mujoco

// Copyright 2025 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_core_util.h"

#include <stddef.h>

#include <mujoco/mjdata.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include "engine/engine_inline.h"
#include "engine/engine_memory.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_sparse.h"
#include "engine/engine_util_spatial.h"



// determine type of constraint Jacobian
int mj_isSparse(const mjModel* m) {
  if (m->opt.jacobian == mjJAC_SPARSE ||
      (m->opt.jacobian == mjJAC_AUTO && m->nv >= 60)) {
    return 1;
  } else {
    return 0;
  }
}


// determine type of friction cone
int mj_isPyramidal(const mjModel* m) {
  if (m->opt.cone == mjCONE_PYRAMIDAL) {
    return 1;
  } else {
    return 0;
  }
}


//-------------------------- sparse chains ---------------------------------------------------------

// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2, int flg_skipcommon) {
  int da1, da2, NV = 0;

  // skip fixed bodies
  b1 = m->body_weldid[b1];
  b2 = m->body_weldid[b2];

  // neither weld root has dofs: empty chain
  if (m->body_dofnum[b1] == 0 && m->body_dofnum[b2] == 0) {
    return 0;
  }

  // initialize last dof address for each body
  da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
  da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;

  // merge chains
  while (da1 >= 0 || da2 >= 0) {
    int da = mjMAX(da1, da2);
    if (flg_skipcommon && da1 == da && da2 == da) {
      break;
    }
    chain[NV] = da;
    if (da1 == da) {
      da1 = m->dof_parentid[da1];
    }
    if (da2 == da) {
      da2 = m->dof_parentid[da2];
    }
    NV++;
  }

  // reverse order of chain: make it increasing
  for (int i=0; i < NV/2; i++) {
    int tmp = chain[i];
    chain[i] = chain[NV-i-1];
    chain[NV-i-1] = tmp;
  }

  return NV;
}


// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
  // swap bodies if wrong order
  if (b1 > b2) {
    int tmp = b1;
    b1 = b2;
    b2 = tmp;
  }

  // init
  int n1 = m->body_dofnum[b1];
  int n2 = m->body_dofnum[b2];

  // both fixed: nothing to do
  if (n1 == 0 && n2 == 0) {
    return 0;
  }

  // copy b1 dofs
  for (int i=0; i < n1; i++) {
    chain[i] = m->body_dofadr[b1] + i;
  }

  // copy b2 dofs
  for (int i=0; i < n2; i++) {
    chain[n1+i] = m->body_dofadr[b2] + i;
  }

  return (n1+n2);
}


// get body chain
int mj_bodyChain(const mjModel* m, int body, int* chain) {
  // simple body
  if (m->body_simple[body]) {
    int dofnum = m->body_dofnum[body];
    for (int i=0; i < dofnum; i++) {
      chain[i] = m->body_dofadr[body] + i;
    }
    return dofnum;
  }

  // general case
  else {
    // skip fixed bodies
    body = m->body_weldid[body];

    // weld root has no dofs: empty chain
    if (m->body_dofnum[body] == 0) {
      return 0;
    }

    // initialize last dof
    int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
    int NV = 0;

    // construct chain from child to parent
    while (da >= 0) {
      chain[NV++] = da;
      da = m->dof_parentid[da];
    }

    // reverse order of chain: make it increasing
    for (int i=0; i < NV/2; i++) {
      int tmp = chain[i];
      chain[i] = chain[NV-i-1];
      chain[NV-i-1] = tmp;
    }

    return NV;
  }
}


//-------------------------- Jacobians -------------------------------------------------------------

// compute 3/6-by-nv Jacobian of global point attached to given body
void mj_jac(const mjModel* m, const mjData* d,
            mjtNum* jacp, mjtNum* jacr, const mjtNum point[3], int body) {
  int nv = m->nv;
  mjtNum offset[3];

  // clear jacobians, compute offset if required
  if (jacp) {
    mju_zero(jacp, 3*nv);
    mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
  }
  if (jacr) {
    mju_zero(jacr, 3*nv);
  }

  // skip fixed bodies
  body = m->body_weldid[body];

  // weld root has no dofs: nothing to do
  if (m->body_dofnum[body] == 0) {
    return;
  }

  // get last dof that affects this (as well as the original) body
  int i = m->body_dofadr[body] + m->body_dofnum[body] - 1;

  // backward pass over dof ancestor chain
  while (i >= 0) {
    mjtNum* cdof = d->cdof+6*i;

    // construct rotation jacobian
    if (jacr) {
      jacr[i+0*nv] = cdof[0];
      jacr[i+1*nv] = cdof[1];
      jacr[i+2*nv] = cdof[2];
    }

    // construct translation jacobian (correct for rotation)
    if (jacp) {
      mjtNum tmp[3];
      mji_cross(tmp, cdof, offset);
      jacp[i+0*nv] = cdof[3] + tmp[0];
      jacp[i+1*nv] = cdof[4] + tmp[1];
      jacp[i+2*nv] = cdof[5] + tmp[2];
    }

    // advance to parent dof
    i = m->dof_parentid[i];
  }
}


// compute body Jacobian
void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body) {
  mj_jac(m, d, jacp, jacr, d->xpos+3*body, body);
}


// compute body-com Jacobian
void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body) {
  mj_jac(m, d, jacp, jacr, d->xipos+3*body, body);
}


// compute subtree-com Jacobian
void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
  int nv = m->nv;
  mj_markStack(d);
  mjtNum* jacp_b = mjSTACKALLOC(d, 3*nv, mjtNum);

  // clear output
  mju_zero(jacp, 3*nv);

  // forward pass starting from body
  for (int b=body; b < m->nbody; b++) {
    // end of body subtree, break from the loop
    if (b > body && m->body_parentid[b] < body) {
      break;
    }

    // b is in the body subtree, add mass-weighted Jacobian into jacp
    mj_jac(m, d, jacp_b, NULL, d->xipos+3*b, b);
    mju_addToScl(jacp, jacp_b, m->body_mass[b], 3*nv);
  }

  // normalize by subtree mass
  mju_scl(jacp, jacp, 1/m->body_subtreemass[body], 3*nv);

  mj_freeStack(d);
}


// compute geom Jacobian
void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom) {
  mj_jac(m, d, jacp, jacr, d->geom_xpos + 3*geom, m->geom_bodyid[geom]);
}


// compute site Jacobian
void mj_jacSite(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int site) {
  mj_jac(m, d, jacp, jacr, d->site_xpos + 3*site, m->site_bodyid[site]);
}


