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

Refactor epaWitness.

Browse Source 
PiperOrigin-RevId: 836597852
Change-Id: Ib75dae07bda47764f937f32a9476ef3266f02056
This commit is contained in:
Kyle Bayes
2025-11-25 03:43:06 -08:00
committed by Copybara-Service
parent 9b0f8dd963
commit b98379372e
1 changed files with 13 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+13 -22
View File
@@ -1269,30 +1269,22 @@ static void horizon(Polytope* pt, Face* face) {
} }
// recover witness points from EPA polytope // recover witness points from EPA polytope, return signed distance between witness points
static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) { static mjtNum epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) {
// compute affine coordinates for witness points on plane defined by face // compute affine coordinates for witness points on plane defined by face
mjtNum lambda[3]; mjtNum lambda[3];
mjtNum* v1 = pt->verts[face->verts[0]].vert; Vertex* v1 = pt->verts + face->verts[0];
mjtNum* v2 = pt->verts[face->verts[1]].vert; Vertex* v2 = pt->verts + face->verts[1];
mjtNum* v3 = pt->verts[face->verts[2]].vert; Vertex* v3 = pt->verts + face->verts[2];
triAffineCoord(lambda, v1, v2, v3, face->v); triAffineCoord(lambda, v1->vert, v2->vert, v3->vert, face->v);
// face on geom 1 // witness point on geom 1
v1 = pt->verts[face->verts[0]].vert1; lincomb(x1, lambda, 3, v1->vert1, v2->vert1, v3->vert1, NULL);
v2 = pt->verts[face->verts[1]].vert1;
v3 = pt->verts[face->verts[2]].vert1;
x1[0] = v1[0]*lambda[0] + v2[0]*lambda[1] + v3[0]*lambda[2];
x1[1] = v1[1]*lambda[0] + v2[1]*lambda[1] + v3[1]*lambda[2];
x1[2] = v1[2]*lambda[0] + v2[2]*lambda[1] + v3[2]*lambda[2];
// face on geom 2 // witness point on geom 2
v1 = pt->verts[face->verts[0]].vert2; lincomb(x2, lambda, 3, v1->vert2, v2->vert2, v3->vert2, NULL);
v2 = pt->verts[face->verts[1]].vert2;
v3 = pt->verts[face->verts[2]].vert2; return -mju_sqrt(face->dist2);
x2[0] = v1[0]*lambda[0] + v2[0]*lambda[1] + v3[0]*lambda[2];
x2[1] = v1[1]*lambda[0] + v2[1]*lambda[1] + v3[1]*lambda[2];
x2[2] = v1[2]*lambda[0] + v2[2]*lambda[1] + v3[2]*lambda[2];
} }
@@ -1434,9 +1426,8 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
status->epa_iterations = k; status->epa_iterations = k;
if (face) { if (face) {
epaWitness(pt, face, status->x1, status->x2); status->dist = epaWitness(pt, face, status->x1, status->x2);
status->nx = 1; status->nx = 1;
status->dist = -mju_sqrt(face->dist2);
} else { } else {
status->nx = 0; status->nx = 0;
status->dist = 0; status->dist = 0;