mujoco

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "engine/engine_collision_driver.h"

#include <stddef.h>
#include <string.h>

#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h>  // IWYU pragma: keep
#include "engine/engine_callback.h"
#include "engine/engine_collision_convex.h"
#include "engine/engine_collision_gjk.h"
#include "engine/engine_collision_primitive.h"
#include "engine/engine_collision_sdf.h"
#include "engine/engine_thread.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_core_util.h"
#include "engine/engine_inline.h"
#include "engine/engine_macro.h"
#include "engine/engine_memory.h"
#include "engine/engine_sleep.h"
#include "engine/engine_sort.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"


// table of pairwise collision functions
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
  /*              PLANE  HFIELD  SPHERE            CAPSULE             ELLIPSOID         CYLINDER            BOX               MESH              SDF */
  /*PLANE     */ {0,     0,      mjc_PlaneSphere,  mjc_PlaneCapsule,   mjc_PlaneConvex,  mjc_PlaneCylinder,  mjc_PlaneBox,     mjc_PlaneConvex,  mjc_PlaneConvex},
  /*HFIELD    */ {0,     0,      mjc_ConvexHField, mjc_ConvexHField,   mjc_ConvexHField, mjc_ConvexHField,   mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF},
  /*SPHERE    */ {0,     0,      mjc_SphereSphere, mjc_SphereCapsule,  mjc_Convex,       mjc_SphereCylinder, mjc_SphereBox,    mjc_Convex,       mjc_SDF},
  /*CAPSULE   */ {0,     0,      0,                mjc_CapsuleCapsule, mjc_Convex,       mjc_Convex,         mjc_CapsuleBox,   mjc_Convex,       mjc_SDF},
  /*ELLIPSOID */ {0,     0,      0,                0,                  mjc_Convex,       mjc_Convex,         mjc_Convex,       mjc_Convex,       mjc_SDF},
  /*CYLINDER  */ {0,     0,      0,                0,                  0,                mjc_Convex,         mjc_Convex,       mjc_Convex,       mjc_SDF},
  /*BOX       */ {0,     0,      0,                0,                  0,                0,                  mjc_BoxBox,       mjc_Convex,       mjc_SDF},
  /*MESH      */ {0,     0,      0,                0,                  0,                0,                  0,                mjc_Convex,       mjc_MeshSDF},
  /*SDF       */ {0,     0,      0,                0,                  0,                0,                  0,                0,                mjc_SDF}
};



//------------------------------------ utility functions ------------------------------------------


// return the maximum number of contacts that can be generated between two geoms
// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0
// indicates that the geoms have a positive margin
int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) {
  int type1 = m->geom_type[g1];
  int type2 = m->geom_type[g2];

  if (type1 == mjGEOM_SDF || type2 == mjGEOM_SDF) {
    return m->opt.sdf_initpoints;
  }

  if (type1 == mjGEOM_HFIELD || type2 == mjGEOM_HFIELD) {
    int type = (type1 == mjGEOM_HFIELD) ? type2 : type1;
    return (type != mjGEOM_PLANE && type != mjGEOM_HFIELD) ? mjMAXCONPAIR : 0;
  }

  // spheres and ellipsoids always generate a single contact
  if (type1 == mjGEOM_SPHERE || type1 == mjGEOM_ELLIPSOID ||
      type2 == mjGEOM_SPHERE || type2 == mjGEOM_ELLIPSOID) {
    return 1;
  }

  // box-box primitive collider
  if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
    return 8;
  }

  // capsule-capsule primitive collider
  if (type1 == mjGEOM_CAPSULE && type2 == mjGEOM_CAPSULE) {
    return 2;
  }

  // capsule-box primitive collider
  if ((type1 == mjGEOM_CAPSULE && type2 == mjGEOM_BOX) ||
      (type1 == mjGEOM_BOX && type2 == mjGEOM_CAPSULE)) {
    return 4;
  }

  // the remaining plane cases
  if (type1 == mjGEOM_PLANE || type2 == mjGEOM_PLANE) {
    int type = (type1 == mjGEOM_PLANE) ? type2 : type1;
    switch (type) {
      case mjGEOM_CAPSULE:
        return 2;
      case mjGEOM_CYLINDER:
      case mjGEOM_BOX:
        return 4;
      case mjGEOM_MESH:
        return 3;
      default:
        return 0;
    }
  }

  int is_multiccd = !mjDISABLED(mjDSBL_MULTICCD);
  if (!is_multiccd) {
    return 1;
  }

  if (type1 == mjGEOM_CAPSULE || type2 == mjGEOM_CAPSULE ||
      type1 == mjGEOM_CYLINDER || type2 == mjGEOM_CYLINDER) {
    return 5;
  }

  if (mjDISABLED(mjDSBL_NATIVECCD)) {
    return is_multiccd ? 5 : 1;  // mesh-mesh or mesh-box with libccd
  }

  // check margin from model
  if (has_margin < 0) {
    has_margin = 0;
    if (mjENABLED(mjENBL_OVERRIDE)) {
      has_margin = m->opt.o_margin > 0.0;
    } else {
      int npair = m->npair;
      int ipair = -1;
      for (int k=0; k < npair; k++) {
        if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) ||
            (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) {
            ipair = k;
            break;
          }
        }

      if (ipair > -1) {
        has_margin = m->pair_margin[ipair] > 0.0;
      } else {
        has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0;
      }
    }
  }

  // 4 contacts for mesh-mesh or mesh-box without margins, 5 with margins
  return has_margin ? 5 : 4;
}


// return the margin for a given geom pair
static inline mjtNum getMargin(const mjModel* m, int g1, int g2, int ipair) {
  if (ipair >= 0) {
    return mj_assignMargin(m, m->pair_margin[ipair]);
  }
  return mj_assignMargin(m, m->geom_margin[g1] + m->geom_margin[g2]);
}


// return the gap for a given geom pair
static inline mjtNum getGap(const mjModel* m, int g1, int g2, int ipair) {
  if (ipair >= 0) {
    return m->pair_gap[ipair];
  }
  return m->geom_gap[g1] + m->geom_gap[g2];
}


// move arena pointer back to the end of the contact array
static inline void resetArena(mjData* d) {
  d->parena = d->ncon * sizeof(mjContact);
#ifdef mjUSEASAN
    ASAN_POISON_MEMORY_REGION(
      (char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
#endif
}


// realign the arena pointer to the specified alignment
static inline size_t alignArena(mjData* d, size_t alignment) {
  size_t misalignment = d->parena % alignment;
  if (misalignment) {
    d->parena += alignment - misalignment;
  }
  return d->parena;
}


// plane to geom_center squared distance, g1 is a plane
static mjtNum planeGeomDist(const mjModel* m, mjData* d, int g1, int g2) {
  mjtNum* mat1 = d->geom_xmat + 9*g1;
  mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
  mjtNum dif[3];

  mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
  return mju_dot3(dif, norm);
}


// return 1 if body has plane geom, 0 otherwise
static int hasPlane(const mjModel* m, int body) {
  int start = m->body_geomadr[body];
  int end = m->body_geomadr[body] + m->body_geomnum[body];

  // scan geoms belonging to body
  int g;
  for (g=start; g < end; g++) {
    if (m->geom_type[g] == mjGEOM_PLANE) {
      return 1;
    }
  }

  return 0;
}


// filter contact based on type and affinity
static int filterBitmask(int contype1, int conaffinity1,
                         int contype2, int conaffinity2) {
  return !(contype1 & conaffinity2) && !(contype2 & conaffinity1);
}


// filter contact based on global AABBs
static int filterBox(const mjtNum aabb1[6], const mjtNum aabb2[6], mjtNum margin) {
  if (aabb1[0]+aabb1[3]+margin < aabb2[0]-aabb2[3]) return 1;
  if (aabb1[1]+aabb1[4]+margin < aabb2[1]-aabb2[4]) return 1;
  if (aabb1[2]+aabb1[5]+margin < aabb2[2]-aabb2[5]) return 1;
  if (aabb2[0]+aabb2[3]+margin < aabb1[0]-aabb1[3]) return 1;
  if (aabb2[1]+aabb2[4]+margin < aabb1[1]-aabb1[4]) return 1;
  if (aabb2[2]+aabb2[5]+margin < aabb1[2]-aabb1[5]) return 1;
  return 0;
}


// filter contact based sphere-box test, treating sphere as box
static int filterSphereBox(const mjtNum s[3], mjtNum bound, const mjtNum aabb[6]) {
  if (s[0]+bound < aabb[0]-aabb[3]) return 1;
  if (s[1]+bound < aabb[1]-aabb[4]) return 1;
  if (s[2]+bound < aabb[2]-aabb[5]) return 1;
  if (s[0]-bound > aabb[0]+aabb[3]) return 1;
  if (s[1]-bound > aabb[1]+aabb[4]) return 1;
  if (s[2]-bound > aabb[2]+aabb[5]) return 1;
  return 0;
}


// filter contact based on bounding sphere test (raw)
static int filterSphere(const mjtNum pos1[3], const mjtNum pos2[3], mjtNum bound) {
  mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
  mjtNum distsqr = dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];

  return (distsqr > bound*bound);
}


// filter contact based on bounding sphere test
static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
  // neither geom is a plane
  if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) {
    return filterSphere(d->geom_xpos + 3*g1, d->geom_xpos + 3*g2,
                        m->geom_rbound[g1] + m->geom_rbound[g2] + margin);
  }

  // one geom is a plane
  if (m->geom_type[g1] == mjGEOM_PLANE && m->geom_rbound[g2] > 0
      && planeGeomDist(m, d, g1, g2) > margin + m->geom_rbound[g2]) {
    return 1;
  }
  if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0
      && planeGeomDist(m, d, g2, g1) > margin + m->geom_rbound[g1]) {
    return 1;
  }
  return 0;
}


// filter body pair; 1: discard, 0: proceed
static int filterBodyPair(int weldbody1, int weldparent1, int asleep1, int dofnum1,
                          int weldbody2, int weldparent2, int asleep2, int dofnum2,
                          int dsbl_filterparent) {
  // same weldbody check
  if (weldbody1 == weldbody2) {
    return 1;
  }

  // both dof-less: no forces can act, skip
  if (dofnum1 == 0 && dofnum2 == 0) {
    return 1;
  }

  // both asleep check
  if (asleep1 && asleep2) {
    return 1;
  }

  // asleep and world-static check (mocap has weldbody != 0, still triggers wake)
  if ((asleep1 && !weldbody2) || (asleep2 && !weldbody1)) {
    return 1;
  }

  // weldparent check
  if ((!dsbl_filterparent && weldbody1 != 0 && weldbody2 != 0) &&
      (weldbody1 == weldparent2 || weldbody2 == weldparent1)) {
    return 1;
  }

  // all tests passed
  return 0;
}


// return 1 if bodyflex can collide, 0 otherwise
static int canCollide(const mjModel* m, int bf) {
  if (bf < m->nbody) {
    return (m->body_contype[bf] || m->body_conaffinity[bf]);
  } else {
    int f = bf - m->nbody;
    return (m->flex_contype[f] || m->flex_conaffinity[f]);
  }
}


// return 1 if two bodyflexes can collide, 0 otherwise
static int canCollide2(const mjModel* m, int bf1, int bf2) {
  int nbody = m->nbody;
  int contype1 = (bf1 < nbody) ? m->body_contype[bf1] : m->flex_contype[bf1-nbody];
  int conaffinity1 = (bf1 < nbody) ? m->body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody];
  int contype2 = (bf2 < nbody) ? m->body_contype[bf2] : m->flex_contype[bf2-nbody];
  int conaffinity2 = (bf2 < nbody) ? m->body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody];

  // opposite of bitmask filter
  return (!filterBitmask(contype1, conaffinity1, contype2, conaffinity2));
}