// compute translation Jacobian of point, and rotation Jacobian of axis
void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacAxis,
                     const mjtNum point[3], const mjtNum axis[3], int body) {
  int nv = m->nv;

  // get full Jacobian of point
  mj_markStack(d);
  mjtNum* jacp = (jacPoint ? jacPoint : mjSTACKALLOC(d, 3*nv, mjtNum));
  mjtNum* jacr = mjSTACKALLOC(d, 3*nv, mjtNum);
  mj_jac(m, d, jacp, jacr, point, body);

  // jacAxis_col = cross(jacr_col, axis)
  if (jacAxis) {
    for (int i=0; i < nv; i++) {
      jacAxis[     i] = jacr[  nv+i]*axis[2] - jacr[2*nv+i]*axis[1];
      jacAxis[  nv+i] = jacr[2*nv+i]*axis[0] - jacr[     i]*axis[2];
      jacAxis[2*nv+i] = jacr[     i]*axis[1] - jacr[  nv+i]*axis[0];
    }
  }

  mj_freeStack(d);
}


// compute 3/6-by-nv sparse Jacobian of global point attached to given body
void mj_jacSparse(const mjModel* m, const mjData* d,
                  mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
                  int NV, const int* chain, int flg_skipcommon) {
  // clear jacobians
  if (jacp) {
    mju_zero(jacp, 3*NV);
  }
  if (jacr) {
    mju_zero(jacr, 3*NV);
  }

  // compute point-com offset
  mjtNum offset[3];
  mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);

  // skip fixed bodies
  body = m->body_weldid[body];

  // weld root has no dofs: nothing to do
  if (m->body_dofnum[body] == 0) {
    return;
  }

  // get last dof that affects this (as well as the original) body
  int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;

  // start and the end of the chain (chain is in increasing order)
  int ci = NV-1;

  // backward pass over dof ancestor chain
  while (da >= 0) {
    // find chain index for this dof
    while (ci >= 0 && chain[ci] > da) {
      ci--;
    }

    // dof not in chain: skip if shared dofs are excluded, otherwise SHOULD NOT OCCUR
    if (ci < 0 || chain[ci] != da) {
      if (flg_skipcommon) {
        da = m->dof_parentid[da];
        continue;
      }
      mjERROR("dof index %d not found in chain", da);
    }

    const mjtNum* cdof = d->cdof + 6*da;

    // construct rotation jacobian
    if (jacr) {
      jacr[ci+0*NV] = cdof[0];
      jacr[ci+1*NV] = cdof[1];
      jacr[ci+2*NV] = cdof[2];
    }

    // construct translation jacobian (correct for rotation)
    if (jacp) {
      mjtNum tmp[3];
      mji_cross(tmp, cdof, offset);

      jacp[ci+0*NV] = cdof[3] + tmp[0];
      jacp[ci+1*NV] = cdof[4] + tmp[1];
      jacp[ci+2*NV] = cdof[5] + tmp[2];
    }

    // advance to parent dof
    da = m->dof_parentid[da];
  }
}


// sparse Jacobian difference for simple body contacts
void mj_jacSparseSimple(const mjModel* m, const mjData* d,
                        mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point,
                        int body, int flg_second, int NV, int start) {
  // compute point-com offset
  mjtNum offset[3];
  mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);

  // skip fixed body
  if (!m->body_dofnum[body]) {
    return;
  }

  // process dofs
  int ci = start;
  int end = m->body_dofadr[body] + m->body_dofnum[body];
  for (int da=m->body_dofadr[body]; da < end; da++) {
    mjtNum *cdof = d->cdof+6*da;

    // construct rotation jacobian
    if (jacdifr) {
      // plus sign
      if (flg_second) {
        jacdifr[ci+0*NV] = cdof[0];
        jacdifr[ci+1*NV] = cdof[1];
        jacdifr[ci+2*NV] = cdof[2];
      }

      // minus sign
      else {
        jacdifr[ci+0*NV] = -cdof[0];
        jacdifr[ci+1*NV] = -cdof[1];
        jacdifr[ci+2*NV] = -cdof[2];
      }
    }

    // construct translation jacobian (correct for rotation)
    if (jacdifp) {
      mjtNum tmp[3];
      mji_cross(tmp, cdof, offset);

      // plus sign
      if (flg_second) {
        jacdifp[ci+0*NV] = (cdof[3] + tmp[0]);
        jacdifp[ci+1*NV] = (cdof[4] + tmp[1]);
        jacdifp[ci+2*NV] = (cdof[5] + tmp[2]);
      }

      // minus sign
      else {
        jacdifp[ci+0*NV] = -(cdof[3] + tmp[0]);
        jacdifp[ci+1*NV] = -(cdof[4] + tmp[1]);
        jacdifp[ci+2*NV] = -(cdof[5] + tmp[2]);
      }
    }

    // advance jacdif counter
    ci++;
  }
}


// dense or sparse Jacobian difference for two body points: pos2 - pos1, global
int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
                  int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3],
                  mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp,
                  mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr,
                  int issparse, int flg_skipcommon) {
  int issimple = (m->body_simple[b1] && m->body_simple[b2]);
  int NV = m->nv;

  // skip if no DOFs
  if (!NV) {
    return 0;
  }

  // construct merged chain of body dofs
  if (issparse) {
    if (issimple) {
      NV = mj_mergeChainSimple(m, chain, b1, b2);
    } else {
      NV = mj_mergeChain(m, chain, b1, b2, flg_skipcommon);
    }
  }

  // skip if empty chain
  if (!NV) {
    return 0;
  }

  // count-only mode
  if (!jacdifp && !jacdifr && !jac1p && !jac1r) {
    return NV;
  }

  // sparse case
  if (issparse) {
    // simple: fast processing
    if (issimple) {
      // first body
      mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos1, b1, 0, NV,
                         b1 < b2 ? 0 : m->body_dofnum[b2]);

      // second body
      mj_jacSparseSimple(m, d, jacdifp, jacdifr, pos2, b2, 1, NV,
                         b2 < b1 ? 0 : m->body_dofnum[b1]);
    }

    // regular processing
    else {
      // Jacobians
      mj_jacSparse(m, d, jac1p, jac1r, pos1, b1, NV, chain, flg_skipcommon);
      mj_jacSparse(m, d, jac2p, jac2r, pos2, b2, NV, chain, flg_skipcommon);

      // differences
      if (jacdifp) {
        mju_sub(jacdifp, jac2p, jac1p, 3*NV);
      }
      if (jacdifr) {
        mju_sub(jacdifr, jac2r, jac1r, 3*NV);
      }
    }
  }

  // dense case
  else {
    // Jacobians
    mj_jac(m, d, jac1p, jac1r, pos1, b1);
    mj_jac(m, d, jac2p, jac2r, pos2, b2);

    // differences
    if (jacdifp) {
      mju_sub(jacdifp, jac2p, jac1p, 3*NV);
    }
    if (jacdifr) {
      mju_sub(jacdifr, jac2r, jac1r, 3*NV);
    }
  }

  return NV;
}


