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

Compute correct witness point info for multicontact.

Browse Source 
PiperOrigin-RevId: 962132590
Change-Id: I6594dc02ad93b1ec63a32157535f1d95b54d1058
This commit is contained in:
Kyle Bayes
2026-08-10 07:08:40 -07:00
committed by Copybara-Service
parent 0dac756d53
commit ab1af24911
1 changed files with 28 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+28 -18
View File
@@ -159,6 +159,11 @@ static inline void scl3(mjtNum res[3], const mjtNum v[3], mjtNum s) {
res[0] = s*v[0], res[1] = s*v[1], res[2] = s*v[2]; res[0] = s*v[0], res[1] = s*v[1], res[2] = s*v[2];
} }
// res = v1 + s*v2
static inline void addScl3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], mjtNum s) {
res[0] = v1[0] + s*v2[0], res[1] = v1[1] + s*v2[1], res[2] = v1[2] + s*v2[2];
}
// cross product: res = v1 x v2 // cross product: res = v1 x v2
static inline void cross3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { static inline void cross3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) {
res[0] = v1[1]*v2[2] - v1[2]*v2[1]; res[0] = v1[1]*v2[2] - v1[2]*v2[1];
@@ -1618,6 +1623,17 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
} }
// compute witness points on face (given by point p and normal n) from clipped vertex
static inline void witnessOnFace(mjtNum w1[3], mjtNum w2[3], const mjtNum v[3],
const mjtNum* p, const mjtNum n[3], const mjtNum dir[3]) {
mjtNum d[3];
sub3(d, v, p);
mjtNum g = mju_abs(dot3(d, n));
addScl3(w1, v, dir, -g);
copy3(w2, v);
}
// clip a polygon against another polygon // clip a polygon against another polygon
static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1, static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
const mjtNum* face2, int nface2, const mjtNum n[3], const mjtNum* face2, int nface2, const mjtNum n[3],
@@ -1697,8 +1713,7 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
mjtNum* rect[4]; mjtNum* rect[4];
polygonQuad(rect, polygon, npolygon); polygonQuad(rect, polygon, npolygon);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
copy3(status->x2 + 3*i, rect[i]); witnessOnFace(status->x1 + 3*i, status->x2 + 3*i, rect[i], face1, n, dir);
sub3(status->x1 + 3*i, status->x2 + 3*i, dir);
} }
return; return;
} }
@@ -1720,10 +1735,8 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
} }
} }
} }
copy3(status->x2, polygon + 3*best1); witnessOnFace(status->x1, status->x2, polygon + 3*best1, face1, n, dir);
sub3(status->x1, status->x2, dir); witnessOnFace(status->x1 + 3, status->x2 + 3, polygon + 3*best2, face1, n, dir);
copy3(status->x2 + 3, polygon + 3*best2);
sub3(status->x1 + 3, status->x2 + 3, dir);
status->nx = 2; status->nx = 2;
return; return;
} }
@@ -1732,8 +1745,7 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
int maxcon = sizeof(status->x2) / (3*sizeof(status->x2[0])); int maxcon = sizeof(status->x2) / (3*sizeof(status->x2[0]));
npolygon = (npolygon < maxcon) ? npolygon : maxcon; npolygon = (npolygon < maxcon) ? npolygon : maxcon;
for (int i = 0; i < npolygon; i++) { for (int i = 0; i < npolygon; i++) {
copy3(status->x2 + 3*i, polygon + 3*i); witnessOnFace(status->x1 + 3*i, status->x2 + 3*i, polygon + 3*i, face1, n, dir);
sub3(status->x1 + 3*i, status->x2 + 3*i, dir);
} }
status->nx = npolygon; status->nx = npolygon;
} }
@@ -2228,15 +2240,13 @@ static void multicontact(int nmeshdegmax, int npolygonmax, uint8_t* buffer, Poly
} }
} }
// TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the // normal direction for witness recovery
// single contact direction's magnitude. This is effective, but polygonClip should compute mjtNum wit_dir[3];
// this for each contact point.
mjtNum approx_dir[3];
// face1 is an edge; clip face1 against face2 // face1 is an edge; clip face1 against face2
if (edgecon1) { if (edgecon1) {
scl3(approx_dir, n2 + 3*j, -norm3(dir)); scl3(wit_dir, n2 + 3*j, -1.0);
polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir, polygon, npolygonmax); polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, wit_dir, polygon, npolygonmax);
// x1 and x2 must be flipped as we flipped the faces in polygonClip // x1 and x2 must be flipped as we flipped the faces in polygonClip
int nx = status->nx; int nx = status->nx;
for (int k = 0; k < nx; k++) { for (int k = 0; k < nx; k++) {
@@ -2250,14 +2260,14 @@ static void multicontact(int nmeshdegmax, int npolygonmax, uint8_t* buffer, Poly
// face2 is an edge; clip face2 against face1 // face2 is an edge; clip face2 against face1
if (edgecon2) { if (edgecon2) {
scl3(approx_dir, n1 + 3*j, -norm3(dir)); scl3(wit_dir, n1 + 3*j, -1.0);
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir, polygon, npolygonmax); polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, wit_dir, polygon, npolygonmax);
return; return;
} }
// face-face collision // face-face collision
scl3(approx_dir, n2 + 3*j, norm3(dir)); copy3(wit_dir, n2 + 3*j);
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir, polygon, npolygonmax); polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, wit_dir, polygon, npolygonmax);
} }