// return 1 if element is active, 0 otherwise
int mj_isElemActive(const mjModel* m, int f, int e) {
  if (m->flex_dim[f] < 3) {
    return 1;
  } else {
    return (m->flex_elemlayer[m->flex_elemadr[f]+e] < m->flex_activelayers[f]);
  }
}


//----------------------------- collision detection entry point ------------------------------------

// internal structure for storing pairs meant for narrowphase collision detection
typedef struct {
  // type of collision pair
  enum {
    mjCPAIR_GEOM_GEOM = 0,  // mjCOLLISIONFUNC[type1][type2]
    mjCPAIR_GEOM_FLEX,      // mj_collidePlaneFlex, mj_collideSdfFlex
    mjCPAIR_GEOM_ELEM,      // mj_collideGeomElem
    mjCPAIR_ELEM_VERT,      // mj_collideElemVert
    mjCPAIR_ELEM_ELEM,      // mj_collideElems
  } type;
  int conpos;
  union {
    struct { int g1, g2, ipair; } geom_geom;
    struct { int g, f; } geom_flex;
    struct { int g, f, e; } geom_elem;
    struct { int f, e, v; } elem_vert;
    struct { int f1, e1, f2, e2; } elem_elem;
  };
} mjcPair;


// initialize a default collision pair
static inline void defaultPair(mjcPair* pair, int type) {
  pair->type = type;
  // clear all fields in the union
  pair->elem_elem.f1 = -1;
  pair->elem_elem.e1 = -1;
  pair->elem_elem.f2 = -1;
  pair->elem_elem.e2 = -1;
}


// binary search between two bodyflex trees
static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
                           int merged, int startadr, int pairadr);

// compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena
static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, int npair,
                           size_t parena);

// test a plane geom and a flex for collision, add to contact list
static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f);

// test an SDF geom and a flex for collision, add to contact list
static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f);

// test for internal flex collisions, add to contact list
static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f);

// compare contact pairs by their geom/elem/vert IDs
static inline int contactcompare(const mjContact* c1, const mjContact* c2, void* context) {
  const mjModel* m = (const mjModel*) context;

  // get colliding object ids
  int con1_obj1 = c1->geom[0] >= 0 ? c1->geom[0] : (c1->elem[0] >= 0 ? c1->elem[0] : c1->vert[0]);
  int con1_obj2 = c1->geom[1] >= 0 ? c1->geom[1] : (c1->elem[1] >= 0 ? c1->elem[1] : c1->vert[1]);
  int con2_obj1 = c2->geom[0] >= 0 ? c2->geom[0] : (c2->elem[0] >= 0 ? c2->elem[0] : c2->vert[0]);
  int con2_obj2 = c2->geom[1] >= 0 ? c2->geom[1] : (c2->elem[1] >= 0 ? c2->elem[1] : c2->vert[1]);

  // for geom:geom, reproduce the order of contacts without mj_collideTree
  // normally sorted by (g1, g2), but g1 and g2 are swapped based on geom_type
  // here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
  if (c1->geom[0] >= 0 && c1->geom[1] >= 0 &&
      c2->geom[0] >= 0 && c2->geom[1] >= 0) {
    if (m->geom_type[con1_obj1] > m->geom_type[con1_obj2]) {
      int tmp = con1_obj1;
      con1_obj1 = con1_obj2;
      con1_obj2 = tmp;
    }
    if (m->geom_type[con2_obj1] > m->geom_type[con2_obj2]) {
      int tmp = con2_obj1;
      con2_obj1 = con2_obj2;
      con2_obj2 = tmp;
    }
  }
  if (con1_obj1 < con2_obj1) return -1;
  if (con1_obj1 > con2_obj1) return 1;
  if (con1_obj2 < con2_obj2) return -1;
  if (con1_obj2 > con2_obj2) return 1;
  return 0;
}

// define contactSort function for sorting contacts
mjSORT(contactSort, mjContact, contactcompare);


// filter flex contacts based on distance
static void filterFlexContacts(mjData* d, int ncon_before) {
  int n = d->ncon - ncon_before;
  if (n <= mjMAXCONPAIR) {
    return;
  }

  mjContact* contacts = d->contact + ncon_before;

  mj_markStack(d);
  mjtByte* selected = mjSTACKALLOC(d, n, mjtByte);
  mjtNum* min_dist = mjSTACKALLOC(d, n, mjtNum);
  memset(selected, 0, n);

  for (int i = 0; i < n; i++) {
    min_dist[i] = mjMAXVAL;
  }

  // start with the deepest penetrating contact
  int nselected = 0;
  int best = 0;
  mjtNum bestdist = -contacts[0].dist;
  for (int i = 1; i < n; i++) {
    if (-contacts[i].dist > bestdist) {
      bestdist = -contacts[i].dist;
      best = i;
    }
  }

  while (nselected < mjMAXCONPAIR && best >= 0) {
    selected[best] = 1;
    mjtNum* bestpos = contacts[best].pos;

    int nextbest = -1;
    mjtNum nextbestdist = -1;
    for (int i = 0; i < n; i++) {
      if (selected[i]) continue;

      mjtNum dx = contacts[i].pos[0] - bestpos[0];
      mjtNum dy = contacts[i].pos[1] - bestpos[1];
      mjtNum dz = contacts[i].pos[2] - bestpos[2];
      mjtNum d2 = dx*dx + dy*dy + dz*dz;
      if (d2 < min_dist[i]) {
        min_dist[i] = d2;
      }
      if (min_dist[i] > nextbestdist) {
        nextbestdist = min_dist[i];
        nextbest = i;
      }
    }

    if (nselected < mjMAXCONPAIR - 1) {
      mjContact temp = contacts[nselected];
      contacts[nselected] = contacts[best];
      contacts[best] = temp;

      if (nextbest == nselected) {
        nextbest = best;
      }
    }

    nselected++;
    best = nextbest;
  }

  mj_freeStack(d);

  d->ncon = ncon_before + nselected;
  resetArena(d);
}


// push a candidate collision pair onto the arena
static void pushPairArena(mjData* d, mjcPair* pair) {
  // allocate geom pair on the arena
  mjcPair* new_pair = (mjcPair*) mj_arenaAllocByte(d, sizeof(mjcPair), _Alignof(mjcPair));
  if (!new_pair) {
    mjERROR("arena too small to allocate geom pair");
  }
  *new_pair = *pair;
}


// push a geom-geom collision pair onto the arena
static void pushGeomGeom(const mjModel* m, mjData* d, int g1, int g2, int ipair) {
  if (m->geom_type[g1] > m->geom_type[g2]) {
    int tmp = g1;
    g1 = g2;
    g2 = tmp;
  }

  mjcPair pair;
  defaultPair(&pair, mjCPAIR_GEOM_GEOM);
  pair.geom_geom.g1 = g1;
  pair.geom_geom.g2 = g2;
  pair.geom_geom.ipair = ipair;
  pushPairArena(d, &pair);
}


// filter candidate collision pair; return 0 if pair should be discarded
static int filterCollisionPair(const mjModel* m, mjData* d, int g1, int g2, int ipair,
                               int merged, int startadr, int pairadr) {
  // merged, find matching pair
  if (merged) {
    for (int k=startadr; k < pairadr; k++) {
      if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) ||
          (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) {
        return 0;
      }
    }
  }

  if (ipair >= 0) {
    if (mjENABLED(mjENBL_SLEEP)) {
      int b1 = m->geom_bodyid[g1];
      int b2 = m->geom_bodyid[g2];
      if (d->body_awake[b1] != mjS_AWAKE && d->body_awake[b2] != mjS_AWAKE) {
        return 0;
      }
    }
  }

  if (ipair < 0) {
    if (mjcb_contactfilter) {
      if (mjcb_contactfilter(m, d, g1, g2)) {
        return 0;
      }
    } else if (filterBitmask(m->geom_contype[g1], m->geom_conaffinity[g1],
                             m->geom_contype[g2], m->geom_conaffinity[g2])) {
      return 0;
    }
  }

  // bounding sphere filter
  mjtNum margin = getMargin(m, g1, g2, ipair);
  mjtNum gap = getGap(m, g1, g2, ipair);
  if (mj_filterSphere(m, d, g1, g2, margin + gap)) {
    return 0;
  }

  // highly unlikely, but check collision function is well-defined
  int type1 = mjMIN(m->geom_type[g1], m->geom_type[g2]);
  int type2 = mjMAX(m->geom_type[g1], m->geom_type[g2]);
  return mjCOLLISIONFUNC[type1][type2] != NULL;
}


