diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 1fac0a77..35211837 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -276,9 +276,9 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { mjtNum local_dir[3], tmp[3]; mulMatTVec3(local_dir, mat, dir); - tmp[0] = mju_sign(local_dir[0]) * size[0]; - tmp[1] = mju_sign(local_dir[1]) * size[1]; - tmp[2] = mju_sign(local_dir[2]) * size[2]; + tmp[0] = (local_dir[0] >= 0 ? 1 : -1) * size[0]; + tmp[1] = (local_dir[1] >= 0 ? 1 : -1) * size[1]; + tmp[2] = (local_dir[2] >= 0 ? 1 : -1) * size[2]; // transform result to global frame localToGlobal(res, mat, tmp, pos); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 59b4380f..3db7aabb 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -147,6 +147,7 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) { // GJK algorithm static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->dist_cutoff > 0; // need to recover geom distances if not in contact + int backup_gjk = !get_dist; // use gjkIntersect if no geom distances needed mjtNum *simplex1 = status->simplex1; // simplex for obj1 mjtNum *simplex2 = status->simplex2; // simplex for obj2 mjtNum *simplex = status->simplex; // simplex in Minkowski difference @@ -203,10 +204,15 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // tetrahedron is generated and only need contact info; fallback to gjkIntersect to // determine contact - if (!get_dist && n == 3) { + if (n == 3 && backup_gjk) { status->gjk_iterations = k; - status->nx = 0; - return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL; + int ret = gjkIntersect(status, obj1, obj2); + if (ret != -1) { + status->nx = 0; + return ret > 0 ? 0 : mjMAXVAL; + } + k = status->gjk_iterations; + backup_gjk = 0; } // run the distance subalgorithm to compute the barycentric coordinates @@ -364,6 +370,12 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { dist[2] = signedDistance(&normals[6], simplex + s[1], simplex + s[0], simplex + s[3]); dist[3] = signedDistance(&normals[9], simplex + s[0], simplex + s[1], simplex + s[2]); + // if origin is on any affine hull, convergence will fail + if (!dist[3] || !dist[2] || !dist[1] || !dist[0]) { + status->gjk_iterations = k; + return -1; + } + // find the face with the smallest distance to the origin int i = (dist[0] < dist[1]) ? 0 : 1; int j = (dist[2] < dist[3]) ? 2 : 3;