// dense or sparse weighted sum of multiple body Jacobians at same point
int mj_jacSum(const mjModel* m, mjData* d, int* chain,
              int n, const int* body, const mjtNum* weight,
              const mjtNum point[3], mjtNum* jacp, mjtNum* jacr, int flg_rot) {
  int nv = m->nv, NV;

  mj_markStack(d);
  mjtNum* jtmp = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum);

  // sparse
  if (mj_isSparse(m)) {
    // the sparse merge produces one packed [jacp; jacr] block; split into the outputs at the end
    mjtNum* jac = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum);
    mjtNum* buf = mjSTACKALLOC(d, flg_rot ? 6*nv : 3*nv, mjtNum);
    int* buf_ind = mjSTACKALLOC(d, nv, int);
    int* bodychain = mjSTACKALLOC(d, nv, int);

    // set first (rotational rows packed right after the translational rows, at offset 3*NV)
    NV = mj_bodyChain(m, body[0], chain);
    if (NV) {
      // get Jacobian
      mjtNum* jr = flg_rot ? jac + 3*NV : NULL;
      if (m->body_simple[body[0]]) {
        mj_jacSparseSimple(m, d, jac, jr, point, body[0], 1, NV, 0);
      } else {
        mj_jacSparse(m, d, jac, jr, point, body[0], NV, chain, /*flg_skipcommon=*/0);
      }

      // apply weight
      mju_scl(jac, jac, weight[0], flg_rot ? 6*NV : 3*NV);
    }

    // accumulate remaining
    for (int i=1; i < n; i++) {
      // get body chain and Jacobian
      int bodyNV = mj_bodyChain(m, body[i], bodychain);
      if (!bodyNV) {
        continue;
      }
      mjtNum* jr = flg_rot ? jtmp + 3*bodyNV : NULL;
      if (m->body_simple[body[i]]) {
        mj_jacSparseSimple(m, d, jtmp, jr, point, body[i], 1, bodyNV, 0);
      } else {
        mj_jacSparse(m, d, jtmp, jr, point, body[i], bodyNV, bodychain, /*flg_skipcommon=*/0);
      }

      // combine sparse matrices
      NV = mju_addToSparseMat(jac, jtmp, nv, flg_rot ? 6 : 3, weight[i],
                              NV, bodyNV, chain, bodychain, buf, buf_ind);
    }

    // split the packed block into the separate output buffers (each NV-packed)
    mju_copy(jacp, jac, 3*NV);
    if (flg_rot) {
      mju_copy(jacr, jac + 3*NV, 3*NV);
    }
  }

  // dense
  else {
    mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL;

    // set first
    mj_jac(m, d, jacp, flg_rot ? jacr : NULL, point, body[0]);
    mju_scl(jacp, jacp, weight[0], 3*nv);
    if (flg_rot) {
      mju_scl(jacr, jacr, weight[0], 3*nv);
    }

    // accumulate remaining
    for (int i=1; i < n; i++) {
      mj_jac(m, d, jtmp, jr, point, body[i]);
      mju_addToScl(jacp, jtmp, weight[i], 3*nv);
      if (flg_rot) {
        mju_addToScl(jacr, jr, weight[i], 3*nv);
      }
    }

    NV = nv;
  }

  mj_freeStack(d);

  return NV;
}


// compute 3/6-by-nv Jacobian time derivative of global point attached to given body
void mj_jacDot(const mjModel* m, const mjData* d,
               mjtNum* jacp, mjtNum* jacr, const mjtNum point[3], int body) {
  int nv = m->nv;
  mjtNum offset[3];
  mjtNum pvel[6];  // point velocity (rot:lin order)

  // clear jacobians, compute offset and pvel if required
  if (jacp) {
    mju_zero(jacp, 3*nv);
    const mjtNum* com = d->subtree_com+3*m->body_rootid[body];
    mju_sub3(offset, point, com);
    mju_transformSpatial(pvel, d->cvel+6*body, 0, point, com, 0);
  }
  if (jacr) {
    mju_zero(jacr, 3*nv);
  }

  // skip fixed bodies
  body = m->body_weldid[body];

  // weld root has no dofs: nothing to do
  if (m->body_dofnum[body] == 0) {
    return;
  }

  // get last dof that affects this (as well as the original) body
  int i = m->body_dofadr[body] + m->body_dofnum[body] - 1;

  // backward pass over dof ancestor chain
  while (i >= 0) {
    mjtNum cdof_dot[6];
    mji_copy6(cdof_dot, d->cdof_dot+6*i);
    mjtNum* cdof = d->cdof+6*i;

    // check for quaternion
    mjtJoint type = m->jnt_type[m->dof_jntid[i]];
    int dofadr = m->jnt_dofadr[m->dof_jntid[i]];
    int is_quat = type == mjJNT_BALL || (type == mjJNT_FREE && i >= dofadr + 3);

    // compute cdof_dot for quaternion (use current body cvel)
    if (is_quat) {
      mji_crossMotion(cdof_dot, d->cvel+6*m->dof_bodyid[i], cdof);
    }

    // construct rotation jacobian
    if (jacr) {
      jacr[i+0*nv] += cdof_dot[0];
      jacr[i+1*nv] += cdof_dot[1];
      jacr[i+2*nv] += cdof_dot[2];
    }

    // construct translation jacobian (correct for rotation)
    if (jacp) {
      // first correction term, account for varying cdof
      mjtNum tmp1[3];
      mji_cross(tmp1, cdof_dot, offset);

      // second correction term, account for point translational velocity
      mjtNum tmp2[3];
      mji_cross(tmp2, cdof, pvel + 3);

      jacp[i+0*nv] += cdof_dot[3] + tmp1[0] + tmp2[0];
      jacp[i+1*nv] += cdof_dot[4] + tmp1[1] + tmp2[1];
      jacp[i+2*nv] += cdof_dot[5] + tmp1[2] + tmp2[2];
    }

    // advance to parent dof
    i = m->dof_parentid[i];
  }
}


