mujoco

// 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 <math.h>

#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include "engine/engine_memory.h"        // mj_arenaAllocByte, mjSTACKALLOC
#include "engine/engine_util_blas.h"     // mju_dot3, mju_mulMatVec3
#include "engine/engine_util_errmem.h"   // mju_warning

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];
    }
  }
}


// geom types mjc_GeomDist supports; a flex vertex has no continuous contact with the others
int mjc_GeomSupported(int type) {
  return type == mjGEOM_PLANE || type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE ||
         type == mjGEOM_BOX || type == mjGEOM_MESH;
}


// 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;
}


// the radius a geom's features carry: a sphere's centre and a capsule's axis stand that far
// inside the surface; a box's or mesh's corners and edges lie on it
static mjtNum featRadius(const mjModel* m, int gi) {
  if (gi < 0) return 0;
  int type = m->geom_type[gi];
  return (type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE) ? m->geom_size[3 * gi] : 0;
}


// world-space VERTICES of a static geom (the features that can poke through a flex triangle
// between its vertices): box -> 8 corners; mesh -> all its vertices; sphere -> its centre;
// capsule -> its two axis endpoints (both carrying the geom's radius, see featRadius); the plane
// 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_SPHERE) {
    for (int k=0; k < 3; k++) out[k] = gpos[k];
    return 1;
  }
  if (type == mjGEOM_CAPSULE) {
    mjtNum axis[3] = {0, 0, m->geom_size[3 * gi + 1]}, wa[3];  // half-length along the local z
    mju_mulMatVec3(wa, gmat, axis);
    for (int k=0; k < 3; k++) {
      out[k] = gpos[k] - wa[k];
      out[3 + k] = gpos[k] + wa[k];
    }
    return 2;
  }
  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); capsule -> its axis (carrying the geom's
// radius, see featRadius). 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_CAPSULE) {
    return mjc_GeomVerts(m, gi, gpos, gmat, out) == 2 ? 1 : 0;  // the two endpoints, one edge
  }
  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;  // MIDSURFACE distance: radii NOT subtracted (delta unchanged; see header)
    }
    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;

      // MIDSURFACE distance: flex radii NOT subtracted (delta unchanged; see header); the geom's
      // own radius is, for a sphere centre or capsule endpoint standing in for a corner
      return dd - featRadius(m, pair->g);
    }
    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 - featRadius(m, pair->g);  // as above, a capsule axis standing in for an edge
    }
    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) {
  int v[4];
  int nv = mjc_pairVerts(v, con);
  mjtNum P[4][3];

  // the conditional keeps the multiply-add from being contracted into an FMA, so a point's
  // advanced position is bit for bit what it was when the displacement was indexed per free dof
  for (int q=0; q < nv; q++)
    for (int k=0; k < 3; k++) {
      mjtNum dk = dxw[3 * v[q] + k];
      P[q][k] = x[3 * v[q] + k] + (dk != 0 ? t * dk : 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);  // midsurface, matching mjc_pairGap
    }
    case mjcGEOM_CORNER_TRI:  // midsurface less the feature's radius, matching mjc_pairGap
      return mjc_PtTri(&gv[3 * con->idx[0]], P[0], P[1], P[2], cp, w) - featRadius(m, con->g);
    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) - featRadius(m, con->g);
    }
    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 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++)
      for (int k=0; k < 3; k++) dp[q][k] = dxw[3 * v[q] + k];
    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)

    // this pair's own collision time, found on its own and capped at the full step: the earliest
    // filter compares these across pairs, so none may be cut short at the running minimum of
    // the others, which would tie every later pair with the earliest one
    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);
      mjtNum room = g - gtarget;
      if (room <= 1e-9 * g0) break;
      t += room / l;
      if (t >= 1.0) {
        t = 1.0;
        break;
      }
    }
    if (toiout) {
      toiout[c] = t;
    }
    if (t < alpha) {
      alpha = t;
    }
  }
  return alpha;
}


