diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index baa23887..f29664bb 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 00000000..1160519a --- /dev/null +++ b/src/engine/engine_collision_continuous.c @@ -0,0 +1,971 @@ +// 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. + +// Continuous-collision geometry for deformables; see the header for scope. + +#include "engine/engine_collision_continuous.h" + +#include + +#include +#include +#include +#include "engine/engine_util_blas.h" // mju_dot3, mju_mulMatVec3 +#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 +// 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 distmax) { + 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 distmax 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 > distmax) { + 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 mjcFlexPair* pair, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* gv, const mjtNum* ge, const mjtNum* radii, mjtNum* n, + int* idv, mjtNum* cw, int* nidx, mjtNum cutoff) { + switch (pair->type) { + case mjcFLEX_VERT_TRI: { // flex self-contact or cross-flex (vertex v vs triangle A,B,C) + int v = pair->idx[0], A = pair->idx[1], B = pair->idx[2], C = pair->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 mjcFLEX_EDGE_EDGE: { // edge a1b1 against edge a2b2: flex self OR cross-flex + int a1 = pair->idx[0], b1 = pair->idx[1], a2 = pair->idx[2], b2 = pair->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 mjcFLEX_VERT_GEOM: { // flex vertex v vs static geom g surface + int v = pair->idx[0], gi = pair->g; + mjtNum dd = mjc_GeomDist(m, gi, d->geom_xpos + 3 * gi, d->geom_xmat + 9 * gi, &x[3 * v], n, + cutoff + radii[v]); + idv[0] = v; + cw[0] = 1; + *nidx = 1; + return dd - radii[v]; + } + case mjcGEOM_CORNER_TRI: { // static geom corner gv[idx0] vs flex triangle A,B,C + const mjtNum* corner = &gv[3 * pair->idx[0]]; + int A = pair->idx[1], B = pair->idx[2], C = pair->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 - radii[A]; // flex triangle radius + } + case mjcGEOM_EDGE_EDGE: { // static geom edge ge[idx0] vs flex edge a,b + const mjtNum* eg = &ge[6 * pair->idx[0]]; + int a = pair->idx[1], b = pair->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 - radii[a]; // flex edge radius + } + default: + mju_error("mjc_pairGap: unknown pair type %d", pair->type); + return 0; + } +} + +// copy flex pair vertices and return number of vertices; geom features (fixed) +// are excluded -- types 3/4 store the geom index in idx[0], so the flex +// vertices start at idx[1] +int mjc_pairVerts(int* v, const mjcFlexPair* pair) { + int nv = 2, off = 0; + switch (pair->type) { + case mjcFLEX_VERT_TRI: + case mjcFLEX_EDGE_EDGE: + nv = 4; + break; + case mjcFLEX_VERT_GEOM: + nv = 1; + break; + case mjcGEOM_CORNER_TRI: + nv = 3; + off = 1; + break; + case mjcGEOM_EDGE_EDGE: + nv = 2; + off = 1; + break; + } + + for (int i = 0; i < nv; i++) { + v[i] = pair->idx[off + i]; + } + return nv; +} + +// per-contact barrier activation distance d_hat. Never exceeds the global band, 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 +// band. +mjtNum mjc_pairBand(const mjcFlexPair* pair, const mjtNum* radii, mjtNum band) { + int vv[4]; + int nvv = mjc_pairVerts(vv, pair); + mjtNum g = band; + for (int q=0; q < nvv; q++) { + if (radii[vv[q]] < g) { + g = radii[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 mjcFlexPair* con, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* dxw, mjtNum t, const mjtNum* gv, const mjtNum* ge, + const mjtNum* radii, const int* fidx) { + int v[4]; + int nv = mjc_pairVerts(v, con); + mjtNum P[4][3]; + 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 mjcFLEX_VERT_TRI: + return mjc_PtTri(P[0], P[1], P[2], P[3], cp, w); // midsurface, matching mjc_pairGap + case mjcFLEX_EDGE_EDGE: + return mjc_SegSeg(P[0], P[1], P[2], P[3], c1, c2, st); // midsurface, matching mjc_pairGap + case mjcFLEX_VERT_GEOM: { + mjtNum nn[3]; + return mjc_GeomDist(m, con->g, d->geom_xpos + 3 * con->g, d->geom_xmat + 9 * con->g, P[0], + nn, 1e30) - + radii[con->idx[0]]; + } + case mjcGEOM_CORNER_TRI: + return mjc_PtTri(&gv[3 * con->idx[0]], P[0], P[1], P[2], cp, w) - radii[con->idx[1]]; + case mjcGEOM_EDGE_EDGE: { + const mjtNum* eg = &ge[6 * con->idx[0]]; + return mjc_SegSeg(eg, eg + 3, P[0], P[1], c1, c2, st) - radii[con->idx[1]]; + } + default: + mju_error("conGapAdv: unknown pair type %d", con->type); + return 0; + } +} + +mjtNum mjc_advance(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* dxw, + const mjtNum* gv, const mjtNum* ge, const mjtNum* radii, int nfv, + const int* fidx, const mjcFlexPair* 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 mjcFlexPair* 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 == mjcFLEX_VERT_TRI) ? con->idx[1] : con->idx[2]; + int v[4], + self = (con->type <= mjcFLEX_EDGE_EDGE) && (con->idx[0] < nfv) && (other < nfv) && + (pt2flex[con->idx[0]] == pt2flex[other]); + int nv = mjc_pairVerts(v, con); + 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 == mjcFLEX_VERT_TRI) { + 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 == mjcFLEX_EDGE_EDGE) { + 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, radii, 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 mjcFlexPair* con, const mjtNum* radii) { + switch (con->type) { + case mjcFLEX_VERT_TRI: + return radii[con->idx[0]] + radii[con->idx[1]]; // vertex + triangle flex radius + case mjcFLEX_EDGE_EDGE: + return radii[con->idx[0]] + radii[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(mjcFlexPair con, const mjModel* m, const mjData* d, const mjtNum* x, + const mjtNum* gv, const mjtNum* ge, const mjtNum* radii, mjtNum thresh, + const mjtNum* dfrom, const mjtNum* dto, mjtNum ghat, mjcFlexPair* 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, radii, 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, radii); + 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 <= mjcFLEX_EDGE_EDGE && 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, radii, 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* radii, 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, mjcFlexPair* 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 + radii[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; + } + mjcFlexPair con = {mjcFLEX_VERT_GEOM, {v, 0, 0, 0}, gi}; + addCand(con, m, d, x, gv, ge, radii, 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]; + mjcFlexPair con = {mjcGEOM_CORNER_TRI, + {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, radii, 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; + mjcFlexPair con = {mjcGEOM_EDGE_EDGE, + {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, radii, 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(radii[v], rk)) + 4.0 * maxdisp; + mjtNum qh[3] = {thv + radii[v], thv + radii[v], thv + radii[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 + mjcFlexPair con = {mjcFLEX_VERT_TRI, {v, A, B, C}, -1}; + addCand(con, m, d, x, gv, ge, radii, 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(radii[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 + radii[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 + mjcFlexPair con = {mjcFLEX_EDGE_EDGE, {a1, b1, a2, b2}, -1}; + addCand(con, m, d, x, gv, ge, radii, 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 00000000..b6bf94e5 --- /dev/null +++ b/src/engine/engine_collision_continuous.h @@ -0,0 +1,127 @@ +// 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 +#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). + +// Pair types for continuous collision. +typedef enum { + mjcFLEX_VERT_TRI = 0, // flex vertex vs flex triangle + mjcFLEX_EDGE_EDGE = 1, // flex edge vs flex edge + mjcFLEX_VERT_GEOM = 2, // flex vertex vs geom surface + mjcGEOM_CORNER_TRI = 3, // geom corner vs flex triangle + mjcGEOM_EDGE_EDGE = 4, // geom edge vs flex edge +} mjcFlexPairType; + +// One candidate contact pair: the geometric identity only. +// idx holds free-point indices (types 0/1: all four; type 2: idx[0]; +// types 3/4: the flex-side points), g 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 { + mjcFlexPairType type; // pair type + int idx[4]; // participant free-point indices, meaning per type (see mjc_pairGap) + int g; // geom id, types 2-4 only +} mjcFlexPair; + +// the standoff of a pair whose detection band is `band`: min(band, cap) +mjtNum mjc_standoff(mjtNum band, mjtNum cap); + +// the involved free-point indices of a pair (up to 4), for iterating its vertices +int mjc_pairVerts(int* v, const mjcFlexPair* pair); + +// 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 mjcFlexPair* pair, const mjtNum* radii, mjtNum band); + +// 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), radii the per-free-point radii. Early-out beyond cutoff. +MJAPI mjtNum mjc_pairGap(const mjcFlexPair* pair, const mjModel* m, const mjData* d, + const mjtNum* x, const mjtNum* gv, const mjtNum* ge, const mjtNum* radii, + 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). +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* radii, 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, mjcFlexPair* 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). +MJAPI mjtNum mjc_advance(const mjModel* m, const mjData* d, const mjtNum* x, const mjtNum* dxw, + const mjtNum* gv, const mjtNum* ge, const mjtNum* radii, int nfv, + const int* fidx, const mjcFlexPair* cand, int ncand, const mjtNum* cgap, + const int* pt2flex, int* approut, mjtNum* toiout); + +// point-triangle distance (closest point cp and barycentric weights w) +MJAPI mjtNum mjc_PtTri(const mjtNum* p, const mjtNum* a, const mjtNum* b, const mjtNum* c, + mjtNum* cp, mjtNum* w); + +// segment-segment distance (closest points and line parameters st) +MJAPI mjtNum mjc_SegSeg(const mjtNum* p1, const mjtNum* p2, const mjtNum* q1, const mjtNum* q2, + mjtNum* cp1, mjtNum* cp2, mjtNum* st); + +// signed distance (+ outward unit normal n) from geom gi's surface, at pose +// gpos/gmat, to world point x; early-out beyond distmax +MJAPI mjtNum mjc_GeomDist(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + const mjtNum* x, mjtNum* n, mjtNum distmax); + +// world-space sharp vertices of geom gi at pose gpos/gmat (out sized by the caller); return count +MJAPI int mjc_GeomVerts(const mjModel* m, int gi, const mjtNum* gpos, const mjtNum* gmat, + mjtNum* out); + +// world-space sharp edges of geom gi at pose gpos/gmat (out sized by the caller); return count +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 85253578..8fd199b6 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 00000000..86345b4d --- /dev/null +++ b/test/engine/engine_collision_continuous_test.cc @@ -0,0 +1,394 @@ +// 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 +#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, MjTol(1e-12, 1e-5)); + + mjtNum p_edge[3] = {-1, 0.5, 0}; // nearest the x=0 edge + EXPECT_NEAR(PtTri(p_edge, a, b, c), 1.0, MjTol(1e-12, 1e-5)); + + mjtNum p_vert[3] = {-3, -4, 0}; // nearest vertex a + EXPECT_NEAR(PtTri(p_vert, a, b, c), 5.0, MjTol(1e-12, 1e-5)); + + mjtNum p_on[3] = {0.25, 0.25, 0}; // on the triangle + EXPECT_NEAR(PtTri(p_on, a, b, c), 0.0, MjTol(1e-12, 1e-5)); +} + +// segment-segment distance: perpendicular crossing, collinear gap, parallel +// offset +TEST_F(ContinuousCollisionTest, SegmentSegmentDistance) { + mjtNum p1[3] = {-1, 0, 0}, p2[3] = {1, 0, 0}; + + // perpendicular, 0.3 above + mjtNum q1[3] = {0, -1, 0.3}, q2[3] = {0, 1, 0.3}; + EXPECT_NEAR(SegSeg(p1, p2, q1, q2), 0.3, MjTol(1e-12, 1e-5)); + + mjtNum r1[3] = {2, 0, 0}, r2[3] = {3, 0, 0}; // collinear, gap 1 + EXPECT_NEAR(SegSeg(p1, p2, r1, r2), 1.0, MjTol(1e-12, 1e-5)); + + 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, MjTol(1e-12, 1e-5)); +} + +// ------------------------------- 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, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[0], 1, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[1], 0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[2], 0, MjTol(1e-12, 1e-5)); + + // 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, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[0], 1, MjTol(1e-12, 1e-5)); + + // 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, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[2], 1, MjTol(1e-12, 1e-5)); + + 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, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(std::fabs(verts[3*i + 1]), 0.2, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(std::fabs(verts[3*i + 2]), 0.3, MjTol(1e-12, 1e-5)); + } + // 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) < MjTol(1e-12, 1e-5) || + std::fabs(len-0.4) < MjTol(1e-12, 1e-5) || + std::fabs(len-0.6) < MjTol(1e-12, 1e-5)) + << "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 radii[4] = {0.005, 0.005, 0.005, 0.005}; + mjcFlexPair pair; + pair.type = mjcFLEX_VERT_TRI; + pair.idx[0] = 0; pair.idx[1] = 1; pair.idx[2] = 2; pair.idx[3] = 3; + pair.g = -1; + + mjtNum n[3], cw[4]; + int idv[4], nidx = 0; + mjtNum g = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, radii, n, idv, cw, + &nidx, 1e30); + // midsurface distance, radii not subtracted + EXPECT_NEAR(g, 0.5, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[0], 0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(n[1], 0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(std::fabs(n[2]), 1, MjTol(1e-12, 1e-5)); + EXPECT_GT(nidx, 0); + + // dg/d(vertex idv[p]) = cw[p]*n, by central differences + mjtNum eps = MjTol(1e-6, 1e-3); + 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, radii, n, + idv, cw, &nidx, 1e30); + x[3*idv[p] + k] = saved - eps; + mjtNum gm = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, radii, n, + idv, cw, &nidx, 1e30); + x[3*idv[p] + k] = saved; + mjtNum g0 = mjc_pairGap(&pair, m, d, x, nullptr, nullptr, radii, n, + idv, cw, &nidx, 1e30); + EXPECT_NEAR(cw[p]*n[k], (gp - gm) / (2*eps), MjTol(1e-6, 1e-3)) + << "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 radii[4] = {0.005, 0.005, 0.005, 0.005}; + int fidx[4] = {0, 1, 2, 3}; // all points free, identity map + // cross-flex pair: no coherent-motion mean removal + int pt2flex[4] = {0, 1, 1, 1}; + mjcFlexPair cand; + cand.type = mjcFLEX_VERT_TRI; + cand.idx[0] = 0; cand.idx[1] = 1; cand.idx[2] = 2; cand.idx[3] = 3; + cand.g = -1; + + mjtNum n[3], cw[4]; + int idv[4], nidx = 0; + mjtNum cgap[1]; + cgap[0] = mjc_pairGap(&cand, m, d, x, nullptr, nullptr, radii, n, idv, cw, + &nidx, 1e30); + ASSERT_NEAR(cgap[0], 0.5, MjTol(1e-12, 1e-5)); + + // 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, radii, 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, radii, 4, fidx, + &cand, 1, cgap, pt2flex, appr, toi); + EXPECT_NEAR(alpha, 1.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(toi[0], 1.0, MjTol(1e-12, 1e-5)); + + // 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, radii, 4, fidx, + &cand, 1, cgap, pt2flex, appr, toi); + EXPECT_NEAR(alpha, 1.0, MjTol(1e-12, 1e-5)); + EXPECT_NEAR(toi[0], 1.0, MjTol(1e-12, 1e-5)); + 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], radii[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]; + radii[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; + mjcFlexPair cand[256]; + int ncand = mjc_candidates(m, d, x, nullptr, nullptr, 0, 0, radii, 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 == mjcFLEX_VERT_TRI || + cand[c].type == mjcFLEX_EDGE_EDGE) + << "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