// compute 3/6-by-NV sparse Jacobian time derivative of global point attached to given body
void mj_jacDotSparse(const mjModel* m, const mjData* d,
                     mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
                     int NV, const int* chain) {
  mjtNum offset[3];
  mjtNum pvel[6];

  // clear jacobians, compute offset and pvel if required
  if (jacp) {
    mju_zero(jacp, 3*NV);
    const mjtNum* com = d->subtree_com+3*m->body_rootid[body];
    mju_sub3(offset, point, com);
    mju_transformSpatial(pvel, d->cvel+6*body, 0, point, com, 0);
  }
  if (jacr) {
    mju_zero(jacr, 3*NV);
  }

  // skip fixed bodies
  body = m->body_weldid[body];

  // weld root has no dofs: nothing to do
  if (m->body_dofnum[body] == 0) {
    return;
  }

  // get last dof that affects this body
  int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;

  // start at end of chain (chain is in increasing order)
  int ci = NV-1;

  // backward pass over dof ancestor chain
  while (da >= 0) {
    // find chain index for this dof
    while (ci >= 0 && chain[ci] > da) {
      ci--;
    }

    // dof not in chain: SHOULD NOT OCCUR
    if (ci < 0 || chain[ci] != da) {
      mjERROR("dof index %d not found in chain", da);
    }

    mjtNum cdof_dot[6];
    mji_copy6(cdof_dot, d->cdof_dot+6*da);
    mjtNum* cdof = d->cdof+6*da;

    // check for quaternion
    mjtJoint type = m->jnt_type[m->dof_jntid[da]];
    int dofadr = m->jnt_dofadr[m->dof_jntid[da]];
    int is_quat = type == mjJNT_BALL || (type == mjJNT_FREE && da >= dofadr + 3);

    // compute cdof_dot for quaternion (use current body cvel)
    if (is_quat) {
      mji_crossMotion(cdof_dot, d->cvel+6*m->dof_bodyid[da], cdof);
    }

    // construct rotation jacobian
    if (jacr) {
      jacr[ci+0*NV] += cdof_dot[0];
      jacr[ci+1*NV] += cdof_dot[1];
      jacr[ci+2*NV] += cdof_dot[2];
    }

    // construct translation jacobian (correct for rotation)
    if (jacp) {
      // first correction term, account for varying cdof
      mjtNum tmp1[3];
      mji_cross(tmp1, cdof_dot, offset);

      // second correction term, account for point translational velocity
      mjtNum tmp2[3];
      mji_cross(tmp2, cdof, pvel + 3);

      jacp[ci+0*NV] += cdof_dot[3] + tmp1[0] + tmp2[0];
      jacp[ci+1*NV] += cdof_dot[4] + tmp1[1] + tmp2[1];
      jacp[ci+2*NV] += cdof_dot[5] + tmp1[2] + tmp2[2];
    }

    // advance to parent dof
    da = m->dof_parentid[da];
  }
}


// compute subtree angular momentum matrix
void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) {
  int nv = m->nv;
  mj_markStack(d);

  // stack allocations
  mjtNum* jacp = mjSTACKALLOC(d, 3*nv, mjtNum);
  mjtNum* jacr = mjSTACKALLOC(d, 3*nv, mjtNum);
  mjtNum* term1 = mjSTACKALLOC(d, 3*nv, mjtNum);
  mjtNum* term2 = mjSTACKALLOC(d, 3*nv, mjtNum);

  // clear output
  mju_zero(mat, 3*nv);

  // save the location of the subtree COM
  mjtNum subtree_com[3];
  mju_copy3(subtree_com, d->subtree_com+3*body);

  for (int b=body; b < m->nbody; b++) {
    // end of body subtree, break from the loop
    if (b > body && m->body_parentid[b] < body) {
      break;
    }

    // linear and angular velocity Jacobian of the body COM (inertial frame)
    mj_jacBodyCom(m, d, jacp, jacr, b);

    // orientation of the COM (inertial) frame of b-th body
    mjtNum ximat[9];
    mji_copy9(ximat, d->ximat+9*b);

    // save the inertia matrix of b-th body
    mjtNum inertia[9] = {0};
    inertia[0] = m->body_inertia[3*b+0];  // inertia(1,1)
    inertia[4] = m->body_inertia[3*b+1];  // inertia(2,2)
    inertia[8] = m->body_inertia[3*b+2];  // inertia(3,3)

    // term1 = body angular momentum about self COM in world frame
    mjtNum tmp1[9], tmp2[9];
    mji_mulMatMat3(tmp1, ximat, inertia);          // tmp1  = ximat * inertia
    mju_mulMatMatT3(tmp2, tmp1, ximat);            // tmp2  = ximat * inertia * ximat^T
    mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv);    // term1 = ximat * inertia * ximat^T * jacr

    // location of body COM w.r.t subtree COM
    mjtNum com[3];
    mji_sub3(com, d->xipos+3*b, subtree_com);

    // skew symmetric matrix representing body_com vector
    mjtNum com_mat[9] = {0};
    com_mat[1] = -com[2];
    com_mat[2] = com[1];
    com_mat[3] = com[2];
    com_mat[5] = -com[0];
    com_mat[6] = -com[1];
    com_mat[7] = com[0];

    // term2 = moment of linear momentum
    mju_mulMatMat(term2, com_mat, jacp, 3, 3, nv);   // term2 = com_mat * jacp
    mju_scl(term2, term2, m->body_mass[b], 3 * nv);  // term2 = com_mat * jacp * mass

    // mat += term1 + term2
    mju_addTo(mat, term1, 3*nv);
    mju_addTo(mat, term2, 3*nv);
  }

  mj_freeStack(d);
}


//-------------------------- spatial frame utilities -----------------------------------------------

// compute object 6D velocity in object-centered frame, world/local orientation
void mj_objectVelocity(const mjModel* m, const mjData* d,
                       int objtype, int objid, mjtNum res[6], int flg_local) {
  int bodyid = 0;
  const mjtNum *pos = 0, *rot = 0;

  // body-inertial
  if (objtype == mjOBJ_BODY) {
    bodyid = objid;
    pos = d->xipos+3*objid;
    rot = (flg_local ? d->ximat+9*objid : 0);
  }

  // body-regular
  else if (objtype == mjOBJ_XBODY) {
    bodyid = objid;
    pos = d->xpos+3*objid;
    rot = (flg_local ? d->xmat+9*objid : 0);
  }

  // geom
  else if (objtype == mjOBJ_GEOM) {
    bodyid = m->geom_bodyid[objid];
    pos = d->geom_xpos+3*objid;
    rot = (flg_local ? d->geom_xmat+9*objid : 0);
  }

  // site
  else if (objtype == mjOBJ_SITE) {
    bodyid = m->site_bodyid[objid];
    pos = d->site_xpos+3*objid;
    rot = (flg_local ? d->site_xmat+9*objid : 0);
  }

  // camera
  else if (objtype == mjOBJ_CAMERA) {
    bodyid = m->cam_bodyid[objid];
    pos = d->cam_xpos+3*objid;
    rot = (flg_local ? d->cam_xmat+9*objid : 0);
  }

  // object without spatial frame
  else {
    mjERROR("invalid object type %d", objtype);
  }

  // dof-less body (static or mocap): quick return
  if (m->body_dofnum[m->body_weldid[bodyid]] == 0) {
    mju_zero(res, 6);
    return;
  }

  // transform velocity
  mju_transformSpatial(res, d->cvel+6*bodyid, 0, pos, d->subtree_com+3*m->body_rootid[bodyid], rot);
}


