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

Implement mesh extrema in a 3x3x3 grid corresponding to each feature of a unit cube. These are used as seeds for a better initial point in mesh hill climbing with up to a 2x speedup in mjc_Convex.

Browse Source 
PiperOrigin-RevId: 959645299
Change-Id: I032ae534704e0cc440ddb7691fd21a29a359f521
This commit is contained in:
Kyle Bayes
2026-08-05 07:22:01 -07:00
committed by Copybara-Service
parent 1362a8bded
commit 83e621d771
14 changed files with 2708 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+20 -1
View File
@@ -407,7 +407,24 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
mulMatTVec3(local_dir, mat, dir);
int prev = -1;
int imax = obj->meshindex >= 0 ? obj->meshindex : 0;
int imax;
// map continuous direction to discrete 3x3x3 grid (-1, 0, 1) indices
int cx = (local_dir[0] > 0.4) - (local_dir[0] < -0.4) + 1;
int cy = (local_dir[1] > 0.4) - (local_dir[1] < -0.4) + 1;
int cz = (local_dir[2] > 0.4) - (local_dir[2] < -0.4) + 1;
int grid_idx = obj->data.mesh.extrema[cx*9 + cy*3 + cz];
if (obj->meshindex >= 0) {
// warm start: pick the better of cached vertex vs grid seed
mjtNum cached_dot = dot3f(local_dir, verts + 3*vert_globalid[obj->meshindex]);
mjtNum seed_dot = dot3f(local_dir, verts + 3*vert_globalid[grid_idx]);
imax = (seed_dot > cached_dot) ? grid_idx : obj->meshindex;
} else {
// cold start: use grid seed
imax = grid_idx;
}
mjtNum max = dot3f(local_dir, verts + 3*vert_globalid[imax]);
// hillclimb until no change
@@ -734,9 +751,11 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
polyadr = m->mesh_polyadr[m->geom_dataid[g]];
if (graphadr < 0 || m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
obj->data.mesh.graph = NULL;
obj->data.mesh.extrema = NULL;
obj->support = mjc_meshSupport;
} else {
obj->data.mesh.graph = m->mesh_graph + graphadr;
obj->data.mesh.extrema = m->mesh_extrema + 27 * m->geom_dataid[g];
obj->support = mjc_hillclimbSupport;
}
obj->data.mesh.vert = m->mesh_vert + 3*vertadr;
src/engine/engine_collision_convex.h
+2 -1
View File
@@ -62,7 +62,8 @@ struct _mjCCDObj {
const int* polyvertnum;
const int* polyvert;
const mjtNum* polynormal;
const int*graph;
const int* graph;
const int* extrema;
} mesh;
// hfield prism data