Add constraint island discovery

PiperOrigin-RevId: 557067599
Change-Id: Ic41e1d0efef02b7a79142518afe49cf9d4e74725

src/engine/engine_island.h

@@ -0,0 +1,46 @@
+// Copyright 2023 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
+#define MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_
+
+#include <mujoco/mjdata.h>
+#include <mujoco/mjexport.h>
+#include <mujoco/mjmodel.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+//-------------------------- utility functions -----------------------------------------------------
+
+// construct sparse matrix from non-unique, unsorted edge array, return number of nonzeros
+MJAPI int mj_edge2Sparse(int* rownnz, int* rowadr, int* colind, int* edge, int ne, int nr);
+
+// find disjoint subgraphs ("islands") given sparse symmetric adjacency matrix
+MJAPI int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, const int* colind,
+                       int* scratch);
+
+//-------------------------- top-level API for island construction ---------------------------------
+
+// discover islands:
+//   nisland, island_dofadr, dof_island, dof_islandnext, island_efcadr, efc_island, efc_islandnext
+MJAPI void mj_island(const mjModel* m, mjData* d);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // MUJOCO_SRC_ENGINE_ENGINE_ISLAND_H_