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

Remove unnecessary norm calculation in EPA.

Browse Source 
PiperOrigin-RevId: 681906350
Change-Id: Ib6a13ca170a8071adde0771c6ba2b38fb4fd4da4
This commit is contained in:
Kyle Bayes
2024-10-03 09:11:48 -07:00
committed by Copybara-Service
parent c7c2edf758
commit dc995ad341
1 changed files with 27 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_gjk.c
+27 -14
View File
@@ -38,7 +38,8 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]);
// helper function to compute the support point in the Minkowski difference
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3]);
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum d[3], mjtNum dnorm);
// support function tweaked for GJK by taking kth iteration point as input and setting both
// support points to recover witness points
@@ -203,11 +204,22 @@ static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj
// helper function to compute the support point in the Minkowski difference
static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum d[3]) {
const mjtNum d[3], mjtNum dnorm) {
mjtNum dir[3], dir_neg[3];
mju_copy3(dir, d);
mju_normalize3(dir); // mjc_support assumes a normalized direction
mju_scl3(dir_neg, dir, -1);
// mjc_support assumes a normalized direction
if (dnorm < mjMINVAL) {
dir[0] = 1, dir_neg[0] = -1;
dir[1] = 0, dir_neg[1] = 0;
dir[2] = 0, dir_neg[2] = 0;
} else {
dir[0] = d[0] / dnorm;
dir[1] = d[1] / dnorm;
dir[2] = d[2] / dnorm;
dir_neg[0] = -dir[0];
dir_neg[1] = -dir[1];
dir_neg[2] = -dir[2];
}
// compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
obj1->support(s1, obj1, dir);
@@ -675,15 +687,15 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
mjtNum v3a[3], v3b[3], v3[3];
support(v3a, v3b, obj1, obj2, d1);
support(v3a, v3b, obj1, obj2, d1, mju_norm3(d1));
mju_sub3(v3, v3a, v3b);
mjtNum v4a[3], v4b[3], v4[3];
support(v4a, v4b, obj1, obj2, d2);
support(v4a, v4b, obj1, obj2, d2, mju_norm3(d2));
mju_sub3(v4, v4a, v4b);
mjtNum v5a[3], v5b[3], v5[3];
support(v5a, v5b, obj1, obj2, d3);
support(v5a, v5b, obj1, obj2, d3, mju_norm3(d3));
mju_sub3(v5, v5a, v5b);
// check that all six faces are valid triangles (not collinear)
@@ -796,20 +808,21 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
mju_sub3(v3, status->simplex1 + 6, status->simplex2 + 6);
// get normals in both directions
mjtNum diff1[3], diff2[3], n[3], nn[3];
mjtNum diff1[3], diff2[3], n[3], n_neg[3];
mju_sub3(diff1, v2, v1);
mju_sub3(diff2, v3, v1);
mju_cross(n, diff1, diff2);
if (mju_norm3(n) < mjMINVAL) {
mjtNum n_norm = mju_norm3(n);
if (n_norm < mjMINVAL) {
return 0;
}
// negative of triangle normal n
mju_scl3(nn, n, -1);
mju_scl3(n_neg, n, -1);
// get 4th vertex in n direction
mjtNum v4a[3], v4b[3], v4[3];
support(v4a, v4b, obj1, obj2, n);
support(v4a, v4b, obj1, obj2, n, n_norm);
mju_sub3(v4, v4a, v4b);
// check that v4 is not contained in the 2-simplex
@@ -819,7 +832,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// get 5th vertex in -n direction
mjtNum v5a[3], v5b[3], v5[3];
support(v5a, v5b, obj1, obj2, nn);
support(v5a, v5b, obj1, obj2, n_neg, n_norm);
mju_sub3(v5, v5a, v5b);
// check that v5 is not contained in the 2-simplex
@@ -1141,7 +1154,7 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o
// compute support point w from the closest face's normal
mjtNum w1[3], w2[3], w[3];
support(w1, w2, obj1, obj2, face->v);
support(w1, w2, obj1, obj2, face->v, dist);
mju_sub3(w, w1, w2);
mjtNum next_dist = mju_dot3(face->v, w) / dist;
if (next_dist - dist < tolerance) {