chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update engine_island.c

Browse Source 
Signed-off-by: teerthsharma <teerths57@gmail.com>
This commit is contained in:
teerthsharma
2026-07-11 11:50:50 +05:30
committed by teerthsharma
parent 52ddcbc81a
commit 03203e2f32
1 changed files with 10 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_island.c
+10 -16
View File
@@ -130,8 +130,9 @@ static void dsuUnion(int* parent, int tree1, int tree2) {
// assign deterministic island ids in ascending canonical-root order // assign deterministic island ids in ascending canonical-root order
static int dsuAssign(int* island, int* parent, int ntree) { static int dsuAssign(int* island, int* parent, const int* tree_dofnum, int ntree, int* nidof) {
int nisland = 0; int nisland = 0;
*nidof = 0;
for (int tree=0; tree < ntree; tree++) { for (int tree=0; tree < ntree; tree++) {
if (parent[tree] == -1) { if (parent[tree] == -1) {
island[tree] = -1; island[tree] = -1;
@@ -140,6 +141,7 @@ static int dsuAssign(int* island, int* parent, int ntree) {
int root = dsuFind(parent, tree); int root = dsuFind(parent, tree);
island[tree] = root == tree ? nisland++ : island[root]; island[tree] = root == tree ? nisland++ : island[root];
*nidof += tree_dofnum[tree];
} }
return nisland; return nisland;
@@ -445,7 +447,8 @@ void mj_island(const mjModel* m, mjData* d) {
dsuInit(parent, ntree); dsuInit(parent, ntree);
unionConstraintTrees(m, d, parent); unionConstraintTrees(m, d, parent);
int* tree_island = mjSTACKALLOC(d, ntree, int); int* tree_island = mjSTACKALLOC(d, ntree, int);
d->nisland = dsuAssign(tree_island, parent, ntree); int nidof;
d->nisland = dsuAssign(tree_island, parent, m->tree_dofnum, ntree, &nidof);
// no islands found: quick return // no islands found: quick return
if (!d->nisland) { if (!d->nisland) {
@@ -454,13 +457,6 @@ void mj_island(const mjModel* m, mjData* d) {
return; return;
} }
// count nidof: total number of dofs in islands
int nidof = 0;
for (int i=0; i < ntree; i++) {
if (tree_island[i] >= 0) {
nidof += m->tree_dofnum[i];
}
}
d->nidof = nidof; d->nidof = nidof;
// allocate island arrays on arena // allocate island arrays on arena
@@ -599,17 +595,15 @@ void mj_island(const mjModel* m, mjData* d) {
int ic = d->island_iefcadr[island] + island_nefc2[island]++; int ic = d->island_iefcadr[island] + island_nefc2[island]++;
d->map_efc2iefc[c] = ic; d->map_efc2iefc[c] = ic;
d->map_iefc2efc[ic] = c; d->map_iefc2efc[ic] = c;
d->iefc_type[ic] = d->efc_type[c];
d->iefc_id[ic] = d->efc_id[c];
d->iefc_frictionloss[ic] = d->efc_frictionloss[c];
d->iefc_D[ic] = d->efc_D[c];
d->iefc_R[ic] = d->efc_R[c];
} }
// SHOULD NOT OCCUR // SHOULD NOT OCCUR
if (!mju_compare(island_nefc2, d->island_nefc, nisland)) mjERROR("island_nefc miscount"); if (!mju_compare(island_nefc2, d->island_nefc, nisland)) mjERROR("island_nefc miscount");
// copy position-dependent efc vectors required by solver
mju_gatherInt(d->iefc_type, d->efc_type, d->map_iefc2efc, nefc);
mju_gatherInt(d->iefc_id, d->efc_id, d->map_iefc2efc, nefc);
mju_gather(d->iefc_frictionloss, d->efc_frictionloss, d->map_iefc2efc, nefc);
mju_gather(d->iefc_D, d->efc_D, d->map_iefc2efc, nefc);
mju_gather(d->iefc_R, d->efc_R, d->map_iefc2efc, nefc);
mj_freeStack(d); mj_freeStack(d);
} }