// main collision function
void mj_collision(const mjModel* m, mjData* d) {
  TM_START1;

  int nexclude = m->nexclude, npair = m->npair, nbody = m->nbody;
  int nbodyflex = m->nbody + m->nflex;
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < nbody;

  // reset the size of the contact array and invalidate efc arrays
  d->ncon = 0;
  resetArena(d);
  mj_clearEfc(d);

  // return if disabled
  if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) || nbodyflex < 2) {
    TM_END1(mjTIMER_POS_COLLISION);
    return;
  }

  // reset the visualization flags
  if (m->vis.global.bvactive) {
    memset(d->bvh_active, 0, m->nbvh);
  }

  mj_markStack(d);

  // broadphase collision detector
  TM_START;
  int nmaxpairs = (nbodyflex*(nbodyflex - 1))/2;
  int* broadphasepair = mjSTACKALLOC(d, nmaxpairs, int);
  int nbfpair = mj_broadphase(m, d, broadphasepair, nmaxpairs);
  unsigned int last_signature = -1;
  TM_END(mjTIMER_COL_BROAD);

  // narrowphase and midphase collision detector
  TM_RESTART;

  // process bodyflex pairs returned by broadphase, merge with predefined geom pairs
  int pairadr = 0, ngeompair = 0, g1, g2;

  // align the arena for candidate collision pairs for narrowphase
  size_t parena = alignArena(d, _Alignof(int));

  for (int i=0; i < nbfpair; i++) {
    // reconstruct bodyflex pair ids
    int bf1 = (broadphasepair[i]>>16) & 0xFFFF;
    int bf2 = broadphasepair[i] & 0xFFFF;

    // compute signature for this bodyflex pair
    unsigned int signature = (bf1<<16) + bf2;
    // pairs come sorted by signature, but may not be unique
    // if signature is repeated, skip it
    if (signature == last_signature) {
      continue;
    }
    last_signature = signature;

    // merge predefined geom pairs
    int merged = 0;
    int startadr = pairadr;
    // test all predefined pairs for which pair_signature <= signature
    for (; pairadr < npair && m->pair_signature[pairadr] <= signature; pairadr++) {
      merged = (m->pair_signature[pairadr] == signature);
      g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr];
      if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) {
        pushGeomGeom(m, d, g1, g2, pairadr);
        ngeompair++;
      }
    }

    // apply bitmask filtering at the bodyflex level
    if (!canCollide2(m, bf1, bf2)) {
      continue;
    }

    // handle body pair exclusion
    int exadr = 0;
    if (nexclude) {
      // advance exadr while exclude_signature < signature
      while (exadr < nexclude && m->exclude_signature[exadr] < signature) {
        exadr++;
      }

      // skip this bodyflex pair if its signature is found in exclude array
      if (exadr < nexclude && m->exclude_signature[exadr] == signature) {
        continue;
      }
    }

    // get bodyflex info
    int isbody1 = (bf1 < nbody);
    int isbody2 = (bf2 < nbody);
    int bvh1 = (isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]);
    int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]);
    int geomadr1 = (isbody1 ? m->body_geomadr[bf1] : -1);
    int geomadr2 = (isbody2 ? m->body_geomadr[bf2] : -1);

    // process bodyflex pair: two single-geom bodies
    if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1) {
      if (filterCollisionPair(m, d, geomadr1, geomadr2, -1, merged, startadr, pairadr)) {
        pushGeomGeom(m, d, geomadr1, geomadr2, -1);
        ngeompair++;
      }
    }

    // process bodyflex pair: midphase
    else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1 >= 0 && bvh2 >= 0) {
      // flush geom pairs before calling mj_collideTree as post sorting needs to happen
      if (ngeompair > 0) {
        mj_narrowphase(m, d, NULL, ngeompair, parena);
        ngeompair = 0;
      }

      int ncon_before = d->ncon;
      mj_collideTree(m, d, bf1, bf2, merged, startadr, pairadr);
      int ncon_after = d->ncon;

      // filter flex contacts (limit per geom-flex or flex-flex pair)
      if (bf1 >= nbody || bf2 >= nbody) {
        filterFlexContacts(d, ncon_before);
        ncon_after = d->ncon;
      }

      // sort contacts
      int n = ncon_after - ncon_before;
      if (n > 1) {
        mj_markStack(d);
        mjContact* buf = mjSTACKALLOC(d, n, mjContact);
        contactSort(d->contact + ncon_before, buf, n, (void*)m);
        mj_freeStack(d);
      }

      // realign the arena after adding contacts
      parena = alignArena(d, _Alignof(int));
    }

    // process bodyflex pair: all-to-all
    else {
      int geomadr_end1 = geomadr1 + m->body_geomnum[bf1];
      int geomadr_end2 = geomadr2 + m->body_geomnum[bf2];

      // body : body
      if (isbody1 && isbody2) {
        for (g1=geomadr1; g1 < geomadr_end1; g1++) {
          for (g2=geomadr2; g2 < geomadr_end2; g2++) {
            if (filterCollisionPair(m, d, g1, g2, -1, merged, startadr, pairadr)) {
              pushGeomGeom(m, d, g1, g2, -1);
              ngeompair++;
            }
          }
        }
      }

      // body : flex
      else if (isbody1) {
        if (ngeompair > 0) {
          mj_narrowphase(m, d, NULL, ngeompair, parena);
          ngeompair = 0;
        }

        int f = bf2 - nbody;

        // process body geoms
        for (int g=m->body_geomadr[bf1]; g < geomadr_end1; g++) {
          // bitmask filtering at the geom-flex level
          if (filterBitmask(m->geom_contype[g], m->geom_conaffinity[g],
                            m->flex_contype[f], m->flex_conaffinity[f])) {
            continue;
          }

          // plane special processing
          if (m->geom_type[g] == mjGEOM_PLANE) {
            int ncon_before = d->ncon;
            mj_collidePlaneFlex(m, d, g, f);
            filterFlexContacts(d, ncon_before);
            continue;
          }

          // SDF special processing
          if (m->geom_type[g] == mjGEOM_SDF) {
            int ncon_before = d->ncon;
            mj_collideSdfFlex(m, d, g, f);
            filterFlexContacts(d, ncon_before);
            continue;
          }

          // collide geom with flex elements
          int ncon_before = d->ncon;
          int elemnum = m->flex_elemnum[f];
          for (int e=0; e < elemnum; e++) {
            mj_collideGeomElem(m, d, g, f, e);
          }
          filterFlexContacts(d, ncon_before);
        }
        // realign the arena after adding contacts
        parena = alignArena(d, _Alignof(int));
      }

      // flex : flex
      else {
        // flush accumulated geompairs before flex:flex processing
        if (ngeompair > 0) {
          mj_narrowphase(m, d, NULL, ngeompair, parena);
          ngeompair = 0;
        }

        int f1 = bf1 - nbody;
        int f2 = bf2 - nbody;

        // collide elements of two flexes
        int ncon_before = d->ncon;
        for (int e1=0; e1 < m->flex_elemnum[f1]; e1++) {
          for (int e2=0; e2 < m->flex_elemnum[f2]; e2++) {
            mj_collideElems(m, d, f1, e1, f2, e2);
          }
        }
        filterFlexContacts(d, ncon_before);

        // realign the arena after adding contacts
        parena = alignArena(d, _Alignof(int));
      }
    }
  }
  mj_freeStack(d);

  // finish merging predefined geom pairs
  for (; pairadr < npair; pairadr++) {
    g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr];
    if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) {
      pushGeomGeom(m, d, g1, g2, pairadr);
      ngeompair++;
    }
  }

  // flush remaining collision pairs
  if (ngeompair > 0) {
    mj_narrowphase(m, d, NULL, ngeompair, parena);
    ngeompair = 0;
  }

  // flex self-collisions
  for (int f=0; f < m->nflex; f++) {
    if (!m->flex_rigid[f] && (m->flex_contype[f] & m->flex_conaffinity[f])) {
      // skip if flex is asleep
      if (sleep_filter && mj_sleepState(m, d, mjOBJ_FLEX, f) == mjS_ASLEEP) continue;

      // internal collisions
      if (m->flex_internal[f]) {
        int ncon_before = d->ncon;
        mj_collideFlexInternal(m, d, f);
        filterFlexContacts(d, ncon_before);
      }

      // active element collisions
      if (m->flex_selfcollide[f] != mjFLEXSELF_NONE) {
        int ncon_before = d->ncon;

        // element-element: midphase
        if (!mjDISABLED(mjDSBL_MIDPHASE) &&
            m->flex_selfcollide[f] != mjFLEXSELF_NARROW &&
            m->flex_bvhadr[f] >= 0) {
          // select midphase mode
          if (m->flex_selfcollide[f] == mjFLEXSELF_BVH ||
              (m->flex_selfcollide[f] == mjFLEXSELF_AUTO && m->flex_dim[f] == 3)) {
            mj_collideTree(m, d, nbody+f, nbody+f, 0, 0, 0);
          } else {
            mj_collideFlexSAP(m, d, f);
          }
        }

        // element-element: direct
        else {
          int flex_elemnum = m->flex_elemnum[f];
          for (int e1=0; e1 < flex_elemnum; e1++) {
            if (mj_isElemActive(m, f, e1)) {
              for (int e2=e1+1; e2 < flex_elemnum; e2++) {
                if (mj_isElemActive(m, f, e2)) {
                  mj_collideElems(m, d, f, e1, f, e2);
                }
              }
            }
          }
        }

        filterFlexContacts(d, ncon_before);
      }
    }
  }

  // end narrowphase and midphase timer
  TM_END(mjTIMER_COL_NARROW);
  TM_END1(mjTIMER_POS_COLLISION);
}


//------------------------------------ binary tree search ------------------------------------------

// collision tree node
typedef struct  {
  int node1;
  int node2;
} mjCollisionTree;

// oriented bounding boxes collision (see Gottschalk et al.)
int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
                  const mjtNum xpos1[3], const mjtNum xmat1[9],
                  const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin,
                  mjtNum product[36], mjtNum offset[12], mjtBool* initialize) {
  // get infinite dimensions (planes only)
  mjtBool inf1[3] = {aabb1[3] >= mjMAXVAL, aabb1[4] >= mjMAXVAL, aabb1[5] >= mjMAXVAL};
  mjtBool inf2[3] = {aabb2[3] >= mjMAXVAL, aabb2[4] >= mjMAXVAL, aabb2[5] >= mjMAXVAL};

  // if a bounding box is infinite, there must be a collision
  if ((inf1[0] && inf1[1] && inf1[2]) || (inf2[0] && inf2[1] && inf2[2])) {
    return 1;
  }

  const mjtNum* aabb[2] = {aabb1, aabb2};
  const mjtNum *xmat[2] = {xmat1, xmat2};
  const mjtNum *xpos[2] = {xpos1, xpos2};
  mjtNum xcenter[2][3], normal[2][3][3];
  mjtNum proj[2], radius[2];
  mjtBool infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]};

  // compute centers in local coordinates
  if (product == NULL) {
    for (int i=0; i < 2; i++) {  // bounding boxes
      if (xmat[i]) {
        mju_mulMatVec3(xcenter[i], xmat[i], aabb[i]);
      } else {
        mju_copy3(xcenter[i], aabb[i]);
      }

      if (xpos[i]) {
        mju_addTo3(xcenter[i], xpos[i]);
      }
    }
  }

  // compute normals in global coordinates
  for (int i=0; i < 2; i++) {  // bounding boxes
    for (int j=0; j < 3; j++) {  // faces
      for (int k=0; k < 3; k++) {  // world axes
        if (xmat[i]) {
          normal[i][j][k] = xmat[i][3*k+j];
        } else {
          normal[i][j][k] = (j == k);
        }
      }
    }
  }

  // precompute dot products
  if (product && offset && *initialize) {
    for (int i=0; i < 2; i++) {  // bodies
      for (int j=0; j < 2; j++) {  // bodies
        for (int k=0; k < 3; k++) {  // axes
          for (int l=0; l < 3; l++) {  // axes
            product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]);
          }
          offset[6*i + 3*j + k] = xpos[i] ? mju_dot3(xpos[i], normal[j][k]) : 0;
        }
      }
    }
    *initialize = 0;
  }

  // check intersections
  for (int j=0; j < 2; j++) {  // bounding boxes
    if (infinite[1-j]) {
      continue;  // skip test against an infinite body
    }
    for (int k=0; k < 3; k++) {  // face
      for (int i=0; i < 2; i++) {  // bounding boxes
        if (product == NULL) {
          proj[i] = mju_dot3(xcenter[i], normal[j][k]);
          radius[i] = mju_abs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) +
                      mju_abs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) +
                      mju_abs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k]));
        } else {
          int adr = 18*i + 9*j + 3*k;
          proj[i] = aabb[i][0] * product[adr + 0] +
                    aabb[i][1] * product[adr + 1] +
                    aabb[i][2] * product[adr + 2] +
                    offset[6*i + 3*j + k];
          radius[i] = mju_abs(aabb[i][3]*product[adr + 0]) +
                      mju_abs(aabb[i][4]*product[adr + 1]) +
                      mju_abs(aabb[i][5]*product[adr + 2]);
        }
      }

      if (radius[0]+radius[1]+margin < mju_abs(proj[1]-proj[0])) {
        return 0;
      }
    }
  }

  return 1;
}