// the radii a midsurface gap reads ABOVE the old skin gap. Used ONLY to keep the broad-phase
// detection band at its pre-midsurface reach; every pair type measures at the flex midsurface,
// so each flex participant contributes its radius (geoms have none: their surface is the surface).
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
    case mjcFLEX_VERT_GEOM:
      return radii[con->idx[0]];  // flex vertex radius
    case mjcGEOM_CORNER_TRI:
      return radii[con->idx[1]];  // flex triangle radius
    case mjcGEOM_EDGE_EDGE:
      return radii[con->idx[1]];  // flex edge radius
    default:
      return 0.0;
  }
}


// append a candidate contact if its gap at x is below the (margin-inflated) detection threshold.
// Candidates are pushed onto the arena one at a time, as mj_collision pushes its pairs: consecutive
// pushes of one type are adjacent, so *base + *nc indexes them as an array. Once the arena is full
// *full is set and nothing more is pushed.
static void addCand(mjcFlexPair con, const mjModel* m, 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** base,
                    int* nc, int* full) {
  if (*full) 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: the gap is a minimum over the two features, each the convex hull of its
  // vertices, so over the step it shrinks by at most the largest displacement of a vertex of one
  // feature relative to a vertex of the other (a geom feature is static); a pair beyond its band
  // plus that bound cannot become active this step -> drop it. Bulk motion cancels in the
  // relative displacement, so a settling bag does not flood the list. The closest points' weights
  // at x are no bound: under deformation the closest points move to another region of the
  // features (ContinuousCollisionTest.CandidatesCoverARotatingTriangle).
  int vv[4], nvv = mjc_pairVerts(vv, &con);
  int n1 = con.type == mjcFLEX_EDGE_EDGE ? 2
           : (con.type == mjcFLEX_VERT_TRI || con.type == mjcFLEX_VERT_GEOM) ? 1 : 0;
  mjtNum dv[5][3] = {{0}};  // per pair vertex, the fifth entry standing in for a static feature
  for (int q=0; q < nvv; q++)
    for (int c=0; c < 3; c++) dv[q][c] = dto[3 * vv[q] + c] - dfrom[3 * vv[q] + c];
  int i0 = n1 ? 0 : 4, i1 = n1 ? n1 : 5, j0 = nvv > n1 ? n1 : 4, j1 = nvv > n1 ? nvv : 5;
  mjtNum bound = 0;
  for (int i=i0; i < i1; i++)
    for (int j=j0; j < j1; j++) {
      mjtNum dd[3];
      mju_sub3(dd, dv[i], dv[j]);
      mjtNum l = mju_sqrt(mju_dot3(dd, dd));
      if (l > bound) bound = l;
    }
  if (g < bo + mjc_pairBand(&con, radii, ghat) + bound) {
    mjcFlexPair* p =
        (mjcFlexPair*)mj_arenaAllocByte(d, sizeof(mjcFlexPair), _Alignof(mjcFlexPair));
    if (!p) {
      *full = 1;
      return;
    }
    *p = con;
    if (*nc == 0) *base = p;
    (*nc)++;
  }
}


// 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;
}


// contype/conaffinity filter, the rule of the native pipeline (filterBitmask in
// engine_collision_driver.c): a pair is filtered unless one side's type meets the other's affinity
static int maskFiltered(int contype1, int conaffinity1, int contype2, int conaffinity2) {
  return !(contype1 & conaffinity2) && !(contype2 & conaffinity1);
}


// the pairs the IPC step owns under the ipc flag (see the header)
int mjc_ipcOwnsFlexFlex(const mjModel* m, int f1, int f2) {
  return mjENABLED(mjENBL_IPC) && m->flex_dim[f1] == 2 && m->flex_dim[f2] == 2;
}


int mjc_ipcOwnsFlexGeom(const mjModel* m, int f, int g) {
  return mjENABLED(mjENBL_IPC) && m->flex_dim[f] == 2 &&
         m->body_weldid[m->geom_bodyid[g]] == 0 && mjc_GeomSupported(m->geom_type[g]);
}