// compute material surface velocity of a geom at a point, in the world frame
void mj_geomSurfaceVelocity(const mjModel* m, const mjData* d, int geomid,
                            const mjtNum point[3], mjtNum linear[3], mjtNum angular[3]) {
  const mjtNum* sv = m->geom_surfacevel + 6*geomid;

  // rotate local linear and angular surface velocities to the world frame
  mji_mulMatVec3(linear, d->geom_xmat + 9*geomid, sv);
  mji_mulMatVec3(angular, d->geom_xmat + 9*geomid, sv + 3);

  // add angular velocity contribution (w x r) at the query point
  mjtNum arm[3], wxr[3];
  mji_sub3(arm, point, d->geom_xpos + 3*geomid);
  mji_cross(wxr, angular, arm);
  mji_addTo3(linear, wxr);
}


// compute object 6D acceleration in object-centered frame, world/local orientation
void mj_objectAcceleration(const mjModel* m, const mjData* d,
                           int objtype, int objid, mjtNum res[6], int flg_local) {
  int bodyid = 0;
  const mjtNum *pos = 0, *rot = 0;

  // body-inertial
  if (objtype == mjOBJ_BODY) {
    bodyid = objid;
    pos = d->xipos+3*objid;
    rot = (flg_local ? d->ximat+9*objid : 0);
  }

  // body-regular
  else if (objtype == mjOBJ_XBODY) {
    bodyid = objid;
    pos = d->xpos+3*objid;
    rot = (flg_local ? d->xmat+9*objid : 0);
  }

  // geom
  else if (objtype == mjOBJ_GEOM) {
    bodyid = m->geom_bodyid[objid];
    pos = d->geom_xpos+3*objid;
    rot = (flg_local ? d->geom_xmat+9*objid : 0);
  }

  // site
  else if (objtype == mjOBJ_SITE) {
    bodyid = m->site_bodyid[objid];
    pos = d->site_xpos+3*objid;
    rot = (flg_local ? d->site_xmat+9*objid : 0);
  }

  // camera
  else if (objtype == mjOBJ_CAMERA) {
    bodyid = m->cam_bodyid[objid];
    pos = d->cam_xpos+3*objid;
    rot = (flg_local ? d->cam_xmat+9*objid : 0);
  }

  // object without spatial frame
  else {
    mjERROR("invalid object type %d", objtype);
  }

  // dof-less body (static or mocap): quick return
  if (m->body_dofnum[m->body_weldid[bodyid]] == 0) {
    mju_zero(res, 6);
    return;
  }

  // transform com-based acceleration to local frame
  mju_transformSpatial(res, d->cacc+6*bodyid, 0, pos, d->subtree_com+3*m->body_rootid[bodyid], rot);

  // transform com-based velocity to local frame
  mjtNum vel[6];
  mju_transformSpatial(vel, d->cvel+6*bodyid, 0, pos, d->subtree_com+3*m->body_rootid[bodyid], rot);

  // add Coriolis correction due to rotating frame:  acc_tran += vel_rot x vel_tran
  mjtNum correction[3];
  mji_cross(correction, vel, vel+3);
  mji_addTo3(res+3, correction);
}


// map from body local to global Cartesian coordinates
void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
                     const mjtNum pos[3], const mjtNum quat[4],
                     int body, mjtByte sameframe) {
  mjtSameFrame sf = sameframe;

  // position
  if (xpos && pos) {
    switch (sf) {
    case mjSAMEFRAME_NONE:
    case mjSAMEFRAME_BODYROT:
    case mjSAMEFRAME_INERTIAROT:
      mji_mulMatVec3(xpos, d->xmat+9*body, pos);
      mji_addTo3(xpos, d->xpos+3*body);
      break;
    case mjSAMEFRAME_BODY:
      mji_copy3(xpos, d->xpos+3*body);
      break;
    case mjSAMEFRAME_INERTIA:
      mji_copy3(xpos, d->xipos+3*body);
      break;
    }
  }

  // orientation
  if (xmat && quat) {
    mjtNum tmp[4];
    switch (sf) {
    case mjSAMEFRAME_NONE:
      mji_mulQuat(tmp, d->xquat+4*body, quat);
      mju_quat2Mat(xmat, tmp);
      break;
    case mjSAMEFRAME_BODY:
    case mjSAMEFRAME_BODYROT:
      mji_copy9(xmat, d->xmat+9*body);
      break;
    case mjSAMEFRAME_INERTIA:
    case mjSAMEFRAME_INERTIAROT:
      mji_copy9(xmat, d->ximat+9*body);
      break;
    }
  }
}


//-------------------------- miscellaneous utilities -----------------------------------------------

// Check weld parent for independent ordered XYZ slides. Used only to select fast paths;
// general attachments use the point Jacobian. The same test sizes the constant factor in mjCFlex.
int mj_flexBodySimple(const mjModel* m, int body) {
  body = m->body_weldid[body];
  if (m->body_dofnum[body] != 3 || m->body_jntnum[body] != 3 ||
      m->body_dofnum[m->body_weldid[m->body_parentid[body]]] != 0) {
    return 0;
  }
  int jadr = m->body_jntadr[body];
  for (int j=0; j < 3; j++) {
    if (m->jnt_type[jadr+j] != mjJNT_SLIDE) return 0;
    for (int k=0; k < 3; k++) {
      if (mju_abs(m->jnt_axis[3*(jadr+j)+k] - (j == k)) > mjMINVAL) return 0;
    }
  }
  return 1;
}


// Check for standard fixed-frame three-DOF assembly flex and compatibility with cached factor.
int mj_flexSimple(const mjModel* m, int f) {
  for (int v=m->flex_vertadr[f]; v < m->flex_vertadr[f]+m->flex_vertnum[f]; v++) {
    int body = m->body_weldid[m->flex_vertbodyid[v]];
    if (m->body_dofnum[body] && !mj_flexBodySimple(m, body)) return 0;
  }
  return 1;
}