// binary search between two bodyflex trees
static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
                           int merged, int startadr, int pairadr) {
  int nbody = m->nbody, nbvhstatic = m->nbvhstatic;
  mjtBool isbody1 = (bf1 < nbody);
  mjtBool isbody2 = (bf2 < nbody);
  int f1 = isbody1 ? -1 : bf1 - nbody;
  int f2 = isbody2 ? -1 : bf2 - nbody;
  int mark_active = m->vis.global.bvactive;
  const int bvhadr1 = isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[f1];
  const int bvhadr2 = isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[f2];
  const int* child1 = m->bvh_child + 2*bvhadr1;
  const int* child2 = m->bvh_child + 2*bvhadr2;
  const mjtNum* bvh1 = isbody1 ? (m->bvh_aabb     + 6*bvhadr1) :
                                 (d->bvh_aabb_dyn + 6*(bvhadr1 - nbvhstatic));
  const mjtNum* bvh2 = isbody2 ? (m->bvh_aabb     + 6*bvhadr2) :
                                 (d->bvh_aabb_dyn + 6*(bvhadr2 - nbvhstatic));

  // used with rotated bounding boxes (when bodies are involved)
  mjtNum product[36];  // 2 bb x 2 bb x 3 axes (body) x 3 axes (world)
  mjtNum offset[12];   // 2 bb x 2 bb x 3 axes (world)
  mjtBool initialize = true;

  // bitmask filter for bodyflex pair
  if (!canCollide2(m, bf1, bf2)) {
    return;
  }

  mj_markStack(d);
  // TODO(b/273737633): Store bvh max depths to make this bound tighter.
  const int max_stack = (isbody1 ? m->body_bvhnum[bf1] : m->flex_bvhnum[f1]) +
                        (isbody2 ? m->body_bvhnum[bf2] : m->flex_bvhnum[f2]);
  mjCollisionTree* stack = mjSTACKALLOC(d, max_stack, mjCollisionTree);

  int nstack = 1;
  stack[0].node1 = stack[0].node2 = 0;

  // for body:flex, if dof-less body has planes, call mj_collidePlaneFlex directly
  if (isbody1 && !isbody2 && m->body_dofnum[m->body_weldid[bf1]] == 0) {
    for (int i=m->body_geomadr[bf1]; i < m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) {
      if (m->geom_type[i] == mjGEOM_PLANE) {
        mj_collidePlaneFlex(m, d, i, f2);
      }
    }
  }

  // for body:flex, if body has SDFs, call mj_collideSdfFlex directly
  if (isbody1 && !isbody2) {
    for (int i=m->body_geomadr[bf1]; i < m->body_geomadr[bf1]+m->body_geomnum[bf1]; i++) {
      if (m->geom_type[i] == mjGEOM_SDF) {
        mj_collideSdfFlex(m, d, i, f2);
      }
    }
  }

  // collide trees
  while (nstack) {
    // pop from stack
    nstack--;
    int node1 = stack[nstack].node1;
    int node2 = stack[nstack].node2;
    mjtBool isleaf1 = (child1[2*node1] < 0) && (child1[2*node1+1] < 0);
    mjtBool isleaf2 = (child2[2*node2] < 0) && (child2[2*node2+1] < 0);
    int nodeid1 = m->bvh_nodeid[bvhadr1 + node1];
    int nodeid2 = m->bvh_nodeid[bvhadr2 + node2];

    // SHOULD NOT OCCUR
    if ((isleaf1 && nodeid1 < 0) || (isleaf2 && nodeid2 < 0)) {
      mju_error("BVH leaf has invalid node id");
    }

    // self-collision: avoid repeated pairs
    if (bf1 == bf2 && node1 > node2) {
      continue;
    }

    // body : body
    if (isbody1 && isbody2) {
      // both are leaves
      if (isleaf1 && isleaf2) {
        mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->geom_margin[nodeid2]);
        mjtNum gap = m->geom_gap[nodeid1] + m->geom_gap[nodeid2];

        if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin + gap)) {
          if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
                            d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
                            d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
                            margin + gap, NULL, NULL, &initialize)) {
            if (filterCollisionPair(m, d, nodeid1, nodeid2, -1, merged, startadr, pairadr)) {
              int n1 = nodeid1, n2 = nodeid2;
              if (m->geom_type[n1] > m->geom_type[n2]) {
                n1 = nodeid2;
                n2 = nodeid1;
              }
              mjcPair pair;
              defaultPair(&pair, mjCPAIR_GEOM_GEOM);
              pair.geom_geom.g1 = n1;
              pair.geom_geom.g2 = n2;
              mj_narrowphase(m, d, &pair, 1, 0);
            }
            if (mark_active) {
              d->bvh_active[node1 + bvhadr1] = true;
              d->bvh_active[node2 + bvhadr2] = true;
            }
          }
        }
        continue;
      }

      // if no intersection at intermediate levels, stop
      mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->body_margin[bf2]);
      if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
                         d->xipos + 3*bf1, d->ximat + 9*bf1,
                         d->xipos + 3*bf2, d->ximat + 9*bf2,
                         margin, product, offset, &initialize)) {
        continue;
      }
    }

    // body : flex
    else if (isbody1 && !isbody2) {
      // both are leaves
      if (isleaf1 && isleaf2) {
        mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]);
        mjtNum gap = m->geom_gap[nodeid1] + m->flex_gap[f2];

        if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1],
                           m->flex_contype[f2], m->flex_conaffinity[f2]) &&
            !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin + gap,
                             bvh2 + 6*node2)) {
          if (mj_collideOBB(m->geom_aabb + 6*nodeid1, bvh2 + 6*node2,
                            d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
                            NULL, NULL,
                            margin + gap, NULL, NULL, &initialize)) {
            // collide unless geom is plane or SDF (handled separately)
            if (m->geom_type[nodeid1] != mjGEOM_PLANE &&
                m->geom_type[nodeid1] != mjGEOM_SDF) {
              mj_collideGeomElem(m, d, nodeid1, f2, nodeid2);
            }
            if (mark_active) {
              d->bvh_active[node1 + bvhadr1] = true;
              d->bvh_active[node2 + bvhadr2] = true;
            }
          }
        }
        continue;
      }

      // if no intersection at intermediate levels, stop
      mjtNum margin = mj_assignMargin(m, m->body_margin[bf1] + m->flex_margin[f2]);
      if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
                         d->xipos + 3*bf1, d->ximat + 9*bf1,
                         NULL, NULL,
                         margin, product, offset, &initialize)) {
        continue;
      }
    }

    // flex : body  SHOULD NOT OCCUR
    else if (!isbody1 && isbody2) {
      mjERROR("BVH flex : body collision should not occur");
    }

    // flex : flex
    else {
      // both are leaves
      // box filter applied in mj_collideElems, bitmask filter applied earlier
      if (isleaf1 && isleaf2) {
        mj_collideElems(m, d, f1, nodeid1, f2, nodeid2);
        if (mark_active) {
          d->bvh_active[node1 + bvhadr1] = true;
          d->bvh_active[node2 + bvhadr2] = true;
        }
        continue;
      }

      // if no intersection at intermediate levels, stop
      mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
      if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) {
        continue;
      }
    }

    if (mark_active) {
      d->bvh_active[node1 + bvhadr1] = true;
      d->bvh_active[node2 + bvhadr2] = true;
    }

    // keep traversing the tree
    if (!isleaf1 && isleaf2) {
      for (int i=0; i < 2; i++) {
        if (child1[2*node1+i] != -1) {
          if (nstack >= max_stack) {
            mjERROR("BVH stack depth exceeded.");  // SHOULD NOT OCCUR
          }
          stack[nstack].node1 = child1[2*node1+i];
          stack[nstack].node2 = node2;
          nstack++;
        }
      }
    } else if (isleaf1 && !isleaf2) {
      for (int i=0; i < 2; i++) {
        if (child2[2*node2+i] != -1) {
          if (nstack >= max_stack) {
            mjERROR("BVH stack depth exceeded.");  // SHOULD NOT OCCUR
          }
          stack[nstack].node1 = node1;
          stack[nstack].node2 = child2[2*node2+i];
          nstack++;
        }
      }
    } else {
      // compute surface areas of bounding boxes
      mjtNum x1 = bvh1[6*node1+3]-bvh1[6*node1+0];
      mjtNum y1 = bvh1[6*node1+4]-bvh1[6*node1+1];
      mjtNum z1 = bvh1[6*node1+5]-bvh1[6*node1+2];
      mjtNum x2 = bvh2[6*node2+3]-bvh2[6*node2+0];
      mjtNum y2 = bvh2[6*node2+4]-bvh2[6*node2+1];
      mjtNum z2 = bvh2[6*node2+5]-bvh2[6*node2+2];
      mjtNum surface1 = x1*y1 + y1*z1 + z1*x1;
      mjtNum surface2 = x2*y2 + y2*z2 + z2*x2;

      // traverse the hierarchy whose bounding box has the larger surface area
      if (surface1 > surface2) {
        for (int i = 0; i < 2; i++) {
          if (child1[2 * node1 + i] != -1) {
            if (nstack >= max_stack) {
              mjERROR("BVH stack depth exceeded.");  // SHOULD NOT OCCUR
            }
            stack[nstack].node1 = child1[2 * node1 + i];
            stack[nstack].node2 = node2;
            nstack++;
          }
        }
      } else {
        for (int i = 0; i < 2; i++) {
          if (child2[2 * node2 + i] != -1) {
            if (nstack >= max_stack) {
              mjERROR("BVH stack depth exceeded.");  // SHOULD NOT OCCUR
            }
            stack[nstack].node1 = node1;
            stack[nstack].node2 = child2[2*node2+i];
            nstack++;
          }
        }
      }
    }
  }
  mj_freeStack(d);
}


//----------------------------- broad-phase collision detection ------------------------------------

// make AAMM (xmin[3], xmax[3]) for one bodyflex
static void makeAAMM(const mjModel* m, mjData* d,
                     mjtNum* x_min, mjtNum* y_min, mjtNum* z_min,
                     mjtNum* x_max, mjtNum* y_max, mjtNum* z_max,
                     int bf, const mjtNum* frame) {
  mjtNum aamm[6];
  mjtNum override_margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5 * m->opt.o_margin : 0;

  // body
  if (bf < m->nbody) {
    int body = bf;
    int body_geomnum = m->body_geomnum[body];

    // process all body geoms (body is collidable, should have geoms)
    for (int i=0; i < body_geomnum; i++) {
      int geom = m->body_geomadr[body]+i;
      mjtNum margin = override_margin ? override_margin
                                       : m->geom_margin[geom] + m->geom_gap[geom];
      mjtNum _aamm[6];

      const mjtNum* aabb = m->geom_aabb + 6*geom;
      const mjtNum* size = m->geom_aabb + 6*geom + 3;
      const mjtNum* xpos = d->geom_xpos + 3*geom;
      const mjtNum* xmat = d->geom_xmat + 9*geom;

      // compute center in global coordinates
      mjtNum pos[3];
      mji_mulMatVec3(pos, xmat, aabb);
      mju_addTo3(pos, xpos);

      mjtNum axis[9];
      mji_transpose3(axis, xmat);
      mjtNum r_half = m->geom_rbound[geom];

      for (int j=0; j < 3; j++) {
        const mjtNum* frame_j = frame + 3*j;
        mjtNum aabb_cen = mju_dot3(pos, frame_j);
        mjtNum aabb_half = mju_abs(size[0] * mju_dot3(axis + 0, frame_j))
                         + mju_abs(size[1] * mju_dot3(axis + 3, frame_j))
                         + mju_abs(size[2] * mju_dot3(axis + 6, frame_j));
        mjtNum r_cen = mju_dot3(xpos, frame_j);
        _aamm[j + 0] = mju_max(r_cen - r_half, aabb_cen - aabb_half) - margin;
        _aamm[j + 3] = mju_min(r_cen + r_half, aabb_cen + aabb_half) + margin;
      }

      // update body aamm
      if (i == 0) {
        mju_copy(aamm, _aamm, 6);
      } else {
        for (int j=0; j < 3; j++) {
          aamm[j]   = mju_min(aamm[j],   _aamm[j]);
          aamm[j+3] = mju_max(aamm[j+3], _aamm[j+3]);
        }
      }
    }
  }

  // flex
  else {
    int f = bf - m->nbody;
    int flex_vertnum = m->flex_vertnum[f];
    const mjtNum* vbase = d->flexvert_xpos + 3*m->flex_vertadr[f];

    // process flex vertices
    for (int i=0; i < flex_vertnum; i++) {
      mjtNum v[3];

      // compute vertex coordinates in given frame
      mju_mulMatVec(v, frame, vbase+3*i, 3, 3);

      // update aamm
      if (i == 0) {
        mju_copy3(aamm, v);
        mju_copy3(aamm+3, v);
      } else {
        for (int j=0; j < 3; j++) {
          aamm[j]   = mju_min(aamm[j], v[j]);
          aamm[j+3] = mju_max(aamm[j+3], v[j]);
        }
      }
    }

    // correct for flex radius and margin
    mjtNum margin = override_margin ? override_margin
                                     : m->flex_margin[f] + m->flex_gap[f];
    mjtNum bound = m->flex_radius[f] + margin;
    aamm[0] -= bound;
    aamm[1] -= bound;
    aamm[2] -= bound;
    aamm[3] += bound;
    aamm[4] += bound;
    aamm[5] += bound;
  }

  // assign outputs
  *x_min = aamm[0];
  *y_min = aamm[1];
  *z_min = aamm[2];
  *x_max = aamm[3];
  *y_max = aamm[4];
  *z_max = aamm[5];
}