// build the candidate-contact list: every pair whose gap can enter the detection band along the
// sweep dfrom -> dto. The reach of each query is the base threshold plus the travel of the points
// involved, taken per point (|dto - dfrom|) and per queried flex (the largest travel of its points,
// plus how far its points sit from the configuration its BVH was built at, the state's), so the
// bounds are conservative for the sweep however far a proposal flings a vertex, without inflating
// every query by the fastest vertex anywhere. Flex-flex and geom-feature-vs-flex pairs are found by
// querying the flex element BVH (bvhBox). The native collision filtering applies: nothing with
// contact (or constraints) disabled, pairs under the contype/conaffinity rule, self-contact under
// the flex's selfcollide.
int mjc_candidates(const mjModel* m, mjData* d, const mjtNum* x, const mjtNum* gv,
                   const mjtNum* ge, const int* gvgeom, const int* gegeom, int ngv, int nge,
                   const mjtNum* radii, mjtNum thresh, mjtNum threshGeom, const mjtNum* dfrom,
                   const mjtNum* dto, mjtNum ghat, int nfv, int npt, const int* fidx,
                   const int* pjnv, const int* flist, const int* fxadr, int nfd,
                   const int* pt2flex, mjcFlexPair** cand) {
  (void)npt;  // increment A: candidates are flex-only (npt==nfv); rigid bodies carry no hard contact
  int nc = 0, full = 0;
  *cand = NULL;
  if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)) return 0;  // as mj_collision
  mj_markStack(d);
  // the sweep: travel per point, and per flex the largest travel of its points (fsweep) and the
  // largest distance of its points from the BVH's configuration (fstale)
  mjtNum* dl = mjSTACKALLOC(d, nfv > 0 ? nfv : 1, mjtNum);
  mjtNum* fsweep = mjSTACKALLOC(d, nfd > 0 ? nfd : 1, mjtNum);
  mjtNum* fstale = mjSTACKALLOC(d, nfd > 0 ? nfd : 1, mjtNum);
  for (int k=0; k < nfd; k++) fsweep[k] = fstale[k] = 0;
  for (int v=0; v < nfv; v++) {
    int k = pt2flex[v];
    int vg = m->flex_vertadr[flist[k]] + v - fxadr[k];
    mjtNum dd[3], ds[3];
    for (int c=0; c < 3; c++) {
      dd[c] = dto[3 * v + c] - dfrom[3 * v + c];
      ds[c] = x[3 * v + c] - d->flexvert_xpos[3 * vg + c];
    }
    dl[v] = mju_sqrt(mju_dot3(dd, dd));
    mjtNum stale = mju_sqrt(mju_dot3(ds, ds));
    if (dl[v] > fsweep[k]) fsweep[k] = dl[v];
    if (stale > fstale[k]) fstale[k] = stale;
  }
  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 (!mjc_GeomSupported(m->geom_type[gi])) continue;  // its native rows are kept instead
    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++) {  // free points and points riding a moving body
      if (fidx[v] < 0 && !(pjnv && pjnv[v] > 0)) continue;
      int f = flist[pt2flex[v]];
      if (maskFiltered(m->geom_contype[gi], m->geom_conaffinity[gi], m->flex_contype[f],
                       m->flex_conaffinity[f]))
        continue;
      mjtNum reach = thresh + dl[v];  // the point's own travel: the geom is static
      mjtNum marg = reach + 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, reach, dfrom, dto, ghat, cand, &nc, &full);
    }
  }

  // (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 self-compatible masks) and INTER-FLEX otherwise (gated by the
  // two flexes' masks). 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 = mjSTACKALLOC(d, maxbvh, int);
  int* outel = mjSTACKALLOC(d, maxel, int);
  int* stampG = mjSTACKALLOC(d, maxen, int);
  for (int e=0; e < maxen; e++) stampG[e] = -1;
  int qid = 0;
  // flex-pair filter fok[k*nfd+kj]: flex flist[kj] against flex flist[k]'s elements
  int* fok = mjSTACKALLOC(d, nfd * nfd, int);
  for (int k=0; k < nfd; k++) {
    for (int kj=0; kj < nfd; kj++) {
      int fk = flist[k], fj = flist[kj];
      int ok = !maskFiltered(m->flex_contype[fk], m->flex_conaffinity[fk], m->flex_contype[fj],
                             m->flex_conaffinity[fj]);
      if (kj == k) ok = ok && (m->flex_selfcollide[fk] != mjFLEXSELF_NONE);
      fok[k * nfd + kj] = ok;
    }
  }

  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 ct_k = m->flex_contype[fk], ca_k = m->flex_conaffinity[fk];

    // (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 (a
    // convex-decomposition bin's ~1600 edges otherwise produce hundreds of thousands of
    // candidates). Only the flex side moves: its reach grows by the flex's travel, and its BVH
    // query by the staleness of the tree as well.
    // A feature carries its geom (con.g) for the masks and for the radius a sphere centre or
    // capsule axis stands inside the surface, which widens its query box.
    mjtNum reachG = threshGeom + fsweep[k], boxG = reachG + rk + fstale[k];
    for (int c=0; c < ngv; c++) {
      int gi = gvgeom[c];
      if (maskFiltered(m->geom_contype[gi], m->geom_conaffinity[gi], ct_k, ca_k)) continue;
      mjtNum bx = boxG + featRadius(m, gi);
      mjtNum qhvG[3] = {bx, bx, bx};
      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]},
                           gi};
        addCand(con, m, d, x, gv, ge, radii, reachG, dfrom, dto, ghat, cand, &nc, &full);
      }
    }
    // geom-edge vs flex edge (type 4); dedup the shared triangle edges per query via stampG.
    for (int c=0; c < nge; c++) {
      int gi = gegeom[c];
      if (maskFiltered(m->geom_contype[gi], m->geom_conaffinity[gi], ct_k, ca_k)) continue;
      const mjtNum* p0 = &ge[6 * c];
      const mjtNum* p1 = &ge[6 * c + 3];
      mjtNum qc[3], qh[3], bx = boxG + featRadius(m, gi);
      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]) + bx;
      }
      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},
                        gi};
          addCand(con, m, d, x, gv, ge, radii, reachG, dfrom, dto, ghat, cand, &nc, &full);
        }
      }
    }
    // flex vertex vs flex triangle (type 0): self (same flex) + inter-flex, both under fok.
    // 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 (!fok[k * nfd + kv]) continue;  // filtered flex pair
      // per-pair band: the thinner flex sets it, capped by the global band; the reach adds the
      // travel of both sides, the BVH query the tree's staleness as well
      mjtNum thv = 3.0 * min2(ghat, min2(radii[v], rk)) + dl[v] + fsweep[k];
      mjtNum bv = thv + radii[v] + fstale[k];
      mjtNum qh[3] = {bv, bv, bv};
      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, &full);
      }
    }
    // flex edge vs flex edge (type 1): symmetric, so canonical -- querying flex kj <= k, and e2 >
    // e1 within a flex. Self (kj==k) and inter-flex (kj<k) both under fok.
    for (int kj=0; kj <= k; kj++) {
      int self = (kj == k);
      if (!fok[k * nfd + kj]) continue;  // filtered flex pair
      int fj = flist[kj], ea_j = m->flex_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];
        // per-pair band plus the travel of both sides; the query adds the tree's staleness
        mjtNum the = 3.0 * min2(ghat, min2(radii[a1], rk)) + (dl[a1] > dl[b1] ? dl[a1] : dl[b1]) +
                     fsweep[k];
        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] + fstale[k];
        }
        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, &full);
          }
        }
      }
    }
  }
  mj_freeStack(d);
  if (full) {
    mju_warning("IPC: arena full after %d candidate pairs; the rest were dropped, so geometry "
                "there can pass through. Increase the model's memory attribute. Time = %.4f",
                nc, d->time);
  }
  return nc;
}