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

Fix bug in computing pos in EPA by replacing S2D with computing affine coordinates directly to recover the witness points.

Browse Source 
PiperOrigin-RevId: 668006152
Change-Id: I1de33c5bb94de1a14523a88a5dff7d6ec984628e
This commit is contained in:
Kyle Bayes
2024-08-27 08:54:52 -07:00
committed by Copybara-Service
parent cad8ca6ffd
commit ddb29e9293
1 changed files with 52 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+52 -14
View File
@@ -906,28 +906,66 @@ static void addEdgeIfUnique(Horizon* h, int v1, int v2) {
// recover witness points from EPA polytope // recover witness points from EPA polytope
static void epa_witness(const Polytope* pt, int index, mjtNum x1[3], mjtNum x2[3]) { static void epa_witness(const Polytope* pt, int index, mjtNum x1[3], mjtNum x2[3]) {
Face* face = &pt->faces[index]; Face* face = &pt->faces[index];
int s1 = face->verts[0], s2 = face->verts[1], s3 = face->verts[2]; int s1i = face->verts[0], s2i = face->verts[1], s3i = face->verts[2];
// run S2D to get barycentric coordinates of witness point // three vertices of face
// witness point is guaranteed to be an internal point of face const mjtNum* s1 = pt->verts[s1i].v;
mjtNum simplex[9], lambda[4]; const mjtNum* s2 = pt->verts[s2i].v;
mju_copy3(simplex, pt->verts[s1].v); const mjtNum* s3 = pt->verts[s3i].v;
mju_copy3(simplex + 3, pt->verts[s2].v); const mjtNum* v = face->v;
mju_copy3(simplex + 6, pt->verts[s3].v);
S2D(lambda, simplex); // compute minors as in S2D
mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1];
mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0];
mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0];
// exclude one of the axes with the largest projection of the simplex using the computed minors
mjtNum M_max = 0;
int x, y;
mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34);
if (mu1 >= mu2 && mu1 >= mu3) {
M_max = M_14;
x = 1;
y = 2;
} else if (mu2 >= mu3) {
M_max = M_24;
x = 0;
y = 2;
} else {
M_max = M_34;
x = 0;
y = 1;
}
// C31 corresponds to the signed area of 2-simplex: (v, s2, s3)
mjtNum C31 = v[x]*s2[y] + v[y]*s3[x] + s2[x]*s3[y]
- v[x]*s3[y] - v[y]*s2[x] - s3[x]*s2[y];
// C32 corresponds to the signed area of 2-simplex: (v, s1, s3)
mjtNum C32 = v[x]*s3[y] + v[y]*s1[x] + s3[x]*s1[y]
- v[x]*s1[y] - v[y]*s3[x] - s1[x]*s3[y];
// C33 corresponds to the signed area of 2-simplex: (v, s1, s2)
mjtNum C33 = v[x]*s1[y] + v[y]*s2[x] + s1[x]*s2[y]
- v[x]*s2[y] - v[y]*s1[x] - s2[x]*s1[y];
// compute affine coordinates
mjtNum lambda[3];
lambda[0] = C31 / M_max;
lambda[1] = C32 / M_max;
lambda[2] = C33 / M_max;
// face on geom 1 // face on geom 1
mjtNum simplex1[9]; mjtNum simplex1[9];
mju_copy3(simplex1, pt->verts[s1].v1); mju_copy3(simplex1, pt->verts[s1i].v1);
mju_copy3(simplex1 + 3, pt->verts[s2].v1); mju_copy3(simplex1 + 3, pt->verts[s2i].v1);
mju_copy3(simplex1 + 6, pt->verts[s3].v1); mju_copy3(simplex1 + 6, pt->verts[s3i].v1);
lincomb(x1, lambda, simplex1, 3); lincomb(x1, lambda, simplex1, 3);
// face on geom 2 // face on geom 2
mjtNum simplex2[9]; mjtNum simplex2[9];
mju_copy3(simplex2, pt->verts[s1].v2); mju_copy3(simplex2, pt->verts[s1i].v2);
mju_copy3(simplex2 + 3, pt->verts[s2].v2); mju_copy3(simplex2 + 3, pt->verts[s2i].v2);
mju_copy3(simplex2 + 6, pt->verts[s3].v2); mju_copy3(simplex2 + 6, pt->verts[s3i].v2);
lincomb(x2, lambda, simplex2, 3); lincomb(x2, lambda, simplex2, 3);
} }