// add bodyflex pair in buffer; do not filter if m is NULL
static void add_pair(const mjModel* m, int bf1, int bf2,
                     int* npair, int* pair, int maxpair) {
  // add pair if there is room in buffer
  if ((*npair) < maxpair) {
    // contact filtering if m is not NULL
    if (m) {
      int nbody = m->nbody;
      int contype1, conaffinity1, contype2, conaffinity2;

      // get contype and conaffinity for bodyflex 1
      if (bf1 < nbody) {
        int body_geomadr1 = m->body_geomadr[bf1];
        int body_geomnum1 = m->body_geomnum[bf1];
        contype1 = conaffinity1 = 0;
        for (int i=body_geomadr1; i < body_geomadr1+body_geomnum1; i++) {
          contype1 |= m->geom_contype[i];
          conaffinity1 |= m->geom_conaffinity[i];
        }
      } else {
        contype1 = m->flex_contype[bf1-nbody];
        conaffinity1 = m->flex_conaffinity[bf1-nbody];
      }

      // get contype and conaffinity for bodyflex 2
      if (bf2 < nbody) {
        int body_geomadr2 = m->body_geomadr[bf2];
        int body_geomnum2 = m->body_geomnum[bf2];
        contype2 = conaffinity2 = 0;
        for (int i=body_geomadr2; i < body_geomadr2+body_geomnum2; i++) {
          contype2 |= m->geom_contype[i];
          conaffinity2 |= m->geom_conaffinity[i];
        }
      } else {
        contype2 = m->flex_contype[bf2-nbody];
        conaffinity2 = m->flex_conaffinity[bf2-nbody];
      }

      // compatibility check
      if (!(contype1 & conaffinity2) && !(contype2 & conaffinity1)) {
        return;
      }
    }

    // add pair
    if (bf1 < bf2) {
      pair[*npair] = (bf1<<16) + bf2;
    } else {
      pair[*npair] = (bf2<<16) + bf1;
    }
    (*npair)++;
  } else {
    mjERROR("broadphase buffer full");
  }
}


//----------------------------- general Sweep and Prune algorithm ----------------------------------

// helper structure for SAP sorting
struct _mjtSAP {
  float value;
  int id_ismax;
};
typedef struct _mjtSAP mjtSAP;



// comparison function for SAP
static inline int SAPcmp(mjtSAP* obj1, mjtSAP* obj2, void* context) {
  if (obj1->value < obj2->value) {
    return -1;
  } else if (obj1->value == obj2->value) {
    return 0;
  } else {
    return 1;
  }
}

// define SAPsort function for sorting SAP sorting
mjSORT(SAPsort, mjtSAP, SAPcmp);


// given list of axis-aligned bounding boxes in AAMM (min[3], max[3]) format,
// return list of pairs (i, j) in format (i<<16 + j) that can collide,
// using sweep-and-prune along specified x axis (0, 1 or 2).
static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis_x, int* pair, int maxpair) {
  // check inputs
  if (n >= 0x10000 || axis_x < 0 || axis_x > 2 || maxpair < 1) {
    return -1;
  }

  // allocate sort buffer
  mjtSAP* sortbuf = mjSTACKALLOC(d, 2*n, mjtSAP);    // sorted min and max values projected onto x axis
  mjtSAP* activebuf = mjSTACKALLOC(d, 2*n, mjtSAP);  // list of all AAMMs currently intersecting the sweep plane

  // get AAMM pointers for primary "x" axis
  const mjtNum* x_min = aamm + n*(axis_x + 0);
  const mjtNum* x_max = aamm + n*(axis_x + 3);

  // init sortbuf with specified axis
  for (int i=0; i < n; i++) {
    sortbuf[2*i].id_ismax = i;
    sortbuf[2*i].value = (float)x_min[i];
    sortbuf[2*i+1].id_ismax = i + 0x10000;
    sortbuf[2*i+1].value = (float)x_max[i];
  }

  // sort along specified axis
  mjtSAP* buf = mjSTACKALLOC(d, 2*n, mjtSAP);
  SAPsort(sortbuf, buf, 2*n, NULL);

  // define the other two axes
  int axis_y, axis_z;
  if (axis_x == 0) {
    axis_y = 1;
    axis_z = 2;
  } else if (axis_x == 1) {
    axis_y = 0;
    axis_z = 2;
  } else {
    axis_y = 0;
    axis_z = 1;
  }

  // get AAMM pointers to secondary "y, z" axes
  const mjtNum* y_min = aamm + n*(axis_y + 0);
  const mjtNum* y_max = aamm + n*(axis_y + 3);
  const mjtNum* z_min = aamm + n*(axis_z + 0);
  const mjtNum* z_max = aamm + n*(axis_z + 3);

  // sweep and prune
  int cnt = 0;    // size of active list
  int npair = 0;  // number of pairs added
  for (int i=0; i < 2*n; i++) {
    // min value: collide with all in list, add
    if (!(sortbuf[i].id_ismax & 0x10000)) {
      for (int j=0; j < cnt; j++) {
        // get ids: no need to mask ismax because activebuf entries never have the ismax bit,
        // and sortbuf[i].id_ismax is tested above
        int id1 = activebuf[j].id_ismax;
        int id2 = sortbuf[i].id_ismax;

        // use the other two axes to prune if possible
        if (y_min[id1] > y_max[id2] ||
            y_min[id2] > y_max[id1] ||
            z_min[id1] > z_max[id2] ||
            z_min[id2] > z_max[id1]) {
          continue;
        }

        // add pair, check buffer size
        pair[npair++] = (id1<<16) + id2;
        if (npair >= maxpair) {
          return maxpair;
        }
      }

      // add to list
      activebuf[cnt] = sortbuf[i];
      cnt++;
    }

    // max value: remove corresponding min value from list
    else {
      int toremove = sortbuf[i].id_ismax & 0xFFFF;
      for (int j=0; j < cnt; j++) {
        if (activebuf[j].id_ismax == toremove) {
          if (j < cnt-1) {
            memmove(activebuf+j, activebuf+j+1, sizeof(mjtSAP)*(cnt-1-j));
          }
          cnt--;
          break;
        }
      }
    }
  }

  return npair;
}


// add vector to covariance
static void updateCov(mjtNum cov[9], const mjtNum vec[3], const mjtNum cen[3]) {
  mjtNum dif[3] = {vec[0]-cen[0], vec[1]-cen[1], vec[2]-cen[2]};
  mjtNum D00 = dif[0]*dif[0];
  mjtNum D01 = dif[0]*dif[1];
  mjtNum D02 = dif[0]*dif[2];
  mjtNum D11 = dif[1]*dif[1];
  mjtNum D12 = dif[1]*dif[2];
  mjtNum D22 = dif[2]*dif[2];
  cov[0] += D00;
  cov[1] += D01;
  cov[2] += D02;
  cov[3] += D01;
  cov[4] += D11;
  cov[5] += D12;
  cov[6] += D02;
  cov[7] += D12;
  cov[8] += D22;
}


// comparison function for unsigned ints
static inline int uintcmp(int* i, int* j, void* context) {
  if ((unsigned) *i < (unsigned) *j) {
    return -1;
  } else if (*i == *j) {
    return 0;
  } else {
    return 1;
  }
}

// define bfsort function for sorting bodyflex pairs
mjSORT(bfsort, int, uintcmp);


// broadphase collision detector
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
  int npair = 0, nbody = m->nbody, ngeom = m->ngeom;
  int nvert = m->nflexvert, nflex = m->nflex, nbodyflex = m->nbody + m->nflex;
  int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
  int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->nbody_awake < nbody;
  mjtNum cov[9], cen[3], eigval[3], frame[9], quat[4];

  // init with pairs involving always-colliding bodies
  for (int b1=0; b1 < nbody; b1++) {
    // cannot collide
    if (!canCollide(m, b1)) {
      continue;
    }

    // b1 is world body with geoms, or dof-less body with plane (static or mocap)
    if ((b1 == 0 && m->body_geomnum[b1] > 0) ||
        (m->body_dofnum[m->body_weldid[b1]] == 0 && hasPlane(m, b1))) {
      int weld1 = m->body_weldid[b1];
      int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
      int dofnum1 = m->body_dofnum[weld1];

      // add b1:b2 pairs that are not welded together
      for (int b2=0; b2 < nbody; b2++) {
        // cannot collide
        if (!canCollide(m, b2)) {
          continue;
        }

        int weld2 = m->body_weldid[b2];
        int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
        int asleep2 = sleep_filter ? d->body_awake[b2] == mjS_ASLEEP : 0;
        int dofnum2 = m->body_dofnum[weld2];
        if (filterBodyPair(weld1, parent_weld1, 0, dofnum1,
                           weld2, parent_weld2, asleep2, dofnum2,
                           dsbl_filterparent)) {
          continue;
        }

        // add pair
        add_pair(m, b1, b2, &npair, bfpair, maxpair);
      }

      // add body:flex pairs, skip if flex asleep
      for (int f=0; f < nflex; f++) {
        if (sleep_filter && mj_sleepState(m, d, mjOBJ_FLEX, f) == mjS_ASLEEP) continue;
        add_pair(m, b1, nbody+f, &npair, bfpair, maxpair);
      }
    }
  }

  // find center of non-world geoms and flex vertices; return if none
  int cnt = 0;
  mju_zero3(cen);
  for (int i=0; i < ngeom; i++) {
    if (m->geom_bodyid[i]) {
      mju_addTo3(cen, d->geom_xpos+3*i);
      cnt++;
    }
  }
  for (int i=0; i < nvert; i++) {
    if (m->flex_vertbodyid[i]) {
      mju_addTo3(cen, d->flexvert_xpos+3*i);
      cnt++;
    }
  }
  if (cnt == 0) {
    return npair;
  }
  mju_scl3(cen, cen, 1.0/cnt);

  // compute covariance
  mju_zero(cov, 9);
  for (int i=0; i < ngeom; i++) {
    if (m->geom_bodyid[i]) {
      updateCov(cov, d->geom_xpos+3*i, cen);
    }
  }
  for (int i=0; i < nvert; i++) {
    if (m->flex_vertbodyid[i]) {
      updateCov(cov, d->flexvert_xpos+3*i, cen);
    }
  }
  mju_scl(cov, cov, 1.0/cnt, 9);

  // construct covariance-aligned 3D frame
  mju_eig3(eigval, frame, quat, cov);

  // allocate collidable bodyflex ids, construct list
  mj_markStack(d);
  int* bfid = mjSTACKALLOC(d, nbodyflex, int);
  int ncollide = 0;
  for (int i=1; i < nbodyflex; i++) {
    if (canCollide(m, i)) {
      bfid[ncollide++] = i;
    }
  }

  if (ncollide > 1) {
    // allocate and construct AAMMs for collidable only
    mjtNum* aamm = mjSTACKALLOC(d, 6*ncollide, mjtNum);
    for (int i=0; i < ncollide; i++) {
      // aamm is column-major (grouped per-axis rather than per-box for better cache locality)
      makeAAMM(m, d, aamm + 0*ncollide + i,
                     aamm + 1*ncollide + i,
                     aamm + 2*ncollide + i,
                     aamm + 3*ncollide + i,
                     aamm + 4*ncollide + i,
                     aamm + 5*ncollide + i, bfid[i], frame);
    }

    // call SAP
    int maxsappair = ncollide*(ncollide-1)/2;
    int* sappair = mjSTACKALLOC(d, maxsappair, int);
    int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair);
    if (nsappair < 0) {
      mjERROR("SAP failed");
    }

    // filter SAP pairs, convert to bodyflex pairs
    for (int i=0; i < nsappair; i++) {
      int bf1 = bfid[sappair[i] >> 16];
      int bf2 = bfid[sappair[i] & 0xFFFF];

      // body pair: prune based on sleep filter and weld filter
      if (bf1 < nbody && bf2 < nbody) {
        int weld1 = m->body_weldid[bf1];
        int weld2 = m->body_weldid[bf2];
        int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
        int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
        int asleep1 = sleep_filter ? d->body_awake[bf1] == mjS_ASLEEP : 0;
        int asleep2 = sleep_filter ? d->body_awake[bf2] == mjS_ASLEEP : 0;
        int dofnum1 = m->body_dofnum[weld1];
        int dofnum2 = m->body_dofnum[weld2];
        if (filterBodyPair(weld1, parent_weld1, asleep1, dofnum1,
                           weld2, parent_weld2, asleep2, dofnum2,
                           dsbl_filterparent)) {
          continue;
        }
      }

      // flex pair: skip if neither side is dynamically awake
      else if (sleep_filter) {
        int awake1 = (bf1 >= nbody) ? mj_sleepState(m, d, mjOBJ_FLEX, bf1-nbody) == mjS_AWAKE
                                    : d->body_awake[bf1] == mjS_AWAKE && m->body_treeid[bf1] >= 0;
        int awake2 = (bf2 >= nbody) ? mj_sleepState(m, d, mjOBJ_FLEX, bf2-nbody) == mjS_AWAKE
                                    : d->body_awake[bf2] == mjS_AWAKE && m->body_treeid[bf2] >= 0;
        if (!awake1 && !awake2) continue;
      }

      // add bodyflex pair if there is room in buffer
      add_pair(m, bf1, bf2, &npair, bfpair, maxpair);
    }
  }

  // sort bodyflex pairs by signature
  if (npair > 1) {
    int* buf = mjSTACKALLOC(d, npair, int);
    bfsort(bfpair, buf, npair, NULL);
  }

  mj_freeStack(d);
  return npair;
}


