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
static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) {
// recover witness points from EPA polytope, return signed distance between witness points
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
mjtNum lambda[3];
mjtNum* v1 = pt->verts[face->verts[0]].vert;
mjtNum* v2 = pt->verts[face->verts[1]].vert;
mjtNum* v3 = pt->verts[face->verts[2]].vert;
triAffineCoord(lambda, v1, v2, v3, face->v);
Vertex* v1 = pt->verts + face->verts[0];
Vertex* v2 = pt->verts + face->verts[1];
Vertex* v3 = pt->verts + face->verts[2];
triAffineCoord(lambda, v1->vert, v2->vert, v3->vert, face->v);
// face on geom 1
v1 = pt->verts[face->verts[0]].vert1;
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];
// witness point on geom 1
lincomb(x1, lambda, 3, v1->vert1, v2->vert1, v3->vert1, NULL);
// face on geom 2
v1 = pt->verts[face->verts[0]].vert2;
v2 = pt->verts[face->verts[1]].vert2;
v3 = pt->verts[face->verts[2]].vert2;
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];
// witness point on geom 2
lincomb(x2, lambda, 3, v1->vert2, v2->vert2, v3->vert2, NULL);
return -mju_sqrt(face->dist2);
}
@@ -1434,9 +1426,8 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
status->epa_iterations = k;
if (face) {
epaWitness(pt, face, status->x1, status->x2);
status->dist = epaWitness(pt, face, status->x1, status->x2);
status->nx = 1;
status->dist = -mju_sqrt(face->dist2);
} else {
status->nx = 0;
status->dist = 0;