From ca485f34ac5a309f6a4632e0cce6bfc400a82c57 Mon Sep 17 00:00:00 2001 From: Alessio Date: Wed, 5 Aug 2026 13:21:13 +0100 Subject: [PATCH] Add continuous-collision infrastructure for deformables The discrete collision pipeline generates contact points at a configuration; this module prices gaps along trajectories: differentiable vertex-triangle, edge-edge and vertex-geom distance kernels with closest-point barycentrics, swept-volume candidate generation over the flex bounding-volume hierarchy, per-pair gap evaluation with the gradient's vertex weights, and a conservative advancement that bounds each contact pair's time of impact. Engine-internal, with no consumer in this change: it is the groundwork for continuous-contact (IPC-style) solvers for flex, which will arrive as callers. The mjcPair type carries the geometric identity of a candidate pair only; solver state (multipliers, ages) and cached linearizations belong to the consumer. The two lengths the module needs -- the standoff cap and the detection band -- are caller-supplied parameters, not constants. Flex-flex pairs measure their gap at the midsurface rather than skin-to-skin: where mesh geometry is tighter than the combined radii (a string threaded through a hem) a skin gap is permanently negative and the pair would be discarded as invalid, losing CCD coverage exactly where tunneling is likeliest. The broad phase adds the radii back into its reach, so detection range is unchanged. Tests cover the distance kernels, the geom sharp features, the pair gap with its gradient checked by central differences at every involved vertex, the conservative advancement (the analytic cap on a crossing sweep, the conservative closing-rate bound, the small-motion early-out), and candidate generation on stacked cloths (pairs within reach found, distant ones not). --- doc/changelog.rst | 7 + src/engine/CMakeLists.txt | 2 + src/engine/engine_collision_continuous.c | 959 ++++++++++++++++++ src/engine/engine_collision_continuous.h | 115 +++ test/engine/CMakeLists.txt | 2 + .../engine_collision_continuous_test.cc | 363 +++++++ 6 files changed, 1448 insertions(+) create mode 100644 src/engine/engine_collision_continuous.c create mode 100644 src/engine/engine_collision_continuous.h create mode 100644 test/engine/engine_collision_continuous_test.cc diff --git a/doc/changelog.rst b/doc/changelog.rst index e42fc33cd62..6eb378f0d1f 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -34,6 +34,13 @@ Actuation Engine ^^^^^^ +- Added continuous-collision infrastructure for deformables (``engine_collision_continuous``): differentiable + vertex-triangle, edge-edge and vertex-geom distance kernels with closest-point barycentrics, swept-volume contact + candidate generation over the flex bounding-volume hierarchy, per-pair gap evaluation with the gradient's vertex + weights, and a conservative advancement that bounds each pair's time of impact. Engine-internal with no consumer in + this change: the discrete pipeline generates contact points at a configuration, this module prices gaps along + trajectories, and it is the groundwork for continuous-contact (IPC-style) solvers for flex. + - Replaced the per-step sparse Cholesky factorization of the flex block of the implicit effective metric M + K with its prefactored per-vertex 3x3 diagonal blocks. The blocks precondition the CG constraint solver and drive an iterative solve for ``qacc_smooth``, which now converges on :ref:`tolerance` rather than a fixed diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index baa23887a1d..f29664bbe82 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -17,6 +17,8 @@ set(MUJOCO_ENGINE_SRCS engine_callback.c engine_callback.h engine_collision_box.c + engine_collision_continuous.c + engine_collision_continuous.h engine_collision_convex.c engine_collision_convex.h engine_collision_driver.c diff --git a/src/engine/engine_collision_continuous.c b/src/engine/engine_collision_continuous.c new file mode 100644 index 00000000000..7cafcec92f0 --- /dev/null +++ b/src/engine/engine_collision_continuous.c @@ -0,0 +1,959 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Continuous-collision geometry for deformables; see the header for scope. + +#include "engine/engine_collision_continuous.h" + +#include +#include + +#include +#include +#include +#include "engine/engine_core_util.h" // mj_local2Global (anchor body-local -> world) +#include "engine/engine_util_blas.h" // mju_dot3, mju_mulMatVec3 +#include "engine/engine_util_spatial.h" // mju_cross +#include "engine/engine_util_errmem.h" // mju_malloc, mju_free + +static inline mjtNum min2(mjtNum a, mjtNum b) { return a < b ? a : b; } + +// point-triangle: distance, closest point cp, barycentric weights w of cp (for the barrier +// gradient). +// TODO(consolidation): same closest-point-on-triangle math as mjraw_SphereTriangle +// (engine_collision_primitive.c) and GJK's S2D -- NOT a duplicate by accident. The barrier +// gradient/Hessian needs the BARYCENTRIC weights w (to spread the reaction onto the 3 verts), which +// mjraw_SphereTriangle never forms (it finds the closest 3D point in a rotated frame, no w), and it +// early-exits on margin so it can't return the unconditional signed distance this module uses in +// broadphase/CCD. Reuse = factor the core out of mjraw_SphereTriangle AND add a barycentric output +// (a collision-core change), not a drop-in arg add; deferred (regression-risky, small payoff). +mjtNum mjc_PtTri(const mjtNum* p, const mjtNum* a, const mjtNum* b, const mjtNum* c, mjtNum* cp, + mjtNum* w) { + mjtNum ab[3], ac[3], ap[3]; + for (int k=0; k < 3; k++) { + ab[k] = b[k] - a[k]; + ac[k] = c[k] - a[k]; + ap[k] = p[k] - a[k]; + } + mjtNum d1 = mju_dot3(ab, ap), d2 = mju_dot3(ac, ap); + if (d1 <= 0 && d2 <= 0) { + w[0] = 1; + w[1] = 0; + w[2] = 0; + } else { + mjtNum bp[3]; + for (int k=0; k < 3; k++) bp[k] = p[k] - b[k]; + mjtNum d3 = mju_dot3(ab, bp), d4 = mju_dot3(ac, bp); + if (d3 >= 0 && d4 <= d3) { + w[0] = 0; + w[1] = 1; + w[2] = 0; + } else { + mjtNum vc = d1 * d4 - d3 * d2; + if (vc <= 0 && d1 >= 0 && d3 <= 0) { + mjtNum t = d1 / (d1 - d3); + w[0] = 1 - t; + w[1] = t; + w[2] = 0; + } else { + mjtNum cq[3]; + for (int k=0; k < 3; k++) cq[k] = p[k] - c[k]; + mjtNum d5 = mju_dot3(ab, cq), d6 = mju_dot3(ac, cq); + if (d6 >= 0 && d5 <= d6) { + w[0] = 0; + w[1] = 0; + w[2] = 1; + } else { + mjtNum vb = d5 * d2 - d1 * d6; + if (vb <= 0 && d2 >= 0 && d6 <= 0) { + mjtNum t = d2 / (d2 - d6); + w[0] = 1 - t; + w[1] = 0; + w[2] = t; + } else { + mjtNum va = d3 * d6 - d5 * d4; + if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { + mjtNum t = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + w[0] = 0; + w[1] = 1 - t; + w[2] = t; + } else { + mjtNum den = 1.0 / (va + vb + vc), t = vb * den, u = vc * den; + w[0] = 1 - t - u; + w[1] = t; + w[2] = u; + } + } + } + } + } + } + for (int k=0; k < 3; k++) cp[k] = w[0] * a[k] + w[1] * b[k] + w[2] * c[k]; + mjtNum dd[3]; + for (int k=0; k < 3; k++) dd[k] = p[k] - cp[k]; + return sqrt(mju_dot3(dd, dd)); +} + +// closest distance between segment p1p2 and segment q1q2; closest points cp1, cp2 and the segment +// parameters st = {s, t} (cp1 = p1+s*(p2-p1), cp2 = q1+t*(q2-q1)). +// TODO(consolidation): same segment-segment math as mjraw_CapsuleCapsule's NON-parallel branch +// (engine_collision_primitive.c:442-470, identical x1/x2 clip). Kept separate because contact-gen returns +// (s,t) (mjraw_ discards them into mjraw_SphereSphere), the PARALLEL branch diverges (mjraw_ emits +// up to TWO contacts at the overlap ends -- a contact-gen stability trick -- vs the one pair + +// mollifier), and mjraw_ early-exits on margin. Reuse needs a non-static closest-point primitive +// factored out, not an arg add; deferred. +// TODO(mollifier): no parallel-edge mollifier yet -- the near-parallel branch falls back to s=0 +// (fine for non-parallel crossings) but gives a DISCONTINUOUS edge-edge gap gradient the Newton +// step feels. Add the Li-et-al EE mollifier (smooth vanish as |e1 x e2| -> 0) if/when near-parallel +// EE contacts (folded/stacked cloth) start to bite. +mjtNum mjc_SegSeg(const mjtNum* p1, const mjtNum* p2, const mjtNum* q1, const mjtNum* q2, + mjtNum* cp1, mjtNum* cp2, mjtNum* st) { + mjtNum d1[3], d2[3], rr[3]; + for (int k=0; k < 3; k++) { + d1[k] = p2[k] - p1[k]; + d2[k] = q2[k] - q1[k]; + rr[k] = p1[k] - q1[k]; + } + mjtNum a = mju_dot3(d1, d1), e = mju_dot3(d2, d2), fq = mju_dot3(d2, rr); + mjtNum s, t; + if (a <= 1e-12 && e <= 1e-12) { + s = 0; + t = 0; + } else if (a <= 1e-12) { + s = 0; + t = fq / e; + } else { + mjtNum c = mju_dot3(d1, rr); + if (e <= 1e-12) { + t = 0; + s = -c / a; + } else { + mjtNum b = mju_dot3(d1, d2), den = a * e - b * b; + s = (den > 1e-12) ? (b * fq - c * e) / den + : 0.0; // parallel: s=0 (mollifier territory, deferred) + s = (s < 0) ? 0 : (s > 1 ? 1 : s); + t = (b * s + fq) / e; + if (t < 0) { + t = 0; + s = -c / a; + } else if (t > 1) { + t = 1; + s = (b - c) / a; + } + } + } + s = (s < 0) ? 0 : (s > 1 ? 1 : s); + t = (t < 0) ? 0 : (t > 1 ? 1 : t); + st[0] = s; + st[1] = t; + for (int k=0; k < 3; k++) { + cp1[k] = p1[k] + s * d1[k]; + cp2[k] = q1[k] + t * d2[k]; + } + mjtNum dd[3]; + for (int k=0; k < 3; k++) dd[k] = cp1[k] - cp2[k]; + return sqrt(mju_dot3(dd, dd)); +} + +// closest point (out) on a convex polygon face to point p: project onto the face plane; if the +// projection is inside the polygon use it, else clamp to the nearest boundary edge. pv = the face's +// mesh-local vertex indices (nv of them) into the vertex array vbase; nrm = the face's plane +// normal. Helper for mjc_GeomDist's convex-mesh SDF branch; the engine has no +// point-on-convex-face util. +static void closestOnPoly(const mjtNum* p, const float* vbase, const int* pv, int nv, + const mjtNum* nrm, mjtNum* out) { + const float* a0 = vbase + 3 * pv[0]; + mjtNum dpl = nrm[0] * (p[0] - a0[0]) + nrm[1] * (p[1] - a0[1]) + nrm[2] * (p[2] - a0[2]); + mjtNum pp[3]; + for (int k=0; k < 3; k++) pp[k] = p[k] - dpl * nrm[k]; // projection onto face plane + int npos = 0, nneg = 0; + for (int i=0; i < nv; i++) { + const float* a = vbase + 3 * pv[i]; + const float* b = vbase + 3 * pv[(i + 1) % nv]; + mjtNum e[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]}, + w[3] = {pp[0] - a[0], pp[1] - a[1], pp[2] - a[2]}; + mjtNum cr = (e[1] * w[2] - e[2] * w[1]) * nrm[0] + (e[2] * w[0] - e[0] * w[2]) * nrm[1] + + (e[0] * w[1] - e[1] * w[0]) * nrm[2]; + if (cr > 0) + npos++; + else + nneg++; + } + if (npos == 0 || nneg == 0) { + for (int k=0; k < 3; k++) out[k] = pp[k]; + return; + } // inside polygon + mjtNum best = 1e30; // clamp to edges + for (int i=0; i < nv; i++) { + const float* a = vbase + 3 * pv[i]; + const float* b = vbase + 3 * pv[(i + 1) % nv]; + mjtNum e[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]}, + w[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]}; + mjtNum t = (e[0] * w[0] + e[1] * w[1] + e[2] * w[2]) / + (e[0] * e[0] + e[1] * e[1] + e[2] * e[2] + 1e-18); + t = t < 0 ? 0 : (t > 1 ? 1 : t); + mjtNum c[3] = {a[0] + t * e[0], a[1] + t * e[1], a[2] + t * e[2]}; + mjtNum d2 = (p[0] - c[0]) * (p[0] - c[0]) + (p[1] - c[1]) * (p[1] - c[1]) + + (p[2] - c[2]) * (p[2] - c[2]); + if (d2 < best) { + best = d2; + for (int k=0; k < 3; k++) out[k] = c[k]; + } + } +} + +// signed distance from a static geom's surface to world point x (positive outside) + outward unit +// normal n. Closed-form for plane/sphere/capsule/box; returns +large (no contact) for other types. +// A continuous-contact solver needs a differentiable POINT-to-geom signed distance; +// mj_geomDistance is geom-vs-geom (can't take a flex vertex) and mjc_distance/mjc_gradient need an +// mjSDF object + miss convex meshes. +mjtNum mjc_GeomDist(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + const mjtNum* x, mjtNum* n, mjtNum cutoff) { + int type = m->geom_type[gi]; + const mjtNum* size = m->geom_size + 3 * gi; + mjtNum dx[3]; + for (int k=0; k < 3; k++) dx[k] = x[k] - gpos[k]; + if (type == mjGEOM_MESH) { + // point vs CONVEX-hull mesh, using MuJoCo's precomputed convex polygon faces. First the max + // signed face-plane distance (cheap; gives the inside test and a far/cheap reject). If the + // point is INSIDE (maxd<=0) return maxd with the max-face normal. If clearly FAR (maxd large) + // return maxd (barrier is off there, normal direction irrelevant). Only NEAR the surface do the + // proper point-to-convex-hull CLOSEST POINT (min over faces of the clamped projection) -> + // correct distance AND a normal that points at the actual nearest surface (no max-plane normal + // chatter). + int meshid = m->geom_dataid[gi]; + const float* vbase = m->mesh_vert + 3 * m->mesh_vertadr[meshid]; + int polyadr = m->mesh_polyadr[meshid], pn = m->mesh_polynum[meshid]; + if (pn <= 0) { + n[0] = 0; + n[1] = 0; + n[2] = 1; + return 1e30; + } + mjtNum pl[3]; + mju_mulMatTVec3(pl, gmat, dx); // world -> mesh-local + mjtNum maxd = -1e30; + const mjtNum* bestn = m->mesh_polynormal + 3 * polyadr; + for (int p=0; p < pn; p++) { + const mjtNum* pnl = m->mesh_polynormal + 3 * (polyadr + p); + const float* v0 = vbase + 3 * m->mesh_polyvert[m->mesh_polyvertadr[polyadr + p]]; + mjtNum c = pnl[0] * v0[0] + pnl[1] * v0[1] + pnl[2] * v0[2]; + mjtNum dd = pnl[0] * pl[0] + pnl[1] * pl[1] + pnl[2] * pl[2] - c; + if (dd > maxd) { + maxd = dd; + bestn = pnl; + } + } + if (maxd <= 0) { + mju_mulMatVec3(n, gmat, bestn); + return maxd; + } // inside: penetration + // far reject: maxd (max signed plane distance) is a LOWER bound on the true distance for a + // convex hull, so if it already exceeds the cutoff the closest-point search can't bring it into + // range -- skip the O(faces) loop (this is what makes the per-vertex broadphase against a mesh + // affordable). + if (maxd > cutoff) { + mju_mulMatVec3(n, gmat, bestn); + return maxd; + } + mjtNum best = 1e30, bc[3] = {0, 0, 0}; // outside: true closest point + for (int p=0; p < pn; p++) { + const mjtNum* pnl = m->mesh_polynormal + 3 * (polyadr + p); + const int* pv = m->mesh_polyvert + m->mesh_polyvertadr[polyadr + p]; + int nv = m->mesh_polyvertnum[polyadr + p]; + mjtNum cc[3]; + closestOnPoly(pl, vbase, pv, nv, pnl, cc); + mjtNum d2 = (pl[0] - cc[0]) * (pl[0] - cc[0]) + (pl[1] - cc[1]) * (pl[1] - cc[1]) + + (pl[2] - cc[2]) * (pl[2] - cc[2]); + if (d2 < best) { + best = d2; + bc[0] = cc[0]; + bc[1] = cc[1]; + bc[2] = cc[2]; + } + } + mjtNum dist = sqrt(best); + mjtNum nl[3]; + for (int k=0; k < 3; k++) nl[k] = (dist > 1e-12) ? (pl[k] - bc[k]) / dist : bestn[k]; + mju_mulMatVec3(n, gmat, nl); // outward normal (point - closest) -> world + return dist; + } + if (type == mjGEOM_PLANE) { + n[0] = gmat[2]; + n[1] = gmat[5]; + n[2] = gmat[8]; // local +z axis in world coords + return mju_dot3(n, dx); + } + if (type == mjGEOM_SPHERE) { + mjtNum L = sqrt(mju_dot3(dx, dx)); + if (L < 1e-12) { + n[0] = 0; + n[1] = 0; + n[2] = 1; + return -size[0]; + } + for (int k=0; k < 3; k++) n[k] = dx[k] / L; + return L - size[0]; + } + mjtNum p[3]; + mju_mulMatTVec3(p, gmat, dx); // world -> geom-local + mjtNum nl[3] = {0, 0, 0}, dist; + if (type == mjGEOM_CAPSULE) { + mjtNum hz = size[1], zc = p[2] > hz ? hz : (p[2] < -hz ? -hz : p[2]); + mjtNum q[3] = {p[0], p[1], p[2] - zc}; // vector from nearest axis point + mjtNum L = sqrt(mju_dot3(q, q)); + if (L < 1e-12) { + nl[0] = 1; + } else { + for (int k=0; k < 3; k++) nl[k] = q[k] / L; + } + dist = L - size[0]; + } else if (type == mjGEOM_BOX) { + mjtNum q[3]; + int outside = 0; + for (int k=0; k < 3; k++) { + mjtNum c = p[k] > size[k] ? size[k] : (p[k] < -size[k] ? -size[k] : p[k]); + q[k] = p[k] - c; + if (q[k] != 0) outside = 1; + } + if (outside) { + mjtNum L = sqrt(mju_dot3(q, q)); + dist = L; + for (int k=0; k < 3; k++) nl[k] = q[k] / L; + } else { // inside: least-penetration face + int ax = 0; + mjtNum best = 1e30; + for (int k=0; k < 3; k++) { + mjtNum pen = size[k] - (p[k] < 0 ? -p[k] : p[k]); + if (pen < best) { + best = pen; + ax = k; + } + } + dist = -best; + nl[ax] = p[ax] > 0 ? 1 : -1; + } + } else { + n[0] = 0; + n[1] = 0; + n[2] = 1; + return 1e30; // unsupported geom -> no barrier + } + mju_mulMatVec3(n, gmat, nl); // geom-local normal -> world + return dist; +} + +// world-space VERTICES of a static geom (sharp features that can poke through a flex triangle): +// box -> 8 corners; mesh -> all its vertices; smooth/infinite geoms none. Returns the count. +int mjc_GeomVerts(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, mjtNum* out) { + int type = m->geom_type[gi]; + if (type == mjGEOM_BOX) { + const mjtNum* size = m->geom_size + 3 * gi; + int n = 0; + for (int sx=-1; sx <= 1; sx += 2) + for (int sy=-1; sy <= 1; sy += 2) + for (int sz=-1; sz <= 1; sz += 2) { + mjtNum loc[3] = {sx * size[0], sy * size[1], sz * size[2]}, wc[3]; + mju_mulMatVec3(wc, gmat, loc); + for (int k=0; k < 3; k++) out[3 * n + k] = gpos[k] + wc[k]; + n++; + } + return n; + } + if (type == mjGEOM_MESH) { + int mid = m->geom_dataid[gi], nv = m->mesh_vertnum[mid]; + const float* vb = m->mesh_vert + 3 * m->mesh_vertadr[mid]; + for (int i=0; i < nv; i++) { + mjtNum lv[3] = {vb[3 * i], vb[3 * i + 1], vb[3 * i + 2]}, wv[3]; + mju_mulMatVec3(wv, gmat, lv); + for (int k=0; k < 3; k++) out[3 * i + k] = gpos[k] + wv[k]; + } + return nv; + } + return 0; +} + +// world-space EDGES of a static geom (a geom edge can slice through a flex triangle between flex +// vertices): box -> 12 edges; mesh -> its convex-polygon edges (deduped: each shared edge emitted +// once, by the polygon traversing it low->high index). Each edge = two endpoints. Returns count. +int mjc_GeomEdges(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, mjtNum* out) { + int type = m->geom_type[gi]; + if (type == mjGEOM_BOX) { + const mjtNum* size = m->geom_size + 3 * gi; + int n = 0; + for (int axis=0; axis < 3; axis++) { + int a1 = (axis + 1) % 3, a2 = (axis + 2) % 3; + for (int s1=-1; s1 <= 1; s1 += 2) + for (int s2=-1; s2 <= 1; s2 += 2) { + mjtNum lo[3], hi[3], w1[3], w2[3]; + lo[axis] = -size[axis]; + hi[axis] = size[axis]; + lo[a1] = hi[a1] = s1 * size[a1]; + lo[a2] = hi[a2] = s2 * size[a2]; + mju_mulMatVec3(w1, gmat, lo); + mju_mulMatVec3(w2, gmat, hi); + for (int k=0; k < 3; k++) { + out[6 * n + k] = gpos[k] + w1[k]; + out[6 * n + 3 + k] = gpos[k] + w2[k]; + } + n++; + } + } + return n; + } + if (type == mjGEOM_MESH) { + int mid = m->geom_dataid[gi]; + const float* vb = m->mesh_vert + 3 * m->mesh_vertadr[mid]; + int pa = m->mesh_polyadr[mid], pn = m->mesh_polynum[mid], n = 0; + for (int p=0; p < pn; p++) { + int adr = m->mesh_polyvertadr[pa + p], nvp = m->mesh_polyvertnum[pa + p]; + for (int j=0; j < nvp; j++) { + int a = m->mesh_polyvert[adr + j], b = m->mesh_polyvert[adr + (j + 1) % nvp]; + if (a >= b) continue; // dedup: emit each hull edge once (the low->high traversal) + mjtNum la[3] = {vb[3 * a], vb[3 * a + 1], vb[3 * a + 2]}, + lb[3] = {vb[3 * b], vb[3 * b + 1], vb[3 * b + 2]}, wa[3], wb[3]; + mju_mulMatVec3(wa, gmat, la); + mju_mulMatVec3(wb, gmat, lb); + for (int k=0; k < 3; k++) { + out[6 * n + k] = gpos[k] + wa[k]; + out[6 * n + 3 + k] = gpos[k] + wb[k]; + } + n++; + } + } + return n; + } + return 0; +} + +// contact standoff from a pair's detection band: the rest gap min(band, cap). See the header for +// the standoff/band semantics; cap is the caller's skin thickness ceiling. +mjtNum mjc_standoff(mjtNum band, mjtNum cap) { return band < cap ? band : cap; } + +// unit gap normal from a closest-point pair. Coincident features (dd == 0) have no defined +// direction, so the normal is zeroed and the contact goes inert for this linearization instead of +// emitting NaN; it is re-linearized at the next configuration. The CCD certificate keeps dd > 0, +// so this only guards configurations that are already degenerate on entry. +static void gapNormal(mjtNum* n, const mjtNum* p1, const mjtNum* p2, mjtNum dd) { + if (dd < mjMINVAL) { + n[0] = n[1] = n[2] = 0; + return; + } + for (int k=0; k < 3; k++) n[k] = (p1[k] - p2[k]) / dd; +} + +// gap g of a contact at configuration x, plus the barrier gradient direction n, the involved flex +// vertices idv[*nidx] and their weights cw (dg/d(vertex_p) = cw[p]*n). gv/ge are the precomputed +// world-space static-geom corners/edges. Single source of the per-type contact geometry. +// The engine's collision generators only emit an mjContact (dist/normal); this +// returns gap + normal +// + barycentric weights cw (dg/dx) for the barrier gradient/Hessian, which they do not expose. +mjtNum mjc_pairGap(const mjcPair* con, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* gv, const mjtNum* ge, const mjtNum* rad, mjtNum* n, + int* idv, mjtNum* cw, int* nidx, mjtNum cutoff) { + switch (con->type) { + case 0: { // vertex-triangle: flex self-contact or cross-flex (vertex v vs triangle A,B,C) + int v = con->idx[0], A = con->idx[1], B = con->idx[2], C = con->idx[3]; + mjtNum cp[3], w[3], dd = mjc_PtTri(&x[3 * v], &x[3 * A], &x[3 * B], &x[3 * C], cp, w); + gapNormal(n, &x[3 * v], cp, dd); + idv[0] = v; + idv[1] = A; + idv[2] = B; + idv[3] = C; + cw[0] = 1; + cw[1] = -w[0]; + cw[2] = -w[1]; + cw[3] = -w[2]; + *nidx = 4; + return dd; // MIDSURFACE distance: radii NOT subtracted (delta unchanged; see header) + } + case 1: { // edge-edge contact (edge a1b1 against edge a2b2): flex self OR cross-flex + int a1 = con->idx[0], b1 = con->idx[1], a2 = con->idx[2], b2 = con->idx[3]; + mjtNum cp1[3], cp2[3], st[2], + dd = mjc_SegSeg(&x[3 * a1], &x[3 * b1], &x[3 * a2], &x[3 * b2], cp1, cp2, st); + gapNormal(n, cp1, cp2, dd); + idv[0] = a1; + idv[1] = b1; + idv[2] = a2; + idv[3] = b2; + cw[0] = 1 - st[0]; + cw[1] = st[0]; + cw[2] = -(1 - st[1]); + cw[3] = -st[1]; + *nidx = 4; + return dd; // MIDSURFACE distance: radii NOT subtracted (delta unchanged; see header) + } + case 2: { // flex vertex v vs static geom gi surface + int v = con->idx[0], gi = con->gi; + mjtNum dd = mjc_GeomDist(m, gi, d->geom_xpos + 3 * gi, d->geom_xmat + 9 * gi, &x[3 * v], n, + cutoff + rad[v]); + idv[0] = v; + cw[0] = 1; + *nidx = 1; + return dd - rad[v]; + } + case 3: { // static geom corner gv[idx0] vs flex triangle A,B,C + const mjtNum* corner = &gv[3 * con->idx[0]]; + int A = con->idx[1], B = con->idx[2], C = con->idx[3]; + mjtNum cp[3], w[3], dd = mjc_PtTri(corner, &x[3 * A], &x[3 * B], &x[3 * C], cp, w); + gapNormal(n, corner, cp, dd); + idv[0] = A; + idv[1] = B; + idv[2] = C; + cw[0] = -w[0]; + cw[1] = -w[1]; + cw[2] = -w[2]; + *nidx = 3; + return dd - rad[A]; // flex triangle radius + } + default: { // case 4: static geom edge ge[idx0] vs flex edge a,b + const mjtNum* eg = &ge[6 * con->idx[0]]; + int a = con->idx[1], b = con->idx[2]; + mjtNum cp1[3], cp2[3], st[2], dd = mjc_SegSeg(eg, eg + 3, &x[3 * a], &x[3 * b], cp1, cp2, st); + gapNormal(n, cp1, cp2, dd); + idv[0] = a; + idv[1] = b; + cw[0] = -(1 - st[1]); + cw[1] = -st[1]; + *nidx = 2; + return dd - rad[a]; // flex edge radius + } + } +} + +// the flex vertices a contact involves (geom features are fixed and excluded): fills v[*nv] +void mjc_pairVerts(const mjcPair* con, int* v, int* nv) { + switch (con->type) { + case 0: + case 1: + v[0] = con->idx[0]; + v[1] = con->idx[1]; + v[2] = con->idx[2]; + v[3] = con->idx[3]; + *nv = 4; + break; + case 2: + v[0] = con->idx[0]; + *nv = 1; + break; + case 3: + v[0] = con->idx[1]; + v[1] = con->idx[2]; + v[2] = con->idx[3]; + *nv = 3; + break; // flex triangle A,B,C + default: + v[0] = con->idx[1]; + v[1] = con->idx[2]; + *nv = 2; + break; // flex edge a,b + } +} + +// per-contact barrier activation distance d_hat. Never exceeds the global ghat, but shrinks to the +// thinnest participating radius: a thin flex (e.g. a drawstring sitting in a thick bag's sleeve) +// then gets a proportionally thin barrier zone instead of resting deep inside the thick neighbour's +// zone (which keeps it permanently active + ratchets kappa -> ill-conditioned). Geom features carry +// no radius (excluded by mjc_pairVerts), so geom/flex and sphere/flex contacts keep the global ghat. +mjtNum mjc_pairBand(const mjcPair* con, const mjtNum* rad, mjtNum ghat) { + int vv[4], nvv; + mjc_pairVerts(con, vv, &nvv); + mjtNum g = ghat; + for (int q=0; q < nvv; q++) + if (rad[vv[q]] < g) g = rad[vv[q]]; + return g; +} + +// surface gap of a contact with its flex vertices advanced by t*dxw (geom features fixed); +// gap-only, for the CCD conservative advancement (recomputes the closest feature at the advanced +// configuration). The engine has no advanced-configuration gap evaluator for +// conservative advancement (mjc_ccd is single-config). +static mjtNum conGapAdv(const mjcPair* con, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* dxw, mjtNum t, const mjtNum* gv, const mjtNum* ge, + const mjtNum* rad, const int* fidx) { + int v[4], nv; + mjtNum P[4][3]; + mjc_pairVerts(con, v, &nv); + for (int q=0; q < nv; q++) { + int fq = fidx[v[q]]; + for (int k=0; k < 3; k++) P[q][k] = x[3 * v[q] + k] + (fq >= 0 ? t * dxw[3 * fq + k] : 0.0); + } + mjtNum cp[3], w[3], c1[3], c2[3], st[2]; + switch (con->type) { + case 0: + return mjc_PtTri(P[0], P[1], P[2], P[3], cp, w); // midsurface, matching mjc_pairGap + case 1: + return mjc_SegSeg(P[0], P[1], P[2], P[3], c1, c2, st); // midsurface, matching mjc_pairGap + case 2: { + mjtNum nn[3]; + return mjc_GeomDist(m, con->gi, d->geom_xpos + 3 * con->gi, d->geom_xmat + 9 * con->gi, P[0], + nn, 1e30) - + rad[con->idx[0]]; + } + case 3: + return mjc_PtTri(&gv[3 * con->idx[0]], P[0], P[1], P[2], cp, w) - rad[con->idx[1]]; + default: { + const mjtNum* eg = &ge[6 * con->idx[0]]; + return mjc_SegSeg(eg, eg + 3, P[0], P[1], c1, c2, st) - rad[con->idx[1]]; + } + } +} + +mjtNum mjc_advance(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* dxw, + const mjtNum* gv, const mjtNum* ge, const mjtNum* rad, int nfv, + const int* fidx, const mjcPair* cand, int ncand, const mjtNum* cgap, + const int* pt2flex, int* approut, mjtNum* toiout) { + mjtNum alpha = 1.0; + if (approut) + for (int c=0; c < ncand; c++) + approut[c] = 0; // Alg.3: per-pair "the proxy approaches this contact" + if (toiout) + for (int c=0; c < ncand; c++) + toiout[c] = 1.0; // Alg.3: per-pair collision time, 1 == does not collide this step + for (int c=0; c < ncand; c++) { + const mjcPair* con = &cand[c]; + // mean-removal (don't throttle COHERENT motion) is valid ONLY for a true SAME-FLEX + // self-contact. For INTER-flex (e.g. drawstring vs bag) the two sides move independently, so + // removing the mean underestimates the closing speed and lets one tunnel through the other. + // Gate it on same-flex (both sides in the same flex via pt2flex). + int other = (con->type == 0) ? con->idx[1] : con->idx[2]; + int v[4], nv, + self = (con->type <= 1) && (con->idx[0] < nfv) && (other < nfv) && + (pt2flex[con->idx[0]] == pt2flex[other]); + mjc_pairVerts(con, v, &nv); + mjtNum dp[4][3], mean[3] = {0, 0, 0}; + for (int q=0; q < nv; q++) { + int fq = fidx[v[q]]; + for (int k=0; k < 3; k++) dp[q][k] = (fq >= 0 ? dxw[3 * fq + k] : 0.0); + } + if (self) { + for (int q=0; q < nv; q++) + for (int k=0; k < 3; k++) mean[k] += dp[q][k]; + for (int k=0; k < 3; k++) mean[k] /= nv; + for (int q=0; q < nv; q++) + for (int k=0; k < 3; k++) dp[q][k] -= mean[k]; + } + mjtNum l; // bound on the gap-shrink rate per unit alpha + if (con->type == 0) { + mjtNum a0 = sqrt(mju_dot3(dp[0], dp[0])), b0 = 0; + for (int q=1; q < 4; q++) { + mjtNum s = sqrt(mju_dot3(dp[q], dp[q])); + if (s > b0) b0 = s; + } + l = a0 + b0; + } else if (con->type == 1) { + mjtNum a0 = 0, b0 = 0; + for (int q=0; q < 2; q++) { + mjtNum s = sqrt(mju_dot3(dp[q], dp[q])); + if (s > a0) a0 = s; + } + for (int q=2; q < 4; q++) { + mjtNum s = sqrt(mju_dot3(dp[q], dp[q])); + if (s > b0) b0 = s; + } + l = a0 + b0; + } else { + l = 0; + for (int q=0; q < nv; q++) { + mjtNum s = sqrt(mju_dot3(dp[q], dp[q])); + if (s > l) l = s; + } + } + if (l < 1e-12) continue; + mjtNum g0 = + cgap[c]; // true gap at x (>= standoff delta at rest, so the CCD always has room: no lock) + if (g0 <= 0) continue; // already at/under the surface: the contact solver owns it + if (l <= 0.8 * g0) + continue; // full alpha=1 step shrinks gap by <= l, stays above the 20% floor + if (approut) + approut[c] = + 1; // reaches the bisection -> the proxy closes this pair's gap this step (Alg.3 add) + mjtNum gtarget = 0.2 * g0, t = 0; + for (int it=0; it < 32; it++) { + mjtNum g = conGapAdv(con, m, d, x, dxw, t, gv, ge, rad, fidx); + mjtNum room = g - gtarget; + if (room <= 1e-9 * g0) break; + t += room / l; + if (t >= alpha) { + t = alpha; + break; + } + } + if (toiout) { + toiout[c] = t; // this pair's own collision time, for the per-vertex earliest filter + } + if (t < alpha) { + alpha = t; + } + } + return alpha; +} + +// the radii sum a flex-flex midsurface gap reads ABOVE the old skin gap. Used ONLY to keep the +// broad-phase detection band at its pre-midsurface reach; types 2/3/4 still subtract their radius +// inside mjc_pairGap, so they contribute nothing here. +static mjtNum bandOffset(const mjcPair* con, const mjtNum* rad) { + switch (con->type) { + case 0: + return rad[con->idx[0]] + rad[con->idx[1]]; // vertex + triangle flex radius + case 1: + return rad[con->idx[0]] + rad[con->idx[2]]; // both edges (flex) radii + default: + return 0.0; // geom side: mjc_pairGap already subtracted the flex radius for types 2/3/4 + } +} + +// append a candidate contact if its gap at x is below the (margin-inflated) detection threshold +static void addCand(mjcPair con, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* gv, const mjtNum* ge, const mjtNum* rad, mjtNum thresh, + const mjtNum* dfrom, const mjtNum* dto, mjtNum ghat, mjcPair* cand, int* nc, + int candmax) { + if (*nc >= candmax) return; + mjtNum n[3], cw[4]; + int idv[4], nidx; + mjtNum g = mjc_pairGap(&con, m, d, x, gv, ge, rad, n, idv, cw, &nidx, thresh); + // DETECTION band. A flex-flex gap is now the midsurface distance, so it reads r1+r2 larger than + // the pre-midsurface convention; add that back HERE (and only here) so the broad-phase reaches + // exactly as far as it used to. This offset belongs to the band, NOT to the rest target + // -- the rest target (mjc_standoff(mjc_pairBand)) is unchanged and must stay that way. + mjtNum bo = bandOffset(&con, rad); + if (g >= bo + thresh) return; + // KEEP penetrating geom pairs (g < 0): the normal is still defined there and the solver's restoring + // force is what pushes the flex back out, so dropping them leaves the penetration unopposed; + // mjc_advance (g0 <= 0 -> no cap) and the merge (cgap <= 0 -> admit) already expect them. + // Flex-flex distances are UNSIGNED, so g <= 0 means coincident features with no defined normal. + if (con.type <= 1 && g <= 0) return; + // closing-bound prune: over the step the gap changes by at most |sum_p cw[p]*(dto-dfrom)[idv[p]]| + // (Cauchy-Schwarz, |n|=1), so a pair beyond its per-contact ghc + that bound cannot become active + // this step -> drop it. Replaces the crude GLOBAL 4*maxdisp band (which inflated by the fastest + // vertex anywhere, flooding correlated bulk motion like a settling bag+string). No-tunnel safe: + // the per-outer re-query at xfree (dfrom=xfree, dto=x) recaptures any pair whose closest feature + // flips under the inner step. + mjtNum rel[3] = {0, 0, 0}; + for (int p=0; p < nidx; p++) { + int vp = idv[p]; + if (vp < 0) continue; + for (int c=0; c < 3; c++) rel[c] += cw[p] * (dto[3 * vp + c] - dfrom[3 * vp + c]); + } + if (g < bo + mjc_pairBand(&con, rad, ghat) + sqrt(mju_dot3(rel, rel))) cand[(*nc)++] = con; +} + +// descend flex f's element BVH (built and AABB-refreshed by mj_flex at the step's xold), collecting +// the leaf element ids whose (radius-inflated) node AABB overlaps the query box [c +/- h]. Replaces +// the hand-rolled uniform spatial hash: same "nearby elements" query, but the engine's hierarchy. +// The node AABBs already include flex_radius, so a query half of thresh is a conservative superset +// (no pair within thresh is missed; the narrowphase then filters). stack must hold flex_bvhnum +// ints. +static int bvhBox(const mjModel* m, const mjData* d, int f, const mjtNum* c, const mjtNum* h, + int* stack, int* out, int maxout) { + int bvhadr = m->flex_bvhadr[f]; + if (bvhadr < 0) return 0; + const int* child = m->bvh_child + 2 * bvhadr; + const int* nodeid = m->bvh_nodeid + bvhadr; + const mjtNum* aabb = d->bvh_aabb_dyn + 6 * (bvhadr - m->nbvhstatic); + int ns = 0, nout = 0; + stack[ns++] = 0; + while (ns) { + int node = stack[--ns]; + const mjtNum* na = aabb + 6 * node; // [center(3), halfsize(3)] + if (mju_abs(na[0] - c[0]) > na[3] + h[0] || mju_abs(na[1] - c[1]) > na[4] + h[1] || + mju_abs(na[2] - c[2]) > na[5] + h[2]) + continue; // box-box separation -> prune + int c0 = child[2 * node], c1 = child[2 * node + 1]; + if (c0 < 0 && c1 < 0) { + if (nout < maxout) out[nout++] = nodeid[node]; + } // leaf -> element id + else { + if (c0 >= 0) stack[ns++] = c0; + if (c1 >= 0) stack[ns++] = c1; + } + } + return nout; +} + +// build the candidate-contact list once per step, gated by a velocity-aware threshold so any pair +// that could close within the step is captured (the Newton loop then only re-tests candidates). +// Flex-flex and geom-feature-vs-flex pairs are found by querying the flex element BVH (bvhBox). +int mjc_candidates(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* gv, + const mjtNum* ge, int ngv, int nge, const mjtNum* rad, mjtNum thresh, + mjtNum threshGeom, mjtNum maxdisp, const mjtNum* dfrom, const mjtNum* dto, + mjtNum ghat, int nfv, int npt, const int* fidx, const int* flist, + const int* fxadr, int nfd, const int* pt2flex, mjcPair* cand, int candmax) { + (void) + npt; // increment A: candidates are flex-only (npt==nfv); rigid bodies carry no hard contact + int nc = 0; + for (int gi=0; gi < m->ngeom; gi++) { // free point (flex vert) vs STATIC geom + if (m->geom_contype[gi] == 0 && m->geom_conaffinity[gi] == 0) continue; // skip non-colliding + if (m->body_weldid[m->geom_bodyid[gi]] != 0) + continue; // STATIC geoms ONLY: a MOVABLE rigid geom (ball/limb) is a soft type-3 contact, + // never a frozen penetration-free obstacle + // geom-level cull: bound the points by the geom's true world AABB (the rotated local geom_aabb) + // + per-point margin, before the per-face distance. Much tighter than the bounding sphere for + // the elongated convex-decomposition slabs. Planes are infinite -> no cull (their distance is + // O(1) anyway). + int isplane = (m->geom_type[gi] == mjGEOM_PLANE); + mjtNum wc[3], wh[3]; + if (!isplane) { + const mjtNum* la = m->geom_aabb + 6 * gi; + const mjtNum* gp = d->geom_xpos + 3 * gi; + const mjtNum* gR = d->geom_xmat + 9 * gi; + mju_mulMatVec3(wc, gR, la); + for (int k=0; k < 3; k++) wc[k] += gp[k]; // world AABB center + for (int k=0; k < 3; k++) + wh[k] = mju_abs(gR[3 * k]) * la[3] + mju_abs(gR[3 * k + 1]) * la[4] + + mju_abs(gR[3 * k + 2]) * la[5]; + } + for (int v=0; v < nfv; v++) { // flex verts only (rigid bodies carry no hard barrier) + if (fidx[v] < 0) continue; + mjtNum marg = thresh + rad[v]; + if (!isplane) { // world-AABB cull + if (mju_abs(x[3 * v] - wc[0]) > wh[0] + marg || + mju_abs(x[3 * v + 1] - wc[1]) > wh[1] + marg || + mju_abs(x[3 * v + 2] - wc[2]) > wh[2] + marg) + continue; + } + mjcPair con = {2, {v, 0, 0, 0}, gi}; + addCand(con, m, d, x, gv, ge, rad, thresh, dfrom, dto, ghat, cand, &nc, candmax); + } + } + + // (Rigid sphere-sphere and sphere-vs-flex HARD contacts removed in increment A: rigid bodies live + // in their own solver block and carry no continuous contact yet. Soft flex-rigid contact is a + // separate next increment.) + + // ---- BVH-based candidates over ALL dim-2 flexes: geom-feature and flex-vs-flex VT/EE. + // Each query is against one flex's element BVH (bvhBox); triangle/edge vertices are mapped + // from the queried flex's local indices to the combined free-point space (fxadr[k] + local). + // flex-vs-flex contact is SELF when the querying vertex/edge is in the queried flex (gated by + // that flex's selfcollide) and INTER-FLEX otherwise (always on). Scratch buffers are sized for + // the largest flex. ---- + int maxbvh = 1, maxel = 1, maxen = 1; + for (int k=0; k < nfd; k++) { + int fk = flist[k]; + if (m->flex_bvhnum[fk] > maxbvh) maxbvh = m->flex_bvhnum[fk]; + if (m->flex_elemnum[fk] > maxel) maxel = m->flex_elemnum[fk]; + if (m->flex_edgenum[fk] > maxen) maxen = m->flex_edgenum[fk]; + } + int* stk = (int*)mju_malloc(maxbvh * sizeof(int)); + int* outel = (int*)mju_malloc(maxel * sizeof(int)); + int* stampG = (int*)mju_malloc(maxen * sizeof(int)); + for (int e=0; e < maxen; e++) stampG[e] = -1; + int qid = 0; + + for (int k=0; k < nfd; k++) { // query flex fk's element BVH + int fk = flist[k]; + int ne_k = m->flex_elemnum[fk], ea_k = m->flex_edgeadr[fk], off_k = fxadr[k]; + if (m->flex_bvhadr[fk] < 0 || ne_k == 0) continue; + const int* el_k = m->flex_elem + m->flex_elemdataadr[fk]; + const int* eme_k = m->flex_elemedge + m->flex_elemedgeadr[fk]; + mjtNum rk = m->flex_radius[fk]; + int doself_k = (m->flex_selfcollide[fk] != mjFLEXSELF_NONE); + + // (rigid sphere-vs-flex-triangle hard contact removed in increment A -- see note above.) + // geom-corner vs flex triangle (type 3); geom is static (one-sided) -> tighter threshGeom (the + // convex-decomposition bin's ~1600 edges otherwise overflow candmax and drop the bag-bin + // contacts). + mjtNum qhvG[3] = {threshGeom + rk, threshGeom + rk, threshGeom + rk}; + for (int c=0; c < ngv; c++) { + int n = bvhBox(m, d, fk, &gv[3 * c], qhvG, stk, outel, ne_k); + for (int i=0; i < n; i++) { + int e = outel[i]; + mjcPair con = { + 3, {c, off_k + el_k[3 * e], off_k + el_k[3 * e + 1], off_k + el_k[3 * e + 2]}, -1}; + addCand(con, m, d, x, gv, ge, rad, threshGeom, dfrom, dto, ghat, cand, &nc, candmax); + } + } + // geom-edge vs flex edge (type 4); dedup the shared triangle edges per query via stampG. + for (int c=0; c < nge; c++) { + const mjtNum* p0 = &ge[6 * c]; + const mjtNum* p1 = &ge[6 * c + 3]; + mjtNum qc[3], qh[3]; + for (int kk=0; kk < 3; kk++) { + qc[kk] = 0.5 * (p0[kk] + p1[kk]); + qh[kk] = 0.5 * mju_abs(p1[kk] - p0[kk]) + threshGeom + rk; + } + int n = bvhBox(m, d, fk, qc, qh, stk, outel, ne_k); + qid++; + for (int i=0; i < n; i++) { + int e = outel[i]; + for (int j=0; j < 3; j++) { + int e2 = eme_k[3 * e + j]; + if (stampG[e2] == qid) continue; + stampG[e2] = qid; + mjcPair con = {4, + {c, off_k + m->flex_edge[2 * (ea_k + e2)], + off_k + m->flex_edge[2 * (ea_k + e2) + 1], 0}, + -1}; + addCand(con, m, d, x, gv, ge, rad, threshGeom, dfrom, dto, ghat, cand, &nc, candmax); + } + } + } + // flex vertex vs flex triangle (type 0): self (same flex, gated by selfcollide) + inter-flex + // (always). Asymmetric (vert vs tri), so all verts query every flex's BVH -- both directions + // are distinct contacts. + for (int v=0; v < nfv; v++) { + int kv = pt2flex[v]; + if (kv == k && !doself_k) continue; // self-contact disabled for this flex + // per-pair band: the thinner flex sets it, capped by the global band + mjtNum thv = 3.0 * min2(ghat, min2(rad[v], rk)) + 4.0 * maxdisp; + mjtNum qh[3] = {thv + rad[v], thv + rad[v], thv + rad[v]}; + int n = bvhBox(m, d, fk, &x[3 * v], qh, stk, outel, ne_k); + for (int i=0; i < n; i++) { + int e = outel[i]; + int A = off_k + el_k[3 * e], B = off_k + el_k[3 * e + 1], C = off_k + el_k[3 * e + 2]; + if (kv == k && (v == A || v == B || v == C)) continue; // skip the self-adjacent triangle + mjcPair con = {0, {v, A, B, C}, -1}; + addCand(con, m, d, x, gv, ge, rad, thv, dfrom, dto, ghat, cand, &nc, candmax); + } + } + // flex edge vs flex edge (type 1): symmetric, so canonical -- querying flex kj <= k, and e2 > + // e1 within a flex. Self (kj==k) gated by selfcollide; inter-flex (kjflex_edgeadr[fj], en_j = m->flex_edgenum[fj], off_j = fxadr[kj]; + for (int e1=0; e1 < en_j; e1++) { + int a1 = off_j + m->flex_edge[2 * (ea_j + e1)], + b1 = off_j + m->flex_edge[2 * (ea_j + e1) + 1]; + mjtNum the = 3.0 * min2(ghat, min2(rad[a1], rk)) + 4.0 * maxdisp; // per-pair band + mjtNum qc[3], qh[3]; + for (int kk=0; kk < 3; kk++) { + qc[kk] = 0.5 * (x[3 * a1 + kk] + x[3 * b1 + kk]); + qh[kk] = 0.5 * mju_abs(x[3 * a1 + kk] - x[3 * b1 + kk]) + the + rad[a1]; + } + int n = bvhBox(m, d, fk, qc, qh, stk, outel, ne_k); + qid++; + for (int i=0; i < n; i++) { + int e = outel[i]; + for (int j=0; j < 3; j++) { + int e2 = eme_k[3 * e + j]; + if (self && e2 <= e1) continue; // canonical within a flex + if (stampG[e2] == qid) continue; + stampG[e2] = qid; + int a2 = off_k + m->flex_edge[2 * (ea_k + e2)], + b2 = off_k + m->flex_edge[2 * (ea_k + e2) + 1]; + if (a1 == a2 || a1 == b2 || b1 == a2 || b1 == b2) + continue; // shared vertex -> adjacent, skip + mjcPair con = {1, {a1, b1, a2, b2}, -1}; + addCand(con, m, d, x, gv, ge, rad, the, dfrom, dto, ghat, cand, &nc, candmax); + } + } + } + } + } + mju_free(stk); + mju_free(outel); + mju_free(stampG); + return nc; +} diff --git a/src/engine/engine_collision_continuous.h b/src/engine/engine_collision_continuous.h new file mode 100644 index 00000000000..5cc74d2a24f --- /dev/null +++ b/src/engine/engine_collision_continuous.h @@ -0,0 +1,115 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_CONTINUOUS_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_CONTINUOUS_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// Continuous collision for deformables. The discrete pipeline (engine_collision_*) generates +// contact points at a configuration; this module prices gaps along trajectories: differentiable +// vertex-triangle / edge-edge / vertex-geom distance kernels with closest-point barycentrics, +// swept-volume candidate generation over the flex BVH, per-pair gap evaluation with the gradient's +// vertex weights, and a conservative advancement (CCD) that bounds each pair's time of impact. +// A consumer supplies two lengths: +// cap -- the standoff ceiling: a pair's rest gap is min(band, cap), so thin participants keep +// a proportionally thin skin and thick ones do not carry a fat layer; +// band -- the detection reach: how early the broad phase starts tracking a pair. Not a physical +// length; wider costs candidates, narrower hands pairs to the solver later. +// FLEX-FLEX pairs (types 0/1) measure their gap at the MIDSURFACE: mjc_pairGap does not subtract +// the radii for those types, because where the mesh geometry is tighter than the combined radii +// (a string threaded through a hem) a skin-to-skin gap is permanently negative and the pair would +// be discarded as invalid -- no CCD coverage, so the region could tunnel. The broad phase adds the +// radii back into its reach so detection range is unchanged (see addCand). + +// One candidate contact pair: the geometric identity only. type 0: flex vertex vs flex triangle, +// 1: flex edge vs flex edge, 2: flex vertex vs geom surface, 3: geom corner vs flex triangle, +// 4: geom edge vs flex edge. idx holds free-point indices (types 0/1: all four; type 2: idx[0]; +// types 3/4: the flex-side points), gi the geom for types 2-4. Solver state (multipliers, ages) +// and any cached linearization of the gap belong to the consumer, not to this struct. +typedef struct { + int type; // pair type, 0-4 as above + int idx[4]; // participant free-point indices, meaning per type (see mjc_pairGap) + int gi; // geom id, types 2-4 only +} mjcPair; + +// the standoff of a pair whose detection band is `band`: min(band, cap) +mjtNum mjc_standoff(mjtNum band, mjtNum cap); + +// per-pair detection band: min over the pair's flex radii and `band` (see the header note on +// midsurface gaps for why the radii enter the band and not the gap) +mjtNum mjc_pairBand(const mjcPair* pair, const mjtNum* rad, mjtNum band); + +// the involved free-point indices of a pair (up to 4), for iterating its vertices +void mjc_pairVerts(const mjcPair* pair, int* v, int* nv); + +// Gap of a pair at configuration x, plus the gradient's direction n and its vertex weights: +// dg/d(vertex idv[p]) = cw[p]*n, p < *nidx. gv/ge are the precomputed world-space geom corners and +// edges (mjc_GeomVerts/mjc_GeomEdges), rad the per-free-point radii. Early-out beyond cutoff. +// Exported for tests; internal, not a supported API. +MJAPI mjtNum mjc_pairGap(const mjcPair* pair, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* gv, const mjtNum* ge, const mjtNum* rad, mjtNum* n, + int* idv, mjtNum* cw, int* nidx, mjtNum cutoff); + +// Swept candidate generation: all pairs whose gap can enter the detection band along the segment +// dfrom -> dto, gathered over the flex BVH (self and cross-flex) and the geom features. thresh / +// threshGeom bound the flex-flex / flex-geom reach, maxdisp the per-vertex motion the collar must +// absorb, ghat the detection band. Returns the number of candidates written to cand (at most +// candmax). Exported for tests; internal, not a supported API. +MJAPI int mjc_candidates(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* gv, + const mjtNum* ge, int ngv, int nge, const mjtNum* rad, mjtNum thresh, + mjtNum threshGeom, mjtNum maxdisp, const mjtNum* dfrom, const mjtNum* dto, + mjtNum ghat, int nfv, int npt, const int* fidx, const int* flist, + const int* fxadr, int nfd, const int* pt2flex, mjcPair* cand, int candmax); + +// Conservative advancement: the largest alpha in [0, 1] such that moving the free points from x by +// alpha*dxw keeps every candidate's gap above a fraction of its value at x (no pair's gap is +// closed by more than 80%), so the advanced configuration stays intersection-free. cgap holds each +// candidate's gap at x (from mjc_pairGap). Optional outputs: approut[c] = 1 if the full step +// closes candidate c into its active zone; toiout[c] = candidate c's own time of impact (1 if it +// does not collide this step). Exported for tests; internal, not a supported API. +MJAPI mjtNum mjc_advance(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* dxw, + const mjtNum* gv, const mjtNum* ge, const mjtNum* rad, int nfv, + const int* fidx, const mjcPair* cand, int ncand, const mjtNum* cgap, + const int* pt2flex, int* approut, mjtNum* toiout); + +// Distance kernels, exported for tests; internal, not a supported API. +// mjc_PtTri: point-triangle distance (closest point cp and barycentric weights w). +// mjc_SegSeg: segment-segment distance (closest points and line parameters st). +// mjc_GeomDist: signed distance (+ outward unit normal n) from geom gi's surface, at pose +// gpos/gmat, to world point x; early-out beyond distmax. +// mjc_GeomVerts / mjc_GeomEdges: world-space sharp vertices / edges of geom gi at pose +// gpos/gmat (out sized by the caller); return the count. +MJAPI mjtNum mjc_PtTri(const mjtNum* p, const mjtNum* a, const mjtNum* b, const mjtNum* c, + mjtNum* cp, mjtNum* w); +MJAPI mjtNum mjc_SegSeg(const mjtNum* p1, const mjtNum* p2, const mjtNum* q1, const mjtNum* q2, + mjtNum* cp1, mjtNum* cp2, mjtNum* st); +MJAPI mjtNum mjc_GeomDist(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + const mjtNum* x, mjtNum* n, mjtNum distmax); +MJAPI int mjc_GeomVerts(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + mjtNum* out); +MJAPI int mjc_GeomEdges(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + mjtNum* out); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_CONTINUOUS_H_ diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index 852535782e2..8fd199b6037 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -14,6 +14,8 @@ mujoco_test(engine_collision_box_test ADDITIONAL_LINK_LIBRARIES ccd) +mujoco_test(engine_collision_continuous_test) + mujoco_test(engine_collision_convex_test) mujoco_test(engine_collision_driver_test) diff --git a/test/engine/engine_collision_continuous_test.cc b/test/engine/engine_collision_continuous_test.cc new file mode 100644 index 00000000000..56fa446c694 --- /dev/null +++ b/test/engine/engine_collision_continuous_test.cc @@ -0,0 +1,363 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_collision_continuous.c: the distance kernels, the per-pair gap and its +// gradient (finite-difference checked), the swept candidate generation, and the conservative +// advancement. Everything is exercised directly on hand-built pairs or tiny flex models; no +// contact solver is involved. + +#include "src/engine/engine_collision_continuous.h" + +#include + +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +// convenience shims over the MJAPI geometry kernels (pose taken from d, scratch dropped) +static mjtNum PtTri(const mjtNum* p, const mjtNum* a, const mjtNum* b, const mjtNum* c) { + mjtNum cp[3], w[3]; + return mjc_PtTri(p, a, b, c, cp, w); +} +static mjtNum SegSeg(const mjtNum* p1, const mjtNum* p2, const mjtNum* q1, const mjtNum* q2) { + mjtNum cp1[3], cp2[3], st[2]; + return mjc_SegSeg(p1, p2, q1, q2, cp1, cp2, st); +} +static mjtNum GeomDist(const mjModel* m, const mjData* d, int gi, const mjtNum* x, mjtNum* n) { + return mjc_GeomDist(m, gi, d->geom_xpos + 3*gi, d->geom_xmat + 9*gi, x, n, 1e30); +} +static int GeomVerts(const mjModel* m, const mjData* d, int gi, mjtNum* out) { + return mjc_GeomVerts(m, gi, d->geom_xpos + 3*gi, d->geom_xmat + 9*gi, out); +} +static int GeomEdges(const mjModel* m, const mjData* d, int gi, mjtNum* out) { + return mjc_GeomEdges(m, gi, d->geom_xpos + 3*gi, d->geom_xmat + 9*gi, out); +} + +using ::testing::NotNull; +using ContinuousCollisionTest = MujocoTest; + +static mjModel* Load(const char* xml) { + char error[1024]; + MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); + EXPECT_THAT(model.get(), NotNull()) << error; + return model.release(); +} + +// id of the first geom in the model +static int FirstGeom(const mjModel* m) { return 0; } + +//---------------------------------- element distances -------------------------------------------- + +// point-triangle distance: interior (perpendicular), edge region, vertex region +TEST_F(ContinuousCollisionTest, PointTriangleDistance) { + mjtNum a[3] = {0, 0, 0}, b[3] = {1, 0, 0}, c[3] = {0, 1, 0}; + + mjtNum p_above[3] = {0.2, 0.2, 0.5}; // over the interior + EXPECT_NEAR(PtTri(p_above, a, b, c), 0.5, 1e-12); + + mjtNum p_edge[3] = {-1, 0.5, 0}; // nearest the x=0 edge + EXPECT_NEAR(PtTri(p_edge, a, b, c), 1.0, 1e-12); + + mjtNum p_vert[3] = {-3, -4, 0}; // nearest vertex a + EXPECT_NEAR(PtTri(p_vert, a, b, c), 5.0, 1e-12); + + mjtNum p_on[3] = {0.25, 0.25, 0}; // on the triangle + EXPECT_NEAR(PtTri(p_on, a, b, c), 0.0, 1e-12); +} + +// segment-segment distance: perpendicular crossing, collinear gap, parallel offset +TEST_F(ContinuousCollisionTest, SegmentSegmentDistance) { + mjtNum p1[3] = {-1, 0, 0}, p2[3] = {1, 0, 0}; + + mjtNum q1[3] = {0, -1, 0.3}, q2[3] = {0, 1, 0.3}; // perpendicular, 0.3 above + EXPECT_NEAR(SegSeg(p1, p2, q1, q2), 0.3, 1e-12); + + mjtNum r1[3] = {2, 0, 0}, r2[3] = {3, 0, 0}; // collinear, gap 1 + EXPECT_NEAR(SegSeg(p1, p2, r1, r2), 1.0, 1e-12); + + mjtNum s1[3] = {-1, 0, 0.5}, s2[3] = {1, 0, 0.5}; // parallel, 0.5 above + EXPECT_NEAR(SegSeg(p1, p2, s1, s2), 0.5, 1e-12); +} + +//---------------------------------- geom distance ------------------------------------------------ + +constexpr char kPrimitivesXml[] = R"( + + + + + + + +)"; + +TEST_F(ContinuousCollisionTest, GeomDistance) { + mjModel* m = Load(kPrimitivesXml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + int box = mj_name2id(m, mjOBJ_GEOM, "box"); + int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); + int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); + mjtNum n[3]; + + // box (half-extent 0.1 in x): point on +x at 0.5 -> surface distance 0.4, normal +x + mjtNum px[3] = {0.5, 0, 0}; + EXPECT_NEAR(GeomDist(m, d, box, px, n), 0.4, 1e-9); + EXPECT_NEAR(n[0], 1, 1e-9); EXPECT_NEAR(n[1], 0, 1e-9); EXPECT_NEAR(n[2], 0, 1e-9); + + // interior point -> negative signed distance + mjtNum pc[3] = {0, 0, 0}; + EXPECT_LT(GeomDist(m, d, box, pc, n), 0); + + // sphere radius 0.1 at (1,0,0): point at (1.3,0,0) -> 0.2, normal +x + mjtNum ps[3] = {1.3, 0, 0}; + EXPECT_NEAR(GeomDist(m, d, sphere, ps, n), 0.2, 1e-9); + EXPECT_NEAR(n[0], 1, 1e-9); + + // plane at z=-1: point at z=0 -> 1.0, normal +z + mjtNum pp[3] = {0.3, -0.2, 0}; + EXPECT_NEAR(GeomDist(m, d, plane, pp, n), 1.0, 1e-9); + EXPECT_NEAR(n[2], 1, 1e-9); + + mj_deleteData(d); + mj_deleteModel(m); +} + +//---------------------------------- geom sharp features ------------------------------------------ + +// a box exposes its 8 corners (at +/-size) and 12 edges +TEST_F(ContinuousCollisionTest, BoxFeatures) { + constexpr char xml[] = R"( + + + )"; + mjModel* m = Load(xml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + mjtNum verts[8*3], edges[12*6]; + int nv = GeomVerts(m, d, FirstGeom(m), verts); + int ne = GeomEdges(m, d, FirstGeom(m), edges); + EXPECT_EQ(nv, 8); + EXPECT_EQ(ne, 12); + for (int i = 0; i < nv; i++) { + EXPECT_NEAR(std::fabs(verts[3*i + 0]), 0.1, 1e-9); + EXPECT_NEAR(std::fabs(verts[3*i + 1]), 0.2, 1e-9); + EXPECT_NEAR(std::fabs(verts[3*i + 2]), 0.3, 1e-9); + } + // every box edge has unit length along exactly one axis (here 0.2, 0.4, or 0.6) + for (int i = 0; i < ne; i++) { + mjtNum dx = edges[6*i+3] - edges[6*i+0]; + mjtNum dy = edges[6*i+4] - edges[6*i+1]; + mjtNum dz = edges[6*i+5] - edges[6*i+2]; + mjtNum len = std::sqrt(dx*dx + dy*dy + dz*dz); + EXPECT_TRUE(std::fabs(len-0.2) < 1e-9 || std::fabs(len-0.4) < 1e-9 || std::fabs(len-0.6) < 1e-9) + << "edge " << i << " length " << len; + } + mj_deleteData(d); + mj_deleteModel(m); +} + +// a convex mesh exposes its vertices and its (deduplicated) hull edges; a tetrahedron has 4 and 6 +TEST_F(ContinuousCollisionTest, MeshFeatures) { + constexpr char xml[] = R"( + + + + )"; + mjModel* m = Load(xml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + mjtNum verts[64*3], edges[256*6]; + int nv = GeomVerts(m, d, FirstGeom(m), verts); + int ne = GeomEdges(m, d, FirstGeom(m), edges); + EXPECT_EQ(nv, 4); // tetrahedron vertices + EXPECT_EQ(ne, 6); // tetrahedron edges (each shared hull edge emitted once) + mj_deleteData(d); + mj_deleteModel(m); +} + +//---------------------------------- pair gap ----------------------------------------------------- + +// vertex-triangle pair: the gap is the point-triangle distance (midsurface: radii not subtracted), +// and (n, cw) is its exact gradient, checked by central differences at every involved vertex +TEST_F(ContinuousCollisionTest, PairGapVertexTriangleGradient) { + mjModel* m = Load(kPrimitivesXml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + // free points: vertex 0 above the interior of triangle (1, 2, 3) + mjtNum x[12] = {0.2, 0.2, 0.5, 0, 0, 0, 1, 0, 0, 0, 1, 0}; + mjtNum rad[4] = {0.005, 0.005, 0.005, 0.005}; + mjcPair pair; + pair.type = 0; + pair.idx[0] = 0; pair.idx[1] = 1; pair.idx[2] = 2; pair.idx[3] = 3; + pair.gi = -1; + + mjtNum n[3], cw[4]; + int idv[4], nidx = 0; + mjtNum g = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, rad, n, idv, cw, &nidx, 1e30); + EXPECT_NEAR(g, 0.5, 1e-12); // midsurface distance, radii not subtracted + EXPECT_NEAR(n[0], 0, 1e-12); + EXPECT_NEAR(n[1], 0, 1e-12); + EXPECT_NEAR(std::fabs(n[2]), 1, 1e-12); + EXPECT_GT(nidx, 0); + + // dg/d(vertex idv[p]) = cw[p]*n, by central differences + mjtNum eps = 1e-6; + for (int p = 0; p < nidx; p++) { + for (int k = 0; k < 3; k++) { + mjtNum saved = x[3*idv[p] + k]; + x[3*idv[p] + k] = saved + eps; + mjtNum gp = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, rad, n, idv, cw, &nidx, 1e30); + x[3*idv[p] + k] = saved - eps; + mjtNum gm = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, rad, n, idv, cw, &nidx, 1e30); + x[3*idv[p] + k] = saved; + mjtNum g0 = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, rad, n, idv, cw, &nidx, 1e30); + EXPECT_NEAR(cw[p]*n[k], (gp - gm) / (2*eps), 1e-6) + << "gradient mismatch at involved vertex " << p << " axis " << k << " (gap " << g0 << ")"; + } + } + mj_deleteData(d); + mj_deleteModel(m); +} + +//---------------------------------- conservative advancement ------------------------------------- + +// a vertex sweeping through a triangle: the advance caps alpha so the gap keeps 20% of its value, +// reports the pair's own time of impact, and flags it as approaching; motion away is uncapped +TEST_F(ContinuousCollisionTest, AdvanceCapsCrossing) { + mjModel* m = Load(kPrimitivesXml); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + mjtNum x[12] = {0.2, 0.2, 0.5, 0, 0, 0, 1, 0, 0, 0, 1, 0}; + mjtNum rad[4] = {0.005, 0.005, 0.005, 0.005}; + int fidx[4] = {0, 1, 2, 3}; // all points free, identity map + int pt2flex[4] = {0, 1, 1, 1}; // cross-flex pair: no coherent-motion mean removal + mjcPair cand; + cand.type = 0; + cand.idx[0] = 0; cand.idx[1] = 1; cand.idx[2] = 2; cand.idx[3] = 3; + cand.gi = -1; + + mjtNum n[3], cw[4]; + int idv[4], nidx = 0; + mjtNum cgap[1]; + cgap[0] = mjc_pairGap(&cand, m, d, x, nullptr, nullptr, rad, n, idv, cw, &nidx, 1e30); + ASSERT_NEAR(cgap[0], 0.5, 1e-12); + + // vertex 0 moves straight down by 1: the full step would end 0.5 below the triangle + mjtNum dxw[12] = {0, 0, -1}; + int appr[1]; + mjtNum toi[1]; + mjtNum alpha = mjc_advance(m, d, x, dxw, nullptr, nullptr, rad, 4, fidx, &cand, 1, + cgap, pt2flex, appr, toi); + // the advance stops when the gap has dropped to 20% of its value: alpha = (0.5 - 0.1)/1 = 0.4 + EXPECT_NEAR(alpha, 0.4, 1e-3); + EXPECT_LT(toi[0], 1.0); + EXPECT_EQ(appr[0], 1); + + // moving away at speed 1: the closing-rate bound is conservative (it does not project onto the + // normal), so the pair still reaches the bisection and is flagged approaching -- but the actual + // gap grows along the path, so the advance is uncapped and there is no impact + mjtNum dxw_up[12] = {0, 0, +1}; + alpha = mjc_advance(m, d, x, dxw_up, nullptr, nullptr, rad, 4, fidx, &cand, 1, + cgap, pt2flex, appr, toi); + EXPECT_NEAR(alpha, 1.0, 1e-12); + EXPECT_NEAR(toi[0], 1.0, 1e-12); + + // slow motion (well under 80% of the gap): absorbed by the 20% floor without any bisection, + // whatever its direction + mjtNum dxw_slow[12] = {0, 0, -0.1}; + alpha = mjc_advance(m, d, x, dxw_slow, nullptr, nullptr, rad, 4, fidx, &cand, 1, + cgap, pt2flex, appr, toi); + EXPECT_NEAR(alpha, 1.0, 1e-12); + EXPECT_NEAR(toi[0], 1.0, 1e-12); + EXPECT_EQ(appr[0], 0); + + mj_deleteData(d); + mj_deleteModel(m); +} + +//---------------------------------- candidate generation ----------------------------------------- + +// two stacked cloths: the swept broad phase finds cross-flex pairs when they are within the +// detection reach and none when they are far apart +TEST_F(ContinuousCollisionTest, CandidatesFindApproachingPairs) { + constexpr char xml[] = R"( + + + + + + )"; + + for (mjtNum dz : {0.002, 0.5}) { + char xml_filled[1024]; + snprintf(xml_filled, sizeof(xml_filled), xml, 0.5 + dz); + mjModel* m = Load(xml_filled); + mjData* d = mj_makeData(m); + mj_forward(m, d); + + // free-point arrays over the two dim-2 flexes, in flex order + int nfd = m->nflex; + ASSERT_EQ(nfd, 2); + int flist[2], fxadr[2], nfv = 0; + for (int k = 0; k < nfd; k++) { + flist[k] = k; + fxadr[k] = nfv; + nfv += m->flex_vertnum[k]; + } + ASSERT_EQ(nfv, 8); + mjtNum x[8*3], rad[8]; + int fidx[8], pt2flex[8]; + for (int k = 0; k < nfd; k++) { + for (int v = 0; v < m->flex_vertnum[k]; v++) { + int pt = fxadr[k] + v, vg = m->flex_vertadr[k] + v; + for (int c = 0; c < 3; c++) x[3*pt + c] = d->flexvert_xpos[3*vg + c]; + rad[pt] = m->flex_radius[k]; + fidx[pt] = pt; + pt2flex[pt] = k; + } + } + + // static query (no sweep): reach = 3*band, band 3 mm + mjtNum band = 0.003; + mjcPair cand[256]; + int ncand = mjc_candidates(m, d, x, nullptr, nullptr, 0, 0, rad, 3*band, 3*band, 0.0, + x, x, band, nfv, nfv, fidx, flist, fxadr, nfd, pt2flex, + cand, 256); + if (dz < 0.01) { + EXPECT_GT(ncand, 0) << "2 mm apart, within reach: pairs expected"; + for (int c = 0; c < ncand; c++) { + EXPECT_TRUE(cand[c].type == 0 || cand[c].type == 1) << "flex-flex pair types only"; + } + } else { + EXPECT_EQ(ncand, 0) << "0.5 m apart, beyond reach: no pairs expected"; + } + mj_deleteData(d); + mj_deleteModel(m); + } +} + +} // namespace +} // namespace mujoco