//----------------------------- narrow-phase collision detection -----------------------------------

// compute contact condim, solref, solimp, friction, adhesion
static void mj_contactParam(const mjModel* m, int* condim,
                            mjtNum* solref, mjtNum* solimp, mjtNum* friction,
                            mjtNum* adhesion, int g1, int g2, int f1, int f2) {
  mjtNum fri[3];

  // get parameters from geom1 or flex1
  int priority1 =           (f1 < 0) ? m->geom_priority[g1]     : m->flex_priority[f1];
  int condim1 =             (f1 < 0) ? m->geom_condim[g1]       : m->flex_condim[f1];
  mjtNum solmix1 =          (f1 < 0) ? m->geom_solmix[g1]       : m->flex_solmix[f1];
  const mjtNum* solref1 =   (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF;
  const mjtNum* solimp1 =   (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP;
  const mjtNum* friction1 = (f1 < 0) ? m->geom_friction+g1*3    : m->flex_friction+f1*3;
  mjtNum adhesion1 =        (f1 < 0) ? m->geom_adhesion[g1]     : 0;

  // get parameters from geom2 or flex2
  int priority2 =           (f2 < 0) ? m->geom_priority[g2]     : m->flex_priority[f2];
  int condim2 =             (f2 < 0) ? m->geom_condim[g2]       : m->flex_condim[f2];
  mjtNum solmix2 =          (f2 < 0) ? m->geom_solmix[g2]       : m->flex_solmix[f2];
  const mjtNum* solref2 =   (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF;
  const mjtNum* solimp2 =   (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP;
  const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3    : m->flex_friction+f2*3;
  mjtNum adhesion2 =        (f2 < 0) ? m->geom_adhesion[g2]     : 0;

  // adhesion: each surface contributes its own attraction
  *adhesion = adhesion1 + adhesion2;

  // different priority: copy from item with higher priority
  if (priority1 > priority2) {
    *condim = condim1;
    mju_copy(solref, solref1, mjNREF);
    mju_copy(solimp, solimp1, mjNIMP);
    mju_copy(fri, friction1, 3);
    *adhesion = adhesion1;
  }
  else if (priority1 < priority2) {
    *condim = condim2;
    mju_copy(solref, solref2, mjNREF);
    mju_copy(solimp, solimp2, mjNIMP);
    mju_copy(fri, friction2, 3);
    *adhesion = adhesion2;
  }

  // same priority
  else {
    // condim: max
    *condim = mjMAX(condim1, condim2);

    // compute solver mix factor
    mjtNum mix;
    if (solmix1 >= mjMINVAL && solmix2 >= mjMINVAL) {
      mix = solmix1 / (solmix1 + solmix2);
    } else if (solmix1 < mjMINVAL && solmix2 < mjMINVAL) {
      mix = 0.5;
    } else if (solmix1 < mjMINVAL) {
      mix = 0.0;
    } else {
      mix = 1.0;
    }

    // reference standard: mix
    if (solref1[0] > 0 && solref2[0] > 0) {
      for (int i=0; i < mjNREF; i++) {
        solref[i] = mix*solref1[i] + (1-mix)*solref2[i];
      }
    }

    // reference direct: min
    else {
      for (int i=0; i < mjNREF; i++) {
        solref[i] = mju_min(solref1[i], solref2[i]);
      }
    }

    // impedance: mix
    for (int i=0; i < mjNIMP; i++) {
      solimp[i] = mix*solimp1[i] + (1-mix)*solimp2[i];
    }

    // friction: max
    for (int i=0; i < 3; i++) {
      fri[i] = mju_max(friction1[i], friction2[i]);
    }
  }

  // unpack 5D friction
  friction[0] = fri[0];
  friction[1] = fri[0];
  friction[2] = fri[1];
  friction[3] = fri[2];
  friction[4] = fri[2];

  // SHOULD NOT OCCUR
  if (*condim > 6 || *condim < 1) {
    mjERROR("Invalid condim value: %d", *condim);
  }
}


// set contact parameters
static void mj_setContact(const mjModel* m, mjContact* con,
                          int condim, mjtNum includemargin,
                          const mjtNum* solref, const mjtNum* solreffriction,
                          const mjtNum* solimp, const mjtNum* friction,
                          mjtNum adhesion) {
  // set parameters
  con->dim = condim;
  con->includemargin = includemargin;
  con->adhesion = adhesion;
  mj_assignRef(m, con->solref, solref);
  mj_assignRef(m, con->solreffriction, solreffriction);
  mj_assignImp(m, con->solimp, solimp);
  mj_assignFriction(m, con->friction, friction);

  // exclude in gap; adhesive contacts remain active in the gap, producing
  // rows with positive violation whose reference acceleration pulls (tether)
  con->exclude = (con->dist >= includemargin) && !adhesion;

  // in-gap adhesive contacts have no surface contact and therefore no
  // friction: reduce to normal-only rows (with tangential rows present, fast
  // slip would recruit normal force and brake tangential motion at a distance)
  if (adhesion && con->dist >= includemargin) {
    con->dim = 1;
  }

  // complete frame
  mju_makeFrame(con->frame);

  // clear fields that are computed later
  con->efc_address = -1;
  con->mu = 0;
  mju_zero(con->H, 36);

  // set deprecated fields
  con->geom1 = con->geom[0];
  con->geom2 = con->geom[1];
}


// make capsule from two flex vertices
static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2],
                           mjtNum pos[3], mjtNum mat[9], mjtNum size[2]) {
  // get vertex positions
  mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[f] + vid[0]);
  mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[f] + vid[1]);

  // construct capsule from vertices
  mjtNum dif[3] = {v1[0]-v2[0], v1[1]-v2[1], v1[2]-v2[2]};
  size[0] = m->flex_radius[f];
  size[1] = 0.5*mju_normalize3(dif);

  mju_add3(pos, v1, v2);
  mju_scl3(pos, pos, 0.5);

  mjtNum quat[4];
  mju_quatZ2Vec(quat, dif);
  mju_quat2Mat(mat, quat);
}


// struct holding extra data for a flex precontact
typedef struct {
  int elem[2];
  int vert[2];
} mjPreFlex;

// struct for collision task
typedef struct {
  mjPreContact* conbuffer;    // pre-contact buffer returned by collision functions
  mjPreFlex* flexbuffer;      // pre-flex buffer returned by collision functions
  int* nconbuffer;            // contact count for each collision pair
  char* epabuffer;            // buffer for nativeccd
  int ccd_size;               // size of nativeccd buffer
  const mjcPair* pairbuffer;  // collision pairs
  int npair;                  // number of collision pairs
  int chunksize;              // number of pairs to process per task
  int maxcon;                 // maximum number of contacts (size of conbuffer)
} mjContactArg;


static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id, int idx) {
  mjContactArg* conargs = (mjContactArg*)arg;
  mjPreContact* conbuffer = conargs->conbuffer;
  char* epabuffer = conargs->epabuffer;
  int chunksize = conargs->chunksize;
  int globalidx = chunksize * idx;
  const mjcPair* pair = conargs->pairbuffer + globalidx;
  int* ncon = conargs->nconbuffer + chunksize * idx;

  int npair = conargs->npair;
  int n = mjMIN(chunksize, npair - globalidx);

  mjc_setCCDBuffer(epabuffer + thread_id * conargs->ccd_size);
  for (int i = 0; i < n; i++) {
    int g1 = pair[i].geom_geom.g1;
    int g2 = pair[i].geom_geom.g2;
    int ipair = pair[i].geom_geom.ipair;
    int conpos = pair[i].conpos;

    mjfCollision collision_func = mjCOLLISIONFUNC[m->geom_type[g1]][m->geom_type[g2]];
    mjtNum margin = getMargin(m, g1, g2, ipair);
    mjtNum gap = getGap(m, g1, g2, ipair);
    ncon[i] = collision_func(m, d, conbuffer + conpos, g1, g2, margin + gap);

    // SHOULD NOT OCCUR
    int expected_max = (globalidx + i + 1 < npair ? pair[i+1].conpos : conargs->maxcon) - conpos;
    if (ncon[i] > expected_max) {
      mjERROR("collision function returned %d contacts for geom pair (%d, %d), "
              "expected at most %d from mj_maxContact", ncon[i], g1, g2, expected_max);
    }
  }
  mjc_setCCDBuffer(NULL);
}


