From dcdf0a9c5cc6e116046e7f137138c74cd7b2231a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 13 Aug 2025 07:49:00 -0700 Subject: [PATCH] Use mj_rayMesh for computing an inexact SDF for mesh-SDF collisions. PiperOrigin-RevId: 794576123 Change-Id: I3c688056a5ed617a0787ccf1f4aa683f9409040f --- model/plugin/sdf/asset/die.obj | 296 ++++++++++++++++++++++++++++++ model/plugin/sdf/mesh.xml | 34 ++++ model/plugin/sdf/mug.xml | 2 +- src/engine/engine_collision_sdf.c | 282 +++------------------------- 4 files changed, 356 insertions(+), 258 deletions(-) create mode 100644 model/plugin/sdf/asset/die.obj create mode 100644 model/plugin/sdf/mesh.xml diff --git a/model/plugin/sdf/asset/die.obj b/model/plugin/sdf/asset/die.obj new file mode 100644 index 00000000..91964032 --- /dev/null +++ b/model/plugin/sdf/asset/die.obj @@ -0,0 +1,296 @@ +v 0.740000 0.740000 -1.000000 +v 0.740000 1.000000 -0.740000 +v 1.000000 0.740000 -0.740000 +v 0.740000 0.807269 -0.991147 +v 0.740000 0.870021 -0.965154 +v 0.740000 0.923848 -0.923848 +v 0.807269 0.740000 -0.991147 +v 0.809387 0.809958 -0.980606 +v 0.808254 0.870420 -0.954318 +v 0.806858 0.917583 -0.917748 +v 0.870021 0.740000 -0.965154 +v 0.870490 0.808646 -0.954150 +v 0.863847 0.863908 -0.932116 +v 0.858399 0.903670 -0.903688 +v 0.890111 0.890111 -0.890111 +v 0.807269 0.991147 -0.740000 +v 0.870021 0.965154 -0.740000 +v 0.923848 0.923848 -0.740000 +v 0.740000 0.991147 -0.807269 +v 0.809958 0.980606 -0.809387 +v 0.870420 0.954318 -0.808254 +v 0.917583 0.917748 -0.806858 +v 0.740000 0.965154 -0.870021 +v 0.808646 0.954150 -0.870490 +v 0.863908 0.932116 -0.863847 +v 0.903670 0.903688 -0.858399 +v 0.991147 0.740000 -0.807269 +v 0.965154 0.740000 -0.870021 +v 0.923848 0.740000 -0.923848 +v 0.991147 0.807269 -0.740000 +v 0.980606 0.809387 -0.809958 +v 0.954318 0.808254 -0.870420 +v 0.917748 0.806858 -0.917583 +v 0.965154 0.870021 -0.740000 +v 0.954150 0.870490 -0.808646 +v 0.932116 0.863847 -0.863908 +v 0.903688 0.858399 -0.903670 +v 0.740000 -1.000000 -0.740000 +v 0.740000 -0.740000 -1.000000 +v 1.000000 -0.740000 -0.740000 +v 0.740000 -0.991147 -0.807269 +v 0.740000 -0.965154 -0.870021 +v 0.740000 -0.923848 -0.923848 +v 0.807269 -0.991147 -0.740000 +v 0.809387 -0.980606 -0.809958 +v 0.808254 -0.954318 -0.870420 +v 0.806858 -0.917748 -0.917583 +v 0.870021 -0.965154 -0.740000 +v 0.870490 -0.954150 -0.808646 +v 0.863847 -0.932116 -0.863908 +v 0.858399 -0.903688 -0.903670 +v 0.890111 -0.890111 -0.890111 +v 0.807269 -0.740000 -0.991147 +v 0.870021 -0.740000 -0.965154 +v 0.923848 -0.740000 -0.923848 +v 0.740000 -0.807269 -0.991147 +v 0.809958 -0.809387 -0.980606 +v 0.870420 -0.808254 -0.954318 +v 0.917583 -0.806858 -0.917748 +v 0.740000 -0.870021 -0.965154 +v 0.808646 -0.870490 -0.954150 +v 0.863908 -0.863847 -0.932116 +v 0.903670 -0.858399 -0.903688 +v 0.991147 -0.807269 -0.740000 +v 0.965154 -0.870021 -0.740000 +v 0.923848 -0.923848 -0.740000 +v 0.991147 -0.740000 -0.807269 +v 0.980606 -0.809958 -0.809387 +v 0.954318 -0.870420 -0.808254 +v 0.917748 -0.917583 -0.806858 +v 0.965154 -0.740000 -0.870021 +v 0.954150 -0.808646 -0.870490 +v 0.932116 -0.863908 -0.863847 +v 0.903688 -0.903670 -0.858399 +v 1.000000 0.740000 0.740000 +v 0.740000 1.000000 0.740000 +v 0.740000 0.740000 1.000000 +v 0.991147 0.807269 0.740000 +v 0.965154 0.870021 0.740000 +v 0.923848 0.923848 0.740000 +v 0.991147 0.740000 0.807269 +v 0.980606 0.809958 0.809387 +v 0.954318 0.870420 0.808254 +v 0.917748 0.917583 0.806858 +v 0.965154 0.740000 0.870021 +v 0.954150 0.808646 0.870490 +v 0.932116 0.863908 0.863847 +v 0.903688 0.903670 0.858399 +v 0.890111 0.890111 0.890111 +v 0.740000 0.991147 0.807269 +v 0.740000 0.965154 0.870021 +v 0.740000 0.923848 0.923848 +v 0.807269 0.991147 0.740000 +v 0.809387 0.980606 0.809958 +v 0.808254 0.954318 0.870420 +v 0.806858 0.917748 0.917583 +v 0.870021 0.965154 0.740000 +v 0.870490 0.954150 0.808646 +v 0.863847 0.932116 0.863908 +v 0.858399 0.903688 0.903670 +v 0.807269 0.740000 0.991147 +v 0.870021 0.740000 0.965154 +v 0.923848 0.740000 0.923848 +v 0.740000 0.807269 0.991147 +v 0.809958 0.809387 0.980606 +v 0.870420 0.808254 0.954318 +v 0.917583 0.806858 0.917748 +v 0.740000 0.870021 0.965154 +v 0.808646 0.870490 0.954150 +v 0.863908 0.863847 0.932116 +v 0.903670 0.858399 0.903688 +v 1.000000 -0.740000 0.740000 +v 0.740000 -0.740000 1.000000 +v 0.740000 -1.000000 0.740000 +v 0.991147 -0.740000 0.807269 +v 0.965154 -0.740000 0.870021 +v 0.923848 -0.740000 0.923848 +v 0.991147 -0.807269 0.740000 +v 0.980606 -0.809387 0.809958 +v 0.954318 -0.808254 0.870420 +v 0.917748 -0.806858 0.917583 +v 0.965154 -0.870021 0.740000 +v 0.954150 -0.870490 0.808646 +v 0.932116 -0.863847 0.863908 +v 0.903688 -0.858399 0.903670 +v 0.890111 -0.890111 0.890111 +v 0.740000 -0.807269 0.991147 +v 0.740000 -0.870021 0.965154 +v 0.740000 -0.923848 0.923848 +v 0.807269 -0.740000 0.991147 +v 0.809387 -0.809958 0.980606 +v 0.808254 -0.870420 0.954318 +v 0.806858 -0.917583 0.917748 +v 0.870021 -0.740000 0.965154 +v 0.870490 -0.808646 0.954150 +v 0.863847 -0.863908 0.932116 +v 0.858399 -0.903670 0.903688 +v 0.807269 -0.991147 0.740000 +v 0.870021 -0.965154 0.740000 +v 0.923848 -0.923848 0.740000 +v 0.740000 -0.991147 0.807269 +v 0.809958 -0.980606 0.809387 +v 0.870420 -0.954318 0.808254 +v 0.917583 -0.917748 0.806858 +v 0.740000 -0.965154 0.870021 +v 0.808646 -0.954150 0.870490 +v 0.863908 -0.932116 0.863847 +v 0.903670 -0.903688 0.858399 +v -0.740000 0.740000 -1.000000 +v -1.000000 0.740000 -0.740000 +v -0.740000 1.000000 -0.740000 +v -0.807269 0.740000 -0.991147 +v -0.870021 0.740000 -0.965154 +v -0.923848 0.740000 -0.923848 +v -0.740000 0.807269 -0.991147 +v -0.809958 0.809387 -0.980606 +v -0.870420 0.808254 -0.954318 +v -0.917583 0.806858 -0.917748 +v -0.740000 0.870021 -0.965154 +v -0.808646 0.870490 -0.954150 +v -0.863908 0.863847 -0.932116 +v -0.903670 0.858399 -0.903688 +v -0.890111 0.890111 -0.890111 +v -0.991147 0.807269 -0.740000 +v -0.965154 0.870021 -0.740000 +v -0.923848 0.923848 -0.740000 +v -0.991147 0.740000 -0.807269 +v -0.980606 0.809958 -0.809387 +v -0.954318 0.870420 -0.808254 +v -0.917748 0.917583 -0.806858 +v -0.965154 0.740000 -0.870021 +v -0.954150 0.808646 -0.870490 +v -0.932116 0.863908 -0.863847 +v -0.903688 0.903670 -0.858399 +v -0.740000 0.991147 -0.807269 +v -0.740000 0.965154 -0.870021 +v -0.740000 0.923848 -0.923848 +v -0.807269 0.991147 -0.740000 +v -0.809387 0.980606 -0.809958 +v -0.808254 0.954318 -0.870420 +v -0.806858 0.917748 -0.917583 +v -0.870021 0.965154 -0.740000 +v -0.870490 0.954150 -0.808646 +v -0.863847 0.932116 -0.863908 +v -0.858399 0.903688 -0.903670 +v -1.000000 -0.740000 -0.740000 +v -0.740000 -0.740000 -1.000000 +v -0.740000 -1.000000 -0.740000 +v -0.991147 -0.740000 -0.807269 +v -0.965154 -0.740000 -0.870021 +v -0.923848 -0.740000 -0.923848 +v -0.991147 -0.807269 -0.740000 +v -0.980606 -0.809387 -0.809958 +v -0.954318 -0.808254 -0.870420 +v -0.917748 -0.806858 -0.917583 +v -0.965154 -0.870021 -0.740000 +v -0.954150 -0.870490 -0.808646 +v -0.932116 -0.863847 -0.863908 +v -0.903688 -0.858399 -0.903670 +v -0.890111 -0.890111 -0.890111 +v -0.740000 -0.807269 -0.991147 +v -0.740000 -0.870021 -0.965154 +v -0.740000 -0.923848 -0.923848 +v -0.807269 -0.740000 -0.991147 +v -0.809387 -0.809958 -0.980606 +v -0.808254 -0.870420 -0.954318 +v -0.806858 -0.917583 -0.917748 +v -0.870021 -0.740000 -0.965154 +v -0.870490 -0.808646 -0.954150 +v -0.863847 -0.863908 -0.932116 +v -0.858399 -0.903670 -0.903688 +v -0.807269 -0.991147 -0.740000 +v -0.870021 -0.965154 -0.740000 +v -0.923848 -0.923848 -0.740000 +v -0.740000 -0.991147 -0.807269 +v -0.809958 -0.980606 -0.809387 +v -0.870420 -0.954318 -0.808254 +v -0.917583 -0.917748 -0.806858 +v -0.740000 -0.965154 -0.870021 +v -0.808646 -0.954150 -0.870490 +v -0.863908 -0.932116 -0.863847 +v -0.903670 -0.903688 -0.858399 +v -1.000000 0.740000 0.740000 +v -0.740000 0.740000 1.000000 +v -0.740000 1.000000 0.740000 +v -0.991147 0.740000 0.807269 +v -0.965154 0.740000 0.870021 +v -0.923848 0.740000 0.923848 +v -0.991147 0.807269 0.740000 +v -0.980606 0.809387 0.809958 +v -0.954318 0.808254 0.870420 +v -0.917748 0.806858 0.917583 +v -0.965154 0.870021 0.740000 +v -0.954150 0.870490 0.808646 +v -0.932116 0.863847 0.863908 +v -0.903688 0.858399 0.903670 +v -0.890111 0.890111 0.890111 +v -0.740000 0.807269 0.991147 +v -0.740000 0.870021 0.965154 +v -0.740000 0.923848 0.923848 +v -0.807269 0.740000 0.991147 +v -0.809387 0.809958 0.980606 +v -0.808254 0.870420 0.954318 +v -0.806858 0.917583 0.917748 +v -0.870021 0.740000 0.965154 +v -0.870490 0.808646 0.954150 +v -0.863847 0.863908 0.932116 +v -0.858399 0.903670 0.903688 +v -0.807269 0.991147 0.740000 +v -0.870021 0.965154 0.740000 +v -0.923848 0.923848 0.740000 +v -0.740000 0.991147 0.807269 +v -0.809958 0.980606 0.809387 +v -0.870420 0.954318 0.808254 +v -0.917583 0.917748 0.806858 +v -0.740000 0.965154 0.870021 +v -0.808646 0.954150 0.870490 +v -0.863908 0.932116 0.863847 +v -0.903670 0.903688 0.858399 +v -0.740000 -1.000000 0.740000 +v -0.740000 -0.740000 1.000000 +v -1.000000 -0.740000 0.740000 +v -0.740000 -0.991147 0.807269 +v -0.740000 -0.965154 0.870021 +v -0.740000 -0.923848 0.923848 +v -0.807269 -0.991147 0.740000 +v -0.809387 -0.980606 0.809958 +v -0.808254 -0.954318 0.870420 +v -0.806858 -0.917748 0.917583 +v -0.870021 -0.965154 0.740000 +v -0.870490 -0.954150 0.808646 +v -0.863847 -0.932116 0.863908 +v -0.858399 -0.903688 0.903670 +v -0.890111 -0.890111 0.890111 +v -0.807269 -0.740000 0.991147 +v -0.870021 -0.740000 0.965154 +v -0.923848 -0.740000 0.923848 +v -0.740000 -0.807269 0.991147 +v -0.809958 -0.809387 0.980606 +v -0.870420 -0.808254 0.954318 +v -0.917583 -0.806858 0.917748 +v -0.740000 -0.870021 0.965154 +v -0.808646 -0.870490 0.954150 +v -0.863908 -0.863847 0.932116 +v -0.903670 -0.858399 0.903688 +v -0.991147 -0.807269 0.740000 +v -0.965154 -0.870021 0.740000 +v -0.923848 -0.923848 0.740000 +v -0.991147 -0.740000 0.807269 +v -0.980606 -0.809958 0.809387 +v -0.954318 -0.870420 0.808254 +v -0.917748 -0.917583 0.806858 +v -0.965154 -0.740000 0.870021 +v -0.954150 -0.808646 0.870490 +v -0.932116 -0.863908 0.863847 +v -0.903688 -0.903670 0.858399 diff --git a/model/plugin/sdf/mesh.xml b/model/plugin/sdf/mesh.xml new file mode 100644 index 00000000..2e41f044 --- /dev/null +++ b/model/plugin/sdf/mesh.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + diff --git a/model/plugin/sdf/mug.xml b/model/plugin/sdf/mug.xml index 11fcc891..def6c527 100644 --- a/model/plugin/sdf/mug.xml +++ b/model/plugin/sdf/mug.xml @@ -27,7 +27,7 @@ - + diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index a8ca12f1..aa5b264b 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -14,6 +14,7 @@ #include "engine/engine_collision_sdf.h" +#include #include #include @@ -23,17 +24,13 @@ #include "engine/engine_collision_primitive.h" #include "engine/engine_io.h" #include "engine/engine_plugin.h" -#include "engine/engine_sort.h" +#include "engine/engine_ray.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" -#define MAXSDFFACE 1300 -#define MAXMESHPNT 500 - - //---------------------------- interpolated sdf ------------------------------------------- mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) { @@ -274,7 +271,18 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p return oct_distance(m, x, i); } case mjGEOM_MESH: - return oct_distance(m, x, i); + if (m->mesh_octnum[i]) { + return oct_distance(m, x, i); + } else { + mjtNum dir[3] = {-x[0], -x[1], -x[2]}; + mjtNum r = mju_normalize3(dir); + mjtNum dist = mj_rayMesh(m, d, i, x, dir); + if (dist > r) { + mju_scl3(dir, dir, -1); + return -mj_rayMesh(m, d, i, x, dir); + } + return dist; + } default: mjERROR("sdf collisions not available for geom type %d", type); return 0; @@ -380,7 +388,16 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, } break; case mjGEOM_MESH: - oct_gradient(m, gradient, x, i); + if (m->mesh_octnum[i]) { + oct_gradient(m, gradient, x, i); + } else { + mjtNum dir[3] = {-x[0], -x[1], -x[2]}; + mjtNum r = mju_normalize3(dir); + mjtNum dist = mj_rayMesh(m, d, i, x, dir); + gradient[0] = dist > r ? 1 : -1; + gradient[1] = dist > r ? 1 : -1; + gradient[2] = dist > r ? 1 : -1; + } break; default: mjERROR("sdf collisions not available for geom type %d", type); @@ -493,22 +510,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4], //---------------------------- narrow phase ----------------------------------------------- -// comparison function for contact sorting -static inline int distcmp(int* i, int* j, void* context) { - mjtNum d1 = ((mjtNum*)context)[*i]; - mjtNum d2 = ((mjtNum*)context)[*j]; - if (d1 < d2) { - return -1; - } else if (d1 == d2) { - return 0; - } else { - return 1; - } -} - -// define distSort function for contact sorting -mjSORT(distSort, int, distcmp) - // check if the collision point already exists static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { for (int i = 0; i < cnt; i++) { @@ -548,36 +549,6 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], return cnt+1; } -// finds minimum of Frank-Wolfe objective -static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, - const mjModel* m, const mjSDF* sdf, mjData* d) { - for (int step=0; step < m->opt.sdf_iterations; step++) { - mjtNum best = 1e10, fun, s[3], grad[3]; - - // evaluate gradient - mjc_gradient(m, d, sdf, grad, x); - - // evaluate all corners - for (int i=0; i < ncorners; i++) { - // compute sdf - fun = mju_dot3(corners + 3*i, grad); - - // save argmin - if (fun < best) { - best = fun; - mju_copy3(s, corners + 3*i); - } - } - - // update collision point - mju_subFrom3(s, x); - mju_addToScl3(x, s, 2. / (step+2.)); - } - - // compute distance - return mjc_distance(m, d, sdf, x); -} - // finds minimum using gradient descent static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s, mjData* d, int niter) { @@ -626,129 +597,7 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s, return dist; } -//---------------------------- bounding box vs sdf ------------------------------------------------- -// stricter triangle collision -static int triangleIntersect(const mjtNum triangle[9], const mjModel* m, - const mjSDF* sdf, mjData* d) { - mjtNum edges[6]; - mjtNum normal[3], center[3]; - mjtNum v[9], cross[9], p[3]; - mjtNum kDistanceScl = 10.; - - // triangle normal - mju_sub3(edges+0, triangle+3, triangle); - mju_sub3(edges+3, triangle+6, triangle); - mju_cross(normal, edges, edges+3); - mju_normalize3(normal); - - // fourth point - mju_scl3(p, triangle, 1./3.); - mju_addToScl3(p, triangle+3, 1./3.); - mju_addToScl3(p, triangle+6, 1./3.); - mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl; - mju_addToScl3(p, normal, -h); - - // circumsphere center - mju_sub3(v+0, triangle+0, p); - mju_sub3(v+3, triangle+3, p); - mju_sub3(v+6, triangle+6, p); - mju_cross(cross+0, v+3, v+6); - mju_cross(cross+3, v+6, v+0); - mju_cross(cross+6, v+0, v+3); - mju_scl3(center, cross, mju_dot3(v, v)); - mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3)); - mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6)); - mju_scl3(center, center, 1./(2.*mju_dot3(v, cross))); - - // circumsphere radius - mjtNum r = mju_sqrt(mju_dot3(center, center)); - - // coordinate change - mju_addTo3(center, p); - - return mjc_distance(m, d, sdf, center) < r; -} - -// intersect with circumsphere of bounding box -static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3], - const mjtNum rotation[9], const mjModel* m, - const mjSDF* s, mjData* d) { - mjtNum candidate[3]; - mjtNum r = mju_norm3(bvh+3); - - mju_mulMatVec3(candidate, rotation, bvh); - mju_addTo3(candidate, offset); - - // check if inside the bounding box - return mjc_distance(m, d, s, candidate) < r; -} - -//---------------------------- mesh vs sdf broad phase -------------------------------------------- - -// tree vs sdf binary search -static void collideBVH(const mjModel* m, mjData* d, int g, - const mjtNum offset[3], const mjtNum rotation[9], - int* faces, int* npoints, int* n0, - const mjSDF* sdf) { - const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]]; - const int* faceid = m->bvh_nodeid + bvhadr; - const mjtNum* bvh = m->bvh_aabb + 6*bvhadr; - const int* child = m->bvh_child + 2*bvhadr; - mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL; - - mj_markStack(d); - // TODO(quaglino): Store bvh max depths to make this bound tighter. - int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; - struct CollideTreeArgs_ { - int node; - }; - typedef struct CollideTreeArgs_ CollideTreeArgs; - CollideTreeArgs* stack = mjSTACKALLOC(d, max_stack, CollideTreeArgs); - int nstack = 0; - stack[nstack].node = 0; - nstack++; - - while (nstack) { - (*n0)++; - - // pop from stack - nstack--; - int node = stack[nstack].node; - - // node1 is a leaf - if (faceid[node] != -1) { - if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { - faces[*npoints] = faceid[node]; - if (++(*npoints) == MAXSDFFACE) { - mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed"); - mj_freeStack(d); - return; - } - if (bvh_active) bvh_active[node] = 1; - } - continue; - } - - // if no intersection at intermediate levels, stop - if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { - continue; - } - - if (bvh_active) bvh_active[node] = 1; - - // recursive call - for (int i=0; i < 2; i++) { - if (child[2*node+i] != -1) { - if (nstack >= max_stack) mjERROR("BVH stack depth exceeded."); - stack[nstack].node = child[2*node+i]; - nstack++; - } - } - } - - mj_freeStack(d); -} //------------------------------ collision functions ----------------------------------------------- @@ -760,88 +609,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int // collision between a mesh and a signed distance field int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjGETINFO; - - mjtNum offset[3], rotation[9], corners[9], x[3], depth; - mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT]; - int vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; - int faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; - int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT]; - - // get sdf plugin - int instance = m->geom_plugin[g2]; - const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2); - instance = instance == -1 ? m->geom_dataid[g2] : instance; - mjtGeom geomtype = mjGEOM_SDF; - - // copy into data - mjSDF sdf; - sdf.id = &instance; - sdf.type = mjSDFTYPE_SINGLE; - sdf.plugin = &sdf_ptr; - sdf.geomtype = &geomtype; - - // compute transformation from g1 to g2 - mjtNum sdf_quat[4], quat1[4]; - mju_mat2Quat(quat1, mat1); - mju_mat2Quat(sdf_quat, mat2); - mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation); - - // binary tree search - collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); - - // Frank-Wolfe algorithm - for (int i=0; i < npoints; i++) { - int face = faceadr + faces[i]; - for (int v=0; v < 3; v++) { - mjtNum vec[3] = { - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0], - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1], - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2], - }; - - // transform local 1 (mesh) to local 2 (sdf) - mju_mulMatVec3(corners+3*v, rotation, vec); - mju_addTo3(corners+3*v, offset); - } - - // stricter culling - if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) { - continue; - } - - // starting point - x[0] = (corners[0]+corners[3]+corners[6])/3; - x[1] = (corners[1]+corners[4]+corners[7])/3; - x[2] = (corners[2]+corners[5]+corners[8])/3; - - // SHOULD NOT OCCUR - if (ncandidate == MAXMESHPNT)mjERROR("too many contact points"); - - // Frank-Wolfe - depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); - - // store candidate if there is penetration - if (depth < 0) { - mju_copy3(candidate + 3*ncandidate, x); - index[ncandidate] = ncandidate; - dist[ncandidate++] = depth; - } - } - - // sort contacts using depth - if (ncandidate > 1) { - int buf[MAXMESHPNT]; - distSort(index, buf, ncandidate, dist); - } - - // add only the first mjMAXCONPAIR pairs - for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) { - cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat, - dist[index[i]], cnt, m, &sdf, (mjData*)d); - } - - return cnt; + return mjc_SDF(m, d, con, g1, g2, margin); } // collision between two SDFs