From 61721f8d3c9972cc859029c99023797197a4b7ee Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Sun, 8 Sep 2024 13:48:55 -0700 Subject: [PATCH] Set tolerance in GJK to zero for colliding discrete geoms. PiperOrigin-RevId: 672326091 Change-Id: Iddec335272461c9aee13c1960640a453ff59f841 --- src/engine/engine_collision_gjk.c | 30 +++++++++++++- src/engine/engine_util_blas.c | 69 +++++++++++++++++-------------- src/engine/engine_util_blas.h | 5 ++- 3 files changed, 72 insertions(+), 32 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 058027ee..a9b657c2 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -97,6 +97,22 @@ static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* o +// returns true if both geoms are discrete shapes (i.e. meshes or boxes) +static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { + // non-zero margin makes geoms smooth + if (obj1->margin != 0 || obj2->margin != 0) return 0; + + // negative geom indices correspond to flex objects, return + if (obj1->geom < 0 || obj2->geom < 0) return 0; + + int g1 = obj1->model->geom_type[obj1->geom]; + int g2 = obj2->model->geom_type[obj2->geom]; + return (g1 == mjGEOM_MESH || g1 == mjGEOM_BOX) && + (g2 == mjGEOM_MESH || g2 == mjGEOM_BOX); +} + + + // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions @@ -110,6 +126,11 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mju_sub3(x_k, x1_k, x2_k); mjtNum epsilon = status->tolerance * status->tolerance; +// if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0 + if (discreteGeoms(obj1, obj2)) { + epsilon = 0; + } + int k = 0, N = status->max_iterations; for (; k < N; k++) { mjtNum s1[3], s2[3]; // the support points in obj1 and obj2 @@ -146,8 +167,9 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // run the distance subalgorithm to compute the barycentric coordinates // of the closest point to the origin in the simplex + mjtNum tmp[3]; signedVolume(lambda, simplex, ++n); - lincomb(x_k, lambda, simplex, 4); + lincomb(tmp, lambda, simplex, 4); // compute the approximate witness points lincomb(x1_k, lambda, simplex1, 4); @@ -165,6 +187,12 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mju_copy3(simplex + 3*n++, simplex + 3*i); } + // x_k has converged to minimum + if (mju_equal3(tmp, x_k)) { + break; + } + mju_copy3(x_k, tmp); + // we have a tetrahedron containing the origin so return early if (n == 4) { break; diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 77ed4194..af1a0447 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -38,6 +38,15 @@ void mju_zero3(mjtNum res[3]) { +// vec1 == vec2 +int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]) { + return mju_abs(vec1[0] - vec2[0]) < mjMINVAL && + mju_abs(vec1[1] - vec2[1]) < mjMINVAL && + mju_abs(vec1[2] - vec2[2]) < mjMINVAL; +} + + + // res = vec void mju_copy3(mjtNum res[3], const mjtNum data[3]) { res[0] = data[0]; @@ -195,46 +204,46 @@ void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) { // multiply 3x3 matrices, -void mju_mulMatMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) { - res[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6]; - res[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7]; - res[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8]; - res[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6]; - res[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7]; - res[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8]; - res[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6]; - res[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7]; - res[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8]; +void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { + res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[3] + mat1[2]*mat2[6]; + res[1] = mat1[0]*mat2[1] + mat1[1]*mat2[4] + mat1[2]*mat2[7]; + res[2] = mat1[0]*mat2[2] + mat1[1]*mat2[5] + mat1[2]*mat2[8]; + res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[3] + mat1[5]*mat2[6]; + res[4] = mat1[3]*mat2[1] + mat1[4]*mat2[4] + mat1[5]*mat2[7]; + res[5] = mat1[3]*mat2[2] + mat1[4]*mat2[5] + mat1[5]*mat2[8]; + res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[3] + mat1[8]*mat2[6]; + res[7] = mat1[6]*mat2[1] + mat1[7]*mat2[4] + mat1[8]*mat2[7]; + res[8] = mat1[6]*mat2[2] + mat1[7]*mat2[5] + mat1[8]*mat2[8]; } // multiply 3x3 matrices, first argument transposed -void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) { - res[0] = a[0]*b[0] + a[3]*b[3] + a[6]*b[6]; - res[1] = a[0]*b[1] + a[3]*b[4] + a[6]*b[7]; - res[2] = a[0]*b[2] + a[3]*b[5] + a[6]*b[8]; - res[3] = a[1]*b[0] + a[4]*b[3] + a[7]*b[6]; - res[4] = a[1]*b[1] + a[4]*b[4] + a[7]*b[7]; - res[5] = a[1]*b[2] + a[4]*b[5] + a[7]*b[8]; - res[6] = a[2]*b[0] + a[5]*b[3] + a[8]*b[6]; - res[7] = a[2]*b[1] + a[5]*b[4] + a[8]*b[7]; - res[8] = a[2]*b[2] + a[5]*b[5] + a[8]*b[8]; +void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { + res[0] = mat1[0]*mat2[0] + mat1[3]*mat2[3] + mat1[6]*mat2[6]; + res[1] = mat1[0]*mat2[1] + mat1[3]*mat2[4] + mat1[6]*mat2[7]; + res[2] = mat1[0]*mat2[2] + mat1[3]*mat2[5] + mat1[6]*mat2[8]; + res[3] = mat1[1]*mat2[0] + mat1[4]*mat2[3] + mat1[7]*mat2[6]; + res[4] = mat1[1]*mat2[1] + mat1[4]*mat2[4] + mat1[7]*mat2[7]; + res[5] = mat1[1]*mat2[2] + mat1[4]*mat2[5] + mat1[7]*mat2[8]; + res[6] = mat1[2]*mat2[0] + mat1[5]*mat2[3] + mat1[8]*mat2[6]; + res[7] = mat1[2]*mat2[1] + mat1[5]*mat2[4] + mat1[8]*mat2[7]; + res[8] = mat1[2]*mat2[2] + mat1[5]*mat2[5] + mat1[8]*mat2[8]; } // multiply 3x3 matrices, second argument transposed -void mju_mulMatMatT3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]) { - res[0] = a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; - res[1] = a[0]*b[3] + a[1]*b[4] + a[2]*b[5]; - res[2] = a[0]*b[6] + a[1]*b[7] + a[2]*b[8]; - res[3] = a[3]*b[0] + a[4]*b[1] + a[5]*b[2]; - res[4] = a[3]*b[3] + a[4]*b[4] + a[5]*b[5]; - res[5] = a[3]*b[6] + a[4]*b[7] + a[5]*b[8]; - res[6] = a[6]*b[0] + a[7]*b[1] + a[8]*b[2]; - res[7] = a[6]*b[3] + a[7]*b[4] + a[8]*b[5]; - res[8] = a[6]*b[6] + a[7]*b[7] + a[8]*b[8]; +void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]) { + res[0] = mat1[0]*mat2[0] + mat1[1]*mat2[1] + mat1[2]*mat2[2]; + res[1] = mat1[0]*mat2[3] + mat1[1]*mat2[4] + mat1[2]*mat2[5]; + res[2] = mat1[0]*mat2[6] + mat1[1]*mat2[7] + mat1[2]*mat2[8]; + res[3] = mat1[3]*mat2[0] + mat1[4]*mat2[1] + mat1[5]*mat2[2]; + res[4] = mat1[3]*mat2[3] + mat1[4]*mat2[4] + mat1[5]*mat2[5]; + res[5] = mat1[3]*mat2[6] + mat1[4]*mat2[7] + mat1[5]*mat2[8]; + res[6] = mat1[6]*mat2[0] + mat1[7]*mat2[1] + mat1[8]*mat2[2]; + res[7] = mat1[6]*mat2[3] + mat1[7]*mat2[4] + mat1[8]*mat2[5]; + res[8] = mat1[6]*mat2[6] + mat1[7]*mat2[7] + mat1[8]*mat2[8]; } diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index 882c9783..77f9a6a7 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -67,6 +67,9 @@ extern "C" { // res = 0 MJAPI void mju_zero3(mjtNum res[3]); +// vec1 == vec2 +MJAPI int mju_equal3(const mjtNum vec1[3], const mjtNum vec2[3]); + // res = vec MJAPI void mju_copy3(mjtNum res[3], const mjtNum data[3]); @@ -119,7 +122,7 @@ MJAPI void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9 MJAPI void mju_mulMatMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]); // multiply 3x3 matrices, first argument transposed -MJAPI void mju_mulMatTMat3(mjtNum res[9], const mjtNum a[9], const mjtNum b[9]); +MJAPI void mju_mulMatTMat3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]); // multiply 3x3 matrices, second argument transposed MJAPI void mju_mulMatMatT3(mjtNum res[9], const mjtNum mat1[9], const mjtNum mat2[9]);