// compute contacts for a batch of collision pairs contained in a buffer of
// stride 3 ints (g1, g2, ipair)
// if buffer is NULL, results are read from arena starting at parena
static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, int npair,
                           size_t parena) {
  int nthread = mju_numThread(d);
  int npolygonmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->npolygonmax;
  int nmeshdegmax = mjDISABLED(mjDSBL_MULTICCD) ? 0 : m->nmeshdegmax;
  int ccd_size = mjc_ccdSize(npolygonmax, nmeshdegmax, m->opt.ccd_iterations);
  mjtNum margin, gap;

  // try to balance load of 5 chunks per thread (chunksize should be divisible by 16)
  int chunksize = npair / mjMAX(1, 5 * nthread);
  chunksize = mjMAX(16, (chunksize + 15) & ~15);  // round up to next 16
  int nchunk = (npair + chunksize - 1) / chunksize;

  // set buffer and arena pointer
  if (!buffer) {
    buffer = (const mjcPair*) ((char*) d->arena + parena);
  } else {
    parena = d->parena;
  }

  mj_markStack(d);

  // buffer for pair data
  mjcPair* pairbuffer = mjSTACKALLOC(d, npair, mjcPair);
  int maxcon = 0;
  for (int i = 0; i < npair; i++) {
    int g1 = buffer[i].geom_geom.g1;
    int g2 = buffer[i].geom_geom.g2;
    int ipair = buffer[i].geom_geom.ipair;

    pairbuffer[i] = buffer[i];
    pairbuffer[i].conpos = maxcon;
    margin = getMargin(m, g1, g2, ipair);
    gap = getGap(m, g1, g2, ipair);
    maxcon += mj_maxContact(m, g1, g2, margin + gap > 0);
  }

  // buffer data has been copied to metadata on the stack;
  // reclaim arena space so contacts can overwrite the buffer region
  d->parena = parena;

  mjContactArg arg;
  arg.ccd_size = ccd_size;
  arg.pairbuffer = pairbuffer;
  arg.nconbuffer = mjSTACKALLOC(d, npair, int);
  arg.conbuffer = mjSTACKALLOC(d, maxcon, mjPreContact);
  arg.npair = npair;
  arg.chunksize = chunksize;
  arg.maxcon = maxcon;

  // dispatch narrowphase to threads with local stack allocation for EPA
  {
    mj_markStack(d);
    arg.epabuffer = mj_stackAllocByte(d, ccd_size * nthread, sizeof(mjtNum));
    mju_dispatch(m, d, collisionTask, &arg, nchunk);
    mj_freeStack(d);
  }

  int ncon = 0;
  for (int i = 0; i < npair; i++) {
    ncon += arg.nconbuffer[i];
  }

  if (ncon == 0) {
    mj_freeStack(d);
    return;
  }

  // try allocate contact buffer in arena
  mjContact* con =
    (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * ncon, _Alignof(mjContact));
  if (!con) {
    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
    mj_freeStack(d);
    return;
  }
  d->ncon += ncon;

  // fill in contact data
  int conpos = 0;
  for (int i = 0; i < npair; i++) {
    if (!(ncon = arg.nconbuffer[i]))
      continue;

    int condim;
    mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
    mjtNum solreffriction[mjNREF] = {0};
    int g1 = pairbuffer[i].geom_geom.g1;
    int g2 = pairbuffer[i].geom_geom.g2;
    int ipair = pairbuffer[i].geom_geom.ipair;

    if (ipair >= 0) {
      condim = m->pair_dim[ipair];
      mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF);
      mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP);
      mju_copy(friction, m->pair_friction+5*ipair, 5);
      adhesion = m->pair_adhesion[ipair];
      if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) {
        mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF);
      }
    } else {
      mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g1, g2, -1, -1);
    }

    mjPreContact* bc = arg.conbuffer + pairbuffer[i].conpos;
    margin = getMargin(m, g1, g2, ipair);
    for (int j=0; j < ncon; j++) {
      mjContact* c = con + conpos + j;
      c->dist = bc[j].dist;
      mji_copy3(c->pos, bc[j].pos);
      mji_copy3(c->frame, bc[j].normal);
      mji_copy3(c->frame + 3, bc[j].tangent);
      c->geom[0] = g1;
      c->geom[1] = g2;
      c->flex[0] = -1;
      c->flex[1] = -1;
      c->elem[0] = -1;
      c->elem[1] = -1;
      c->vert[0] = -1;
      c->vert[1] = -1;
      mj_setContact(m, c, condim, margin, solref, solreffriction, solimp, friction, adhesion);
    }
    conpos += ncon;
  }
  mj_freeStack(d);
}


// test a plane geom and a flex for collision, add to contact list
static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) {
  mjContact con;
  mjtNum radius = m->flex_radius[f];
  mjtNum* pos = d->geom_xpos + 3*g;
  mjtNum* mat = d->geom_xmat + 9*g;
  mjtNum nrm[3] = {mat[2], mat[5], mat[8]};

  // prepare contact parameters (same for all vertices)
  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
  int condim;
  int flex_vertnum = m->flex_vertnum[f];
  mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // collide all flex vertices with plane
  for (int i=0; i < flex_vertnum; i++) {
    mjtNum* v = d->flexvert_xpos + 3*(m->flex_vertadr[f]+i);

    // distance from plane to vertex
    mjtNum dif[3] = {v[0]-pos[0], v[1]-pos[1], v[2]-pos[2]};
    mjtNum dist = mju_dot3(dif, nrm);

    // no contact
    if (dist > margin + gap + radius) {
      continue;
    }

    // create contact
    con.dist = dist - radius;
    mju_addScl3(con.pos, v, nrm, -con.dist*0.5 - radius);
    mju_copy3(con.frame, nrm);
    mju_zero3(con.frame+3);

    // set contact ids
    con.geom[0] = g;
    con.geom[1] = -1;
    con.flex[0] = -1;
    con.flex[1] = f;
    con.elem[0] = -1;
    con.elem[1] = -1;
    con.vert[0] = -1;
    con.vert[1] = i;

    // set remaining contact parameters
    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion);

    // add to mjData, abort if too many contacts
    if (mj_addContact(m, d, &con)) {
      return;
    }
  }
}


// test an SDF geom and a flex for collision, add to contact list
static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) {
  // only support dim==2 (triangular elements)
  if (m->flex_dim[f] != 2) {
    return;
  }

  // prepare contact parameters (same for all contacts)
  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
  mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
  int condim;
  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // allocate temporary contact array on stack (zero-initialized)
  mj_markStack(d);
  mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);
  int* elem = mjSTACKALLOC(d, mjMAXCONPAIR, int);

  // call batched flex-SDF collision
  int num = mjc_FlexSDF(m, d, precon, elem, g, f, margin + gap);

  // add contacts to mjData
  mjContact con;
  memset(&con, 0, sizeof(mjContact));
  for (int i = 0; i < num; i++) {
    // copy properties from mjPreContact to mjContact
    con.dist = precon[i].dist;
    mju_copy3(con.pos, precon[i].pos);
    mju_copy3(con.frame, precon[i].normal);
    mju_copy3(con.frame + 3, precon[i].tangent);

    // set contact ids
    con.geom[0] = g;
    con.geom[1] = -1;
    con.flex[0] = -1;
    con.flex[1] = f;
    con.elem[0] = -1;
    con.elem[1] = elem[i];
    con.vert[0] = -1;
    con.vert[1] = -1;

    // set remaining contact parameters
    mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction, adhesion);

    // add to mjData, abort if too many contacts
    if (mj_addContact(m, d, &con)) {
      mj_freeStack(d);
      return;
    }
  }

  mj_freeStack(d);
}


// test single triangle plane : vertex
static int planeVertex(mjPreContact* con, const mjtNum* pos, mjtNum rad,
                       int t0, int t1, int t2, int v) {
  // make t0 the origin
  mjtNum e1[3], e2[3], ev[3];
  mju_sub3(e1, pos+3*t1, pos+3*t0);
  mju_sub3(e2, pos+3*t2, pos+3*t0);
  mju_sub3(ev, pos+3*v,  pos+3*t0);

  // compute normal
  mjtNum nrm[3];
  mju_cross(nrm, e1, e2);
  mju_normalize3(nrm);

  // project, check distance
  mjtNum dst = mju_dot3(ev, nrm);
  if (dst <= -2*rad) {
    return 0;
  }

  // construct contact
  con->dist = -dst-2*rad;
  mju_scl3(con->normal, nrm, -1);
  mju_zero3(con->tangent);
  mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst);
  return 1;
}