// Apply the point Jacobian or its transpose without constructing a matrix. The general
// path walks the same motion axes as mj_jac, including every ancestor and the vertex offset.
// Gather overwrites 3*nvert world components; scatter adds to nv generalized components.
static void flexMap(const mjModel* m, const mjData* d, int f, mjtNum* res,
                    const mjtNum* vec, mjtNum scale, int transpose) {
  int vadr = m->flex_vertadr[f];
  for (int v=0; v < m->flex_vertnum[f]; v++) {
    int body = m->body_weldid[m->flex_vertbodyid[vadr+v]];
    if (!transpose) mji_zero3(res + 3*v);
    if (!m->body_dofnum[body]) continue;
    int da = m->body_dofadr[body];
    if (m->body_simple[body] == 2) {
      // The world-space slide axes are already in cdof, including axis order and signs.
      for (int j=0; j < m->body_dofnum[body]; j++) {
        const mjtNum* axis = d->cdof + 6*(da+j)+3;
        if (transpose) {
          res[da+j] += scale*mju_dot3(axis, vec + 3*v);
        } else {
          mji_addToScl3(res + 3*v, axis, vec[da+j]);
        }
      }
      continue;
    }
    mjtNum offset[3];
    mji_sub3(offset, d->flexvert_xpos + 3*(vadr+v),
             d->subtree_com + 3*m->body_rootid[body]);
    for (int i=da + m->body_dofnum[body]-1; i >= 0; i=m->dof_parentid[i]) {
      mjtNum column[3];
      mji_cross(column, d->cdof + 6*i, offset);
      mji_addTo3(column, d->cdof + 6*i+3);
      if (transpose) {
        res[i] += scale*mju_dot3(column, vec + 3*v);
      } else {
        mji_addToScl3(res + 3*v, column, vec[i]);
      }
    }
  }
}


// Gather an arbitrary generalized vector (including qvel) in world vertex coordinates.
void mj_flexGather(const mjModel* m, const mjData* d, int f, mjtNum* res, const mjtNum* vec) {
  flexMap(m, d, f, res, vec, 1, 0);
}


// Accumulate world vertex forces in generalized coordinates, including pin reactions.
void mj_flexScatter(const mjModel* m, const mjData* d, int f, mjtNum* res,
                    const mjtNum* vec, mjtNum scale) {
  flexMap(m, d, f, res, vec, scale, 1);
}


// gather global node positions and velocities
void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel) {
  int nodenum = m->flex_nodenum[f];
  int nstart = m->flex_nodeadr[f];
  int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];

  // compute positions and velocities
  for (int i=0; i < nodenum; i++) {
    int bid = bodyid[i];
    if (m->flex_centered[f] ||
        (m->flex_node[3*(i+nstart)+0] == 0 &&
         m->flex_node[3*(i+nstart)+1] == 0 &&
         m->flex_node[3*(i+nstart)+2] == 0)) {
      mju_copy3(xpos + 3*i, d->xpos + 3*bid);
    } else {
      mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bid, m->flex_node + 3*(i+nstart));
      mju_addTo3(xpos + 3*i, d->xpos + 3*bid);
    }

    if (vel) {
      mjtNum body_vel[6];
      mj_objectVelocity(m, d, mjOBJ_BODY, bid, body_vel, 0);  // returns [omega, v_CoM] in world frame

      // linear velocity at CoM
      mju_copy3(vel + 3*i, body_vel + 3);

      // add omega x (xpos - xipos)
      mjtNum r[3], cross[3];
      mju_sub3(r, xpos + 3*i, d->xipos + 3*bid);
      mju_cross(cross, body_vel, r);
      mju_addTo3(vel + 3*i, cross);
    }
  }

  // shell mode: reconstruct interior node positions and velocities via TFI
  int interp = m->flex_interp[f];
  if (interp < 0) {
    int order = -interp;
    int cx = m->flex_cellnum[3*f+0];
    int cy = m->flex_cellnum[3*f+1];
    int cz = m->flex_cellnum[3*f+2];
    int nx_g = cx * order + 1;
    int ny_g = cy * order + 1;
    int nz_g = cz * order + 1;

    mju_shellTrackInterior(xpos, nx_g, ny_g, nz_g);
    if (vel) {
      mju_shellTrackInterior(vel, nx_g, ny_g, nz_g);
    }
  }
}


// extract 6D force:torque for one contact, in contact frame
void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) {
  mjContact* con;

  // clear result
  mju_zero(result, 6);

  // make sure contact is valid
  if (id >= 0 && id < d->ncon && d->contact[id].efc_address >= 0) {
    // get contact pointer
    con = d->contact + id;

    if (mj_isPyramidal(m)) {
      mju_decodePyramid(result, d->efc_force + con->efc_address, con->friction, con->dim);
    } else {
      mju_copy(result, d->efc_force + con->efc_address, con->dim);
    }

    // report the net interface force: the solver's cone force minus the adhesive pull
    result[0] -= con->adhesion;
  }
}


// count the number of length limit violations for tendon i (0, 1 or 2)
int tendonLimit(const mjModel* m, const mjtNum* ten_length, int i) {
  if (!m->tendon_limited[i]) {
    return 0;
  }

  int nl = 0;
  mjtNum value = ten_length[i];
  mjtNum margin = m->tendon_margin[i];

  // tendon limits can be bilateral, check both sides
  for (int side = -1; side <= 1; side += 2) {
    mjtNum dist = side * (m->tendon_range[2 * i + (side + 1) / 2] - value);
    if (dist < margin) nl++;
  }

  return nl;
}


// compute spring and damper forces along tendon i, zero when disabled
void mj_tendonSpringDamper(const mjModel* m, const mjData* d, int i,
                           mjtNum* frc_spring, mjtNum* frc_damper) {
  *frc_spring = 0;
  *frc_damper = 0;

  // spring force: displacement outside the spring range
  if (!mjDISABLED(mjDSBL_SPRING)) {
    mjtNum stiffness = m->tendon_stiffness[i];
    const mjtNum* spoly = m->tendon_stiffnesspoly + mjNPOLY*i;
    if (stiffness || !mju_isZero(spoly, mjNPOLY)) {
      mjtNum length = d->ten_length[i];
      mjtNum lower = m->tendon_lengthspring[2*i];
      mjtNum upper = m->tendon_lengthspring[2*i+1];
      mjtNum x = (length > upper) ? length - upper : (length < lower) ? length - lower : 0;
      *frc_spring = -x * mju_polyForce(stiffness, spoly, x, mjNPOLY, 0);
    }
  }

  // damper force: velocity, damping includes the contribution of actuators
  if (!mjDISABLED(mjDSBL_DAMPER)) {
    mjtNum dpoly[mjNPOLY];
    mju_copy(dpoly, m->tendon_dampingpoly + mjNPOLY*i, mjNPOLY);
    mjtNum damping = m->tendon_damping[i] + mj_actuatorDamping(m, mjOBJ_TENDON, i, dpoly);
    if (damping || !mju_isZero(dpoly, mjNPOLY)) {
      mjtNum v = d->ten_velocity[i];
      *frc_damper = -v * mju_polyForce(damping, dpoly, v, mjNPOLY, 1);
    }
  }
}