// test for internal flex collisions, add to contact list
// ignore margin to avoid permament self-collision
static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) {
  int flex_evpairnum = m->flex_evpairnum[f];

  // predefined element-vertex
  for (int i=0; i < flex_evpairnum; i++) {
    const int* ev = m->flex_evpair + 2*m->flex_evpairadr[f] + 2*i;
    mj_collideElemVert(m, d, f, ev[0], ev[1]);
  }

  // within-element for tetrahedral only
  if (m->flex_dim[f] != 3) {
    return;
  }

  // initialize contact
  mjContact con;
  con.geom[0] = con.geom[1] = -1;
  con.flex[0] = con.flex[1] = f;
  con.elem[1] = con.vert[0] = -1;

  // prepare contact parameters
  int condim;
  int flex_elemnum = m->flex_elemnum[f];
  mjtNum radius = m->flex_radius[f];
  mjtNum solref[mjNREF], solimp[mjNIMP], friction[5], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f);
  condim = 1;

  // process all elements
  const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
  for (int e=0; e < flex_elemnum; e++) {
    mjPreContact precon;
    const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4;
    con.elem[0] = e;

    // face (0,1,2)
    if (planeVertex(&precon, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) {
      con.vert[1] = edata[3];
      con.dist = precon.dist;
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

    // face (0,2,3)
    if (planeVertex(&precon, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) {
      con.vert[1] = edata[1];
      con.dist = precon.dist;
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

    // face (0,3,1)
    if (planeVertex(&precon, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) {
      con.vert[1] = edata[2];
      con.dist = precon.dist;
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }

    // face (1,3,2)
    if (planeVertex(&precon, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) {
      con.vert[1] = edata[0];
      con.dist = precon.dist;
      mju_copy3(con.pos, precon.pos);
      mju_copy3(con.frame, precon.normal);
      mju_copy3(con.frame + 3, precon.tangent);
      mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction, adhesion);
      if (mj_addContact(m, d, &con)) return;
    }
  }
}


// test active element self-collisions with SAP
// ignore margin to avoid permanent self-collision
void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) {
  mj_markStack(d);

  // allocate and construct active element ids
  int* elid = mjSTACKALLOC(d, m->flex_elemnum[f], int);
  int nactive = 0;
  int flex_elemnum = m->flex_elemnum[f];
  for (int i=0; i < flex_elemnum; i++) {
    if (mj_isElemActive(m, f, i)) {
      elid[nactive++] = i;
    }
  }

  // nothing active
  if (nactive < 2) {
    mj_freeStack(d);
    return;
  }

  // allocate and construct AAMMs for active elements
  mjtNum* aamm = mjSTACKALLOC(d, 6*nactive, mjtNum);
  const mjtNum* elemaabb = d->flexelem_aabb + 6*m->flex_elemadr[f];
  for (int i=0; i < nactive; i++) {
    const mjtNum* center = elemaabb+6*elid[i];
    const mjtNum* radius = elemaabb+6*elid[i]+3;
    aamm[nactive*0 + i] = center[0] - radius[0];
    aamm[nactive*1 + i] = center[1] - radius[1];
    aamm[nactive*2 + i] = center[2] - radius[2];
    aamm[nactive*3 + i] = center[0] + radius[0];
    aamm[nactive*4 + i] = center[1] + radius[1];
    aamm[nactive*5 + i] = center[2] + radius[2];
  }

  // select largest axis from flex bvh
  const mjtNum* bvh = d->bvh_aabb_dyn + 6*(m->flex_bvhadr[f] - m->nbvhstatic);
  int axis = (bvh[3] > bvh[4] && bvh[3] > bvh[5]) ? 0 : (bvh[4] > bvh[5] ? 1 : 2);

  // call SAP; hard limit on number of pairs to avoid out-of-memory
  int maxsappair = mjMIN(nactive*(nactive-1)/2, 1000000);
  int* sappair = mjSTACKALLOC(d, maxsappair, int);
  int nsappair = mj_SAP(d, aamm, nactive, axis, sappair, maxsappair);
  if (nsappair < 0) {
    mjERROR("SAP failed");
  }

  // send SAP pairs to nearphase
  for (int i=0; i < nsappair; i++) {
    int e1 = elid[sappair[i] >> 16];
    int e2 = elid[sappair[i] & 0xFFFF];
    mj_collideElems(m, d, f, e1, f, e2);
  }

  mj_freeStack(d);
}


// test a geom and an elem for collision, add to contact list
void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
  mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]);
  mjtNum gap = m->geom_gap[g] + m->flex_gap[f];
  int dim = m->flex_dim[f], type = m->geom_type[g];
  int num;

  // bounding sphere test: only if midphase is disabled
  if (mjDISABLED(mjDSBL_MIDPHASE)) {
    int eglobal = m->flex_elemadr[f] + e;
    if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin+gap,
                        d->flexelem_aabb+6*eglobal)) {
      return;
    }
  }

  // skip if element has vertices on the same body as geom
  int b = m->geom_bodyid[g];
  const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
  const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f];
  for (int i=0; i <= dim; i++) {
    if (b >= 0 && b == bdata[edata[i]]) {
      return;
    }
  }

  // allocate mjPreContact[mjMAXCONPAIR] on the stack
  mj_markStack(d);
  mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);

  // sphere/capsule/box : capsule
  if (dim == 1 && (type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE || type == mjGEOM_BOX)) {
    // make capsule from vertices
    mjtNum pos[3], mat[9], size[2];
    mj_makeCapsule(m, d, f, m->flex_elem + m->flex_elemdataadr[f] + e*2,
                   pos, mat, size);

    // call raw primitive for corresponding geom type
    switch (type) {
      case mjGEOM_SPHERE:
        num = mjraw_SphereCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g,
                                  m->geom_size+3*g, pos, mat, size);
        break;
      case mjGEOM_CAPSULE:
        num = mjraw_CapsuleCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g,
                                   m->geom_size+3*g, pos, mat, size);
        break;
      case mjGEOM_BOX:
        num = mjraw_CapsuleBox(precon, margin + gap, pos, mat, size, d->geom_xpos+3*g,
                               d->geom_xmat+9*g, m->geom_size+3*g);
        break;
      default:
        num = 0;
    }
  }

  // heightfield : elem
  else if (type == mjGEOM_HFIELD) {
    num = mjc_HFieldElem(m, d, precon, g, f, e, margin + gap);
  }

  // sphere : triangle
  else if (type == mjGEOM_SPHERE && dim == 2) {
    const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
    num = mjraw_SphereTriangle(precon, margin + gap,
                               d->geom_xpos+3*g, m->geom_size[3*g],
                               vertxpos + 3*edata[0], vertxpos + 3*edata[1],
                               vertxpos + 3*edata[2], m->flex_radius[f]);
  }

  // box : triangle
  else if (type == mjGEOM_BOX && dim == 2) {
    const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
    num = mjraw_BoxTriangle(precon, margin + gap, d->geom_xpos + 3 * g,
                            d->geom_xmat + 9 * g, m->geom_size + 3 * g,
                            vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
                            vertxpos + 3 * edata[2], m->flex_radius[f]);
  }

  // capsule : triangle
  else if (type == mjGEOM_CAPSULE && dim == 2) {
    const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f];
    num = mjraw_CapsuleTriangle(
        precon, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g,
        m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1],
        vertxpos + 3 * edata[2], m->flex_radius[f]);
  }

  // general geom : elem
  else {
    num = mjc_ConvexElem(m, d, precon, g, -1, -1, -1, f, e, margin + gap);
  }

  // check contacts
  if (!num) {
    mj_freeStack(d);
    return;
  }

  // get contact parameters
  int condim;
  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, g, -1, -1, f);

  // allocate mjContact[num] on the arena
  mjContact* con =
    (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact));
  if (!con) {
    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
    mj_freeStack(d);
    return;
  }

  // add contacts
  for (int i=0; i < num; i++) {
    // set contact ids
    con[i].geom[0] = g;
    con[i].geom[1] = -1;
    con[i].flex[0] = -1;
    con[i].flex[1] = f;
    con[i].elem[0] = -1;
    con[i].elem[1] = e;
    con[i].vert[0] = -1;
    con[i].vert[1] = -1;

    con[i].dist = precon[i].dist;
    mju_copy3(con[i].pos, precon[i].pos);
    mju_copy3(con[i].frame + 0, precon[i].normal);
    mju_copy3(con[i].frame + 3, precon[i].tangent);

    // reverse contact normals, since box geom is second
    if (dim == 1 && type == mjGEOM_BOX) {
      mju_scl3(con[i].frame, con[i].frame, -1);
    }

    // set remaining contact parameters
    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
  d->ncon += num;
  mj_freeStack(d);
}


// test two elems for collision, add to contact list
void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) {
  mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]);
  mjtNum gap = m->flex_gap[f1] + m->flex_gap[f2];
  int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2];
  int num;

  // ignore margin and gap in self-collisions
  if (f1 == f2) {
    margin = 0;
    gap = 0;
  }

  // bounding box filter (not applied in midphase)
  if (filterBox(d->flexelem_aabb+6*(m->flex_elemadr[f1]+e1),
                d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin + gap)) {
    return;
  }

  // skip if elements have vertices on the same body
  const int* edata1 = m->flex_elem + m->flex_elemdataadr[f1] + e1*(dim1+1);
  const int* edata2 = m->flex_elem + m->flex_elemdataadr[f2] + e2*(dim2+1);
  const int* bdata1 = m->flex_vertbodyid + m->flex_vertadr[f1];
  const int* bdata2 = m->flex_vertbodyid + m->flex_vertadr[f2];
  for (int i1=0; i1 <= dim1; i1++) {
    int b1 = bdata1[edata1[i1]];
    for (int i2=0; i2 <= dim2; i2++) {
      if (b1 >= 0 && b1 == bdata2[edata2[i2]]) {
        return;
      }
    }
  }

  // allocate mjPreContact[mjMAXCONPAIR] on the stack
  mj_markStack(d);
  mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);

  // capsule : capsule
  if (dim1 == 1 && dim2 == 1) {
    // make capsules from vertices
    mjtNum pos1[3], mat1[9], size1[2];
    mjtNum pos2[3], mat2[9], size2[2];
    mj_makeCapsule(m, d, f1, m->flex_elem + m->flex_elemdataadr[f1] + e1*2,
                   pos1, mat1, size1);
    mj_makeCapsule(m, d, f2, m->flex_elem + m->flex_elemdataadr[f2] + e2*2,
                   pos2, mat2, size2);

    // raw primitive
    num = mjraw_CapsuleCapsule(precon, margin + gap, pos1, mat1, size1, pos2, mat2, size2);
  }

  // general convex collision
  else {
    num = mjc_ConvexElem(m, d, precon, -1, f1, e1, -1, f2, e2, margin + gap);
  }

  // check contacts
  if (!num) {
    mj_freeStack(d);
    return;
  }

  // get contact parameters
  int condim;
  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f1, f2);

  mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact));
  if (!con) {
    mj_freeStack(d);
    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
    return;
  }

  // add contacts
  for (int i=0; i < num; i++) {
    // set contact ids
    con[i].geom[0] = -1;
    con[i].geom[1] = -1;
    con[i].flex[0] = f1;
    con[i].flex[1] = f2;
    con[i].elem[0] = e1;
    con[i].elem[1] = e2;
    con[i].vert[0] = -1;
    con[i].vert[1] = -1;

    con[i].dist = precon[i].dist;
    mju_copy3(con[i].pos, precon[i].pos);
    mju_copy3(con[i].frame + 0, precon[i].normal);
    mju_copy3(con[i].frame + 3, precon[i].tangent);

    // set remaining contact parameters
    mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
  d->ncon += num;
  mj_freeStack(d);
}


// test element and vertex for collision, add to contact list
void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) {
  mjtNum margin = mj_assignMargin(m, m->flex_margin[f]);
  mjtNum radius = m->flex_radius[f];
  const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + v);
  int dim = m->flex_dim[f];
  const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
  int num;

  // box-box filter (sphere treated as box)
  const mjtNum* aabb = d->flexelem_aabb + 6*(m->flex_elemadr[f] + e);
  mjtNum rbound = margin + radius;
  if (aabb[0]-aabb[3] > vert[0]+rbound) return;
  if (aabb[1]-aabb[4] > vert[1]+rbound) return;
  if (aabb[2]-aabb[5] > vert[2]+rbound) return;
  if (aabb[0]+aabb[3] < vert[0]-rbound) return;
  if (aabb[1]+aabb[4] < vert[1]-rbound) return;
  if (aabb[2]+aabb[5] < vert[2]-rbound) return;

  // allocate mjPreContact[mjMAXCONPAIR] on the stack
  mj_markStack(d);
  mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact);

  // sphere : capsule
  if (dim == 1) {
    mjtNum pos[3], mat[9], size[2];
    mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
    mj_makeCapsule(m, d, f, edata, pos, mat, size);
    num = mjraw_SphereCapsule(precon, 0, vert, I, &radius, pos, mat, size);
  }

  // sphere : triangle
  else if (dim == 2) {
    const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
    num = mjraw_SphereTriangle(precon, 0, vert, radius,
                               vertxpos + 3*edata[0], vertxpos + 3*edata[1],
                               vertxpos + 3*edata[2], radius);
  }

  // sphere : tetrahdron
  else {
    num = mjc_ConvexElem(m, d, precon, -1, f, -1, v, f, e, 0);
  }

  // check contacts
  if (!num) {
    mj_freeStack(d);
    return;
  }

  // get contact parameters
  int condim;
  mjtNum friction[5], solref[mjNREF], solimp[mjNIMP], adhesion;
  mjtNum solreffriction[mjNREF] = {0};
  mj_contactParam(m, &condim, solref, solimp, friction, &adhesion, -1, -1, f, f);


  // allocate mjContact[num] on the arena
  mjContact* con =
    (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact));
  if (!con) {
    mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
    mj_freeStack(d);
    return;
  }


  // add contacts
  for (int i=0; i < num; i++) {
    // set contact ids
    con[i].geom[0] = -1;
    con[i].geom[1] = -1;
    con[i].flex[0] = f;
    con[i].flex[1] = f;
    con[i].elem[0] = -1;
    con[i].elem[1] = e;
    con[i].vert[0] = v;
    con[i].vert[1] = -1;

    con[i].dist = precon[i].dist;
    mju_copy3(con[i].pos, precon[i].pos);
    mju_copy3(con[i].frame + 0, precon[i].normal);
    mju_copy3(con[i].frame + 3, precon[i].tangent);

    // set remaining contact parameters
    mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction, adhesion);
  }

  // add to ncon
  d->ncon += num;

  mj_freeStack(d);
}