// return actuator damping contribution to joint or tendon
mjtNum mj_actuatorDamping(const mjModel* m, mjtObj type, int id, mjtNum poly[mjNPOLY]) {
  if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) {
    mjERROR("only joint and tendon objects can inherit damping from actuators");
    return 0;
  }

  // get actuator id
  int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id];

  if (actuatorid == -1) {
    return 0;
  }

  mjtNum damping = 0;

  // single actuator contributes damping
  if (actuatorid >= 0) {
    mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]];
    damping = m->actuator_damping[actuatorid] * gear2;
    for (int k = 0; k < mjNPOLY; k++) {
      poly[k] += m->actuator_dampingpoly[mjNPOLY*actuatorid+k] * gear2;
    }
  }

  // actuatorid < -1: scan all actuators for contributions
  else {
    for (int k = 0; k < m->nactuator; k++) {
      // skip actuators that don't actuate the given joint/tendon
      if (m->actuator_trnid[2*k] != id) {
        continue;
      }
      if (type == mjOBJ_JOINT &&
          m->actuator_trntype[k] != mjTRN_JOINT &&
          m->actuator_trntype[k] != mjTRN_JOINTINPARENT) {
        continue;
      }
      if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) {
        continue;
      }

      // accumulate damping contribution
      mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]];
      damping += m->actuator_damping[k] * gear2;
      for (int j = 0; j < mjNPOLY; j++) {
        poly[j] += m->actuator_dampingpoly[mjNPOLY*k+j] * gear2;
      }
    }
  }

  return damping;
}


// return actuator armature contribution to joint or tendon
mjtNum mj_actuatorArmature(const mjModel* m, mjtObj type, int id) {
  if (type != mjOBJ_TENDON && type != mjOBJ_JOINT) {
    mjERROR("only joint and tendon objects can inherit armature from actuators");
    return 0;
  }

  // get actuator id
  int actuatorid = type == mjOBJ_JOINT ? m->jnt_actuatorid[id] : m->tendon_actuatorid[id];

  // no actuator contribution
  if (actuatorid == -1) {
    return 0;
  }

  mjtNum armature = 0;

  // single actuator contributes armature
  if (actuatorid >= 0) {
    mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[actuatorid]] * m->actuator_gear[6*m->actuator_outadr[actuatorid]];
    armature = m->actuator_armature[actuatorid] * gear2;
  }

  // actuatorid < -1: scan all actuators for contributions
  else {
    for (int k = 0; k < m->nactuator; k++) {
      // skip actuators that don't actuate the given joint/tendon
      if (m->actuator_trnid[2*k] != id) {
        continue;
      }
      if (type == mjOBJ_JOINT &&
          m->actuator_trntype[k] != mjTRN_JOINT &&
          m->actuator_trntype[k] != mjTRN_JOINTINPARENT) {
        continue;
      }
      if (type == mjOBJ_TENDON && m->actuator_trntype[k] != mjTRN_TENDON) {
        continue;
      }

      // accumulate armature contribution
      mjtNum gear2 = m->actuator_gear[6*m->actuator_outadr[k]] * m->actuator_gear[6*m->actuator_outadr[k]];
      armature += m->actuator_armature[k] * gear2;
    }
  }

  return armature;
}


// return DC motor winding resistance at the current temperature
mjtNum mj_dcmotorResistance(const mjModel* m, const mjData* d, int id) {
  const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*id;
  const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*id;
  mjtNum R = gainprm[0];
  mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm);

  // account for temperature if thermal model is enabled
  if (slots.temperature >= 0) {
    mjtNum T = d->act[m->actuator_actadr[id]+slots.temperature];
    mjtNum alpha = gainprm[2];  // temperature coefficient
    mjtNum T0 = gainprm[3];     // reference temperature
    mjtNum Ta = dynprm[4];      // ambient temperature
    R *= 1 + alpha * (T + Ta - T0);
  }

  return mju_max(mjMINVAL, R);
}


// count warnings, print only the first time
void mj_warning(mjData* d, int warning, int info) {
  // check type
  if (warning < 0 || warning >= mjNWARNING) {
    mjERROR("invalid warning type %d", warning);
  }

  // save info (override previous)
  d->warning[warning].lastinfo = info;

  // print message only the first time this warning is encountered
  if (!d->warning[warning].number) {
    mju_warning("%s Time = %.4f.", mju_warningText(warning, info), d->time);
  }

  // increase counter
  d->warning[warning].number++;
}


//-------------------------- effective-metric predicates ------------------------------------------

// the selected integrator performs the constraint solve in the effective metric.
// The option-level gate decision; d->efm_active reports whether the per-step build ran
int mj_isMetric(const mjModel* m) {
  return m->opt.integrator == mjINT_DISCRETE;
}


// do the tendon and actuator classes enter the metric. Under solver=PGS -- and only
// there -- they are excluded and their forces integrate explicitly: the dual assembles
// its constraint-space AR from the backbone factor, which cannot carry their couplings,
// and a consistent backbone metric beats a solve whose forces and accelerations disagree.
// Noslip atop a primal solver keeps the couplings: the main solve runs in the full
// metric and the post-pass consumes the backbone AR as an approximation. Flex, which is
// too stiff to exclude, is rejected by mj_checkDiscrete instead
int mj_effCouplings(const mjModel* m) {
  return mj_isMetric(m) && m->opt.solver != mjSOL_PGS;
}


// tendon i has a spring: nonzero stiffness or stiffness polynomial
int mj_tendonHasStiffness(const mjModel* m, int i) {
  return m->tendon_stiffness[i] != 0 ||
         !mju_isZero(m->tendon_stiffnesspoly + mjNPOLY*i, mjNPOLY);
}


// tendon i has a damper: nonzero damping, damping polynomial, or an attached actuator
int mj_tendonHasDamping(const mjModel* m, int i) {
  return m->tendon_damping[i] != 0 ||
         !mju_isZero(m->tendon_dampingpoly + mjNPOLY*i, mjNPOLY) ||
         m->tendon_actuatorid[i] != -1;
}


// does flex f use the penalty form of passive contact: a standard deformable flex of dim >= 2
// that asks for it, and not under the ipc flag, which solves the same law for every supported
// flex itself (running both would apply each pair's force twice)
int mj_effFlexContactPossible(const mjModel* m, int f) {
  return m->flex_passive[f] && !m->flex_rigid[f] && !m->flex_interp[f] && m->flex_dim[f] >= 2 &&
         !mjENABLED(mjENBL_IPC);
}


// does flex f contribute elastic stiffness to the metric. Unlike the assembler gate
// flexStiff_active (engine_derivative.c), interpolated flexes are included: their
// stiffness is carried matrix-free
int mj_effFlexStiffPossible(const mjModel* m, int f) {
  // rigid or 1D flexes do not contribute stiffness
  if (m->flex_rigid[f] || m->flex_dim[f] < 2) {
    return 0;
  }

  // stretch stiffness present (the strain equality mode stores its constraint
  // eigenmodes in this block instead)
  int sadr = m->flex_stiffnessadr[f];
  if (sadr >= 0 && m->flex_stiffness[sadr] != 0 && m->flex_edgeequality[f] != 3) {
    return 1;
  }

  // bending: an allocated block does not imply stiffness
  // (strain-constrained and zero-elasticity flexes carry an all-zero block)
  int badr = m->flex_bendingadr[f];
  if (badr < 0) {
    return 0;
  }
  int end = m->nflexbending;
  for (int g=f+1; g < m->nflex; g++) {
    if (m->flex_bendingadr[g] >= 0) {
      end = m->flex_bendingadr[g];
      break;
    }
  }
  return !mju_isZero(m->flex_bending + badr, end - badr);
}


// does flex f need the implicit metric treatment: elastic stiffness or passive contact
int mj_effFlexPossible(const mjModel* m, int f) {
  return mj_effFlexStiffPossible(m, f) || mj_effFlexContactPossible(m, f);
}


// can this tendon contribute to the metric (model-level; mirrored by island discovery
// and the sleep wake rule)
int mj_effTendonPossible(const mjModel* m, int i) {
  return (!mjDISABLED(mjDSBL_SPRING) && mj_tendonHasStiffness(m, i)) ||
         (!mjDISABLED(mjDSBL_DAMPER) && mj_tendonHasDamping(m, i));
}


// can this actuator contribute to the metric (model-level type check; mirrored by island discovery)
int mj_effActuatorPossible(const mjModel* m, int i) {
  if (mjDISABLED(mjDSBL_ACTUATION)) {
    return 0;
  }
  return m->actuator_biastype[i] == mjBIAS_AFFINE  ||
         m->actuator_biastype[i] == mjBIAS_SO3     ||
         m->actuator_biastype[i] == mjBIAS_DCMOTOR ||
         m->actuator_biastype[i] == mjBIAS_MUSCLE  ||
         m->actuator_gaintype[i] == mjGAIN_AFFINE  ||
         m->actuator_gaintype[i] == mjGAIN_SO3     ||
         m->actuator_gaintype[i] == mjGAIN_MUSCLE  ||
         m->actuator_gaintype[i] == mjGAIN_DCMOTOR;
}


//-------------------------- Stable Neo-Hookean tetrahedra -----------------------------------------

// cubic Gram determinant P(s) = a*b*c + 2*d*e*f - a*f*f - b*e*e - c*d*d,
// where D(s) = [[a,d,e], [d,b,f], [e,f,c]] is the Gram difference at vertex 0.
// Add its derivatives to the same edge tension/metric used for the quadratic energy.
void mj_snhCubic(mjtNum metric[36], mjtNum tension[6], const mjtNum s[6],
                 mjtNum gamma, int flg_stiffness) {
  mjtNum a = s[0], b = s[2], c = s[4];
  mjtNum d = 0.5 * (s[0] + s[2] - s[1]);
  mjtNum e = 0.5 * (s[0] + s[4] - s[5]);
  mjtNum f = 0.5 * (s[2] + s[4] - s[3]);
  mjtNum cof[6] = {b*c-f*f, a*c-e*e, a*b-d*d, e*f-c*d, d*f-b*e, d*e-a*f};
  mjtNum g[6] = {cof[0]+cof[3]+cof[4], -cof[3], cof[1]+cof[3]+cof[5],
                 -cof[5], cof[2]+cof[4]+cof[5], -cof[4]};
  mju_addToScl(tension, g, 2*gamma, 6);
  if (!flg_stiffness) return;

  // columns of the constant map s -> (a,b,c,d,e,f)
  static const mjtNum basis[6][6] = {
    {1, 0, 0, .5, .5, 0}, {0, 0, 0, -.5, 0, 0},
    {0, 1, 0, .5, 0, .5}, {0, 0, 0, 0, 0, -.5},
    {0, 0, 1, 0, .5, .5}, {0, 0, 0, 0, -.5, 0}
  };
  for (int j = 0; j < 6; j++) {
    const mjtNum* h = basis[j];
    mjtNum dc[6] = {
      h[1]*c+b*h[2]-2*f*h[5], h[0]*c+a*h[2]-2*e*h[4],
      h[0]*b+a*h[1]-2*d*h[3], h[4]*f+e*h[5]-h[2]*d-c*h[3],
      h[3]*f+d*h[5]-h[1]*e-b*h[4], h[3]*e+d*h[4]-h[0]*f-a*h[5]
    };
    mjtNum dg[6] = {dc[0]+dc[3]+dc[4], -dc[3], dc[1]+dc[3]+dc[5],
                    -dc[5], dc[2]+dc[4]+dc[5], -dc[4]};
    for (int i = 0; i <= j; i++) {
      mjtNum value = 2*gamma*dg[i];
      metric[6*i+j] += value;
      if (i != j) metric[6*j+i] += value;
    }
  }
}


// exact Hessian-vector product of the quadratic, cubic, and signed-volume energies
void mj_snhStiffnessMul(mjtNum result[4][3], const mjtNum metric[36], const mjtNum tension[6],
                        mjtNum edgevec[6][3], mjtNum grad[4][3], mjtNum pressure,
                        const mjtNum k[24], mjtNum vec[4][3], mjtNum scale) {
  const int (*edge)[2] = mj_stretchEdges[1];
  mjtNum delta[6][3], edgeforce[6][3];
  for (int e = 0; e < 6; e++) {
    mju_sub3(delta[e], vec[edge[e][0]], vec[edge[e][1]]);
  }
  mj_stretchStiffnessMul(edgeforce, metric, tension, edgevec, delta, 6, scale);
  mju_zero(result[0], 12);
  for (int e = 0; e < 6; e++) {
    mju_addTo3(result[edge[e][0]], edgeforce[e]);
    mju_subFrom3(result[edge[e][1]], edgeforce[e]);
  }

  // differentiate the three cross products in grad(J), without forming a 12x12 matrix
  mjtNum dg[4][3], tmp[3];
  mju_cross(dg[1], delta[4], edgevec[2]);
  mju_cross(tmp, edgevec[4], delta[2]);
  mju_addTo3(dg[1], tmp);
  mju_cross(dg[2], delta[4], edgevec[0]);
  mju_cross(tmp, edgevec[4], delta[0]);
  mju_addTo3(dg[2], tmp);
  mju_cross(dg[3], delta[2], edgevec[0]);
  mju_cross(tmp, edgevec[2], delta[0]);
  mju_addTo3(dg[3], tmp);
  for (int x = 0; x < 3; x++) {
    dg[0][x] = -dg[1][x]-dg[2][x]-dg[3][x];
  }
  mjtNum dJ = 0;
  for (int v = 0; v < 4; v++) {
    dJ += mju_dot3(grad[v], vec[v]);
  }
  for (int v = 0; v < 4; v++) {
    mju_addToScl3(result[v], grad[v], 2*scale*k[22]*dJ);
    mju_addToScl3(result[v], dg[v], scale*pressure*k[23]);
  }
}