Merge branch 'google-deepmind:main' into mjx-warp-segmentation

doc/APIreference/functions.rst

@@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors<sensor-user>` of :ref:`stage<sensor-
 The computed force arrays ``cfrc_int`` and ``cfrc_ext`` currently suffer from a know bug, they do not take into account
 the effect of spatial tendons, see :issue:`832`.
 
+.. _mj_maxContact:
+
+`mj_maxContact <#mj_maxContact>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_maxContact
+
+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.
+
 .. _mj_collision:
 
 `mj_collision <#mj_collision>`__
@@ -1507,6 +1519,24 @@ Add file to VFS from buffer; return 0: success, 2: repeated name, -1: failed to
 
 Delete file from VFS; return 0: success, -1: not found in VFS.
 
+.. _mj_containsBufferVFS:
+
+`mj_containsBufferVFS <#mj_containsBufferVFS>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_containsBufferVFS
+
+Check if buffer exists in VFS; return 1: exists, 0: not found.
+
+.. _mj_containsFileVFS:
+
+`mj_containsFileVFS <#mj_containsFileVFS>`__
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mj_containsFileVFS
+
+Check if file exists in VFS; return 1: exists, 0: not found.
+
 .. _mj_deleteVFS:
 
 `mj_deleteVFS <#mj_deleteVFS>`__

doc/XMLreference.rst

@@ -677,7 +677,7 @@ from its default.
 
 .. _option-flag-multiccd:
 
-:at:`multiccd`: :at-val:`[disable, enable], "disable"`
+:at:`multiccd`: :at-val:`[disable, enable], "enable"`
    This flag enables multiple-contact collision detection for geom pairs that use a general-purpose convex-convex
    collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface and the
    single contact point generated by the convex-convex collider cannot accurately capture the surface contact, leading
@@ -3593,7 +3593,7 @@ saving the XML:
 
 .. _body-flexcomp-dof:
 
-:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"`
+:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"`
    The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the
    different parametrizations with deformable spheres. The three models in the video are respectively
    `sphere_full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_full.xml>`__,
@@ -3608,6 +3608,10 @@ saving the XML:
      requires a free joint at the flex's parent in order for free body motion to be possible. This type of
      parametrization is appropriate for shapes that are relatively spherical.
 
+   **2d**
+     Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes
+     parallel to the parent body's X-Y plane.
+
    **trilinear**
      Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire
      flex, independent of the number of vertices. The positions of the vertices are updated using trilinear
@@ -4324,7 +4328,7 @@ stress-strain relationship. See also :ref:`deformable <CDeformable>` objects and
 
 :at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"`
    Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only,
-   "both": bending and stretching.
+   "both": bending and stretching. Not yet supported by :ref:`dof<body-flexcomp-dof>` **trilinear** and **quadratic**.
 
 .. _flex-contact:
 
@@ -4888,8 +4892,11 @@ constraint type is only supported for dimension 3 flexes with trilinear or quadr
 :at:`flex`: :at-val:`string, required`
    Name of the flex whose strain is being constrained.
 
+.. _equality-flexstrain-cell:
 
-
+:at:`cell`: :at-val:`int(3), optional`
+   3D grid index (i, j, k) identifying the cell in the flex object. The grid size is specified in the :ref:`cellcount
+   <deformable-flex-cellcount>` attribute.
 
 
 .. _tendon:

doc/XMLschema.rst

@@ -2062,6 +2062,9 @@
             .. grid-item::
                :ref:`flex<equality-flexstrain-flex>`
 
+            .. grid-item::
+               :ref:`cell<equality-flexstrain-cell>`
+
             .. grid-item::
                :ref:`active<equality-flexstrain-active>`
 

doc/changelog.rst

@@ -7,9 +7,29 @@ Upcoming version (not yet released)
 
 General
 ^^^^^^^
-
+- Added new :ref:`mj_maxContact<mj_maxContact>` function to get the maximum number of possible contacts returned by
+  two geoms.
+- Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS.
 - Added :ref:`multi-cell support<body-flexcomp-cellnum>` for trilinear and quadratic flexes. Note that the implicit
   integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes.
+- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the
+  number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new
+  attribute ":ref:`cell <equality-flexstrain-cell>`
+
+  .. admonition:: Breaking API changes
+   :class: attention
+
+   - The feature :ref:`multiccd<coMultiCCD>` is now enabled by default. This feature has little performance overhead
+     and gives better contact behavior for stability.
+
+     **Migration:** The flag :ref:`multiccd<option-flag-multiccd>` must be explicitly disabled.
+
+Bug fixes
+^^^^^^^^^
+
+- Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather
+  than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child
+  spec.
 
 Version 3.7.0 (April 14, 2026)
 ------------------------------

doc/computation/index.rst

@@ -1656,23 +1656,25 @@ Both pipelines are controlled by a tolerance (in units of distance) and maximum
 
 Multiple contacts
 ^^^^^^^^^^^^^^^^^
-Some colliders can return more than one contact per colliding pair to model line or surface contacts, as when two flat
+Some colliders can return more than one contact per colliding pair to model edge or surface contacts, as when two flat
 objects touch. For example the capsule-plane and box-plane colliders can return up to two or four contacts,
-respectively. Standard general-purpose convex collision algorithms like MPR and GJK always return a single contact
+respectively. Standard general-purpose convex collision algorithms like MPR and GJK/EPA always return a single contact
 point, which is problematic for surface contact scenarios (e.g., box-stacking). Both of MuJoCo's CCD pipelines can
 return multiple points per contacting pair ("multiccd"). This behavior is controlled by the
 :ref:`multiccd<option-flag-multiccd>` flag, but is implemented in different ways with different trade-offs:
 
-libccd pipeline (legacy)
+multi-run pipeline (legacy)
   Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and
   re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact
-  points. This method is effective, but increases the cost of each collision call by a factor of 5.
+  points. This method is effective, but increases the cost of each collision call by a factor of 5.  This method is
+  used when the :ref:`nativeccd<option-flag-nativeccd>` flag is disabled, and for geoms collisions involving cylinders
+  and capsules or with :ref:`positive contact margins<body-geom-margin>`.
 
-native pipeline
-  Native multiccd discovers multiple contacts using a novel analysis of the contacting surfaces at the solution,
-  avoiding full re-runs of the collision routine, and is thus effectively "free". Note that native multiccd currently
-  does not support positive contact margins. If one of the two geoms has a positive margin, native multiccd will fall
-  back to legacy algorithm.
+single-shot pipeline
+  The single-shot pipeline is used in conjunction with the native CCD pipeline, i.e., when the
+  :ref:`nativeccd<option-flag-nativeccd>` flag is enabled. As this pipeline is one-shot and most of the geom analysis
+  is done at compilation time, there is very little performance overhead. Supported geoms are boxes and meshes without
+  :ref:`positive contact margins<body-geom-margin>`.
 
 .. _coDistance:
 
@@ -1716,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati
 Pair-wise colliders
 ^^^^^^^^^^^^^^^^^^^
 
-The table below provides information about the colliders used for different geom pairs. The second row in each cell
-lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh``
-will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
+The table below provides information about the colliders used for different geom pairs. These values can be computed
+dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the
+parameters :ref:`nativeccd<option-flag-nativeccd>`, :ref:`multiccd<option-flag-multiccd>`, and
+:ref:`margin<body-geom-margin>`.
+
+.. raw:: html
+
+   <div class="pairwise-toggles">
+     <div class="pairwise-toggle-item">
+       <label class="pairwise-switch">
+         <input type="checkbox" id="nativeccd-checkbox" checked>
+         <span class="pairwise-slider"></span>
+       </label>
+       <span>nativeccd</span>
+     </div>
+     <div class="pairwise-toggle-item">
+       <label class="pairwise-switch">
+         <input type="checkbox" id="multiccd-checkbox">
+         <span class="pairwise-slider"></span>
+       </label>
+       <span>multiccd</span>
+     </div>
+     <div class="pairwise-toggle-item">
+       <label class="pairwise-switch">
+         <input type="checkbox" id="margin-checkbox">
+         <span class="pairwise-slider"></span>
+       </label>
+       <span>with margin</span>
+     </div>
+   </div>
 
 .. list-table::
    :header-rows: 1
@@ -1742,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
      - | primitive
        | **1**
      - | primitive
-       | **2**
+       | **4**
      - | primitive
        | **4**
      - | primitive
@@ -1785,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
        | **2**
      - | CCD
        | **1**
-     - | CCD
-       | **1**, **4**
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>5</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
      - | primitive
        | **2**
-     - | CCD
-       | **1**, **4**
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>5</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
      - | SDF
        | :ref:`sdf_initpoints <option-sdf_initpoints>`
    * - Ellipsoid
@@ -1810,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
      -
      -
      -
-     - | CCD
-       | **1**, **4**
-     - | CCD
-       | **1**, **4**
-     - | CCD
-       | **1**, **4**
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>5</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>5</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>5</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
      - | SDF
        | :ref:`sdf_initpoints <option-sdf_initpoints>`
    * - Box
@@ -1825,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
      -
      - | primitive
        | **8**
-     - | CCD
-       | **1**, **4**
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+           <div class="multiccd-off"><strong>1</strong></div>
+           <div class="multiccd-native"><strong>4</strong></div>
+           <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
      - | SDF
        | :ref:`sdf_initpoints <option-sdf_initpoints>`
    * - Mesh
@@ -1835,8 +1912,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
      -
      -
      -
-     - | CCD
-       | **1**, **4**
+     -
+       .. raw:: html
+
+         <div class="line">CCD</div>
+         <div class="line">
+            <div class="multiccd-off"><strong>1</strong></div>
+            <div class="multiccd-native"><strong>4</strong></div>
+            <div class="multiccd-legacy"><strong>5</strong></div>
+         </div>
+
      - | MeshSDF
        | :ref:`sdf_initpoints <option-sdf_initpoints>`
    * - SDF
@@ -1849,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts.
      - | SDF
        | :ref:`sdf_initpoints <option-sdf_initpoints>`
 
+.. raw:: html
+
+   <script>
+     const pairwiseToggles = () => {
+       const table = document.querySelector('.table-pairwise');
+       const toggles = [
+         {id: 'nativeccd-checkbox', cls: 'nativeccd-enabled'},
+         {id: 'multiccd-checkbox', cls: 'multiccd-enabled'},
+         {id: 'margin-checkbox', cls: 'margin-enabled'}
+       ];
+
+       toggles.forEach(toggle => {
+         const cb = document.getElementById(toggle.id);
+         if (cb.checked) {
+           table.classList.add(toggle.cls);
+         }
+         cb.addEventListener('change', () => {
+           table.classList.toggle(toggle.cls, this.checked);
+         });
+       });
+     };
+     pairwiseToggles();
+   </script>
+
+
+
 .. _Sleeping:
 
 Sleeping islands

doc/css/theme_overrides.css

@@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) {
   font-size: 85%;
 }
 
+.pairwise-toggles {
+  display: flex;
+  align-items: center;
+  gap: 1.5em;
+  margin-bottom: 0.75em;
+}
+
+.pairwise-toggle-item {
+  display: flex;
+  align-items: center;
+  gap: 0.5em;
+}
+
+.pairwise-switch {
+  position: relative;
+  display: inline-block;
+  width: 36px;
+  height: 20px;
+}
+
+.pairwise-switch input {
+  opacity: 0;
+  width: 0;
+  height: 0;
+}
+
+.pairwise-slider {
+  position: absolute;
+  cursor: pointer;
+  inset: 0;
+  background-color: #ccc;
+  transition: 0.3s;
+  border-radius: 20px;
+}
+
+.pairwise-slider:before {
+  content: "";
+  position: absolute;
+  height: 14px;
+  width: 14px;
+  left: 3px;
+  bottom: 3px;
+  background-color: white;
+  transition: 0.3s;
+  border-radius: 50%;
+}
+
+.pairwise-switch input:checked + .pairwise-slider {
+  background-color: var(--secondary-header-color, #123693);
+}
+
+.pairwise-switch input:checked + .pairwise-slider:before {
+  transform: translateX(16px);
+}
+
+.multiccd-off,
+.multiccd-native,
+.multiccd-legacy {
+  display: none;
+  margin: 0;
+}
+
+.multiccd-off {
+  display: inline;
+}
+
+.multiccd-enabled .multiccd-off {
+  display: none;
+}
+
+.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native {
+  display: inline;
+}
+
+.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy,
+.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy {
+  display: inline;
+}
+
+.margin-show {
+  display: none;
+}
+
+.margin-enabled .margin-hide {
+  display: none;
+}
+
+.margin-enabled .margin-show {
+  display: inline;
+}
+
+
 .small-centered td, .small-centered th,
 .table-pairwise td, .table-pairwise th {
   text-align: center !important;

doc/includes/references.h

@@ -494,8 +494,9 @@ typedef enum mjtDisableBit_ {     // disable default feature bitflags
   mjDSBL_AUTORESET    = 1<<16,    // automatic reset when numerical issues are detected
   mjDSBL_NATIVECCD    = 1<<17,    // native convex collision detection
   mjDSBL_ISLAND       = 1<<18,    // constraint island discovery
+  mjDSBL_MULTICCD     = 1<<19,    // multiple CCD contact points
 
-  mjNDISABLE          = 19        // number of disable flags
+  mjNDISABLE          = 20        // number of disable flags
 } mjtDisableBit;
 typedef enum mjtEnableBit_ {      // enable optional feature bitflags
   mjENBL_OVERRIDE     = 1<<0,     // override contact parameters
@@ -503,10 +504,9 @@ typedef enum mjtEnableBit_ {      // enable optional feature bitflags
   mjENBL_FWDINV       = 1<<2,     // record solver statistics
   mjENBL_INVDISCRETE  = 1<<3,     // discrete-time inverse dynamics
                                   // experimental features:
-  mjENBL_MULTICCD     = 1<<4,     // multi-point convex collision detection
-  mjENBL_SLEEP        = 1<<5,     // sleeping
+  mjENBL_SLEEP        = 1<<4,     // sleeping
 
-  mjNENABLE           = 6         // number of enable flags
+  mjNENABLE           = 5         // number of enable flags
 } mjtEnableBit;
 typedef enum mjtJoint_ {          // type of degree of freedom
   mjJNT_FREE          = 0,        // global position and orientation (quat)       (7)
@@ -3161,6 +3161,8 @@ int mj_unmountVFS(mjVFS* vfs, const char* filename);
 int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename);
 int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer);
 int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
+int mj_containsBufferVFS(mjVFS* vfs, const char* name);
+int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename);
 void mj_deleteVFS(mjVFS* vfs);
 size_t mj_getCacheSize(const mjCache* cache);
 size_t mj_getCacheCapacity(const mjCache* cache);
@@ -3272,6 +3274,7 @@ void mj_passive(const mjModel* m, mjData* d);
 void mj_subtreeVel(const mjModel* m, mjData* d);
 void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
 void mj_rnePostConstraint(const mjModel* m, mjData* d);
+int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin);
 void mj_collision(const mjModel* m, mjData* d);
 void mj_makeConstraint(const mjModel* m, mjData* d);
 void mj_island(const mjModel* m, mjData* d);

doc/mjwarp/index.rst

@@ -1265,8 +1265,6 @@ is available by setting the ``NATIVECCD`` disable flag:
 The specialized collider generates up to 8 contact points, compared to up to 4 for the convex pipeline, and may improve
 contact stability for tasks involving box stacking or manipulation.
 
-.. TODO(taylorhowell): update this section once multiccd is on by default.
-
 CCD margin
 ----------
 
@@ -1283,8 +1281,8 @@ CCD colliders and will raise a ``NotImplementedError`` when calling :func:`mjw.p
      - Scenario
      - Workaround
    * - box-box, box-mesh, mesh-mesh
-     - :ref:`MULTICCD <option-flag-multiccd>` enabled
-     - Set margin to ``0`` or do not enable ``MULTICCD``
+     - :ref:`MULTICCD <option-flag-multiccd>` enabled (on by default)
+     - Set margin to ``0`` or disable ``MULTICCD``
    * - box-box
      - :ref:`NATIVECCD <option-flag-nativeccd>` enabled (on by default)
      - Set margin to ``0`` or disable ``NATIVECCD``

doc/mjwarp/update_types.py

@@ -29,7 +29,7 @@ This script updates such instances with valid types
 @dataclasses.dataclass
 class Option:
   ...
-  timestep: wp.array(dtype=float)
+  timestep: wp.array[float]
   ...
 """
 
@@ -48,13 +48,13 @@ def replace_array_calls(match):
   dtype = args[-1]
 
   if n_args == 2:
-    return f'wp.array(dtype={dtype})'
+    return f'wp.array[{dtype}]'
   elif n_args == 3:
-    return f'wp.array2d(dtype={dtype})'
+    return f'wp.array2d[{dtype}]'
   elif n_args == 4:
-    return f'wp.array3d(dtype={dtype})'
+    return f'wp.array3d[{dtype}]'
   elif n_args == 5:
-    return f'wp.array4d(dtype={dtype})'
+    return f'wp.array4d[{dtype}]'
   else:
     return match.group(0)
 

doc/modeling.rst

@@ -1767,10 +1767,11 @@ better visualize and understand the contact configuration and resulting forces.
 
   a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat
      geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components.
-  b. If contacts are between flat surfaces, try enabling the :ref:`multiccd<option-flag-multiccd>` flag, which allows
-     the detector to find more contacts than the single contact returned by the convex-convex collider.
-  c. Try enabling the native collision detection pipeline by setting the :ref:`nativeccd<option-flag-nativeccd>` flag,
-     which uses a more accurate and efficient convex collision detection algorithm.
+  b. If contacts are between flat surfaces, make sure that the flag :ref:`multiccd<option-flag-multiccd>` is not
+     disabled (enabled by default), as it allows the detector to find more contacts than the single contact
+     returned by the convex-convex collider.
+  c. Make sure that the flag :ref:`nativeccd<option-flag-nativeccd>` is not disabled (enabled by default),
+     as NativeCCD is a more accurate and efficient convex collision detection algorithm.
 
 **High-frequency vibration**
   High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in

doc/programming/index.rst

@@ -116,9 +116,10 @@ target directory.
 
 **Notes:**
 
-- When building on Windows, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or later is
-  installed (see :issue:`862` for more details).
-- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release``
+- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release``.
+- When building on Windows with MSVC, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or
+  later is installed (see :issue:`862` for more details).
+- We've found that performance on Windows is best when building with Clang, rather than MSVC.
 
 .. tip::
    As a reference, a working build configuration can be found in MuJoCo's

include/mujoco/mjmodel.h

@@ -70,8 +70,9 @@ typedef enum mjtDisableBit_ {     // disable default feature bitflags
   mjDSBL_AUTORESET    = 1<<16,    // automatic reset when numerical issues are detected
   mjDSBL_NATIVECCD    = 1<<17,    // native convex collision detection
   mjDSBL_ISLAND       = 1<<18,    // constraint island discovery
+  mjDSBL_MULTICCD     = 1<<19,    // multiple CCD contact points
 
-  mjNDISABLE          = 19        // number of disable flags
+  mjNDISABLE          = 20        // number of disable flags
 } mjtDisableBit;
 
 
@@ -81,10 +82,9 @@ typedef enum mjtEnableBit_ {      // enable optional feature bitflags
   mjENBL_FWDINV       = 1<<2,     // record solver statistics
   mjENBL_INVDISCRETE  = 1<<3,     // discrete-time inverse dynamics
                                   // experimental features:
-  mjENBL_MULTICCD     = 1<<4,     // multi-point convex collision detection
-  mjENBL_SLEEP        = 1<<5,     // sleeping
+  mjENBL_SLEEP        = 1<<4,     // sleeping
 
-  mjNENABLE           = 6         // number of enable flags
+  mjNENABLE           = 5         // number of enable flags
 } mjtEnableBit;
 
 

include/mujoco/mujoco.h

@@ -94,6 +94,12 @@ MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int
 // Delete file from VFS; return 0: success, -1: not found in VFS.
 MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename);
 
+// Check if buffer exists in VFS; return 1: exists, 0: not found.
+MJAPI int mj_containsBufferVFS(mjVFS* vfs, const char* name);
+
+// Check if file exists in VFS; return 1: exists, 0: not found.
+MJAPI int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename);
+
 // Delete all files from VFS and deallocates VFS internal memory.
 MJAPI void mj_deleteVFS(mjVFS* vfs);
 
@@ -460,6 +466,11 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result);
 // RNE with complete data: compute cacc, cfrc_ext, cfrc_int.
 MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d);
 
+// 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.
+MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin);
+
 // Run collision detection.
 MJAPI void mj_collision(const mjModel* m, mjData* d);
 

mjx/mujoco/mjx/_src/types.py

@@ -92,8 +92,6 @@ class EnableBit(enum.IntFlag):
 
   INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE
   # unsupported: OVERRIDE, ENERGY, FWDINV, ISLAND
-  # required by the C implementation only, ignored otherwise: MULTICCD
-  MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD
   SLEEP = mujoco.mjtEnableBit.mjENBL_SLEEP
 
 

mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py

@@ -36,7 +36,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAHORIZON
 from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR
 from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL
 from mujoco.mjx.third_party.mujoco_warp._src.types import Data
-from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit
+from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit
 from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType
 from mujoco.mjx.third_party.mujoco_warp._src.types import Model
 from mujoco.mjx.third_party.mujoco_warp._src.types import mat43
@@ -1127,7 +1127,7 @@ def convex_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_table
   epa_iterations = 16 if nboxbox == ncollision else m.opt.ccd_iterations
 
   # set to true to enable multiccd
-  use_multiccd = m.opt.enableflags & EnableBit.MULTICCD
+  use_multiccd = m.opt.disableflags & DisableBit.MULTICCD == 0
 
   # need at least 4 (square sides) if there's a box collision needing multiccd
   nmaxpolygon = 4 if nboxbox > 0 else 0

mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py

@@ -370,7 +370,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model:
   )
 
   # check for unsupported margin + multicontact / box-box CCD combinations
-  use_multiccd = mjm.opt.enableflags & types.EnableBit.MULTICCD
+  use_multiccd = (mjm.opt.disableflags & types.DisableBit.MULTICCD) == 0
   nativeccd_disabled = mjm.opt.disableflags & types.DisableBit.NATIVECCD
   BOX = int(mujoco.mjtGeom.mjGEOM_BOX)
   MESH = int(mujoco.mjtGeom.mjGEOM_MESH)

mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py

@@ -184,6 +184,7 @@ class DisableBit(enum.IntFlag):
     EULERDAMP:    implicit damping for Euler integration
     NATIVECCD:    native convex collision detection (ignored in MJWarp)
     ISLAND:       constraint islands
+    MULTICCD:     multiple CCD contact points
   """
 
   CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT
@@ -203,6 +204,7 @@ class DisableBit(enum.IntFlag):
   EULERDAMP = mujoco.mjtDisableBit.mjDSBL_EULERDAMP
   NATIVECCD = mujoco.mjtDisableBit.mjDSBL_NATIVECCD
   ISLAND = mujoco.mjtDisableBit.mjDSBL_ISLAND
+  MULTICCD = mujoco.mjtDisableBit.mjDSBL_MULTICCD
   # unsupported: MIDPHASE, AUTORESET
 
 
@@ -212,12 +214,10 @@ class EnableBit(enum.IntFlag):
   Attributes:
     ENERGY: energy computation
     INVDISCRETE: discrete-time inverse dynamics
-    MULTICCD: multiple contacts with CCD
   """
 
   ENERGY = mujoco.mjtEnableBit.mjENBL_ENERGY
   INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE
-  MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD
   # unsupported: OVERRIDE, FWDINV, ISLAND
 
 

model/flex/bunny.xml

@@ -31,7 +31,7 @@
     </body>
     <flexcomp type="mesh" file="bunny.obj" pos="0 0 .1" dim="2" euler="90 0 0"
               radius=".001" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="trilinear">
-      <elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="stretch"/>
+      <elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="none"/>
       <contact selfcollide="none" internal="false"/>
     </flexcomp>
   </worldbody>

model/flex/bunny_multicell.xml

@@ -31,7 +31,7 @@
     </body>
     <flexcomp type="mesh" file="bunny.obj" pos="0 0 0" dim="2" euler="90 0 0" cellcount="3 3 3"
               radius=".001" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="trilinear">
-      <elasticity young="1e3" poisson="0.1" damping="0.01" elastic2d="stretch"/>
+      <elasticity young="1e3" poisson="0.1" damping="0.01" elastic2d="none"/>
       <contact selfcollide="none" internal="false"/>
     </flexcomp>
   </worldbody>

model/flex/bunny_quadratic.xml

@@ -31,7 +31,7 @@
     </body>
     <flexcomp type="mesh" file="bunny.obj" pos="0 0 -.01" dim="2" euler="90 0 0"
               radius=".002" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="quadratic">
-      <elasticity young="1e3" poisson="0.1" damping="0.0001" elastic2d="stretch"/>
+      <elasticity young="1e3" poisson="0.1" damping="0.0001" elastic2d="none"/>
       <contact selfcollide="none" internal="false"/>
     </flexcomp>
   </worldbody>

model/flex/bunny_with_uv.xml

@@ -37,7 +37,7 @@
     </body>
     <flexcomp type="mesh" file="bunny_with_uv.obj" pos="0 0 .1" dim="2" euler="90 0 0"
               radius=".001" material="matsponge" mass=".05" name="softbody" dof="trilinear">
-      <elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="stretch"/>
+      <elasticity young="1e3" poisson="0.1" damping="0.001" elastic2d="none"/>
       <contact selfcollide="none" internal="false"/>
     </flexcomp>
   </worldbody>

model/flex/gripper_2d.xml

@@ -0,0 +1,429 @@
+<!-- Copyright 2026 DeepMind Technologies Limited
+
+     Licensed under the Apache License, Version 2.0 (the "License");
+     you may not use this file except in compliance with the License.
+     You may obtain a copy of the License at
+
+         http://www.apache.org/licenses/LICENSE-2.0
+
+     Unless required by applicable law or agreed to in writing, software
+     distributed under the License is distributed on an "AS IS" BASIS,
+     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+     See the License for the specific language governing permissions and
+     limitations under the License.
+-->
+
+<mujoco model="Gripper 2D Fin Ray">
+  <include file="scene.xml"/>
+
+  <option cone="elliptic" impratio="10" integrator="implicitfast"/>
+
+  <worldbody>
+    <body name="hand" pos="0 0 .37">
+      <joint name="lift" type="slide" range="-.13 .05"/>
+      <geom type="box" size=".12 .06 .03" rgba=".2 .2 .2 1"/>
+
+      <body name="right_arm">
+        <joint name="right" type="slide" axis="-1 0 0"/>
+        <geom type="box" size=".015 .06 .06" pos=".10 0 -.09"
+              rgba=".2 .2 .2 1"/>
+        <body name="right_finger" pos=".1 0 -.15" euler="90 0 0">
+          <!--
+            Fin Ray skeleton mesh (right finger).
+
+            The mesh is the OUTLINE of the fin ray structure:
+            two tapered vertical walls connected by horizontal ribs.
+            Each strut is a thin strip of 2 triangles; the interior
+            between ribs is empty. The radius property inflates the
+            visual geometry to give the skeleton thickness.
+
+            Vertex layout (XY plane, 7 rib levels, 4 verts per level):
+              Level i: L_front, L_back, R_front, R_back
+              Each pair offset by h=0.0015 along Y to create in-plane thickness.
+              Out-of-plane thickness set with radius parameter.
+
+            Side view of skeleton:
+              |===================|    level 0 (base, pinned)
+              |                   |
+              |=================|      level 1
+              |                 |
+              |===============|        level 2
+              |               |
+              |============|           level 3
+              |            |
+              |==========|             level 4
+              |          |
+              |========|               level 5
+              |        |
+              |=====|                  level 6
+          -->
+          <flexcomp name="right_fin" type="direct" dim="2"
+                    radius=".004" rgba="0 .7 .7 1" mass=".3" dof="2d"
+                    point="
+                      -0.0210  0.0015 0.0000
+                      -0.0170  0.0015 0.0000
+                      -0.020538 -0.0015 0.0000
+                      -0.016538 -0.0015 0.0000
+                      0.0170  0.0015 0.0000
+                      0.0210  0.0015 0.0000
+                      0.016538 -0.0015 0.0000
+                      0.020538 -0.0015 0.0000
+                      -0.0190 -0.0115 0.0000
+                      -0.0150 -0.0115 0.0000
+                      -0.018538 -0.0145 0.0000
+                      -0.014538 -0.0145 0.0000
+                      0.0150 -0.0115 0.0000
+                      0.0190 -0.0115 0.0000
+                      0.014538 -0.0145 0.0000
+                      0.018538 -0.0145 0.0000
+                      -0.0170 -0.0245 0.0000
+                      -0.0130 -0.0245 0.0000
+                      -0.016308 -0.0275 0.0000
+                      -0.012308 -0.0275 0.0000
+                      0.0130 -0.0245 0.0000
+                      0.0170 -0.0245 0.0000
+                      0.012308 -0.0275 0.0000
+                      0.016308 -0.0275 0.0000
+                      -0.0140 -0.0375 0.0000
+                      -0.0100 -0.0375 0.0000
+                      -0.013308 -0.0405 0.0000
+                      -0.009308 -0.0405 0.0000
+                      0.0100 -0.0375 0.0000
+                      0.0140 -0.0375 0.0000
+                      0.009308 -0.0405 0.0000
+                      0.013308 -0.0405 0.0000
+                      -0.0110 -0.0505 0.0000
+                      -0.0070 -0.0505 0.0000
+                      -0.010308 -0.0535 0.0000
+                      -0.006308 -0.0535 0.0000
+                      0.0070 -0.0505 0.0000
+                      0.0110 -0.0505 0.0000
+                      0.006308 -0.0535 0.0000
+                      0.010308 -0.0535 0.0000
+                      -0.0080 -0.0635 0.0000
+                      -0.0040 -0.0635 0.0000
+                      -0.007200 -0.0665 0.0000
+                      -0.003200 -0.0665 0.0000
+                      0.0040 -0.0635 0.0000
+                      0.0080 -0.0635 0.0000
+                      0.003200 -0.0665 0.0000
+                      0.007200 -0.0665 0.0000
+                      -0.0040 -0.0785 0.0000
+                      -0.0001 -0.0785 0.0000
+                      -0.003200 -0.0815 0.0000
+                      0.000700 -0.0815 0.0000
+                      0.0001 -0.0785 0.0000
+                      0.0040 -0.0785 0.0000
+                      -0.000700 -0.0815 0.0000
+                      0.003200 -0.0815 0.0000
+                    "
+                    element="
+                        0  2  3
+                        0  3  1
+
+                        4  6  7
+                        4  7  5
+
+                        1  3  6
+                        1  6  4
+
+                        8 10 11
+                        8 11  9
+
+                       12 14 15
+                       12 15 13
+
+                        9 11 14
+                        9 14 12
+
+                       16 18 19
+                       16 19 17
+
+                       20 22 23
+                       20 23 21
+
+                       17 19 22
+                       17 22 20
+
+                       24 26 27
+                       24 27 25
+
+                       28 30 31
+                       28 31 29
+
+                       25 27 30
+                       25 30 28
+
+                       32 34 35
+                       32 35 33
+
+                       36 38 39
+                       36 39 37
+
+                       33 35 38
+                       33 38 36
+
+                       40 42 43
+                       40 43 41
+
+                       44 46 47
+                       44 47 45
+
+                       41 43 46
+                       41 46 44
+
+                       48 50 51
+                       48 51 49
+
+                       52 54 55
+                       52 55 53
+
+                       49 51 54
+                       49 54 52
+
+                        2  8  9
+                        2  9  3
+
+                        6 12 13
+                        6 13  7
+
+                       10 16 17
+                       10 17 11
+
+                       14 20 21
+                       14 21 15
+
+                       18 24 25
+                       18 25 19
+
+                       22 28 29
+                       22 29 23
+
+                       26 32 33
+                       26 33 27
+
+                       30 36 37
+                       30 37 31
+
+                       34 40 41
+                       34 41 35
+
+                       38 44 45
+                       38 45 39
+
+                       42 48 49
+                       42 49 43
+
+                       46 52 53
+                       46 53 47
+                    ">
+            <edge equality="true" damping="0.02"
+                  solimp="0.99 0.999 0.0001 0.5 2" solref="0.001 1"/>
+            <contact selfcollide="none" internal="false"
+                     contype="2" conaffinity="2"/>
+            <pin id="0 1 2 3 4 5 6 7"/>
+          </flexcomp>
+        </body>
+      </body>
+
+      <body name="left_arm">
+        <joint name="left" type="slide" axis="1 0 0"/>
+        <geom type="box" size=".015 .06 .06" pos="-.10 0 -.09"
+              rgba=".2 .2 .2 1"/>
+        <body name="left_finger" pos="-.1 0 -.15" euler="90 0 0">
+          <flexcomp name="left_fin" type="direct" dim="2"
+                    radius=".004" rgba="0 .7 .7 1" mass=".3" dof="2d"
+                    point="
+                      -0.0210  0.0015 0.0000
+                      -0.0170  0.0015 0.0000
+                      -0.020538 -0.0015 0.0000
+                      -0.016538 -0.0015 0.0000
+                      0.0170  0.0015 0.0000
+                      0.0210  0.0015 0.0000
+                      0.016538 -0.0015 0.0000
+                      0.020538 -0.0015 0.0000
+                      -0.0190 -0.0115 0.0000
+                      -0.0150 -0.0115 0.0000
+                      -0.018538 -0.0145 0.0000
+                      -0.014538 -0.0145 0.0000
+                      0.0150 -0.0115 0.0000
+                      0.0190 -0.0115 0.0000
+                      0.014538 -0.0145 0.0000
+                      0.018538 -0.0145 0.0000
+                      -0.0170 -0.0245 0.0000
+                      -0.0130 -0.0245 0.0000
+                      -0.016308 -0.0275 0.0000
+                      -0.012308 -0.0275 0.0000
+                      0.0130 -0.0245 0.0000
+                      0.0170 -0.0245 0.0000
+                      0.012308 -0.0275 0.0000
+                      0.016308 -0.0275 0.0000
+                      -0.0140 -0.0375 0.0000
+                      -0.0100 -0.0375 0.0000
+                      -0.013308 -0.0405 0.0000
+                      -0.009308 -0.0405 0.0000
+                      0.0100 -0.0375 0.0000
+                      0.0140 -0.0375 0.0000
+                      0.009308 -0.0405 0.0000
+                      0.013308 -0.0405 0.0000
+                      -0.0110 -0.0505 0.0000
+                      -0.0070 -0.0505 0.0000
+                      -0.010308 -0.0535 0.0000
+                      -0.006308 -0.0535 0.0000
+                      0.0070 -0.0505 0.0000
+                      0.0110 -0.0505 0.0000
+                      0.006308 -0.0535 0.0000
+                      0.010308 -0.0535 0.0000
+                      -0.0080 -0.0635 0.0000
+                      -0.0040 -0.0635 0.0000
+                      -0.007200 -0.0665 0.0000
+                      -0.003200 -0.0665 0.0000
+                      0.0040 -0.0635 0.0000
+                      0.0080 -0.0635 0.0000
+                      0.003200 -0.0665 0.0000
+                      0.007200 -0.0665 0.0000
+                      -0.0040 -0.0785 0.0000
+                      -0.0001 -0.0785 0.0000
+                      -0.003200 -0.0815 0.0000
+                      0.000700 -0.0815 0.0000
+                      0.0001 -0.0785 0.0000
+                      0.0040 -0.0785 0.0000
+                      -0.000700 -0.0815 0.0000
+                      0.003200 -0.0815 0.0000
+                    "
+                    element="
+                        0  2  3
+                        0  3  1
+
+                        4  6  7
+                        4  7  5
+
+                        1  3  6
+                        1  6  4
+
+                        8 10 11
+                        8 11  9
+
+                       12 14 15
+                       12 15 13
+
+                        9 11 14
+                        9 14 12
+
+                       16 18 19
+                       16 19 17
+
+                       20 22 23
+                       20 23 21
+
+                       17 19 22
+                       17 22 20
+
+                       24 26 27
+                       24 27 25
+
+                       28 30 31
+                       28 31 29
+
+                       25 27 30
+                       25 30 28
+
+                       32 34 35
+                       32 35 33
+
+                       36 38 39
+                       36 39 37
+
+                       33 35 38
+                       33 38 36
+
+                       40 42 43
+                       40 43 41
+
+                       44 46 47
+                       44 47 45
+
+                       41 43 46
+                       41 46 44
+
+                       48 50 51
+                       48 51 49
+
+                       52 54 55
+                       52 55 53
+
+                       49 51 54
+                       49 54 52
+
+                        2  8  9
+                        2  9  3
+
+                        6 12 13
+                        6 13  7
+
+                       10 16 17
+                       10 17 11
+
+                       14 20 21
+                       14 21 15
+
+                       18 24 25
+                       18 25 19
+
+                       22 28 29
+                       22 29 23
+
+                       26 32 33
+                       26 33 27
+
+                       30 36 37
+                       30 37 31
+
+                       34 40 41
+                       34 41 35
+
+                       38 44 45
+                       38 45 39
+
+                       42 48 49
+                       42 49 43
+
+                       46 52 53
+                       46 53 47
+                    ">
+            <edge equality="true" damping="0.02"
+                  solimp="0.99 0.999 0.0001 0.5 2" solref="0.001 1"/>
+            <contact selfcollide="none" internal="false"
+                     contype="2" conaffinity="2"/>
+            <pin id="0 1 2 3 4 5 6 7"/>
+          </flexcomp>
+        </body>
+      </body>
+    </body>
+
+    <body>
+      <freejoint/>
+      <geom type="cylinder" size=".025 .05" pos="0 0 .1" rgba=".5 .5 0 1" euler="90 0 0" mass=".01"
+            priority="1" contype="2" condim="6" friction="2" solref="0.001 1" solimp="0.99 0.999 0.0001 0.5 2"/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <joint joint1="right" joint2="left"/>
+  </equality>
+
+  <tendon>
+    <fixed name="grasp">
+      <joint joint="right" coef="1"/>
+      <joint joint="left" coef="1"/>
+    </fixed>
+  </tendon>
+
+  <actuator>
+    <position name="lift" joint="lift" kp="600" dampratio="1"
+              ctrlrange="-1 1"/>
+    <position name="grasp" tendon="grasp" kp="200" dampratio="1"
+              ctrlrange="0 1"/>
+  </actuator>
+
+</mujoco>

plugin/usd_decoder/usd_decoder.cc

@@ -704,7 +704,7 @@ void ParseUsdPhysicsScene(mjSpec* spec,
 
   bool multiccd_flag;
   mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag);
-  spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0);
+  spec->option.disableflags |= (!multiccd_flag ? mjDSBL_MULTICCD : 0);
 
   // Compiler attributes
   auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr();

python/mujoco/bindings_test.py

@@ -966,7 +966,7 @@ Euler integrator, semi-implicit in velocity.
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0)
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1)
     self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2)
-    self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6)
+    self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1)
     self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2)
@@ -1216,13 +1216,15 @@ Euler integrator, semi-implicit in velocity.
       mujoco.set_mjcb_control(lambda m, d: None)
       mujoco.mj_step(model_instances[-1], data_instances[-1])
     mujoco.set_mjcb_control(None)
+    # Reference counting changed in Python 3.14.
+    expected_refcount = 2 if sys.version_info < (3, 14) else 1
     while data_instances:
       d = data_instances.pop()
-      self.assertEqual(sys.getrefcount(d), 2)
+      self.assertEqual(sys.getrefcount(d), expected_refcount)
       del d
     while model_instances:
       m = model_instances.pop()
-      self.assertEqual(sys.getrefcount(m), 2)
+      self.assertEqual(sys.getrefcount(m), expected_refcount)
 
   # This test is disabled on PyPy as it uses sys.getrefcount
   # However PyPy is not officially supported by MuJoCo
@@ -1236,7 +1238,9 @@ Euler integrator, semi-implicit in velocity.
     # passed to getrefcount.
     self.assertEqual(sys.getrefcount(data.model), 3)
     del data
-    self.assertEqual(sys.getrefcount(model), 2)
+    # Reference counting changed in Python 3.14.
+    expected_refcount = 2 if sys.version_info < (3, 14) else 1
+    self.assertEqual(sys.getrefcount(model), expected_refcount)
 
   def test_can_initialize_mjv_structs(self):
     self.assertIsInstance(mujoco.MjvScene(), mujoco.MjvScene)

python/mujoco/functions.cc

@@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) {
             m, d, flg_acc, result.data());
       });
   Def<traits::mj_rnePostConstraint>(pymodule);
+  Def<traits::mj_maxContact>(pymodule);
   Def<traits::mj_collision>(pymodule);
   Def<traits::mj_makeConstraint>(pymodule);
   Def<traits::mj_island>(pymodule);

python/mujoco/introspect/enums.py

@@ -46,7 +46,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjDSBL_AUTORESET', 65536),
              ('mjDSBL_NATIVECCD', 131072),
              ('mjDSBL_ISLAND', 262144),
-             ('mjNDISABLE', 19),
+             ('mjDSBL_MULTICCD', 524288),
+             ('mjNDISABLE', 20),
          ]),
      )),
     ('mjtEnableBit',
@@ -58,9 +59,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjENBL_ENERGY', 2),
              ('mjENBL_FWDINV', 4),
              ('mjENBL_INVDISCRETE', 8),
-             ('mjENBL_MULTICCD', 16),
-             ('mjENBL_SLEEP', 32),
-             ('mjNENABLE', 6),
+             ('mjENBL_SLEEP', 16),
+             ('mjNENABLE', 5),
          ]),
      )),
     ('mjtJoint',

python/mujoco/introspect/enums_test.py

@@ -42,9 +42,8 @@ class EnumsTest(absltest.TestCase):
                                           ('mjENBL_ENERGY', 1<<1),
                                           ('mjENBL_FWDINV', 1<<2),
                                           ('mjENBL_INVDISCRETE', 1<<3),
-                                          ('mjENBL_MULTICCD', 1<<4),
-                                          ('mjENBL_SLEEP', 1<<5),
-                                          ('mjNENABLE', 6)))
+                                          ('mjENBL_SLEEP', 1<<4),
+                                          ('mjNENABLE', 5)))
 
   # values mostly increment by one with occasional overrides
   def test_mjtGeom(self):  # pylint: disable=invalid-name

python/mujoco/introspect/functions.py

@@ -162,6 +162,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Delete file from VFS; return 0: success, -1: not found in VFS.',
      )),
+    ('mj_containsBufferVFS',
+     FunctionDecl(
+         name='mj_containsBufferVFS',
+         return_type=ValueType(name='int'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='vfs',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjVFS'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='name',
+                 type=PointerType(
+                     inner_type=ValueType(name='char', is_const=True),
+                 ),
+             ),
+         ),
+         doc='Check if buffer exists in VFS; return 1: exists, 0: not found.',
+     )),
+    ('mj_containsFileVFS',
+     FunctionDecl(
+         name='mj_containsFileVFS',
+         return_type=ValueType(name='int'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='vfs',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjVFS'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='directory',
+                 type=PointerType(
+                     inner_type=ValueType(name='char', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='filename',
+                 type=PointerType(
+                     inner_type=ValueType(name='char', is_const=True),
+                 ),
+             ),
+         ),
+         doc='Check if file exists in VFS; return 1: exists, 0: not found.',
+     )),
     ('mj_deleteVFS',
      FunctionDecl(
          name='mj_deleteVFS',
@@ -2407,6 +2453,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.',
      )),
+    ('mj_maxContact',
+     FunctionDecl(
+         name='mj_maxContact',
+         return_type=ValueType(name='int'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='g1',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='g2',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='has_margin',
+                 type=ValueType(name='int'),
+             ),
+         ),
+         doc='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.',  # pylint: disable=line-too-long
+     )),
     ('mj_collision',
      FunctionDecl(
          name='mj_collision',

simulate/simulate.cc

@@ -1192,7 +1192,7 @@ void MakeJointSection(mj::Simulate* sim) {
 
       // set range
       if (sim->jnt_range_[i].has_value())
-        mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
+        mju::sprintf_arr(defSlider[0].other, "%.17g %.17g",
                          sim->jnt_range_[i]->first, sim->jnt_range_[i]->second);
       else if (sim->jnt_type_[i]==mjJNT_SLIDE) {
         mju::strcpy_arr(defSlider[0].other, "-1 1");
@@ -1251,7 +1251,7 @@ void MakeControlSection(mj::Simulate* sim) {
 
     // set range
     if (sim->actuator_ctrlrange_[i].has_value())
-      mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
+      mju::sprintf_arr(defSlider[0].other, "%.17g %.17g",
                        sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second);
     else {
       mju::strcpy_arr(defSlider[0].other, "-1 1");

src/engine/engine_collision_convex.c

@@ -828,7 +828,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o
   // reduce mesh collisions to 4 contacts max
   if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) {
     if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) {
-      return mjENABLED(mjENBL_MULTICCD) ? 4 : 1;
+      return mjDISABLED(mjDSBL_MULTICCD) ? 1 : 4;
     }
   }
 
@@ -857,7 +857,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
   }
 
   // look for additional contacts
-  if (ncon == 1 && mjENABLED(mjENBL_MULTICCD)  // TODO(tassa) leave as bitflag or make geom attribute (?)
+  if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD)  // TODO(tassa) leave as bitflag or make geom attribute (?)
       && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE
       && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) {
     // multiCCD parameters

src/engine/engine_collision_driver.c

@@ -38,7 +38,7 @@
 #include "engine/engine_util_spatial.h"
 
 
-// table of pair-wise collision functions
+// 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},
@@ -56,6 +56,104 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
 
 //------------------------------------ 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;
+}
+
+
 // move arena pointer back to the end of the contact array
 static inline void resetArena(mjData* d) {
   d->parena = d->ncon * sizeof(mjContact);

src/engine/engine_collision_driver.h

@@ -18,6 +18,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjtnum.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -26,10 +27,15 @@ extern "C" {
 // collision function pointers and max contact pairs
 MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
 
+// 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
+MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin);
+
 // collision detection entry point
 MJAPI void mj_collision(const mjModel* m, mjData* d);
 
-// applies Separating Axis Theorem for rotated AABBs
+// apply the Separating Axis Theorem for rotated AABBs
 MJAPI 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,

src/engine/engine_core_constraint.c

@@ -47,249 +47,85 @@
 //-------------------------- utility functions -----------------------------------------------------
 
 
-// compute 3x3 matrix inverse, storing result in out
-// assumes matrix is invertible (det != 0)
-static void mat3_inverse(const mjtNum* mat, mjtNum* out) {
-  mjtNum det = mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) -
-               mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) +
-               mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]);
+// compute cell node Jacobians and combined chain for flex strain constraints
+// npc: number of nodes per cell
+// gindices: global indices of cell nodes in flex
+// cell_node_jac: output array of size 3*npc*cell_nnz (allocated on stack)
+// mj_{mark/free}Stack in calling function
+static mjtNum* cell_pos_and_jac(const mjModel* m, mjData* d, int flex_id, int npc, const int* gindices,
+                                int nv, const mjtNum* xpos_c, int* cell_chain, int* cell_nnz) {
+  int* nstart = m->flex_nodeadr + flex_id;
+  int* bodyid = m->flex_nodebodyid + *nstart;
 
-  out[0] =  (mat[4]*mat[8] - mat[5]*mat[7]) / det;
-  out[1] = -(mat[1]*mat[8] - mat[2]*mat[7]) / det;
-  out[2] =  (mat[1]*mat[5] - mat[2]*mat[4]) / det;
-  out[3] = -(mat[3]*mat[8] - mat[5]*mat[6]) / det;
-  out[4] =  (mat[0]*mat[8] - mat[2]*mat[6]) / det;
-  out[5] = -(mat[0]*mat[5] - mat[2]*mat[3]) / det;
-  out[6] =  (mat[3]*mat[7] - mat[4]*mat[6]) / det;
-  out[7] = -(mat[0]*mat[7] - mat[1]*mat[6]) / det;
-  out[8] =  (mat[0]*mat[4] - mat[1]*mat[3]) / det;
-}
-
-
-// compute 3x3 matrix cofactor, storing result in out
-static void mat3_cofactor(const mjtNum* mat, mjtNum* out) {
-  out[0] =   mat[4]*mat[8] - mat[5]*mat[7];
-  out[1] = -(mat[3]*mat[8] - mat[5]*mat[6]);
-  out[2] =   mat[3]*mat[7] - mat[4]*mat[6];
-  out[3] = -(mat[1]*mat[8] - mat[2]*mat[7]);
-  out[4] =   mat[0]*mat[8] - mat[2]*mat[6];
-  out[5] = -(mat[0]*mat[7] - mat[1]*mat[6]);
-  out[6] =   mat[1]*mat[5] - mat[2]*mat[4];
-  out[7] = -(mat[0]*mat[5] - mat[2]*mat[3]);
-  out[8] =   mat[0]*mat[4] - mat[1]*mat[3];
-}
-
-
-// compute 3x3 matrix determinant
-static mjtNum mat3_det(const mjtNum* mat) {
-  return mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) -
-         mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) +
-         mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]);
-}
-
-
-// compute node positions and Jacobians for flex strain constraints
-// xpos: output array of size 3*nodenum (global node positions)
-// node_jac: output array of size 3*nodenum*nv (dense Jacobians)
-// combined_chain: output array of DOF indices used by any node (sparse mode)
-// combined_nnz: output number of entries in combined_chain
-static void node_pos_and_jac(const mjModel* m, mjData* d, int f, int nv, int issparse, mjtNum* xpos,
-                             mjtNum* node_jac, int* combined_chain, int* combined_nnz) {
-  int nodenum = m->flex_nodenum[f];
-  int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
-  int nstart = m->flex_nodeadr[f];
-
-  for (int n = 0; n < nodenum; n++) {
-    if (m->flex_centered[f]) {
-      mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]);
-    } else {
-      mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart));
-      mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]);
+  // build per-cell sparse chain: union of bodyChain for npc nodes
+  *cell_nnz = 0;
+  int* dof_used = mjSTACKALLOC(d, nv, int);
+  int* temp_chain = mjSTACKALLOC(d, nv, int);
+  mju_zeroInt(dof_used, nv);
+  for (int n = 0; n < npc; n++) {
+    int temp_nnz = mj_bodyChain(m, bodyid[gindices[n]], temp_chain);
+    for (int k = 0; k < temp_nnz; k++) {
+      dof_used[temp_chain[k]] = 1;
+    }
+  }
+  for (int q = 0; q < nv; q++) {
+    if (dof_used[q]) {
+      cell_chain[(*cell_nnz)++] = q;
     }
   }
 
+  // build per-cell node Jacobians: 3*npc x cell_nnz
+  mjtNum* cell_node_jac = mjSTACKALLOC(d, 3*npc*(*cell_nnz), mjtNum);
+  mju_zero(cell_node_jac, 3*npc*(*cell_nnz));
   int* chain_col = mjSTACKALLOC(d, nv, int);
   mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
-  mju_zero(node_jac, 3*nodenum*nv);
-
-  for (int n = 0; n < nodenum; n++) {
-    int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col);
-    mju_zero(blk_jac, 3*nv);
-    mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col, 0);
-
+  for (int n = 0; n < npc; n++) {
+    int body = bodyid[gindices[n]];
+    int chain_n = mj_bodyChain(m, body, chain_col);
+    mju_zero(blk_jac, 3*chain_n);
+    mj_jacSparse(m, d, blk_jac, NULL, xpos_c + 3*n,
+                 body, chain_n, chain_col, 0);
+    // map node's sparse chain into cell_chain indexing
     for (int r = 0; r < 3; r++) {
-      for (int k = 0; k < chain_nnz; k++) {
-        node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k];
+      for (int k = 0; k < chain_n; k++) {
+        // find chain_col[k] in cell_chain via linear scan (chain is short)
+        for (int cc = 0; cc < *cell_nnz; cc++) {
+          if (cell_chain[cc] == chain_col[k]) {
+            cell_node_jac[(3*n + r)*(*cell_nnz) + cc] = blk_jac[r*chain_n + k];
+            break;
+          }
+        }
       }
     }
   }
 
-  *combined_nnz = 0;
-  if (issparse) {
-    int* dof_used = mjSTACKALLOC(d, nv, int);
-    mju_zeroInt(dof_used, nv);
-    for (int n = 0; n < nodenum; n++) {
-      int temp_chain[200];
-      int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain);
-      for (int k = 0; k < temp_nnz; k++) {
-        dof_used[temp_chain[k]] = 1;
-      }
-    }
-
-    for (int q = 0; q < nv; q++) {
-      if (dof_used[q]) {
-        combined_chain[(*combined_nnz)++] = q;
-      }
-    }
-  }
+  return cell_node_jac;
 }
 
 
-// compute strain Jacobian from strain derivative w.r.t. node positions
-// dSdx: input array of size 3*nodenum (dStrain/dNodePosition)
-// node_jac: input array of size 3*nodenum*nv (dense Jacobians)
-// strain_jac: output array of size nv (dStrain/dq)
-static void strain_jacobian(int nodenum, int nv, const mjtNum* dSdx, const mjtNum* node_jac,
-                            mjtNum* strain_jac) {
-  mju_zero(strain_jac, nv);
-  for (int n = 0; n < nodenum; n++) {
+
+// compute strain Jacobian from strain derivative w.r.t. cell-local node positions
+// dSdx_local: input array of size 3*npc (dStrain/dNodePosition for cell nodes)
+// cell_node_jac: input array of size 3*npc*cell_nnz (sparse Jacobians)
+// strain_jac: output array of size cell_nnz (dStrain/dq)
+static void cell_strain_jacobian(int npc, int cell_nnz,
+                                 const mjtNum* dSdx_local,
+                                 const mjtNum* cell_node_jac,
+                                 mjtNum* strain_jac) {
+  mju_zero(strain_jac, cell_nnz);
+  for (int n = 0; n < npc; n++) {
     for (int c = 0; c < 3; c++) {
+      mjtNum w = dSdx_local[3*n + c];
+      if (w == 0) continue;
       int row = 3*n + c;
-      for (int q = 0; q < nv; q++) {
-        strain_jac[q] += dSdx[row] * node_jac[row*nv + q];
+      for (int k = 0; k < cell_nnz; k++) {
+        strain_jac[k] += w * cell_node_jac[row*cell_nnz + k];
       }
     }
   }
 }
 
 
-// basis functions for flex strain constraints
-static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) {
-  if (order == 1) {
-    *phi = (i == 0 ? 1 - p : p);
-    *dphi = (i == 0 ? -1 : 1);
-  } else {
-    if (i == 0) {
-      *phi = 2 * p * p - 3 * p + 1;
-      *dphi = 4 * p - 3;
-    } else if (i == 1) {
-      *phi = 4 * (p - p * p);
-      *dphi = 4 * (1 - 2 * p);
-    } else {
-      *phi = 2 * p * p - p;
-      *dphi = 4 * p - 1;
-    }
-  }
-}
-
-
-// compute shape function gradients at a parametric point
-// grad: output array of size nodenum x 3 (gradient w.r.t. parametric coords)
-static void shape_gradients(
-    int order, const mjtNum* p, mjtNum grad[][3]) {
-  int npoint = (order + 1) * (order + 1) * (order + 1);
-  int stride = order + 1;
-
-  for (int n = 0; n < npoint; n++) {
-    int ix = n / (stride * stride);
-    int iy = (n / stride) % stride;
-    int iz = n % stride;
-
-    mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z;
-    basis(order, ix, p[0], &phi_x, &dphi_x);
-    basis(order, iy, p[1], &phi_y, &dphi_y);
-    basis(order, iz, p[2], &phi_z, &dphi_z);
-
-    grad[n][0] = dphi_x * phi_y * phi_z;
-    grad[n][1] = phi_x * dphi_y * phi_z;
-    grad[n][2] = phi_x * phi_y * dphi_z;
-  }
-}
-
-
-// compute dStrain/dNodePosition for volumetric invariants (I1 or J-1)
-// dSdx: output array of size 3*nodenum
-static void volumetric_dSdx(int invariant_type, int nodenum, mjtNum grad[][3],
-                            const mjtNum* F, const mjtNum* Fref_inv, mjtNum* dSdx) {
-  mju_zero(dSdx, 3*nodenum);
-
-  if (invariant_type == 0) {
-    mjtNum dSdE[9] = {1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0};
-
-    for (int n = 0; n < nodenum; n++) {
-      for (int c = 0; c < 3; c++) {
-        mjtNum dS = 0;
-        for (int ij = 0; ij < 9; ij++) {
-          int ii = ij / 3;
-          int jj = ij % 3;
-
-          mjtNum dF_ci = 0;
-          for (int k = 0; k < 3; k++) {
-            dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
-          }
-          mjtNum dF_cj = 0;
-          for (int k = 0; k < 3; k++) {
-            dF_cj += grad[n][k] * Fref_inv[k*3 + jj];
-          }
-
-          mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj;
-          dS += dSdE[ij] * 0.5 * dC_ij;
-        }
-        dSdx[3*n + c] = dS;
-      }
-    }
-  } else {
-    mjtNum cofF[9];
-    mat3_cofactor(F, cofF);
-
-    for (int n = 0; n < nodenum; n++) {
-      for (int c = 0; c < 3; c++) {
-        mjtNum dJ = 0;
-        for (int b = 0; b < 3; b++) {
-          mjtNum dF_cb = 0;
-          for (int k = 0; k < 3; k++) {
-            dF_cb += grad[n][k] * Fref_inv[k*3 + b];
-          }
-          dJ += cofF[c*3 + b] * dF_cb;
-        }
-        dSdx[3*n + c] = dJ;
-      }
-    }
-  }
-}
-
-
-// compute dStrain/dNodePosition for general strain invariants
-// dSdx: output array of size 3*nodenum
-static void invariant_dSdx(int nodenum, mjtNum grad[][3], const mjtNum* F,
-                           const mjtNum* Fref_inv, const mjtNum* dSdE, mjtNum* dSdx) {
-  mju_zero(dSdx, 3*nodenum);
-
-  for (int n = 0; n < nodenum; n++) {
-    for (int c = 0; c < 3; c++) {
-      mjtNum dS = 0;
-      for (int ij = 0; ij < 9; ij++) {
-        int ii = ij / 3;
-        int jj = ij % 3;
-
-        mjtNum dF_ci = 0;
-        for (int k = 0; k < 3; k++) {
-          dF_ci += grad[n][k] * Fref_inv[k*3 + ii];
-        }
-        mjtNum dF_cj = 0;
-        for (int k = 0; k < 3; k++) {
-          dF_cj += grad[n][k] * Fref_inv[k*3 + jj];
-        }
-
-        mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj;
-        dS += dSdE[ij] * 0.5 * dC_ij;
-      }
-      dSdx[3*n + c] = dS;
-    }
-  }
-}
-
-
 // allocate efc arrays on arena, return 1 on success, 0 on failure
 static int arenaAllocEfc(const mjModel* m, mjData* d) {
 #undef MJ_M
@@ -439,6 +275,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v,
   }
 
   int order = m->flex_interp[f];
+  order = order < 0 ? -order : order;
   int npc = (order+1)*(order+1)*(order+1);  // number of nodes per cell
 
   // cell lookup: get local coords and node indices
@@ -660,12 +497,12 @@ static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id,
 // equality constraints
 void mj_instantiateEquality(const mjModel* m, mjData* d) {
   int issparse = mj_isSparse(m), nv = m->nv;
-  int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
+  int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
   int flex_edgeadr, flex_edgenum;
   int flex_vertadr, flex_vertnum;
   mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
   mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
-  mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
+  mjtNum *jac[2], *jacdif, *data;
 
   // disabled or no equality constraints: return
   if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -684,8 +521,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
   if (issparse) {
     chain = mjSTACKALLOC(d, nv, int);
     chain2 = mjSTACKALLOC(d, nv, int);
-    buf_ind = mjSTACKALLOC(d, nv, int);
-    sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
   }
 
   // find active equality constraints
@@ -853,8 +688,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
 
         // compute Jacobian: sparse or dense
         if (issparse) {
-          NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain,
-                                  chain2, sparse_buf, buf_ind);
+          NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, chain2);
         } else {
           mju_addToScl(jac[0], jac[1], -deriv, nv);
         }
@@ -872,217 +706,137 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
       break;
 
     case mjEQ_FLEXSTRAIN: {
+      // each constraint represents a single cell; cell index in eq_data
       int f = id[0];
       int nodenum = m->flex_nodenum[f];
       int order = m->flex_interp[f];
+      order = order < 0 ? -order : order;
 
       // skip if not interpolated (order == 0 or no nodes)
       if (!order || !nodenum) {
         break;
       }
 
+      // only order 1 (trilinear) and 2 (quadratic) are supported
+      if (order > 2) {
+        mjERROR("flex strain constraints only support order 1 and 2, got %d", order);
+      }
+
       int npc = (order+1)*(order+1)*(order+1);
-      int cx = m->flex_cellnum[3*f+0];
       int cy = m->flex_cellnum[3*f+1];
       int cz = m->flex_cellnum[3*f+2];
-
-      // allocate stack for node positions and Jacobians
-      mj_markStack(d);
-      mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
-      mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum);
-      int* combined_chain = mjSTACKALLOC(d, nv, int);
-      mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum);
-      int combined_nnz = 0;
-
-      node_pos_and_jac(m, d, f, nv, issparse, xpos, node_jac, combined_chain, &combined_nnz);
-
-      // Gauss-Legendre quadrature points in [0,1]^3
-      // order=1: 2x2x2=8 points, order=2: 3x3x3=27 points
-      int nquad = order + 1;
-      int ngauss = nquad * nquad * nquad;
-
-      // 1D Gauss points
-      mjtNum gp1d[3];
-      if (nquad == 2) {
-        gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0);
-        gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0);
-      } else {
-        gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6);
-        gp1d[1] = 0.5;
-        gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6);
-      }
-
-      // build 3D Gauss points array (max 27 points)
-      mjtNum gauss[27][3];
-      for (int gi = 0; gi < nquad; gi++) {
-        for (int gj = 0; gj < nquad; gj++) {
-          for (int gk = 0; gk < nquad; gk++) {
-            int idx = gi*nquad*nquad + gj*nquad + gk;
-            gauss[idx][0] = gp1d[gi];
-            gauss[idx][1] = gp1d[gj];
-            gauss[idx][2] = gp1d[gk];
-          }
-        }
-      }
-
-      // reference positions for all nodes
       int nstart = m->flex_nodeadr[f];
-      mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
-      for (int n = 0; n < nodenum; n++) {
-        mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart));
-      }
+      int* bodyid = m->flex_nodebodyid + nstart;
 
-      // per-cell arrays
+      // read cell index from eq_data
+      int ci = (int)data[0];
+      int cj = (int)data[1];
+      int ck = (int)data[2];
+
+      mj_markStack(d);
+
+      // get cell node indices
+      int gindices[125];  // max npc = 125 for quadratic
+      mju_flexGatherCellState(order, cy, cz, ci, cj, ck,
+                              NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
+
+      // compute positions only for cell nodes (npc << nodenum)
       mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
       mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
+      for (int n = 0; n < npc; n++) {
+        int gn = gindices[n];
+        if (m->flex_centered[f] ||
+            (m->flex_node[3*(gn + nstart)+0] == 0 &&
+             m->flex_node[3*(gn + nstart)+1] == 0 &&
+             m->flex_node[3*(gn + nstart)+2] == 0)) {
+          mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
+        } else {
+          mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
+          mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
+        }
+        mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart));
+      }
+
+      // compute corotational quaternion from cell-local positions
+      mjtNum cell_quat[4] = {1, 0, 0, 0};
+      {
+        mjtNum center[3] = {0.5, 0.5, 0.5};
+        mjtNum mat[9];
+        mju_defGradient(mat, center, xpos_c, order);
+        mju_mat2Rot(cell_quat, mat);
+        mju_negQuat(cell_quat, cell_quat);
+      }
+
+      // build per-cell sparse chain and node Jacobians
+      int* cell_chain = mjSTACKALLOC(d, nv, int);
+      int cell_nnz = 0;
+      mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain,
+                                               &cell_nnz);
+
+
+      mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum);
       mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum);
-      mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum);
-      int gindices[125];  // max npc = 125 for quadratic
 
-      // loop over cells
-      for (int ci = 0; ci < cx; ci++) {
-        for (int cj = 0; cj < cy; cj++) {
-          for (int ck = 0; ck < cz; ck++) {
-            // gather cell-local node positions
-            mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, refpos, xpos_c, NULL,
-                                    refpos_c, gindices, NULL);
+      // for dense mode: allocate and zero a dense Jacobian buffer once
+      mjtNum* dense_jac = NULL;
+      if (!issparse) {
+        dense_jac = mjSTACKALLOC(d, nv, mjtNum);
+        mju_zero(dense_jac, nv);
+      }
 
-            // B-bar: center-point volumetric constraints (trilinear)
-            if (order == 1) {
-              mjtNum center[3] = {0.5, 0.5, 0.5};
-              mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9];
+      // read eigenmode data from flex_stiffness
+      int ndof_cell = 3 * npc;
+      int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2]
+                   + cj * m->flex_cellnum[3*f+2] + ck;
+      const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
+                           + cell_idx * ndof_cell * ndof_cell;
+      int neig = (int)k_cell[0];
 
-              mju_defGradient(Fcur_c, center, xpos_c, order);
-              mju_defGradient(Fref_c, center, refpos_c, order);
-              mat3_inverse(Fref_c, Fref_inv_c);
-              mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c);
+      // compute displacement in corotational frame
+      mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum);
+      for (int n = 0; n < npc; n++) {
+        // rotate xpos_c to corotational frame
+        mjtNum xrot[3];
+        mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat);
+        displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0];
+        displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1];
+        displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2];
+      }
 
-              mjtNum C_c[9], E_c[9];
-              mju_mulMatTMat3(C_c, F_c, F_c);
-              mju_scl(E_c, C_c, 0.5, 9);
-              E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5;
+      // compute inverse quaternion for rotating eigenvectors to world frame
+      mjtNum cell_quat_inv[4];
+      mju_negQuat(cell_quat_inv, cell_quat);
 
-              mjtNum I1_c = E_c[0] + E_c[4] + E_c[8];
-              mjtNum J_c = mat3_det(F_c);
+      // loop over eigenmodes
+      for (int eig = 0; eig < neig; eig++) {
+        const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell;
 
-              mjtNum grad_c[8][3];
-              shape_gradients(order, center, grad_c);
+        // constraint residual: dot product of scaled eigenvector with displacement
+        mjtNum residual = 0;
+        for (int j = 0; j < ndof_cell; j++) {
+          residual += eigvec[j] * displ_c[j];
+        }
+        cpos[0] = residual;
 
-              for (int inv = 0; inv < 2; inv++) {
-                cpos[0] = (inv == 0) ? I1_c : J_c - 1.0;
+        // rotate eigenvector to world frame for Jacobian
+        // dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d]
+        for (int n = 0; n < npc; n++) {
+          mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv);
+        }
 
-                // compute local dSdx
-                volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local);
+        // contract with cell_node_jac to get sparse Jacobian
+        cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
 
-                // scatter to global dSdx
-                mju_zero(dSdx, 3*nodenum);
-                for (int n = 0; n < npc; n++) {
-                  mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n);
-                }
-
-                strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac);
-
-                if (issparse) {
-                  mj_markStack(d);
-                  mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum);
-                  for (int k = 0; k < combined_nnz; k++) {
-                    sj[k] = strain_jac[combined_chain[k]];
-                  }
-                  mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
-                                   combined_nnz, combined_chain);
-                  mj_freeStack(d);
-                } else {
-                  mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
-                }
-              }
-            }
-
-            // Gauss integration per cell
-            for (int g = 0; g < ngauss; g++) {
-              mjtNum* p = gauss[g];
-
-              // F = Fcur * Fref_inv
-              mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9];
-              mju_defGradient(Fcur, p, xpos_c, order);
-              mju_defGradient(Fref, p, refpos_c, order);
-              mat3_inverse(Fref, Fref_inv);
-              mju_mulMatMat3(F, Fcur, Fref_inv);
-
-              // Green-Lagrange strain E = 0.5*(C - I)
-              mjtNum C[9], E[9];
-              mju_mulMatTMat3(C, F, F);
-              for (int j = 0; j < 9; j++) {
-                E[j] = 0.5 * C[j];
-              }
-              E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5;
-
-              // 3 invariants of E
-              mjtNum I1 = E[0] + E[4] + E[8];
-              mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6]
-                          + E[3]*E[1] + E[4]*E[4] + E[5]*E[7]
-                          + E[6]*E[2] + E[7]*E[5] + E[8]*E[8];
-              mjtNum I2 = 0.5 * (I1*I1 - trE2);
-              mjtNum I3 = mat3_det(E);
-
-              // shape function gradients at Gauss point
-              mjtNum grad[27][3];
-              shape_gradients(order, p, grad);
-
-              for (int s = 0; s < 6; s++) {
-                // skip I1,I2,I3 for trilinear (B-bar handles vol)
-                if (order == 1 && (s == 0 || s == 1 || s == 2)) {
-                  continue;
-                }
-
-                mjtNum dSdE[9];
-                mju_zero(dSdE, 9);
-
-                if (s == 0) {
-                  cpos[0] = I1;
-                  dSdE[0] = dSdE[4] = dSdE[8] = 1.0;
-                } else if (s == 1) {
-                  cpos[0] = I2;
-                  dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4];
-                  dSdE[8] = I1-E[8];
-                  dSdE[1] = -E[1]; dSdE[3] = -E[3];
-                  dSdE[2] = -E[2]; dSdE[6] = -E[6];
-                  dSdE[5] = -E[5]; dSdE[7] = -E[7];
-                } else if (s == 2) {
-                  cpos[0] = I3;
-                  mat3_cofactor(E, dSdE);
-                } else {
-                  int offdiag_idx[3] = {1, 2, 5};
-                  int ij = offdiag_idx[s - 3];
-                  cpos[0] = E[ij];
-                  dSdE[ij] = 1.0;
-                }
-
-                // compute local dS/dx for cell nodes
-                invariant_dSdx(npc, grad, F, Fref_inv, dSdE,
-                               dSdx_local);
-
-                // scatter to global dSdx
-                mju_zero(dSdx, 3*nodenum);
-                for (int n = 0; n < npc; n++) {
-                  mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n);
-                }
-
-                strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac);
-
-                if (issparse) {
-                  mj_markStack(d);
-                  mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum);
-                  for (int k = 0; k < combined_nnz; k++) {
-                    sj[k] = strain_jac[combined_chain[k]];
-                  }
-                  mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
-                                   combined_nnz, combined_chain);
-                  mj_freeStack(d);
-                } else {
-                  mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
-                }
-              }
-            }
+        if (issparse) {
+          mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
+                           cell_nnz, cell_chain);
+        } else {
+          for (int k = 0; k < cell_nnz; k++) {
+            dense_jac[cell_chain[k]] = strain_jac[k];
+          }
+          mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
+          for (int k = 0; k < cell_nnz; k++) {
+            dense_jac[cell_chain[k]] = 0;
           }
         }
       }
@@ -1920,26 +1674,36 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
         break;
 
       case mjEQ_FLEXSTRAIN: {
-        // strain constraints: use average node inv weight
+        // strain constraints: per-cell, use avg inv weight of cell's npc nodes
         int flex_id = m->eq_obj1id[id];
-        int nodenum = m->flex_nodenum[flex_id];
         int nstart = m->flex_nodeadr[flex_id];
         int order = m->flex_interp[flex_id];
+        order = order < 0 ? -order : order;
+        int npc = (order+1)*(order+1)*(order+1);
 
-        // compute constraint count per cell, then multiply by ncells
+        // per-cell constraint count
         int nquad = order + 1;
         int ngauss = nquad * nquad * nquad;
-        int ncells = m->flex_cellnum[3*flex_id+0]
-                   * m->flex_cellnum[3*flex_id+1]
-                   * m->flex_cellnum[3*flex_id+2];
-        int nconstraint = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss));
+        int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss);
+
+        // get cell index from eq_data
+        int eq_id = d->efc_id[i];
+        int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0];
+        int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1];
+        int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2];
+        int cy = m->flex_cellnum[3*flex_id+1];
+        int cz = m->flex_cellnum[3*flex_id+2];
+
+        int gindices[125];
+        mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
+                                NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
 
         mjtNum avg_invweight = 0;
-        for (int n = 0; n < nodenum; n++) {
-          int bodyid = m->flex_nodebodyid[nstart + n];
+        for (int n = 0; n < npc; n++) {
+          int bodyid = m->flex_nodebodyid[nstart + gindices[n]];
           avg_invweight += m->body_invweight0[2*bodyid];
         }
-        avg_invweight /= nodenum;
+        avg_invweight /= npc;
         for (int c = 0; c < nconstraint; c++) {
           dA[i++] = avg_invweight;
         }
@@ -2404,6 +2168,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
     chain2 = mjSTACKALLOC(d, nv, int);
   }
 
+  // pre-allocate buffer for cell body IDs (max npc = 125 for order=2)
+  int* cell_bodies = nnz ? mjSTACKALLOC(d, 125, int) : NULL;
+
   // find active equality constraints
   for (int i=0; i < neq; i++) {
     // skip inactive
@@ -2529,34 +2296,37 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
       break;
 
     case mjEQ_FLEXSTRAIN: {
-      // strain constraints:
-      // Q1: B-bar, 2 center (I1, J-1) + 3*8 shear = 26
-      // Q2: full 3x3x3 Gauss, 6*27 = 162
-      // skip if not interpolated (order == 0 or no nodes)
-      int order = m->flex_interp[id[0]];
-      int nodenum = m->flex_nodenum[id[0]];
-      if (!order || !nodenum) {
+      // per-cell strain constraints: each equality is one cell
+      int f = id[0];
+      int order = m->flex_interp[f];
+      order = order < 0 ? -order : order;
+      if (!order || !m->flex_nodenum[f]) {
         break;
       }
-      int nquad = order + 1;  // 2 for order=1, 3 for order=2
-      int ngauss = nquad * nquad * nquad;  // 8 or 27
-      int ncells = m->flex_cellnum[3*id[0]+0]
-                 * m->flex_cellnum[3*id[0]+1]
-                 * m->flex_cellnum[3*id[0]+2];
-      size = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss));
+      int npc = (order+1)*(order+1)*(order+1);
+
+      // read eigenmode count from flex_stiffness
+      int ndof_cell = 3 * npc;
+      int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
+      int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
+      int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
+      int cy = m->flex_cellnum[3*f+1];
+      int cz = m->flex_cellnum[3*f+2];
+      int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
+      const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
+                           + cell_idx * ndof_cell * ndof_cell;
+      size = (int)k_cell[0];  // neig stored as first element
 
       if (nnz) {
-        // Count unique DOFs across all node bodies (matching instantiation)
-        int nstart = m->flex_nodeadr[id[0]];
-        int* nodebodies = mjSTACKALLOC(d, nodenum, int);
-        for (int n = 0; n < nodenum; n++) {
-          nodebodies[n] = m->flex_nodebodyid[nstart + n];
+        // get the npc node body IDs for this cell
+        int gindices[125];
+        mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
+                                NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
+        int nstart = m->flex_nodeadr[f];
+        for (int n = 0; n < npc; n++) {
+          cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]];
         }
-
-        // mj_jacSumCount deduplicates shared DOFs
-        NV = mj_jacSumCount(m, d, chain, nodenum, nodebodies);
-
-        // each constraint row shares this combined NV
+        NV = mj_jacSumCount(m, d, chain, npc, cell_bodies);  // npc nodes only
         NV = size * NV;
       }
       break;

src/engine/engine_core_smooth.c

@@ -564,16 +564,13 @@ void mj_flex(const mjModel* m, mjData* d) {
 
     // 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs
     if (m->flex_interp[f] == 0) {
-      // centered: copy body position
-      if (m->flex_centered[f]) {
-        for (int i=vstart; i < vend; i++) {
+      for (int i=vstart; i < vend; i++) {
+        if (m->flex_centered[f] ||
+            (m->flex_vert[3*i+0] == 0 &&
+             m->flex_vert[3*i+1] == 0 &&
+             m->flex_vert[3*i+2] == 0)) {
           mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
-        }
-      }
-
-      // non-centered: map from local to global
-      else {
-        for (int i=vstart; i < vend; i++) {
+        } else {
           mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
           mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
         }
@@ -585,19 +582,21 @@ void mj_flex(const mjModel* m, mjData* d) {
       int nodenum = nend - nstart;
       mj_markStack(d);
       mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
-      if (m->flex_centered[f]) {
-        for (int i=nstart; i < nend; i++) {
-          mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]);
-        }
-      } else {
-        for (int i=nstart; i < nend; i++) {
-          int j = i - nstart;
+      for (int i=nstart; i < nend; i++) {
+        int j = i - nstart;
+        if (m->flex_centered[f] ||
+            (m->flex_node[3*i+0] == 0 &&
+             m->flex_node[3*i+1] == 0 &&
+             m->flex_node[3*i+2] == 0)) {
+          mji_copy3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
+        } else {
           mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i);
           mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
         }
       }
 
-      int order = m->flex_interp[f];
+      int interp = m->flex_interp[f];
+      int order = interp < 0 ? -interp : 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];
@@ -2626,7 +2625,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
     case mjEQ_FLEXSTRAIN: {
       // increment: trilinear uses 2 center (I1,J-1) + 3*ngauss shear, quadratic uses 6*ngauss
       k = m->eq_obj1id[id];
-      int order = m->flex_interp[k];
+      int interp_k = m->flex_interp[k];
+      int order = interp_k < 0 ? -interp_k : interp_k;
       int nodenum = m->flex_nodenum[k];
       if (order && nodenum) {
         int nquad = order + 1;

src/engine/engine_core_util.c

@@ -988,28 +988,36 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
 //-------------------------- miscellaneous utilities -----------------------------------------------
 
 // gather global node positions and velocities
-void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) {
+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
-  if (m->flex_centered[f]) {
-    for (int i=0; i < nodenum; i++) {
-      mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
-      if (vel) {
-        mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
-      }
+  // 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);
     }
-  } else {
-    mjtNum screw[6];
-    for (int i=0; i < nodenum; i++) {
-      mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
-      mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
-      if (vel) {
-        mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
-        mju_copy3(vel + 3*i, screw + 3);
-      }
+
+    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);
     }
   }
 }

src/engine/engine_core_util.h

@@ -130,7 +130,7 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
 //-------------------------- miscellaneous ---------------------------------------------------------
 
 // gather global node positions and velocities
-MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel);
+MJAPI void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel);
 
 // extract 6D force:torque for one contact, in contact frame
 MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]);

src/engine/engine_derivative.c

@@ -880,17 +880,63 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
                                   const int* dof_indices, int ndof, int nband) {
   int nv = m->nv;
 
-  // build global2local map for ADDH
-  int* global2local = NULL;
+  // compute upper bounds across all interpolated flexes
+  int max_nodenum = 0;
+  int max_npc = 0;
+  for (int f = 0; f < m->nflex; f++) {
+    if (!m->flex_interp[f]) continue;
+    if (m->flex_rigid[f]) continue;
+    int order = m->flex_interp[f];
+    order = order < 0 ? -order : order;
+    int npc = (order+1)*(order+1)*(order+1);
+    if (npc > max_npc) max_npc = npc;
+    if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f];
+  }
+
+  // nothing to do
+  if (max_npc == 0) {
+    return;
+  }
+
+  int max_dim_c = 3 * max_npc;
+
+  // single unconditional markStack
+  mj_markStack(d);
+
+  // global2local map for ADDH
+  int* global2local = mjSTACKALLOC(d, nv, int);
   if (op == mjFLEXOP_ADDH) {
-    mj_markStack(d);
-    global2local = mjSTACKALLOC(d, nv, int);
     mju_fillInt(global2local, -1, nv);
     for (int i=0; i<ndof; i++) {
       global2local[dof_indices[i]] = i;
     }
   }
 
+  // per-flex node positions (upper bound)
+  mjtNum* xpos = mjSTACKALLOC(d, 3*max_nodenum, mjtNum);
+
+  // per-cell arrays (upper bound)
+  mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npc, mjtNum);
+  mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum);
+
+  // sparse Jacobian for one cell (upper bound)
+  int* J_rownnz = mjSTACKALLOC(d, max_dim_c, int);
+  int* J_rowadr = mjSTACKALLOC(d, max_dim_c, int);
+  mjtNum* J_val = mjSTACKALLOC(d, max_dim_c*nv, mjtNum);
+  int* J_colind = mjSTACKALLOC(d, max_dim_c*nv, int);
+
+  // temp allocations for chain
+  int* chain_colind = mjSTACKALLOC(d, nv, int);
+  mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
+
+  // ADDH-specific allocations (upper bound)
+  mjtNum* J_reduced = NULL;
+  mjtNum* KJ = NULL;
+  if (op == mjFLEXOP_ADDH) {
+    J_reduced = mjSTACKALLOC(d, max_dim_c*ndof, mjtNum);
+    KJ = mjSTACKALLOC(d, max_dim_c*ndof, mjtNum);
+  }
+
   // loop over flexes
   for (int f=0; f < m->nflex; f++) {
     // only process flex_interp
@@ -899,10 +945,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
     }
 
     // get stiffness and damping
-    mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f];
+    mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f];
 
     // skip if rigid or no stiffness
-    if (m->flex_rigid[f] || k[0] == 0) {
+    if (m->flex_rigid[f] || K[0] == 0) {
+      continue;
+    }
+
+    // skip if strain constraints present (stiffness handled by constraint solver)
+    if (m->flex_edgeequality[f] == 3) {
       continue;
     }
 
@@ -916,32 +967,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
     }
 
     int order = m->flex_interp[f];
+    order = order < 0 ? -order : order;
     int npc = (order+1)*(order+1)*(order+1);
     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 nodenum = m->flex_nodenum[f];
     int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
 
-    // standard stack allocation
-    mj_markStack(d);
-    mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum);
-
-    // per-cell arrays
     int dim_c = 3 * npc;
-    mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
-    mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum);
-
-    // sparse Jacobian for one cell
-    int* J_rownnz = mjSTACKALLOC(d, dim_c, int);
-    int* J_rowadr = mjSTACKALLOC(d, dim_c, int);
-    mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum);
-    int* J_colind = mjSTACKALLOC(d, dim_c*nv, int);
-
-    // temp allocations for chain
-    int* chain_colind = mjSTACKALLOC(d, nv, int);
-    mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum);
 
     // gather raw node positions (unrotated)
     mju_flexGatherState(m, d, f, xpos, NULL);
@@ -951,6 +985,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
     for (int ci = 0; ci < cx; ci++) {
       for (int cj = 0; cj < cy; cj++) {
         for (int ck = 0; ck < cz; ck++) {
+          // get cell stiffness
+          mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc;
+
+          // skip empty cells: stiffness buffer is zero-initialized at compile time
+          // (user_model.cc), and non-empty cells have strictly positive diagonal
+          if (k_cell[0] == 0) {
+            cell_idx++;
+            continue;
+          }
+
           // gather cell-local node positions
           int gindices[125];  // max npc = 125 for quadratic
           mjtNum quat[4];
@@ -962,9 +1006,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
           mju_quat2Mat(R, quat);
           mju_transpose(RT, R, 3, 3);
 
-          // get cell stiffness
-          mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
-
           // compute K_rot_cell = RT * K_cell * R (block-wise)
           mju_zero(K_rot_cell, dim_c*dim_c);
           for (int a = 0; a < npc; a++) {
@@ -1020,9 +1061,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
             addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
                               J_val, K_rot_cell, dim_c);
           } else if (op == mjFLEXOP_ADDH) {
-            mj_markStack(d);
             // H -= J_cell^T * K_rot_cell * J_cell (banded format)
-            mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum);
             mju_zero(J_reduced, dim_c*ndof);
 
             for (int i = 0; i < dim_c; i++) {
@@ -1038,7 +1077,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
             }
 
             // KJ = K_rot_cell * J_reduced  (dim_c x ndof)
-            mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum);
             mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof);
 
             // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
@@ -1051,20 +1089,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
                 res[i*nband + nband-1-(i-j)] -= val;
               }
             }
-            mj_freeStack(d);
           }
 
           cell_idx++;
         }
       }
     }
-
-    mj_freeStack(d);
   }
 
-  if (op == mjFLEXOP_ADDH) {
-    mj_freeStack(d);  // free global2local
-  }
+  mj_freeStack(d);
 }
 
 

src/engine/engine_passive.c

@@ -229,8 +229,14 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
       continue;
     }
 
+    // skip interpolated flex with strain constraints (stiffness in constraint solver)
+    if (m->flex_edgeequality[f] == 3) {
+      continue;
+    }
+
     if (m->flex_interp[f]) {
       int order = m->flex_interp[f];
+      order = order < 0 ? -order : order;
       int npc = (order+1)*(order+1)*(order+1);  // nodes per cell
       int cx = m->flex_cellnum[3*f+0];
       int cy = m->flex_cellnum[3*f+1];
@@ -268,16 +274,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
       for (int ci = 0; ci < cx; ci++) {
         for (int cj = 0; cj < cy; cj++) {
           for (int ck = 0; ck < cz; ck++) {
+            // get cell stiffness matrix
+            mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
+
+            // skip empty cells (zero stiffness)
+            if (k_cell[0] == 0) {
+              cell_idx++;
+              continue;
+            }
+
             // gather cell-local node data
             mjtNum quat[4];
-            mjtNum p[3] = {.5, .5, .5};
             mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0,
                                     xpos_c, vel_c, xpos0_c, NULL, quat);
 
             // rotate to corotational frame
             for (int n = 0; n < npc; n++) {
               mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat);
-              mji_addTo3(xpos_c+3*n, p);
               mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat);
             }
 
@@ -286,9 +299,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
               mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1);
             }
 
-            // get cell stiffness matrix
-            mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
-
             // compute force in corotational frame
             if (enbl_spring) {
               mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc);
@@ -329,12 +339,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
       // apply accumulated forces to bodies
       for (int i = 0; i < nodenum; i++) {
         mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]);
-        if (m->flex_centered[f]) {
-          if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i);
-          if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i);
+        int bid = bodyid[i];
+        int nidx = i + m->flex_nodeadr[f];
+
+        // fast path: node at body origin (not pinned), direct DOF write
+        if (m->body_dofnum[bid] > 0 &&
+            (m->flex_centered[f] ||
+             (m->flex_node[3*nidx+0] == 0 &&
+              m->flex_node[3*nidx+1] == 0 &&
+              m->flex_node[3*nidx+2] == 0))) {
+          if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i);
+          if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i);
         } else {
-          if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_spring);
-          if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper);
+          if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring);
+          if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper);
         }
       }
 

src/engine/engine_setconst.c

@@ -714,6 +714,7 @@ static void makeFlexBandwidth(mjModel* m, mjData* d) {
     for (int f = 0; f < m->nflex; f++) {
       if (!m->flex_interp[f]) continue;
       int order = m->flex_interp[f];
+      order = order < 0 ? -order : order;
       int nodeadr = m->flex_nodeadr[f];
       int nodenum = m->flex_nodenum[f];
       int cx = m->flex_cellnum[3*f+0];

src/engine/engine_solver.c

@@ -810,6 +810,12 @@ typedef struct {
   mjtNum* Mgrad;          // M\grad or H\grad                             (nv x 1)
   mjtNum* search;         // linesearch vector                            (nv x 1)
   mjtNum* quad;           // quadratic polynomials for constraint costs   (nefc x 3)
+  int* oldstate;          // previous constraint state                    (nefc x 1)
+
+  // CG arrays (PrimalAllocate, CG only)
+  mjtNum* gradold;        // previous gradient                            (nv x 1)
+  mjtNum* Mgradold;       // previous preconditioned gradient             (nv x 1)
+  mjtNum* Mgraddif;       // gradient difference                          (nv x 1)
 
   // Newton arrays, known-size (PrimalAllocate)
   mjtNum* D;              // constraint inertia                           (nefc x 1)
@@ -823,8 +829,6 @@ typedef struct {
   int* L_rowadr;          // Hessian factor row addresses                 (nv x 1)
   int* LT_rownnz;         // Hessian factor transpose row nonzeros        (nv x 1)
   int* LT_rowadr;         // Hessian factor transpose row addresses       (nv x 1)
-  int* buf_ind;           // index buffer for sparse addition             (nv x 1)
-  mjtNum* buf_val;        // value buffer for sparse addition             (nv x 1)
 
   // Newton arrays, computed-size (MakeHessian)
   int nH;                 // number of nonzeros in Hessian H
@@ -960,55 +964,94 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c
 // allocate fixed-size arrays in mjPrimalContext
 //  mj_{mark/free}Stack in calling function!
 static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) {
-  // local sizes
+  // local sizes and flags
   int nv = ctx->nv;
   int nefc = ctx->nefc;
+  int nJ = ctx->is_sparse ? d->nJ : 0;
+  int is_sparse = ctx->is_sparse;
+  int is_elliptic = ctx->is_elliptic;
 
-  // common arrays
-  ctx->Jaref  = mjSTACKALLOC(d, nefc, mjtNum);
-  ctx->Jv     = mjSTACKALLOC(d, nefc, mjtNum);
-  ctx->Ma     = mjSTACKALLOC(d, nv, mjtNum);
-  ctx->Mv     = mjSTACKALLOC(d, nv, mjtNum);
-  ctx->grad   = mjSTACKALLOC(d, nv, mjtNum);
-  ctx->Mgrad  = mjSTACKALLOC(d, nv, mjtNum);
-  ctx->search = mjSTACKALLOC(d, nv, mjtNum);
-  ctx->quad   = mjSTACKALLOC(d, nefc*3, mjtNum);
+  // compute mjtNum block size
+  size_t nNum = 5*nefc + 5*nv;         // common arrays
+  if (is_sparse) nNum += nJ;           // JT
+  if (flg_Newton) {
+    nNum += nefc + nv;                 // D, cholupd
+    if (is_elliptic) nNum += 6*nv;     // LTJ
+    if (!is_sparse) {
+      nNum += nv*nv;                   // L (dense)
+      if (is_elliptic) nNum += nv*nv;  // Lcone (dense)
+    }
+  } else {
+    nNum += 3*nv;                      // CG arrays
+  }
 
-  // sparse only, compute Jacobian transpose
-  if (ctx->is_sparse) {
-    ctx->JT_rownnz   = mjSTACKALLOC(d, nv, int);
-    ctx->JT_rowadr   = mjSTACKALLOC(d, nv, int);
-    ctx->JT_rowsuper = mjSTACKALLOC(d, nv, int);
-    ctx->JT_colind   = mjSTACKALLOC(d, d->nJ, int);
-    ctx->JT          = mjSTACKALLOC(d, d->nJ, mjtNum);
-    int offset       = ctx->J_rowadr[0];
+  // compute int block size
+  size_t nInt = nefc;                  // oldstate
+  if (is_sparse) {
+    nInt += 3*nv + nJ;                 // JT sparse
+    if (flg_Newton) nInt += 8*nv;      // Newton sparse
+  }
+
+  // allocate mjtNum and int blocks
+  mjtNum* numblock = mjSTACKALLOC(d, nNum, mjtNum);
+  int* intblock    = mjSTACKALLOC(d, nInt, int);
+
+  // carve mjtNum block
+  ctx->Jaref  = numblock;  numblock += nefc;
+  ctx->Jv     = numblock;  numblock += nefc;
+  ctx->Ma     = numblock;  numblock += nv;
+  ctx->Mv     = numblock;  numblock += nv;
+  ctx->grad   = numblock;  numblock += nv;
+  ctx->Mgrad  = numblock;  numblock += nv;
+  ctx->search = numblock;  numblock += nv;
+  ctx->quad   = numblock;  numblock += 3*nefc;
+  if (is_sparse) {
+    ctx->JT   = numblock;  numblock += nJ;
+  }
+  if (flg_Newton) {
+    ctx->D       = numblock;  numblock += nefc;
+    ctx->cholupd = numblock;  numblock += nv;
+    if (is_elliptic) {
+      ctx->LTJ = numblock;  numblock += 6*nv;
+    }
+    if (!is_sparse) {
+      ctx->nL = nv*nv;
+      ctx->L     = numblock;  numblock += ctx->nL;
+      ctx->Lcone = is_elliptic ? numblock : NULL;
+      if (is_elliptic) numblock += ctx->nL;
+    }
+  } else {
+    ctx->gradold  = numblock;  numblock += nv;
+    ctx->Mgradold = numblock;  numblock += nv;
+    ctx->Mgraddif = numblock;  numblock += nv;
+  }
+
+  // carve int block
+  ctx->oldstate = intblock;  intblock += nefc;
+  if (is_sparse) {
+    ctx->JT_rownnz   = intblock;  intblock += nv;
+    ctx->JT_rowadr   = intblock;  intblock += nv;
+    ctx->JT_rowsuper = intblock;  intblock += nv;
+    ctx->JT_colind   = intblock;  intblock += nJ;
+  }
+  if (flg_Newton && is_sparse) {
+    ctx->H_rowadr   = intblock;  intblock += nv;
+    ctx->H_rownnz   = intblock;  intblock += nv;
+    ctx->HT_rownnz  = intblock;  intblock += nv;
+    ctx->HT_rowadr  = intblock;  intblock += nv;
+    ctx->L_rownnz   = intblock;  intblock += nv;
+    ctx->L_rowadr   = intblock;  intblock += nv;
+    ctx->LT_rownnz  = intblock;  intblock += nv;
+    ctx->LT_rowadr  = intblock;  intblock += nv;
+  }
+
+  // sparse: compute Jacobian transpose
+  if (is_sparse) {
+    int offset = ctx->J_rowadr[0];
     mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv,
                         ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper,
                         ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset);
   }
-
-  // Newton only, known-size arrays
-  if (flg_Newton) {
-    ctx->D = mjSTACKALLOC(d, nefc, mjtNum);
-    ctx->cholupd = mjSTACKALLOC(d, nv, mjtNum);
-    if (ctx->is_elliptic) {
-      ctx->LTJ = mjSTACKALLOC(d, 6*nv, mjtNum);
-    }
-
-    // sparse Newton only
-    if (ctx->is_sparse) {
-      ctx->H_rowadr   = mjSTACKALLOC(d, nv, int);
-      ctx->H_rownnz   = mjSTACKALLOC(d, nv, int);
-      ctx->HT_rownnz  = mjSTACKALLOC(d, nv, int);
-      ctx->HT_rowadr  = mjSTACKALLOC(d, nv, int);
-      ctx->L_rownnz   = mjSTACKALLOC(d, nv, int);
-      ctx->L_rowadr   = mjSTACKALLOC(d, nv, int);
-      ctx->LT_rownnz  = mjSTACKALLOC(d, nv, int);
-      ctx->LT_rowadr  = mjSTACKALLOC(d, nv, int);
-      ctx->buf_val    = mjSTACKALLOC(d, nv, mjtNum);
-      ctx->buf_ind    = mjSTACKALLOC(d, nv, int);
-    }
-  }
 }
 
 
@@ -1529,7 +1572,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) {
 
   // sparse
   if (ctx->is_sparse) {
-    // initialize Hessian rowadr, rownnz; get total nonzeros
+    // count Hessian nonzeros, initialize rowadr, rownnz
     ctx->nH = mju_sqrMatTDSparseSymbolic(
         ctx->H_rownnz, ctx->H_rowadr, NULL, NULL,
         nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind,
@@ -1538,17 +1581,19 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) {
     // add M nonzeros to Hessian total (unavoidable overcounting since H_colind is still unknown)
     ctx->nH += ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1];
 
-    // shift H row addresses to make room for C
+    // nH is known: allocate H, H_colind, HT_colind
+    ctx->H          = mjSTACKALLOC(d, ctx->nH, mjtNum);
+    int* H_intblock = mjSTACKALLOC(d, 2*ctx->nH, int);
+    ctx->H_colind   = H_intblock;
+    ctx->HT_colind  = H_intblock + ctx->nH;
+
+    // shift H row addresses to make room for M
     int shift = 0;
     for (int r = 0; r < nv - 1; r++) {
       shift += ctx->M_rownnz[r];
       ctx->H_rowadr[r + 1] += shift;
     }
 
-    // allocate H_colind and H
-    ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int);
-    ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum);
-
     // compute H = J'*D*J: symbolic phase
     mju_sqrMatTDSparseSymbolic(
         ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL,
@@ -1562,13 +1607,11 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) {
         ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind,
         ctx->JT_rowsuper, ctx->D, d);
 
-    // add mass matrix: H = J'*D*J + C
+    // add mass matrix: H = J'*D*J + M
     mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv,
-                       ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind,
-                       ctx->buf_val, ctx->buf_ind);
+                       ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind);
 
-    // compute H' (upper triangle, required for symbolic Cholesky)
-    ctx->HT_colind = mjSTACKALLOC(d, ctx->nH, int);
+    // compute H' sparse structure (upper triangle, required for symbolic Cholesky)
     mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL,
                         ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind);
 
@@ -1578,16 +1621,16 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) {
                                      ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind,
                                      nv, d);
 
-    // allocate L_colind, L, Lcone
-    ctx->L_colind = mjSTACKALLOC(d, ctx->nL, int);
-    ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum);
-    if (ctx->is_elliptic) {
-      ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum);
-    }
-
-    // allocate LT (CSC representation of L)
-    ctx->LT_colind = mjSTACKALLOC(d, ctx->nL, int);
-    ctx->LT_map = mjSTACKALLOC(d, ctx->nL, int);
+    // nL is known: allocate blocks and carve L_colind, LT_colind, LT_map, L, Lcone
+    size_t nL_int = 2*ctx->nL + ctx->nL;                     // L_colind + LT_colind + LT_map
+    size_t nL_num = ctx->is_elliptic ? 2*ctx->nL : ctx->nL;  // L + Lcone
+    int* L_intblock    = mjSTACKALLOC(d, nL_int, int);
+    mjtNum* L_numblock = mjSTACKALLOC(d, nL_num, mjtNum);
+    ctx->L_colind      = L_intblock;
+    ctx->LT_colind     = L_intblock + ctx->nL;
+    ctx->LT_map        = L_intblock + 2*ctx->nL;
+    ctx->L             = L_numblock;
+    ctx->Lcone         = ctx->is_elliptic ? L_numblock + ctx->nL : NULL;
 
     // symbolic Cholesky: populate L_colind and LT structures
     mju_cholFactorSymbolic(ctx->L_colind, ctx->L_rownnz, ctx->L_rowadr,
@@ -1598,13 +1641,6 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) {
 
   // dense
   else {
-    // allocate L, Lcone
-    ctx->nL = nv*nv;
-    ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum);
-    if (ctx->is_elliptic) {
-      ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum);
-    }
-
     // compute H = M + J'*D*J
     mju_sqrMatTD_impl(ctx->L, ctx->J, ctx->D, nefc, nv, /*flg_upper=*/ 0);
     mju_addToSymSparse(ctx->L, ctx->M, ctx->nv,
@@ -1647,8 +1683,7 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute)
 
       // add mass matrix: H = J'*D*J + C
       mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv,
-                         ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind,
-                         ctx->buf_val, ctx->buf_ind);
+                         ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind);
     }
 
     // numeric sparse factorization: L = chol(H) using pre-computed sparsity pattern
@@ -1691,13 +1726,12 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute)
 // elliptic case: Hcone = H + cone_contributions
 static void HessianCone(mjData* d, mjPrimalContext* ctx) {
   int nv = ctx->nv, nefc = ctx->nefc;
+  mjtNum* LTJ = ctx->LTJ;
   mjtNum local[36];
 
   // start with Hcone = H
   mju_copy(ctx->Lcone, ctx->L, ctx->nL);
 
-  mjtNum* LTJ = ctx->LTJ;
-
   // add contributions
   for (int i=0; i < nefc; i++) {
     if (ctx->efc_state[i] == mjCNSTRSTATE_CONE) {
@@ -1818,7 +1852,6 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst
 static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, int flg_Newton) {
   int iter = 0;
   mjtNum alpha, beta;
-  mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL;
   mjPrimalContext ctx;
   mj_markStack(d);
 
@@ -1829,14 +1862,7 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i
   // local copies
   int nv   = ctx.nv;
   int nefc = ctx.nefc;
-
-  // allocate local storage
-  if (!flg_Newton) {
-    gradold     = mjSTACKALLOC(d, nv, mjtNum);
-    Mgradold    = mjSTACKALLOC(d, nv, mjtNum);
-    Mgraddif    = mjSTACKALLOC(d, nv, mjtNum);
-  }
-  int* oldstate = mjSTACKALLOC(d, nefc, int);
+  int* oldstate = ctx.oldstate;
 
   // compute Ma = M * qacc
   mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv,
@@ -1895,8 +1921,8 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i
 
     // save old
     if (!flg_Newton) {
-      mju_copy(gradold, ctx.grad, nv);
-      mju_copy(Mgradold, ctx.Mgrad, nv);
+      mju_copy(ctx.gradold, ctx.grad, nv);
+      mju_copy(ctx.Mgradold, ctx.Mgrad, nv);
     }
     mju_copyInt(oldstate, ctx.efc_state, nefc);
     mjtNum oldcost = ctx.cost;
@@ -1933,9 +1959,9 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i
       mju_scl(ctx.search, ctx.Mgrad, -1, nv);
     } else {
       // Polak-Ribiere
-      mju_sub(Mgraddif, ctx.Mgrad, Mgradold, nv);
-      beta = mju_dot(ctx.grad, Mgraddif, nv) /
-             mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv));
+      mju_sub(ctx.Mgraddif, ctx.Mgrad, ctx.Mgradold, nv);
+      beta = mju_dot(ctx.grad, ctx.Mgraddif, nv) /
+             mju_max(mjMINVAL, mju_dot(ctx.gradold, ctx.Mgradold, nv));
 
       // reset if negative
       if (beta < 0) {

src/engine/engine_support.c

@@ -66,7 +66,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
   "Eulerdamp",
   "AutoReset",
   "NativeCCD",
-  "Island"
+  "Island",
+  "MultiCCD"
 };
 
 
@@ -76,7 +77,6 @@ const char* mjENABLESTRING[mjNENABLE] = {
   "Energy",
   "Fwdinv",
   "InvDiscrete",
-  "MultiCCD",
   "Sleep"
 };
 
@@ -420,17 +420,11 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
 void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
              int* rownnz, int* rowadr, int* colind) {
   int nv = m->nv;
+
   // sparse
   if (rownnz && rowadr && colind) {
-    mj_markStack(d);
-    mjtNum* buf_val = mjSTACKALLOC(d, nv, mjtNum);
-    int* buf_ind = mjSTACKALLOC(d, nv, int);
-
-    mju_addToMatSparse(dst, rownnz, rowadr, colind, nv,
-      d->M, m->M_rownnz, m->M_rowadr, m->M_colind,
-      buf_val, buf_ind);
-
-    mj_freeStack(d);
+    mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, d->M,
+                       m->M_rownnz, m->M_rowadr, m->M_colind);
   }
 
   // dense

src/engine/engine_util_solve.c

@@ -144,10 +144,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
                          int* rownnz, const int* rowadr, int* colind,
                          mjData* d) {
   int rank = n;
-
-  mj_markStack(d);
-  mjtNum* buf = mjSTACKALLOC(d, n, mjtNum);
-  int* buf_ind = mjSTACKALLOC(d, n, int);
+  (void) d;
 
   // backpass over rows
   for (int r=n-1; r >= 0; r--) {
@@ -175,15 +172,13 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
 
       // mat(c,0:c) = mat(c,0:c) - mat(r,c) * mat(r,0:c)
       int nnz_c = mju_combineSparse(mat + rowadr[c], mat+rowadr[r], 1, -mat[adr+i],
-                                    rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r],
-                                    buf, buf_ind);
+                                    rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r]);
 
       // assign new nnz to row c
       rownnz[c] = nnz_c;
     }
   }
 
-  mj_freeStack(d);
   return rank;
 }
 

src/engine/engine_util_sparse.c

@@ -198,15 +198,14 @@ void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int
 }
 
 
-// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers
+// add sparse matrix M to sparse destination matrix
 void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr,
                         const mjtNum* M, const int* M_rownnz, const int* M_rowadr,
-                        const int* M_colind,
-                        mjtNum* buf_val, int* buf_ind) {
+                        const int* M_colind) {
   for (int i=0; i < nr; i++) {
     rownnz[i] = mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], 1, 1,
                                   rownnz[i], M_rownnz[i], colind + rowadr[i],
-                                  M_colind + M_rowadr[i], buf_val, buf_ind);
+                                  M_colind + M_rowadr[i]);
   }
 }
 
@@ -256,8 +255,8 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat,
     for (int k=diag-1; k >= 0; k--) {
       int j = ind[k];
       mjtNum val = row[k];
-      res[i] += val * vec[j]; // strict lower
-      res[j] += val * vec[i]; // strict upper
+      res[i] += val * vec[j];  // strict lower
+      res[j] += val * vec[i];  // strict upper
     }
   }
 }

src/engine/engine_util_sparse.h

@@ -62,11 +62,10 @@ MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec
 MJAPI void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc,
                                 const int* rownnz, const int* rowadr, const int* colind);
 
-// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers
+// add sparse matrix M to sparse destination matrix
 MJAPI void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr,
                               const mjtNum* M, const int* M_rownnz, const int* M_rowadr,
-                              const int* M_colind,
-                              mjtNum* buf_val, int* buf_ind);
+                              const int* M_colind);
 
 // add symmetric matrix (only lower triangle represented) to dense matrix
 MJAPI void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n,
@@ -294,8 +293,7 @@ void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum scl2, i
 // combine two sparse vectors: dst = a*dst + b*src, return nnz of result
 static inline
 int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
-                      int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
-                      mjtNum* buf, int* buf_ind) {
+                      int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) {
   // check for identical pattern
   if (dst_nnz == src_nnz) {
     if (mju_compare(dst_ind, src_ind, dst_nnz)) {
@@ -305,49 +303,54 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
     }
   }
 
-  // copy dst into buf
-  if (dst_nnz) {
-    memcpy(buf, dst, dst_nnz * sizeof(mjtNum));
-    memcpy(buf_ind, dst_ind, dst_nnz * sizeof(int));
-  }
+  // compute total nnz of result
+  int nnz = mju_combineSparseCount(dst_nnz, src_nnz, dst_ind, src_ind);
 
-  // prepare to merge buf and src into dst
-  int bi = 0, si = 0, nnz = 0;
-  int buf_nnz = dst_nnz;
+  // set up read/write pointers at end of arrays
+  int bi = dst_nnz - 1, si = src_nnz - 1, w = nnz - 1;
 
-  // merge vectors
-  while (bi < buf_nnz && si < src_nnz) {
-    int badr = buf_ind[bi];
+  // merge backwards
+  while (bi >= 0 && si >= 0) {
+    int badr = dst_ind[bi];
     int sadr = src_ind[si];
 
     if (badr == sadr) {
-      dst[nnz] = a*buf[bi++] + b*src[si++];
-      dst_ind[nnz++] = badr;
+      dst[w] = a*dst[bi] + b*src[si];
+      dst_ind[w] = badr;
+      bi--;
+      si--;
     }
 
-    // buf only
-    else if (badr < sadr) {
-      dst[nnz] = a*buf[bi++];
-      dst_ind[nnz++] = badr;
+    // dst only
+    else if (badr > sadr) {
+      dst[w] = a*dst[bi];
+      dst_ind[w] = badr;
+      bi--;
     }
 
     // src only
     else {
-      dst[nnz] = b*src[si++];
-      dst_ind[nnz++] = sadr;
+      dst[w] = b*src[si];
+      dst_ind[w] = sadr;
+      si--;
     }
+    w--;
   }
 
-  // the rest of src only
-  while (si < src_nnz) {
-    dst[nnz] = b*src[si];
-    dst_ind[nnz++] = src_ind[si++];
+  // remaining src elements
+  while (si >= 0) {
+    dst[w] = b*src[si];
+    dst_ind[w] = src_ind[si];
+    si--;
+    w--;
   }
 
-  // the rest of buf only
-  while (bi < buf_nnz) {
-    dst[nnz] = a*buf[bi];
-    dst_ind[nnz++] = buf_ind[bi++];
+  // remaining dst elements: already in place, scale by a
+  if (a != 1) {
+    while (bi >= 0) {
+      dst[bi] *= a;
+      bi--;
+    }
   }
 
   return nnz;

src/engine/engine_vis_interact.c

@@ -864,6 +864,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
         if (m->flex_interp[i]) {
           mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid);
           int order = m->flex_interp[i];
+          order = order < 0 ? -order : order;
           int npc = (order+1)*(order+1)*(order+1);
 
           // cell lookup: get local coords and node indices

src/engine/engine_vis_visualize.c

@@ -1452,6 +1452,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
     int cy = m->flex_cellnum[3*f+1];
     int cz = m->flex_cellnum[3*f+2];
     int order = m->flex_interp[f];
+    order = order < 0 ? -order : order;
     int NX = cx * order + 1;
     int NY = cy * order + 1;
     int NZ = cz * order + 1;
@@ -1459,11 +1460,18 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
     for (int i=0; i < NX; i++) {
       for (int j=0; j < NY; j++) {
         for (int k=0; k < NZ; k++) {
-          int offset  = 3*(i*NY*NZ + j*NZ + k);
+          int n0 = i*NY*NZ + j*NZ + k;
+
+          // skip if this node is pinned (no joints on its body)
+          if (m->body_jntnum[bodyid[n0]] == 0) {
+            continue;
+          }
+
+          int offset  = 3*n0;
           int offset1 = 3*((i+1)*NY*NZ + j*NZ + k);
           int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k);
           int offset3 = 3*(i*NY*NZ + j*NZ + (k+1));
-          if (i < NX-1) {
+          if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;
@@ -1472,7 +1480,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
             mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
             releaseGeom(&thisgeom, scn);
           }
-          if (j < NY-1) {
+          if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;
@@ -1481,7 +1489,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
             mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
             releaseGeom(&thisgeom, scn);
           }
-          if (k < NZ-1) {
+          if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) {
             mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
             if (!thisgeom) {
               return;

src/experimental/filament/filament/builtins.cc

@@ -65,7 +65,8 @@ class BuiltinBuilder : MeshData {
   virtual ~BuiltinBuilder() = default;
 
   template <typename T, typename... Args>
-  static MeshPtr Create(filament::Engine* engine, Args&&... args) {
+  static std::unique_ptr<Mesh> Create(filament::Engine* engine,
+                                      Args&&... args) {
     auto builder = new T(std::forward<Args>(args)...);
     MeshData* mesh_data = builder->PrepareMeshData();
     mesh_data->release_callback = +[](void* user_data) {
@@ -621,43 +622,43 @@ class DomeBuilder : public BuiltinBuilder {
   }
 };
 
-MeshPtr CreateLine(filament::Engine* engine) {
+std::unique_ptr<Mesh> CreateLine(filament::Engine* engine) {
   return BuiltinBuilder::Create<LineBuilder>(engine);
 }
 
-MeshPtr CreatePlane(filament::Engine* engine, int nquad) {
+std::unique_ptr<Mesh> CreatePlane(filament::Engine* engine, int nquad) {
   return BuiltinBuilder::Create<PlaneBuilder>(engine, nquad);
 }
 
-MeshPtr CreateTriangle(filament::Engine* engine) {
+std::unique_ptr<Mesh> CreateTriangle(filament::Engine* engine) {
   return BuiltinBuilder::Create<TriangleBuilder>(engine);
 }
 
-MeshPtr CreateBox(filament::Engine* engine, int nquad) {
+std::unique_ptr<Mesh> CreateBox(filament::Engine* engine, int nquad) {
   return BuiltinBuilder::Create<BoxBuilder>(engine, nquad);
 }
 
-MeshPtr CreateLineBox(filament::Engine* engine) {
+std::unique_ptr<Mesh> CreateLineBox(filament::Engine* engine) {
   return BuiltinBuilder::Create<LineBoxBuilder>(engine);
 }
 
-MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) {
+std::unique_ptr<Mesh> CreateSphere(filament::Engine* engine, int nstack, int nslice) {
   return BuiltinBuilder::Create<SphereBuilder>(engine, nstack, nslice);
 }
 
-MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) {
+std::unique_ptr<Mesh> CreateTube(filament::Engine* engine, int nstack, int nslice) {
   return BuiltinBuilder::Create<TubeBuilder>(engine, nstack, nslice);
 }
 
-MeshPtr CreateDisk(filament::Engine* engine, int nslice) {
+std::unique_ptr<Mesh> CreateDisk(filament::Engine* engine, int nslice) {
   return BuiltinBuilder::Create<DiskBuilder>(engine, nslice);
 }
 
-MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) {
+std::unique_ptr<Mesh> CreateDome(filament::Engine* engine, int nstack, int nslice) {
   return BuiltinBuilder::Create<DomeBuilder>(engine, nstack, nslice);
 }
 
-MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) {
+std::unique_ptr<Mesh> CreateCone(filament::Engine* engine, int nstack, int nslice) {
   return BuiltinBuilder::Create<ConeBuilder>(engine, nstack, nslice);
 }
 

src/experimental/filament/filament/builtins.h

@@ -15,22 +15,24 @@
 #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_
 #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_
 
+#include <memory>
+
 #include <filament/Engine.h>
 #include "experimental/filament/filament/mesh.h"
 
 // Generates buffers for built-in shapes.
 namespace mujoco {
 
-MeshPtr CreateLine(filament::Engine* engine);
-MeshPtr CreatePlane(filament::Engine* engine, int nquad);
-MeshPtr CreateTriangle(filament::Engine* engine);
-MeshPtr CreateBox(filament::Engine* engine, int nquad);
-MeshPtr CreateLineBox(filament::Engine* engine);
-MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice);
-MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice);
-MeshPtr CreateDisk(filament::Engine* engine, int nslice);
-MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice);
-MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice);
+std::unique_ptr<Mesh> CreateLine(filament::Engine* engine);
+std::unique_ptr<Mesh> CreatePlane(filament::Engine* engine, int nquad);
+std::unique_ptr<Mesh> CreateTriangle(filament::Engine* engine);
+std::unique_ptr<Mesh> CreateBox(filament::Engine* engine, int nquad);
+std::unique_ptr<Mesh> CreateLineBox(filament::Engine* engine);
+std::unique_ptr<Mesh> CreateSphere(filament::Engine* engine, int nstack, int nslice);
+std::unique_ptr<Mesh> CreateTube(filament::Engine* engine, int nstack, int nslice);
+std::unique_ptr<Mesh> CreateDisk(filament::Engine* engine, int nslice);
+std::unique_ptr<Mesh> CreateDome(filament::Engine* engine, int nstack, int nslice);
+std::unique_ptr<Mesh> CreateCone(filament::Engine* engine, int nstack, int nslice);
 
 }  // namespace mujoco
 

src/experimental/filament/filament/filament_context.cc

@@ -156,7 +156,6 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) {
       request.viewport = viewport;
       request.camera = last_camera_;
       request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain);
-      request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f;
       scene_view_->Render(renderer_, request);
       renderer_->endFrame();
     }
@@ -191,14 +190,17 @@ void FilamentContext::SetFrameBuffer(int framebuffer) {
 }
 
 void FilamentContext::PrepareRenderTargets(int width, int height) {
-  color_target_ = std::make_unique<RenderTarget>(
-      engine_, RenderTargetTextureType::kColor,
-      RenderTargetTextureType::kDepth);
+  RenderTargetConfig config;
+  DefaultRenderTargetConfig(&config);
+
+  config.color_format = mjPIXEL_FORMAT_RGB8;
+  config.depth_format = mjPIXEL_FORMAT_DEPTH32F;
+  color_target_ = std::make_unique<RenderTarget>(engine_, config);
   color_target_->Prepare(width, height);
 
-  depth_target_ = std::make_unique<RenderTarget>(
-      engine_, RenderTargetTextureType::kDepthColor,
-      RenderTargetTextureType::kDepth);
+  config.color_format = mjPIXEL_FORMAT_R32F;
+  config.depth_format = mjPIXEL_FORMAT_DEPTH32F;
+  depth_target_ = std::make_unique<RenderTarget>(engine_, config);
   depth_target_->Prepare(width, height);
 }
 
@@ -230,7 +232,6 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb,
       request.target = color_target_.get();
       request.camera = last_camera_;
       request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain);
-      request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f;
       scene_view_->Render(renderer_, request);
 
       const size_t num_bytes = viewport.width * viewport.height * 3;

src/experimental/filament/filament/imgui_bridge.cc

@@ -21,6 +21,8 @@
 #include <vector>
 
 #include <imgui.h>
+#include <math/mat3.h>
+#include <math/vec3.h>
 #include <math/vec4.h>
 #include <mujoco/mujoco.h>
 #include "experimental/filament/filament/material.h"
@@ -32,6 +34,9 @@
 
 namespace mujoco {
 
+using filament::math::float3;
+using filament::math::mat3f;
+
 ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view)
     : object_mgr_(object_mgr), scene_view_(scene_view) {}
 
@@ -235,11 +240,7 @@ void ImguiBridge::Update() {
       const int height = size.y * scale.y;
 
       auto& renderable = renderables_[renderable_index];
-      if (renderable->GetNumMeshes() == 0) {
-        renderable->AppendMesh(mesh, index_offset, command.ElemCount);
-      } else {
-        renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount);
-      }
+      renderable->SetMesh(mesh, index_offset, command.ElemCount);
 
       MaterialTextures textures;
       textures.color = textures_[command.GetTexID()].get();
@@ -259,6 +260,8 @@ void ImguiBridge::Update() {
         properties.scissor[3] = height;
       }
       renderable->UpdateMaterial(properties, textures);
+      renderable->SetTransform(
+          {float3{0, 0, 0}, mat3f(), float3(scale.x, scale.y, 1.0f)});
 
       index_offset += command.ElemCount;
       ++renderable_index;
@@ -268,8 +271,11 @@ void ImguiBridge::Update() {
 
 void ImguiBridge::PrepareRenderables(int count) {
   while (renderables_.size() < count) {
+    RenderableParams config;
+    DefaultRenderableParams(&config);
+    config.shading_model = ShadingModel::Ux;
     auto& r = renderables_.emplace_back(
-        std::make_unique<Renderable>(Renderable::Usage::Ux, object_mgr_));
+        std::make_unique<Renderable>(object_mgr_, config));
     r->SetCastShadows(false);
     r->SetReceiveShadows(false);
     r->SetBlendOrder(static_cast<std::uint16_t>(renderables_.size()));
@@ -281,10 +287,6 @@ void ImguiBridge::PrepareRenderables(int count) {
   }
 }
 
-float ImguiBridge::GetScale() const {
-  return ImGui::GetIO().DisplayFramebufferScale.x;
-}
-
 static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) {
   const ImVec2& display_size = ImGui::GetIO().DisplaySize;
   const float pos_x = display_size.x * ((x + 1) * 0.5f);

src/experimental/filament/filament/imgui_bridge.h

@@ -40,9 +40,6 @@ class ImguiBridge {
   // synced.
   void Update();
 
-  // Returns the current ImGui scale factor.
-  float GetScale() const;
-
   // Uploads texture to be used with ImGui's Image and ImageButton functions.
   uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width,
                         int height, int bpp);
@@ -62,7 +59,7 @@ class ImguiBridge {
   ObjectManager* object_mgr_ = nullptr;
   SceneView* scene_view_ = nullptr;
   std::vector<std::unique_ptr<Renderable>> renderables_;
-  std::vector<MeshPtr> meshes_;
+  std::vector<std::unique_ptr<Mesh>> meshes_;
   std::unordered_map<uintptr_t, std::unique_ptr<Texture>> textures_;
 };
 

src/experimental/filament/filament/math_util.h

@@ -46,15 +46,31 @@ inline filament::math::float4 ReadFloat4(const T* arr, int index = 0) {
 
 // Reads a mat3 from an array buffer in the model/scene.
 template <typename T>
-inline filament::math::mat3 ReadMat3(const T* arr, int index = 0) {
+inline filament::math::mat3f ReadMat3(const T* arr, int index = 0) {
   // clang-format off
   const T* ptr = arr + (9 * index);
-  return filament::math::mat3(ptr[0], ptr[3], ptr[6],
-                              ptr[1], ptr[4], ptr[7],
-                              ptr[2], ptr[5], ptr[8]);
+  return filament::math::mat3f(ptr[0], ptr[3], ptr[6],
+                               ptr[1], ptr[4], ptr[7],
+                               ptr[2], ptr[5], ptr[8]);
   // clang-format on
 }
 
+// A tuple of translation, rotation, and size.
+struct Trs {
+  filament::math::float3 translation{0.0f, 0.0f, 0.0f};
+  filament::math::mat3f rotation;
+  // Note: this is _slightly_ different than scale. For example, for capsules,
+  // the size determines the length of the tube and the radius of the domes,
+  // but the shape remains a capsule.
+  filament::math::float3 size{1.0f, 1.0f, 1.0f};
+
+  // Converts the TRS to a transform matrix.
+  filament::math::mat4f ToTransform() const {
+    return filament::math::mat4f(rotation, translation) *
+           filament::math::mat4f::scaling(size);
+  }
+};
+
 // Calculates a reflection matrix for a plane defined by its transform.
 filament::math::mat4 ToReflectionMatrix(const filament::math::mat4& xform);
 

src/experimental/filament/filament/mesh.h

@@ -185,8 +185,6 @@ class Mesh {
   int num_attributes_ = 0;
 };
 
-using MeshPtr = std::unique_ptr<Mesh>;
-
 }  // namespace mujoco
 
 #endif  // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_

src/experimental/filament/filament/model_objects.cc

@@ -630,20 +630,11 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) {
   height_fields_[id] = std::make_unique<Mesh>(engine_, data);
 }
 
-MeshPtr ModelObjects::CreateFlexMesh(const mjvScene* scene,
-                                     const mjvGeom& geom) {
+void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) {
   MeshData data;
   DefaultMeshData(&data);
   UpdateSkinFlexMeshData(&data, model_, scene, geom);
-  return std::make_unique<Mesh>(engine_, data);
-}
-
-MeshPtr ModelObjects::CreateSkinMesh(const mjvScene* scene,
-                                     const mjvGeom& geom) {
-  MeshData data;
-  DefaultMeshData(&data);
-  UpdateSkinFlexMeshData(&data, model_, scene, geom);
-  return std::make_unique<Mesh>(engine_, data);
+  dynamic_meshes_[geom.objid] = std::make_unique<Mesh>(engine_, data);
 }
 
 const Mesh* ModelObjects::GetMeshBuffer(int data_id) const {
@@ -672,6 +663,11 @@ const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const {
   return shapes_[shape].get();
 }
 
+const Mesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const {
+  auto it = dynamic_meshes_.find(geom_id);
+  return it != dynamic_meshes_.end() ? it->second.get() : nullptr;
+}
+
 const Texture* ModelObjects::GetTexture(int tex_id) const {
   auto it = textures_.find(tex_id);
   return it != textures_.end() ? it->second.get() : nullptr;

src/experimental/filament/filament/model_objects.h

@@ -15,6 +15,7 @@
 #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_
 #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_
 
+#include <array>
 #include <memory>
 #include <unordered_map>
 #include <vector>
@@ -55,6 +56,8 @@ class ModelObjects {
 
   void UploadHeightField(const mjModel* model, int id);
 
+  void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom);
+
   // Returns the filament engine used by the ModelObjects to create filament
   // objects.
   filament::Engine* GetEngine() const { return engine_; }
@@ -63,12 +66,10 @@ class ModelObjects {
   const Mesh* GetShapeBuffer(ShapeType shape) const;
   const Mesh* GetMeshBuffer(int data_id) const;
   const Mesh* GetHeightFieldBuffer(int hfield_id) const;
+  const Mesh* GetFlexSkinGeomMesh(int geom_id) const;
   const Texture* GetTexture(int tex_id) const;
   const Texture* GetTexture(int mat_id, int role) const;
 
-  MeshPtr CreateFlexMesh(const mjvScene* scene, const mjvGeom& geom);
-  MeshPtr CreateSkinMesh(const mjvScene* scene, const mjvGeom& geom);
-
   filament::Skybox* CreateSkybox();
   filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity);
 
@@ -86,10 +87,11 @@ class ModelObjects {
   filament::Engine* engine_ = nullptr;
   std::vector<filament::Skybox*> skyboxes_;
   std::vector<filament::IndirectLight*> indirect_lights_;
-  std::array<MeshPtr, kNumShapes> shapes_;
-  std::unordered_map<int, MeshPtr> meshes_;
-  std::unordered_map<int, MeshPtr> convex_hulls_;
-  std::unordered_map<int, MeshPtr> height_fields_;
+  std::array<std::unique_ptr<Mesh>, kNumShapes> shapes_;
+  std::unordered_map<int, std::unique_ptr<Mesh>> meshes_;
+  std::unordered_map<int, std::unique_ptr<Mesh>> convex_hulls_;
+  std::unordered_map<int, std::unique_ptr<Mesh>> height_fields_;
+  std::unordered_map<int, std::unique_ptr<Mesh>> dynamic_meshes_;
   std::unordered_map<int, std::unique_ptr<Texture>> textures_;
   float specular_multiplier_ = 0.2f;
   float shininess_multiplier_ = 0.1f;

src/experimental/filament/filament/render_target.cc

@@ -30,10 +30,14 @@
 
 namespace mujoco {
 
+void DefaultRenderTargetConfig(RenderTargetConfig* config) {
+  config->color_format = mjPIXEL_FORMAT_RGBA8;
+  config->depth_format = mjPIXEL_FORMAT_DEPTH32F;
+}
+
 RenderTarget::RenderTarget(filament::Engine* engine,
-                           RenderTargetTextureType color,
-                           RenderTargetTextureType depth)
-    : engine_(engine), color_type_(color), depth_type_(depth) {}
+                           const RenderTargetConfig& config)
+    : engine_(engine), config_(config) {}
 
 RenderTarget::~RenderTarget() noexcept {
   Destroy();
@@ -47,10 +51,29 @@ void RenderTarget::Prepare(int width, int height) {
   width_ = width;
   height_ = height;
 
-  color_texture_ =
-      std::make_unique<Texture>(engine_, color_type_, width, height);
-  depth_texture_ =
-      std::make_unique<Texture>(engine_, depth_type_, width, height);
+  TextureConfig color_config;
+  DefaultTextureConfig(&color_config);
+  Texture::InternalFlags color_flags;
+  color_config.width = width;
+  color_config.height = height;
+  color_config.target = mjTEXTURE_2D;
+  color_config.format = config_.color_format;
+  color_config.color_space = mjCOLORSPACE_LINEAR;
+  color_config.format = mjPIXEL_FORMAT_RGB8;
+  color_flags.color_attachment = true;
+  color_texture_ = std::make_unique<Texture>(engine_, color_config, color_flags);
+
+  TextureConfig depth_config;
+  DefaultTextureConfig(&depth_config);
+  Texture::InternalFlags depth_flags;
+  depth_config.width = width;
+  depth_config.height = height;
+  depth_config.target = mjTEXTURE_2D;
+  depth_config.format = config_.depth_format;
+  depth_config.color_space = mjCOLORSPACE_LINEAR;
+  depth_config.format = mjPIXEL_FORMAT_DEPTH32F;
+  depth_flags.depth_attachment = true;
+  depth_texture_ = std::make_unique<Texture>(engine_, depth_config, depth_flags);
 
   filament::RenderTarget::Builder builder;
   builder.texture(filament::RenderTarget::AttachmentPoint::COLOR,
@@ -65,19 +88,19 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes,
   filament::backend::PixelDataFormat format;
   filament::backend::PixelDataType type;
   size_t expected_num_bytes = 0;
-  switch (color_type_) {
-    case RenderTargetTextureType::kColor:
+  switch (config_.color_format) {
+    case mjPIXEL_FORMAT_RGB8:
       format = filament::backend::PixelDataFormat::RGB;
       type = filament::backend::PixelDataType::UBYTE;
       expected_num_bytes = width_ * height_ * 3;
       break;
-    case RenderTargetTextureType::kDepthColor:
+    case mjPIXEL_FORMAT_R32F:
       format = filament::backend::PixelDataFormat::R;
       type = filament::backend::PixelDataType::FLOAT;
       expected_num_bytes = width_ * height_ * sizeof(float);
       break;
     default:
-      mju_error("Unsupported pixel format: %d", color_type_);
+      mju_error("Unsupported pixel format: %d", config_.color_format);
       return;
   }
   if (num_bytes != expected_num_bytes) {

src/experimental/filament/filament/render_target.h

@@ -25,13 +25,21 @@
 
 namespace mujoco {
 
+// Defines the basic properties of a render target.
+struct RenderTargetConfig {
+  mjtPixelFormat color_format;
+  mjtPixelFormat depth_format;
+};
+
+// Initializes the RenderTargetConfig to default values.
+void DefaultRenderTargetConfig(RenderTargetConfig* config);
+
 // Manages a filament RenderTarget and the textures which are bound to it.
 class RenderTarget {
  public:
   // Defines the types of textures to create for the color and depth
   // attachments.
-  RenderTarget(filament::Engine* engine, RenderTargetTextureType color,
-               RenderTargetTextureType depth);
+  RenderTarget(filament::Engine* engine, const RenderTargetConfig& config);
   ~RenderTarget() noexcept;
 
   RenderTarget(const RenderTarget&) = delete;
@@ -58,11 +66,10 @@ class RenderTarget {
   void Destroy();
 
   filament::Engine* engine_ = nullptr;
+  RenderTargetConfig config_;
   filament::RenderTarget* render_target_ = nullptr;
   std::unique_ptr<Texture> color_texture_ = nullptr;
   std::unique_ptr<Texture> depth_texture_ = nullptr;
-  RenderTargetTextureType color_type_;
-  RenderTargetTextureType depth_type_;
   int width_ = 0;
   int height_ = 0;
 };

src/experimental/filament/filament/renderable.cc

@@ -16,100 +16,105 @@
 
 #include <algorithm>
 #include <cstdint>
-#include <utility>
+#include <span>
 
 #include <filament/Engine.h>
 #include <filament/Material.h>
 #include <filament/RenderableManager.h>
 #include <filament/Scene.h>
+#include <filament/TransformManager.h>
+#include <math/mat4.h>
 #include <utils/EntityManager.h>
 #include <mujoco/mujoco.h>
 #include "experimental/filament/filament/draw_mode.h"
 #include "experimental/filament/filament/material.h"
+#include "experimental/filament/filament/math_util.h"
 #include "experimental/filament/filament/mesh.h"
 #include "experimental/filament/filament/object_manager.h"
 
 namespace mujoco {
 
-Renderable::Renderable(Usage usage, ObjectManager* object_mgr)
-    : usage_(usage), object_mgr_(object_mgr) {}
+using filament::math::mat4f;
+
+void DefaultRenderableParams(RenderableParams* params) {
+  params->shading_model = ShadingModel::SceneObject;
+}
+
+Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params)
+    : object_mgr_(object_mgr), params_(params) {}
 
 Renderable::~Renderable() noexcept {
-  while (!entities_.empty()) {
-    RemoveLastEntity();
+  filament::Engine* engine = GetEngine();
+  utils::EntityManager& em = utils::EntityManager::get();
+
+  for (Part& part : parts_) {
+    if (assigned_scene_) {
+      assigned_scene_->remove(part.entity);
+    }
+    engine->destroy(part.entity);
+    em.destroy(part.entity);
   }
   for (int i = 0; i < kNumDrawModes; ++i) {
     if (instances_[i] != nullptr) {
-      GetEngine()->destroy(instances_[i]);
+      engine->destroy(instances_[i]);
       instances_[i] = nullptr;
     }
   }
 }
 
-void Renderable::RemoveLastEntity() {
-  if (entities_.empty()) {
-    return;
+void Renderable::SetMesh(const Mesh* mesh, int elem_offset, int elem_count) {
+  if (mesh == nullptr) {
+    mju_error("Cannot set mesh to nullptr.");
   }
-
-  utils::EntityManager& em = utils::EntityManager::get();
-  utils::Entity entity = entities_.back();
-
-  if (assigned_scene_) {
-    assigned_scene_->remove(entity);
-  }
-
-  GetEngine()->destroy(entity);
-  em.destroy(entity);
-  entities_.pop_back();
-  meshes_.pop_back();
-}
-
-void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset,
-                            int elem_count) {
-  MeshInfo& mesh_info = SetMesh(index, mesh, nullptr, elem_offset, elem_count);
-  UpdateEntity(index, mesh_info);
-}
-
-void Renderable::UpdateMesh(int index, MeshPtr mesh, int elem_offset,
-                            int elem_count) {
-  MeshInfo& mesh_info =
-      SetMesh(index, mesh.get(), std::move(mesh), elem_offset, elem_count);
-  UpdateEntity(index, mesh_info);
-}
-
-void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) {
-  MeshInfo& mesh_info = SetMesh(-1, mesh, nullptr, elem_offset, elem_count);
-  AppendEntity(mesh_info);
-}
-
-void Renderable::AppendMesh(MeshPtr mesh, int elem_offset, int elem_count) {
-  MeshInfo& mesh_info =
-      SetMesh(-1, mesh.get(), std::move(mesh), elem_offset, elem_count);
-  AppendEntity(mesh_info);
-}
-
-void Renderable::AppendEntity(const MeshInfo& mesh_info) {
-  const Mesh* mesh = mesh_info.mesh;
   filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer();
   if (vertex_buffer == nullptr) {
     mju_error("Invalid (null) vertex buffer.");
   }
-
   filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer();
   if (index_buffer == nullptr) {
     mju_error("Invalid (null) index buffer.");
   }
 
-  utils::Entity entity = utils::EntityManager::get().create();
-  if (entity.isNull()) {
+  if (elem_count == 0) {
+    elem_count = index_buffer->getIndexCount() - elem_offset;
+  }
+
+  if (parts_.empty()) {
+    Part& part = parts_.emplace_back();
+    part.mesh = mesh;
+    part.elem_offset = elem_offset;
+    part.elem_count = elem_count;
+    InitPartEntity(part);
+  } else if (parts_.size() == 1) {
+    Part& part = parts_[0];
+    part.mesh = mesh;
+    part.elem_offset = elem_offset;
+    part.elem_count = elem_count;
+
+    filament::RenderableManager& rm = GetEngine()->getRenderableManager();
+    rm.setGeometryAt(rm.getInstance(part.entity), 0,
+                     part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer,
+                     part.elem_offset, part.elem_count);
+
+  } else {
+    mju_error("Cannot set mesh for renderable with multiple parts.");
+  }
+}
+
+void Renderable::InitPartEntity(Part& part) {
+  part.entity = utils::EntityManager::get().create();
+  if (part.entity.isNull()) {
     mju_error("Failed to create entity.");
   }
 
+  filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer();
+  filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer();
+
   filament::RenderableManager::Builder builder(1);
-  builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer,
-                   mesh_info.elem_offset, mesh_info.elem_count);
-  if (mesh->HasBounds()) {
-    builder.boundingBox(mesh->GetBounds());
+  builder.geometry(0, part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer,
+                   part.elem_offset, part.elem_count);
+  if (part.mesh->HasBounds()) {
+    builder.boundingBox(part.mesh->GetBounds());
   } else {
     builder.culling(false);
   }
@@ -123,58 +128,50 @@ void Renderable::AppendEntity(const MeshInfo& mesh_info) {
   builder.blendOrder(0, blend_order_);
   builder.screenSpaceContactShadows(true);
 
-  builder.build(*GetEngine(), entity);
+  builder.build(*GetEngine(), part.entity);
   if (assigned_scene_) {
-    assigned_scene_->addEntity(entity);
+    assigned_scene_->addEntity(part.entity);
   }
-  entities_.push_back(entity);
 }
 
-void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) {
-  if (index < 0 || index >= entities_.size()) {
-    mju_error("Invalid index %d for renderable.", index);
-  }
-  utils::Entity entity = entities_[index];
-
-  const Mesh* mesh = mesh_info.mesh;
-  filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer();
-  if (vertex_buffer == nullptr) {
-    mju_error("Invalid (null) vertex buffer.");
+void Renderable::SetTransform(const Trs& trs) {
+  if (parts_.empty()) {
+    transform_ = trs.ToTransform();
+    return;
   }
 
-  filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer();
-  if (index_buffer == nullptr) {
-    mju_error("Invalid (null) index buffer.");
+  filament::TransformManager& tm = GetEngine()->getTransformManager();
+  if (get_transform_fn_) {
+    for (int i = 0; i < parts_.size(); ++i) {
+      const mat4f& transform = get_transform_fn_(i, trs);
+      tm.setTransform(tm.getInstance(parts_[i].entity), transform);
+    }
+  } else {
+    for (Part& part : parts_) {
+      tm.setTransform(tm.getInstance(part.entity), trs.ToTransform());
+    }
   }
-
-  filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-  rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(),
-                   vertex_buffer, index_buffer, mesh_info.elem_offset,
-                   mesh_info.elem_count);
+  transform_ = tm.getTransform(tm.getInstance(parts_[0].entity));
 }
 
-Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh,
-                                          MeshPtr owned_mesh, int elem_offset,
-                                          int elem_count) {
-  if (index == -1) {
-    index = meshes_.size();
-    meshes_.emplace_back();
-  }
-  if (index < 0 || index >= static_cast<int>(meshes_.size())) {
-    mju_error("Invalid index %d for renderable.", index);
+const mat4f& Renderable::GetTransform() const {
+  return transform_;
+}
+
+void Renderable::SetMeshes(std::span<const Mesh*> meshes,
+                           GetTransformFn get_transform_fn) {
+  if (!parts_.empty()) {
+    mju_error("Cannot set meshes for renderable with multiple parts.");
   }
 
-  MeshInfo* mesh_info = &meshes_[index];
-  mesh_info->owned_mesh = std::move(owned_mesh);
-  mesh_info->mesh = mesh;
-  mesh_info->elem_offset = elem_offset;
-  mesh_info->elem_count = elem_count;
-  if (mesh_info->elem_count == 0) {
-    const int total =
-        mesh_info->mesh->GetFilamentIndexBuffer()->getIndexCount();
-    mesh_info->elem_count = total - mesh_info->elem_offset;
+  get_transform_fn_ = get_transform_fn;
+  for (int i = 0; i < meshes.size(); ++i) {
+    Part& part = parts_.emplace_back();
+    part.mesh = meshes[i];
+    part.elem_offset = 0;
+    part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount();
+    InitPartEntity(part);
   }
-  return *mesh_info;
 }
 
 void Renderable::AddToScene(filament::Scene* scene) {
@@ -185,8 +182,8 @@ void Renderable::AddToScene(filament::Scene* scene) {
     // Entities are already added to the scene.
     return;
   }
-  for (utils::Entity& entity : entities_) {
-    scene->addEntity(entity);
+  for (Part& part : parts_) {
+    scene->addEntity(part.entity);
   }
   assigned_scene_ = scene;
 }
@@ -195,26 +192,27 @@ void Renderable::RemoveFromScene(filament::Scene* scene) {
   if (assigned_scene_ != scene) {
     mju_error("Attempting to remove renderable from wrong scene.");
   }
-  for (utils::Entity& entity : entities_) {
-    scene->remove(entity);
+  for (Part& part : parts_) {
+    scene->remove(part.entity);
   }
   assigned_scene_ = nullptr;
 }
 
 void Renderable::UpdateMaterial(const MaterialParams& params,
                                 const MaterialTextures& textures) {
-  params_ = params;
-  textures_ = textures;
+  material_params_ = params;
+  material_textures_ = textures;
 
   AssignMaterial(DrawMode::Color, GetColorMaterialType());
-  if (usage_ == Usage::SceneObject) {
+  if (params_.shading_model == ShadingModel::SceneObject) {
     AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth);
     AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation);
   }
 
   for (int i = 0; i < kNumDrawModes; ++i) {
     if (instances_[i]) {
-      UpdateMaterialInstance(instances_[i], params_, textures_, object_mgr_);
+      UpdateMaterialInstance(instances_[i], material_params_,
+                             material_textures_, object_mgr_);
     }
   }
   SetDrawMode(draw_mode_);
@@ -240,24 +238,24 @@ void Renderable::AssignMaterial(DrawMode mode,
 }
 
 const MaterialParams& Renderable::GetMaterialParams() const {
-  return params_;
+  return material_params_;
 }
 
 const MaterialTextures& Renderable::GetMaterialTextures() const {
-  return textures_;
+  return material_textures_;
 }
 
 void Renderable::SetDrawMode(DrawMode mode) {
   // Only SceneObjects support non-color draw modes.
-  if (usage_ != Usage::SceneObject) {
+  if (params_.shading_model != ShadingModel::SceneObject) {
     mode = DrawMode::Color;
   }
 
   filament::MaterialInstance* instance = instances_[static_cast<int>(mode)];
   if (instance) {
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      filament::RenderableManager::Instance ri = rm.getInstance(entity);
+    for (Part& part : parts_) {
+      filament::RenderableManager::Instance ri = rm.getInstance(part.entity);
       rm.setMaterialInstanceAt(ri, 0, instance);
     }
   }
@@ -270,8 +268,8 @@ std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) {
     layer_mask_ = mask;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      rm.setLayerMask(rm.getInstance(entity), 0xff, layer_mask_);
+    for (Part& part : parts_) {
+      rm.setLayerMask(rm.getInstance(part.entity), 0xff, layer_mask_);
     }
   }
   return prev;
@@ -283,8 +281,8 @@ std::uint8_t Renderable::SetPriority(std::uint8_t priority) {
     priority_ = priority;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      rm.setPriority(rm.getInstance(entity), priority_);
+    for (Part& part : parts_) {
+      rm.setPriority(rm.getInstance(part.entity), priority_);
     }
   }
   return prev;
@@ -296,8 +294,8 @@ std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) {
     blend_order_ = blend_order;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      rm.setBlendOrderAt(rm.getInstance(entity), 0, blend_order_);
+    for (Part& part : parts_) {
+      rm.setBlendOrderAt(rm.getInstance(part.entity), 0, blend_order_);
     }
   }
   return prev;
@@ -308,8 +306,8 @@ void Renderable::SetCastShadows(bool cast_shadows) {
     cast_shadows_ = cast_shadows;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      rm.setCastShadows(rm.getInstance(entity), cast_shadows_);
+  for (Part& part : parts_) {
+        rm.setCastShadows(rm.getInstance(part.entity), cast_shadows_);
     }
   }
 }
@@ -319,8 +317,8 @@ void Renderable::SetReceiveShadows(bool receive_shadows) {
     receive_shadows_ = receive_shadows;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (utils::Entity& entity : entities_) {
-      rm.setReceiveShadows(rm.getInstance(entity), receive_shadows_);
+    for (Part& part : parts_) {
+      rm.setReceiveShadows(rm.getInstance(part.entity), receive_shadows_);
     }
   }
 }
@@ -333,36 +331,33 @@ void Renderable::SetWireframe(bool wireframe) {
     wireframe_ = wireframe;
 
     filament::RenderableManager& rm = GetEngine()->getRenderableManager();
-    for (int i = 0; i < entities_.size(); ++i) {
-      utils::Entity& entity = entities_[i];
-      const Mesh* mesh = meshes_[i].mesh;
-      filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer();
-      filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer();
-      rm.setGeometryAt(rm.getInstance(entity), 0,
-                       wireframe_ ? kWireframeType : mesh->GetPrimitiveType(),
-                       vertex_buffer, index_buffer, meshes_[i].elem_offset,
-                       meshes_[i].elem_count);
+    for (Part& part : parts_) {
+      filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer();
+      filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer();
+      rm.setGeometryAt(rm.getInstance(part.entity), 0,
+                       wireframe_ ? kWireframeType : part.mesh->GetPrimitiveType(),
+                       vertex_buffer, index_buffer, part.elem_offset,
+                       part.elem_count);
     }
   }
 }
 
-
 ObjectManager::MaterialType Renderable::GetColorMaterialType() const {
-  if (usage_ == Usage::DecorLines) {
+  if (params_.shading_model == ShadingModel::DecorLines) {
     return ObjectManager::kUnlitLine;
-  } else if (usage_ == Usage::Decor) {
-    return ObjectManager::kUnlitSegmentation;
-  } else if (usage_ == Usage::Ux) {
+  } else if (params_.shading_model == ShadingModel::Decor) {
+    return ObjectManager::kUnlitDecor;
+  } else if (params_.shading_model == ShadingModel::Ux) {
     return ObjectManager::kUnlitUi;
-  } else if (textures_.orm) {
+  } else if (material_textures_.orm) {
     return ObjectManager::kPbrPacked;
-  } else if (textures_.metallic) {
+  } else if (material_textures_.metallic) {
     return ObjectManager::kPbr;
-  } else if (textures_.roughness) {
+  } else if (material_textures_.roughness) {
     return ObjectManager::kPbr;
-  } else if (params_.metallic >= 0) {
+  } else if (material_params_.metallic >= 0) {
     return ObjectManager::kPbr;
-  } else if (params_.roughness >= 0) {
+  } else if (material_params_.roughness >= 0) {
     return ObjectManager::kPbr;
   }
 
@@ -371,42 +366,42 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const {
   // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if
   // it has them.
   bool has_texcoords = false;
-  if (!meshes_.empty()) {
-    const auto attribs = meshes_[0].mesh->GetVertexAttributes();
+  if (!parts_.empty()) {
+    const auto attribs = parts_[0].mesh->GetVertexAttributes();
     auto it = std::find(attribs.begin(), attribs.end(),
                         filament::VertexAttribute::UV0);
     has_texcoords = (it != attribs.end());
   }
 
-  if (textures_.color == nullptr) {
-    if (params_.color.a < 1.0f) {
+  if (material_textures_.color == nullptr) {
+    if (material_params_.color.a < 1.0f) {
       return ObjectManager::kPhongColorFade;
-    } else if (params_.reflective) {
+    } else if (material_params_.reflective) {
       return ObjectManager::kPhongColorReflect;
     } else {
       return ObjectManager::kPhongColor;
     }
-  } else if (textures_.color->GetFilamentTexture()->getTarget() ==
+  } else if (material_textures_.color->GetFilamentTexture()->getTarget() ==
               filament::Texture::Sampler::SAMPLER_CUBEMAP) {
-    if (params_.color.a < 1.0f) {
+    if (material_params_.color.a < 1.0f) {
       return ObjectManager::kPhongCubeFade;
-    } else if (params_.reflective) {
+    } else if (material_params_.reflective) {
       return ObjectManager::kPhongCubeReflect;
     } else {
       return ObjectManager::kPhongCube;
     }
   } else if (has_texcoords) {
-    if (params_.color.a < 1.0f) {
+    if (material_params_.color.a < 1.0f) {
       return ObjectManager::kPhong2dUvFade;
-    } else if (params_.reflective) {
+    } else if (material_params_.reflective) {
       return ObjectManager::kPhong2dUvReflect;
     } else {
       return ObjectManager::kPhong2dUv;
     }
   } else {
-    if (params_.color.a < 1.0f) {
+    if (material_params_.color.a < 1.0f) {
       return ObjectManager::kPhong2dFade;
-    } else if (params_.reflective) {
+    } else if (material_params_.reflective) {
       return ObjectManager::kPhong2dReflect;
     } else {
       return ObjectManager::kPhong2d;

src/experimental/filament/filament/renderable.h

@@ -16,62 +16,81 @@
 #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_
 
 #include <cstdint>
+#include <functional>
+#include <span>
 #include <vector>
 
 #include <filament/Engine.h>
 #include <filament/Scene.h>
+#include <math/mat4.h>
 #include <utils/Entity.h>
 #include "experimental/filament/filament/draw_mode.h"
 #include "experimental/filament/filament/material.h"
+#include "experimental/filament/filament/math_util.h"
 #include "experimental/filament/filament/mesh.h"
 #include "experimental/filament/filament/object_manager.h"
 
 namespace mujoco {
 
-// A collection of meshes and a material that, together, define an object that
-// can be rendered in a scene.
+// The shading model (material) for a Renderable.
+enum class ShadingModel {
+  SceneObject,
+  Decor,
+  DecorLines,
+  Ux,
+};
+
+// Configuration parameters for a Renderable.
+struct RenderableParams {
+  ShadingModel shading_model;
+};
+
+void DefaultRenderableParams(RenderableParams* params);
+
+// A Renderable is effectively two things: a mesh and a material.
 //
-// Meshes can be added to the Renderable either by unique_ptr or raw pointer.
-// This determines whether or not the Renderable takes ownership of the mesh.
+// The mesh describes the surface geometry of the object and the material
+// describes how that surface interacts with light (i.e. the color of each point
+// on the surface).
 //
-// Internally, the Renderable creates a filament::Entity for each mesh and
-// assigns the same material instance to all of them.
+// Defining the mesh is easy; just call SetMesh.
+//
+// Defining a Material happens in two stages. First, the user specifies the
+// ShadingModel to use for Rendering. This describes the overall intent of
+// how the Renderable will appear (e.g. lit, unlit, wireframe, etc.). Next,
+// the user specifies the MaterialParams and MaterialTextures to use with the
+// ShadingModel. Its these properties that ultimately define the actual material
+// of the Renderable.
 class Renderable {
  public:
-  // How the material is to be used for rendering.
-  enum class Usage {
-    SceneObject,
-    Decor,
-    DecorLines,
-    Ux,
-  };
-
   // Default filament values for priority and layer mask.
   static constexpr std::uint8_t kDefaultPriority = 4;
   static constexpr std::uint8_t kDefaultLayerMask = 0x01;
 
-  Renderable(Usage usage, ObjectManager* object_mgr);
+  Renderable(ObjectManager* object_mgr, const RenderableParams& params);
   ~Renderable() noexcept;
 
   Renderable(const Renderable&) = delete;
   Renderable& operator=(const Renderable&) = delete;
 
-  // Appends a mesh to the renderable. The elem_offset and elem_count parameters
-  // can be used to specify a submesh to append. If elem_count is 0, assumes
-  // the entire mesh should be appended.
-  void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0);
-  void AppendMesh(MeshPtr mesh, int elem_offset = 0, int elem_count = 0);
+  // Sets the mesh of the renderable. The elem_offset and elem_count parameters
+  // can be used to specify a submesh within the mesh. If elem_count is 0,
+  // assumes the entire mesh should be appended.
+  void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0);
 
-  // Replaces the mesh at the index with a new mesh. The elem_offset and
-  // elem_count parameters can be used to specify a submesh to append. If
-  // elem_count is 0, assumes the entire mesh should be appended.
-  void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0,
-              int elem_count = 0);
-  void UpdateMesh(int index, MeshPtr mesh, int elem_offset = 0,
-                  int elem_count = 0);
+  // Sets the transform of the renderable.
+  void SetTransform(const Trs& trs);
 
-  // Returns the number of meshes that define the renderable.
-  int GetNumMeshes() const { return meshes_.size(); }
+  // Returns the current transform of the renderable.
+  const filament::math::mat4f& GetTransform() const;
+
+  // Sets multiple meshes for a renderable. Users can optionally provide a
+  // function that will be used to compute the transform for each (sub)mesh
+  // relative to the transform of the renderable itself. This allows users to
+  // construct compound (but rigid) objects from multiple meshes.
+  using GetTransformFn = std::function<filament::math::mat4f(int, const Trs&)>;
+  void SetMeshes(std::span<const Mesh*> meshes,
+                 GetTransformFn get_transform = nullptr);
 
   // Sets the layer mask for the managed filament Entities. Layer masks can be
   // used to show/hide the renderable in different views. Returns the previous
@@ -103,7 +122,8 @@ class Renderable {
   // Removes the renderable from the given filament Scene.
   void RemoveFromScene(filament::Scene* scene);
 
-  // Sets the material instance for all managed entities.
+  // Further defines the material of the renderable. Only applies to renderables
+  // with a SceneObject shading model.
   void SetDrawMode(DrawMode mode);
 
   // Updates the parameters for the material.
@@ -119,45 +139,30 @@ class Renderable {
   // Returns the filament Engine managing the renderables.
   filament::Engine* GetEngine();
 
-  // Returns the underlying filament::entity for the given mesh.
-  utils::Entity operator[](int index) { return entities_[index]; }
-
  private:
-  struct MeshInfo {
-    MeshPtr owned_mesh;
+  struct Part {
+    utils::Entity entity;
     const Mesh* mesh = nullptr;
     int elem_offset = 0;
     int elem_count = 0;
   };
 
-  // Sets the mesh information for the mesh at the given index. If index is -1,
-  // a new mesh will be appended to the renderable.
-  MeshInfo& SetMesh(int index, const Mesh* mesh, MeshPtr owned_mesh,
-                    int elem_offset, int elem_count);
-
-  // Appends a new filament::Entity to the renderable, configured to use the
-  // given mesh.
-  void AppendEntity(const MeshInfo& mesh_info);
-
-  // Updates the filament::Entity at the given index to use the given mesh.
-  void UpdateEntity(int index, const MeshInfo& mesh_info);
-
-  // Removes the last filament::Entity from the renderable.
-  void RemoveLastEntity();
+  void InitPartEntity(Part& part);
 
   void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type);
 
   ObjectManager::MaterialType GetColorMaterialType() const;
 
-  Usage usage_;
   ObjectManager* object_mgr_;
+  RenderableParams params_;
   filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr};
-  MaterialParams params_;
-  MaterialTextures textures_;
+  MaterialParams material_params_;
+  MaterialTextures material_textures_;
   DrawMode draw_mode_ = DrawMode::Color;
   filament::Scene* assigned_scene_ = nullptr;
-  std::vector<utils::Entity> entities_;
-  std::vector<MeshInfo> meshes_;
+  std::vector<Part> parts_;
+  filament::math::mat4f transform_;
+  GetTransformFn get_transform_fn_;
   std::uint8_t priority_ = kDefaultPriority;
   std::uint8_t layer_mask_ = kDefaultLayerMask;
   std::uint16_t blend_order_ = 0;

src/experimental/filament/filament/scene_bridge.cc

@@ -381,6 +381,10 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) {
       }
     }
 
+    if (geom->type == mjGEOM_FLEX || geom->type == mjGEOM_SKIN) {
+      model_objects_->CreateSkinFlexMesh(scene, *geom);
+    }
+
     std::unique_ptr<Renderable> renderable = CreateGeomRenderable(
         *geom, scene, object_mgr_, model_objects_.get(), headpos);
 

src/experimental/filament/filament/scene_geom_util.cc

@@ -18,6 +18,7 @@
 #include <cstdint>
 #include <memory>
 #include <numbers>
+#include <vector>
 
 #include <filament/Material.h>
 #include <filament/RenderableManager.h>
@@ -44,30 +45,12 @@ namespace mujoco {
 using filament::math::float2;
 using filament::math::float3;
 using filament::math::float4;
-using filament::math::mat4;
+using filament::math::mat4f;
 
 // An arbitrary scale factor for arrows.
 static constexpr float kArrowScale = 1.f / 6.f;
 static constexpr float kArrowHeadSize = 1.75f;
 
-// Some built-in geometries are actually composed of multiple simple shapes. A
-// capsule, for example, is a open-ended tube with two dome ends. We use these
-// constants to help identify which entity (by index) represents which part of
-// the overall shape.
-static constexpr int kCapsuleTopDome = 1;
-static constexpr int kCapsuleBottomDome = 2;
-static constexpr int kCylinderTopDisk = 1;
-static constexpr int kCylinderBottomDisk = 2;
-static constexpr int kArrow0Cone = 1;
-static constexpr int kArrow0ConeDisk = 2;
-static constexpr int kArrow0BottomDisk = 3;
-static constexpr int kArrow1Cone = 1;
-static constexpr int kArrow1BottomDisk = 2;
-static constexpr int kArrow2TopCone = 1;
-static constexpr int kArrow2BottomCone = 2;
-static constexpr int kArrow2TopConeDisk = 3;
-static constexpr int kArrow2BottomConeDisk = 4;
-
 // Returns the tile size for infinite plane texture alignment.
 // This is duplicated from engine_vis_visualize.c (re-center infinite plane)
 // to ensure UV scaling matches the re-centering increments.
@@ -87,106 +70,265 @@ static bool IsBehind(const float* headpos, const float* pos, const float* mat) {
           0.0f);
 }
 
-static void AddMesh(Renderable& renderable, ModelObjects* model_objs,
-                    int data_id) {
+static const Mesh* GetMesh(ModelObjects* model_objs, int data_id) {
   const Mesh* mesh = model_objs->GetMeshBuffer(data_id);
   if (mesh == nullptr) {
     mju_error("Unknown mesh %d", data_id);
   }
-  renderable.AppendMesh(mesh);
+  return mesh;
 }
 
-static void AddGeom(Renderable& renderable, ModelObjects* model_objs,
-                    const mjvScene* scene, const mjvGeom& geom) {
-  if (geom.type == mjGEOM_FLEX) {
-    renderable.AppendMesh(model_objs->CreateFlexMesh(scene, geom));
-  } else if (geom.type == mjGEOM_SKIN) {
-    renderable.AppendMesh(model_objs->CreateSkinMesh(scene, geom));
-  }
+static const Mesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) {
+  return model_objs->GetFlexSkinGeomMesh(objid);
 }
 
-static void AddHeightField(Renderable& renderable, ModelObjects* model_objs,
-                           int hfield_id) {
+static const Mesh* GetHeightField(ModelObjects* model_objs, int hfield_id) {
   const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id);
   if (mesh == nullptr) {
     mju_error("Unknown height field %d", hfield_id);
   }
-  renderable.AppendMesh(mesh);
+  return mesh;
 }
 
-static void AddShape(Renderable& renderable, ModelObjects* model_objs,
-                     ModelObjects::ShapeType shape_type) {
+static const Mesh* GetShape(ModelObjects* model_objs,
+                            ModelObjects::ShapeType shape_type) {
   const Mesh* mesh = model_objs->GetShapeBuffer(shape_type);
   if (mesh == nullptr) {
     mju_error("Unknown shape %d", shape_type);
   }
-  renderable.AppendMesh(mesh);
+  return mesh;
 }
 
 static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom,
                               const mjvScene* scene,
                               ModelObjects* model_objects) {
+  std::vector<const Mesh*> meshes;
+  Renderable::GetTransformFn get_transforms;
+
+  Trs trs = {
+      .translation = ReadFloat3(geom.pos),
+      .rotation = ReadMat3(geom.mat),
+      .size = ReadFloat3(geom.size),
+  };
+
   switch ((mjtGeom)geom.type) {
     case mjGEOM_MESH:
-      AddMesh(renderable, model_objects, geom.dataid);
+      meshes.push_back(GetMesh(model_objects, geom.dataid));
+      // Ignore size for meshes.
+      trs.size = float3{1.0f, 1.0f, 1.0f};
       break;
     case mjGEOM_HFIELD:
-      AddHeightField(renderable, model_objects, geom.dataid);
+      meshes.push_back(GetHeightField(model_objects, geom.dataid));
+      // Ignore size for height fields.
+      trs.size = float3{1.0f, 1.0f, 1.0f};
       break;
-    case mjGEOM_PLANE:
-      AddShape(renderable, model_objects, ModelObjects::kPlane);
+    case mjGEOM_PLANE: {
+      meshes.push_back(GetShape(model_objects, ModelObjects::kPlane));
+      const bool is_infinite = !(trs.size.x > 0 && trs.size.y > 0);
+      if (is_infinite) {
+        // Infinite planes are scaled to match the tile size used by
+        // re-centering in engine_vis_visualize.c.
+        const float plane_scale = static_cast<float>(mjMAXPLANEGRID) / 2.0f;
+        trs.size.x = plane_scale;
+        trs.size.y = plane_scale;
+      }
+      // Planes only define an xy size, so set the z-dimension to 1.0f.
+      trs.size.z = 1.0f;
       break;
+    }
     case mjGEOM_SPHERE:
-      AddShape(renderable, model_objects, ModelObjects::kSphere);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kSphere));
       break;
     case mjGEOM_ELLIPSOID:
-      AddShape(renderable, model_objects, ModelObjects::kSphere);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kSphere));
       break;
     case mjGEOM_BOX:
-      AddShape(renderable, model_objects, ModelObjects::kBox);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kBox));
       break;
-    case mjGEOM_CAPSULE:
-      AddShape(renderable, model_objects, ModelObjects::kTube);
-      AddShape(renderable, model_objects, ModelObjects::kDome);
-      AddShape(renderable, model_objects, ModelObjects::kDome);
+    case mjGEOM_CAPSULE: {
+      // Capsules are a tube with two domes at the ends.
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTube));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDome));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDome));
+
+      get_transforms = [](int index, const Trs& trs) {
+        // We apply an inverse scale to the domes to counteract the capsule's
+        // overall scale so that the domes remain spherical in shape.
+        const float xz_size = 0.5f * (trs.size.x + trs.size.y);
+        if (index == 0) {
+          return trs.ToTransform();
+        } else if (index == 1) {
+          // Move the first dome to the top of the capsule.
+          mat4f top = mat4f(trs.rotation, trs.translation);
+          top *= mat4f::translation(float3{0, 0, trs.size.z});
+          top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size});
+          return top;
+        } else if (index == 2) {
+          // Move the second dome to the bottom of the capsule and rotate it 180
+          // degrees so that it's facing the right way.
+          mat4f bottom = mat4f(trs.rotation, trs.translation);
+          bottom *= mat4f::translation(float3{0, 0, -trs.size.z});
+          bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size});
+          return bottom;
+        } else {
+          mju_error("Invalid index for capsule geom: %d (expected [0,2])", index);
+          return trs.ToTransform();
+        }
+      };
       break;
-    case mjGEOM_CYLINDER:
-      AddShape(renderable, model_objects, ModelObjects::kTube);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
+    }
+    case mjGEOM_CYLINDER: {
+      // Cylinders are a tube with two disks at the ends.
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTube));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+
+      get_transforms = [](int index, const Trs& trs) {
+        if (index == 0) {
+          return trs.ToTransform();
+        } else if (index == 1) {
+          // Move the first disk to the top of the cylinder.
+          mat4f top = mat4f(trs.rotation, trs.translation);
+          top *= mat4f::translation(float3{0, 0, trs.size.z});
+          top *= mat4f::scaling(trs.size);
+          return top;
+        } else if (index == 2) {
+          // Move the second disk to the bottom of the cylinder. Rotate the disk
+          // 180 degrees so that the normals point outwards.
+          mat4f bottom = mat4f(trs.rotation, trs.translation);
+          bottom *= mat4f::translation(float3{0, 0, -trs.size.z});
+          bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          bottom *= mat4f::scaling(trs.size);
+          return bottom;
+        } else {
+          mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index);
+          return trs.ToTransform();
+        }
+      };
       break;
-    case mjGEOM_ARROW:
-      AddShape(renderable, model_objects, ModelObjects::kTube);
-      AddShape(renderable, model_objects, ModelObjects::kCone);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
+    }
+    case mjGEOM_ARROW: {
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTube));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kCone));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+
+      get_transforms = [](int index, const Trs& trs) {
+        mat4f base = mat4f(trs.rotation, trs.translation);
+        base *= mat4f::scaling(float3{1, 1, kArrowScale});
+        base *= mat4f::translation(float3{0, 0, trs.size.z});
+        if (index == 0) {
+          return base * mat4f::scaling(trs.size);
+        } else if (index == 1) {
+          mat4f top = base;
+          top *= mat4f::translation(float3{0, 0, trs.size.z});
+          top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
+          return top * mat4f::scaling(trs.size);
+        } else if (index == 2) {
+          mat4f top_disk = base;
+          top_disk *= mat4f::translation(float3{0, 0, trs.size.z});
+          top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
+          return top_disk * mat4f::scaling(trs.size);
+        } else if (index == 3) {
+          mat4f bottom = base;
+          bottom *= mat4f::translation(float3{0, 0, -trs.size.z});
+          bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          return bottom * mat4f::scaling(trs.size);
+        } else {
+          mju_error("Invalid index for arrow geom: %d (expected [0,3])", index);
+          return trs.ToTransform();
+        }
+      };
       break;
-    case mjGEOM_ARROW1:
-      AddShape(renderable, model_objects, ModelObjects::kTube);
-      AddShape(renderable, model_objects, ModelObjects::kCone);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
+    }
+    case mjGEOM_ARROW1: {
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTube));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kCone));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+
+      get_transforms = [](int index, const Trs& trs) {
+      mat4f base = mat4f(trs.rotation, trs.translation);
+      base *= mat4f::scaling(float3{1, 1, kArrowScale});
+      base *= mat4f::translation(float3{0, 0, trs.size.z});
+        if (index == 0) {
+          return base * mat4f::scaling(trs.size);
+        } else if (index == 1) {
+          mat4f top = base;
+          top *= mat4f::translation(float3{0, 0, trs.size.z});
+          return top * mat4f::scaling(trs.size);
+        } else if (index == 2) {
+          mat4f bottom = base;
+          bottom *= mat4f::translation(float3{0, 0, -trs.size.z});
+          bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          return bottom * mat4f::scaling(trs.size);
+        } else {
+          mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index);
+          return trs.ToTransform();
+        }
+      };
       break;
-    case mjGEOM_ARROW2:
-      AddShape(renderable, model_objects, ModelObjects::kTube);
-      AddShape(renderable, model_objects, ModelObjects::kCone);
-      AddShape(renderable, model_objects, ModelObjects::kCone);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
-      AddShape(renderable, model_objects, ModelObjects::kDisk);
+    }
+    case mjGEOM_ARROW2: {
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTube));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kCone));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kCone));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+      meshes.push_back(GetShape(model_objects, ModelObjects::kDisk));
+
+      get_transforms = [](int index, const Trs& trs) {
+        mat4f base = mat4f(trs.rotation, trs.translation);
+        base *= mat4f::scaling(float3{1, 1, kArrowScale});
+        base *= mat4f::translation(float3{0, 0, trs.size.z});
+        if (index == 0) {
+          return base * mat4f::scaling(trs.size);
+        } else if (index == 1) {
+          mat4f top = base;
+          top *= mat4f::translation(float3{0, 0, trs.size.z});
+          top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
+          return top * mat4f::scaling(trs.size);
+        } else if (index == 2) {
+          mat4f bottom = base;
+          bottom *= mat4f::translation(float3{0, 0, -trs.size.z});
+          bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
+          return bottom * mat4f::scaling(trs.size);
+        } else if (index == 3) {
+          mat4f top_disk = base;
+          top_disk *= mat4f::translation(float3{0, 0, trs.size.z});
+          top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0});
+          top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
+          return top_disk * mat4f::scaling(trs.size);
+        } else if (index == 4) {
+          mat4f bottom_disk = base;
+          bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z});
+          return bottom_disk * mat4f::scaling(trs.size);
+        } else {
+          mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index);
+          return trs.ToTransform();
+        }
+      };
       break;
+    }
     case mjGEOM_LINE:
-      AddShape(renderable, model_objects, ModelObjects::kLine);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kLine));
       break;
     case mjGEOM_LINEBOX:
-      AddShape(renderable, model_objects, ModelObjects::kLineBox);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox));
       break;
     case mjGEOM_TRIANGLE:
-      AddShape(renderable, model_objects, ModelObjects::kTriangle);
+      meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle));
       break;
     case mjGEOM_FLEX:
-      AddGeom(renderable, model_objects, scene, geom);
+      meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid));
+      // Flexes are defined in global space.
+      trs = Trs();
       break;
     case mjGEOM_SKIN:
-      AddGeom(renderable, model_objects, scene, geom);
+      meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid));
+      // Skins are defined in global space.
+      trs = Trs();
       break;
     case mjGEOM_NONE:
     case mjGEOM_LABEL:
@@ -197,124 +339,9 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom,
       mju_warning("Unsupported geom type: %d", geom.type);
       break;
   }
-}
 
-static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) {
-  // Flex and skin geometries are in global space.
-  if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) {
-    return;
-  }
-
-  mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos));
-  renderable.SetLayerMask(geom.category);
-
-  float3 size = ReadFloat3(geom.size);
-  filament::TransformManager& tm =
-      renderable.GetEngine()->getTransformManager();
-  for (int j = 0; j < renderable.GetNumMeshes(); ++j) {
-    const utils::Entity& entity = renderable[j];
-
-    // Update object transform.
-    mat4 entity_transform = transform;
-
-    // Some built-in drawables are composed of multiple entities. For example,
-    // capsules are a combination of a open tube and two dome end caps.
-
-    if (geom.type == mjGEOM_CYLINDER) {
-      // Cylinders are a tube with two disks at the ends. The "bottom" disk is
-      // rotated so that the normals point outwards.
-      if (j == kCylinderTopDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-      } else if (j == kCylinderBottomDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-      }
-    } else if (geom.type == mjGEOM_CAPSULE) {
-      // Capsules are a tube with two domes at the ends. We apply an inverse
-      // scale to the domes to "counteract" the capsule's overall scale so that
-      // the domes remain spherical in shape.
-      const float xz_size = 0.5f * (size.x + size.y);
-      if (j == kCapsuleTopDome) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-        entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z});
-      } else if (j == kCapsuleBottomDome) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-        entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z});
-      }
-    } else if (geom.type == mjGEOM_ARROW) {
-      // An arrow is a tube with a cone at the end and a disk cap at the other
-      // end. Because the cone head's base is larger than the tube, an extra
-      // disk is added to the base of the cone. This disk is rotated such that
-      // its normal points outwards.
-      entity_transform *= mat4::scaling(float3{1, 1, kArrowScale});
-      entity_transform *= mat4::translation(float3{0, 0, size.z});
-      if (j == kArrow0Cone) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      } else if (j == kArrow0ConeDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      } else if (j == kArrow0BottomDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-      }
-    } else if (geom.type == mjGEOM_ARROW1) {
-      // An arrow1 is a tube with a cone at the end and a disk cap at the other
-      // end.
-      entity_transform *= mat4::scaling(float3{1, 1, kArrowScale});
-      entity_transform *= mat4::translation(float3{0, 0, size.z});
-      if (j == kArrow1Cone) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-      } else if (j == kArrow1BottomDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-      }
-    } else if (geom.type == mjGEOM_ARROW2) {
-      // An arrow2 is a tube with a cone at both ends.  Like the standard arrow,
-      // an extra disk is added to the base of each cone.
-      entity_transform *= mat4::scaling(float3{1, 1, kArrowScale});
-      entity_transform *= mat4::translation(float3{0, 0, size.z});
-      if (j == kArrow2TopCone) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      } else if (j == kArrow2BottomCone) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      } else if (j == kArrow2TopConeDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, size.z});
-        entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      } else if (j == kArrow2BottomConeDisk) {
-        entity_transform *= mat4::translation(float3{0, 0, -size.z});
-        entity_transform *=
-            mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f});
-      }
-    }
-    if (geom.type == mjGEOM_PLANE) {
-      const bool is_infinite = !(size.x > 0 && size.y > 0);
-      if (is_infinite) {
-        // Infinite planes are scaled to match the tile size used by
-        // re-centering in engine_vis_visualize.c.
-        const float plane_scale = static_cast<float>(mjMAXPLANEGRID) / 2.0f;
-        entity_transform *=
-            mat4::scaling(float3{plane_scale, plane_scale, 1.0f});
-      } else {
-        // Regular planes are scaled by geom.size.
-        entity_transform *= mat4::scaling(float3{size.x, size.y, 1.0f});
-      }
-    } else if (geom.type != mjGEOM_MESH && geom.type != mjGEOM_HFIELD) {
-      entity_transform *= mat4::scaling(size);
-    }
-    tm.setTransform(tm.getInstance(entity), entity_transform);
-  }
+  renderable.SetMeshes(meshes, get_transforms);
+  renderable.SetTransform(trs);
 }
 
 static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom,
@@ -338,10 +365,12 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom,
           enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f;
     }
   }
-  renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]);
+  renderable.SetLayerMask(geom.category);
   if (geom.category == mjCAT_DECOR) {
     renderable.SetCastShadows(false);
     renderable.SetReceiveShadows(false);
+  } else {
+    renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]);
   }
 
   MaterialTextures textures;
@@ -463,19 +492,19 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom,
 std::unique_ptr<Renderable> CreateGeomRenderable(
     const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr,
     ModelObjects* model_objs, const float headpos[3]) {
-  Renderable::Usage usage = Renderable::Usage::SceneObject;
+  ShadingModel shading_model = ShadingModel::SceneObject;
   if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) {
-    usage = Renderable::Usage::DecorLines;
+    shading_model = ShadingModel::DecorLines;
   } else if (geom.category == mjCAT_DECOR) {
-    usage = Renderable::Usage::Decor;
+    shading_model = ShadingModel::Decor;
   }
 
-  auto renderable = std::make_unique<Renderable>(usage, object_mgr);
+  RenderableParams config;
+  DefaultRenderableParams(&config);
+  config.shading_model = shading_model;
+  auto renderable = std::make_unique<Renderable>(object_mgr, config);
 
-  // The order of these calls is important. e.g. We need to create the filament
-  // renderable entities before we can set their transform.
   PrepareGeomMeshes(*renderable, geom, scene, model_objs);
-  SetGeomTransform(*renderable, geom);
   UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos);
 
   return renderable;

src/experimental/filament/filament/scene_view.cc

@@ -277,13 +277,11 @@ void SceneView::Render(filament::Renderer* renderer,
 
   // Render reflection passes.
   if (request.draw_mode == DrawMode::Color) {
-    filament::TransformManager& tm = engine_->getTransformManager();
     for (size_t i = 0; i < reflectives_.size(); ++i) {
       Renderable* renderable = reflectives_[i];
 
       // We assume the 0th entity is the reflective entity.
-      const utils::Entity entity = (*renderable)[0];
-      const mat4 transform(tm.getTransform(tm.getInstance(entity)));
+      mat4 transform(renderable->GetTransform());
       SetupReflectionCamera(transform, camera_, reflect_camera_);
 
       // Hide reflective surface from its own reflection pass.
@@ -305,8 +303,7 @@ void SceneView::Render(filament::Renderer* renderer,
 
   if (request.enable_ux) {
     ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f,
-                              viewport.width / request.gui_scale,
-                              viewport.height / request.gui_scale, 0.0f, 0.0f,
+                              viewport.width, viewport.height, 0.0f, 0.0f,
                               1.0f);
     ux_view_->setRenderTarget(render_target);
     renderer->render(ux_view_);
@@ -325,9 +322,12 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) {
   // Ensure we have the same number of render targets as we do reflective
   // renderables.
   while (reflect_targets_.size() < reflectives_.size()) {
-    reflect_targets_.push_back(std::make_unique<RenderTarget>(
-        engine_, RenderTargetTextureType::kReflectionColor,
-        RenderTargetTextureType::kDepth));
+    RenderTargetConfig config;
+    DefaultRenderTargetConfig(&config);
+
+    config.color_format = mjPIXEL_FORMAT_RGBA8;
+    config.depth_format = mjPIXEL_FORMAT_DEPTH32F;
+    reflect_targets_.push_back(std::make_unique<RenderTarget>(engine_, config));
   }
 
   // Prepare a render target for the reflective renderable.

src/experimental/filament/filament/scene_view.h

@@ -69,8 +69,6 @@ class SceneView {
     RenderTarget* target = nullptr;
     // Whether or not to render the UX as a separate pass.
     bool enable_ux = false;
-    // The scale factor to use for UX rendering.
-    float gui_scale = 1.0f;
   };
 
   // Renders the scene.

src/experimental/filament/filament/texture.cc

@@ -99,6 +99,10 @@ static filament::Texture::InternalFormat GetTextureInternalFormat(
         return filament::Texture::InternalFormat::RGB8;
       case mjPIXEL_FORMAT_RGBA8:
         return filament::Texture::InternalFormat::RGBA8;
+      case mjPIXEL_FORMAT_R32F:
+        return filament::Texture::InternalFormat::R32F;
+      case mjPIXEL_FORMAT_DEPTH32F:
+        return filament::Texture::InternalFormat::DEPTH32F;
       default:
         mju_error("Unsupported format: %d", (int)config.format);
         return filament::Texture::InternalFormat::UNUSED;
@@ -114,7 +118,8 @@ void DefaultTextureConfig(TextureConfig* config) {
   std::memset(config, 0, sizeof(TextureConfig));
 }
 
-Texture::Texture(filament::Engine* engine, const TextureConfig& config)
+Texture::Texture(filament::Engine* engine, const TextureConfig& config,
+                 InternalFlags flags)
     : engine_(engine), config_(config) {
   if (IsCompressed(config_)) {
     // We defer creation of compressed textures until Upload() is called. In
@@ -139,45 +144,19 @@ Texture::Texture(filament::Engine* engine, const TextureConfig& config)
     builder.sampler(filament::Texture::Sampler::SAMPLER_2D);
   }
 
-  if (config_.color_space != mjCOLORSPACE_SRGB) {
-    builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE |
-                  filament::Texture::Usage::SAMPLEABLE |
-                  filament::Texture::Usage::UPLOADABLE);
+  filament::Texture::Usage usage = filament::Texture::Usage::DEFAULT;
+  if (flags.color_attachment) {
+    usage |= filament::Texture::Usage::COLOR_ATTACHMENT;
+    usage |= filament::Texture::Usage::BLIT_SRC;
+  } else if (flags.depth_attachment) {
+    usage |= filament::Texture::Usage::DEPTH_ATTACHMENT;
+    usage |= filament::Texture::Usage::BLIT_SRC;
+  } else if (config_.color_space != mjCOLORSPACE_SRGB) {
+    usage |= filament::Texture::Usage::GEN_MIPMAPPABLE;
   }
-  texture_ = builder.build(*engine_);
-}
+  builder.usage(usage);
 
-Texture::Texture(filament::Engine* engine, RenderTargetTextureType type,
-                 int width, int height) : engine_(engine) {
-  filament::Texture::Builder builder;
-  builder.width(width);
-  builder.height(height);
-  switch (type) {
-    case RenderTargetTextureType::kColor:
-      builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT |
-                    filament::Texture::Usage::BLIT_SRC);
-      builder.format(filament::Texture::InternalFormat::RGB8);
-      break;
-    case RenderTargetTextureType::kDepth:
-      builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT |
-                    filament::Texture::Usage::SAMPLEABLE);
-      builder.format(filament::Texture::InternalFormat::DEPTH32F);
-      break;
-    case RenderTargetTextureType::kDepthColor:
-      builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT |
-                    filament::Texture::Usage::BLIT_SRC);
-      builder.format(filament::Texture::InternalFormat::R32F);
-      break;
-    case RenderTargetTextureType::kReflectionColor:
-      builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT |
-                    filament::Texture::Usage::BLIT_SRC |
-                    filament::Texture::Usage::SAMPLEABLE);
-      builder.format(filament::Texture::InternalFormat::RGBA8);
-      break;
-    default:
-      mju_error("Unknown type: %d", static_cast<int>(type));
-  }
-  texture_ = builder.build(*engine);
+  texture_ = builder.build(*engine_);
 }
 
 Texture::~Texture() {

src/experimental/filament/filament/texture.h

@@ -25,29 +25,13 @@
 // Functions for creating filament textures.
 namespace mujoco {
 
-// The types of textures we can create. For internal use only.
-enum class TextureTarget {
-  // A standard 2D image with a width and a height.
-  kNormal2d,
-  // A 2D texture split up into the 6 faces of a cube.
-  kCube,
-};
-
-// The different types of textures we can create for a render target.
-// For internal use only.
-enum class RenderTargetTextureType {
-  kColor,
-  kDepth,
-  kDepthColor,
-  kReflectionColor,
-};
-
 // Pixel formats for textures.
 typedef enum mjtPixelFormat_ {
   mjPIXEL_FORMAT_UNKNOWN = 0,
   mjPIXEL_FORMAT_R8,
   mjPIXEL_FORMAT_RGB8,
   mjPIXEL_FORMAT_RGBA8,
+  mjPIXEL_FORMAT_R32F,
   mjPIXEL_FORMAT_DEPTH32F,
   mjPIXEL_FORMAT_KTX,
 } mjtPixelFormat;
@@ -98,12 +82,16 @@ void DefaultTextureConfig(TextureConfig* config);
 // Wrapper around a filament::Texture.
 class Texture {
  public:
-  // Creates a texture with the given data.
-  Texture(filament::Engine* engine, const TextureConfig& config);
+  // Flags for internal use.
+  struct InternalFlags {
+    InternalFlags() : color_attachment(false), depth_attachment(false) {}
+    bool color_attachment;
+    bool depth_attachment;
+  };
 
-  // Creates a texture for use with a render target, for internal use.
-  Texture(filament::Engine* engine, RenderTargetTextureType type, int width,
-          int height);
+  // Creates a texture with the given data.
+  Texture(filament::Engine* engine, const TextureConfig& config,
+          InternalFlags flags = InternalFlags());
 
   ~Texture();
 

src/experimental/platform/helpers.cc

@@ -14,6 +14,7 @@
 
 #include "experimental/platform/helpers.h"
 
+#include <algorithm>
 #include <cstddef>
 #include <cstdint>
 #include <cstdio>
@@ -23,6 +24,7 @@
 #include <ios>
 #include <iterator>
 #include <string>
+#include <vector>
 
 #include "webp/encode.h"
 #include "webp/types.h"
@@ -77,8 +79,13 @@ std::string ResolveFile(const std::string& filename,
       return resolved;
     }
 
+    std::vector<std::filesystem::path> entries;
     for (const auto& it : std::filesystem::recursive_directory_iterator(path)) {
-      resolved = CheckPathForFile(it.path(), filename);
+      entries.push_back(it.path());
+    }
+    std::sort(entries.begin(), entries.end());
+    for (const auto& entry : entries) {
+      resolved = CheckPathForFile(entry, filename);
       if (!resolved.empty()) {
         return resolved;
       }

src/experimental/platform/ux/imgui_widgets.cc

@@ -321,7 +321,7 @@ bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) {
   float f = *value;
   const bool res = ImGui::SliderFloat(name, &f, min, max);
   if (res) {
-    *value = f;
+    *value = mju_clip(f, min, max);
   }
   return res;
 }

src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc

@@ -649,7 +649,6 @@ class ModelWriter {
     };
 
     const std::vector<std::pair<pxr::TfToken, int>> enable_flags = {
-        {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD},
         {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV},
         {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY},
         {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE},
@@ -677,7 +676,8 @@ class ModelWriter {
         {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP},
         {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET},
         {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD},
-        {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}};
+        {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND},
+        {MjcPhysicsTokens->mjcFlagMulticcd, mjDSBL_MULTICCD}};
     for (const auto &[token, flag] : disable_flags) {
       create_flag_attr(token, flag, false);
     }

src/ui/ui_main.c

@@ -849,8 +849,9 @@ static void setslider(mjuiItem* it, mjUI* ui,
   rx = mju_round(rx * it->slider.divisions) / mjMAX(1, it->slider.divisions);
   rx = mjMAX(0, mjMIN(1, rx));
 
-  // compute value
-  mjtNum val = (mjtNum)(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx);
+  // compute value, clamp to range
+  mjtNum val = mju_clip(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx,
+                        it->slider.range[0], it->slider.range[1]);
 
   // set slider position
   if (it->type == mjITEM_SLIDERINT) {

src/user/user_flexcomp.cc

@@ -13,12 +13,14 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <array>
 #include <climits>
 #include <cmath>
 #include <cstddef>
 #include <cstdio>
 #include <cstring>
 #include <iostream>
+#include <queue>
 #include <sstream>
 #include <stdexcept>
 #include <string>
@@ -98,6 +100,156 @@ mjCFlexcomp::mjCFlexcomp(void) {
 }
 
 
+// identify empty cells and pin nodes exclusively in empty cells
+void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points,
+                                 int npnt, const double minmax[6],
+                                 int nx, int ny, int nz) {
+  int cx = flex->spec.cellcount[0];
+  int cy = flex->spec.cellcount[1];
+  int cz = flex->spec.cellcount[2];
+  int ncells = cx * cy * cz;
+  int order = flex->spec.order;
+
+  // determine which cells contain mesh elements (not just vertices)
+  // for each element, compute its AABB and mark all overlapping cells
+  std::vector<bool> has_element(ncells, false);
+
+  double dx = minmax[3] - minmax[0];
+  double dy = minmax[4] - minmax[1];
+  double dz = minmax[5] - minmax[2];
+
+  // vertices per element: dim+1 (edges=2, triangles=3, tets=4)
+  int nvpe = flex->spec.dim + 1;
+
+  if (nvpe > 0 && !element.empty()) {
+    int nelem = element.size() / nvpe;
+    for (int e = 0; e < nelem; e++) {
+      // compute element AABB
+      double elo[3] = {1e30, 1e30, 1e30};
+      double ehi[3] = {-1e30, -1e30, -1e30};
+      for (int v = 0; v < nvpe; v++) {
+        int vid = element[nvpe * e + v];
+        for (int j = 0; j < 3; j++) {
+          elo[j] = std::min(elo[j], points[3 * vid + j]);
+          ehi[j] = std::max(ehi[j], points[3 * vid + j]);
+        }
+      }
+
+      // map element AABB to cell range
+      auto cellIdx = [](double coord, double lo, double d, int nc) {
+        if (d <= 0) return 0;
+        int c = (int)((coord - lo) / d * nc);
+        return std::max(0, std::min(nc - 1, c));
+      };
+
+      int ci0 = cellIdx(elo[0], minmax[0], dx, cx);
+      int ci1 = cellIdx(ehi[0], minmax[0], dx, cx);
+      int cj0 = cellIdx(elo[1], minmax[1], dy, cy);
+      int cj1 = cellIdx(ehi[1], minmax[1], dy, cy);
+      int ck0 = cellIdx(elo[2], minmax[2], dz, cz);
+      int ck1 = cellIdx(ehi[2], minmax[2], dz, cz);
+
+      // mark all overlapping cells as containing elements
+      for (int ci = ci0; ci <= ci1; ci++) {
+        for (int cj = cj0; cj <= cj1; cj++) {
+          for (int ck = ck0; ck <= ck1; ck++) {
+            has_element[ci * cy * cz + cj * cz + ck] = true;
+          }
+        }
+      }
+    }
+  }
+
+  // default: all cells non-empty (only exterior cells will be empty)
+  flex->cell_empty.assign(ncells, false);
+
+  // for dim=2 (surface mesh): check watertightness and flood-fill
+  if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) {
+    // flood-fill from grid boundary to find exterior cells
+    // cells reachable from the boundary through non-element cells
+    // are outside the mesh volume; cells NOT reachable are interior
+    std::vector<bool> visited(ncells, false);
+    std::queue<std::array<int, 3>> bfs;
+
+    // seed BFS from boundary cells that have no elements
+    for (int ci = 0; ci < cx; ci++) {
+      for (int cj = 0; cj < cy; cj++) {
+        for (int ck = 0; ck < cz; ck++) {
+          if (ci == 0 || ci == cx - 1 ||
+              cj == 0 || cj == cy - 1 ||
+              ck == 0 || ck == cz - 1) {
+            int idx = ci * cy * cz + cj * cz + ck;
+            if (!has_element[idx] && !visited[idx]) {
+              visited[idx] = true;
+              flex->cell_empty[idx] = true;
+              bfs.push({ci, cj, ck});
+            }
+          }
+        }
+      }
+    }
+
+    // BFS: spread through non-element cells
+    const int dirs[6][3] = {
+        {-1, 0, 0}, {1, 0, 0},  {0, -1, 0},
+        {0, 1, 0},  {0, 0, -1}, {0, 0, 1}};
+    while (!bfs.empty()) {
+      auto [ci, cj, ck] = bfs.front();
+      bfs.pop();
+      for (auto& d : dirs) {
+        int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2];
+        if (ni < 0 || ni >= cx ||
+            nj < 0 || nj >= cy ||
+            nk < 0 || nk >= cz) {
+          continue;
+        }
+        int nidx = ni * cy * cz + nj * cz + nk;
+        if (!visited[nidx] && !has_element[nidx]) {
+          visited[nidx] = true;
+          flex->cell_empty[nidx] = true;
+          bfs.push({ni, nj, nk});
+        }
+      }
+    }
+  } else {
+    // dim!=2 (e.g., tet mesh): cells without element overlap are empty
+    for (int c = 0; c < ncells; c++) {
+      flex->cell_empty[c] = !has_element[c];
+    }
+  }
+
+  // pin nodes that belong exclusively to empty cells
+  for (int gi = 0; gi < nx; gi++) {
+    for (int gj = 0; gj < ny; gj++) {
+      for (int gk = 0; gk < nz; gk++) {
+        // find all cells that reference this node
+        bool all_empty = true;
+        int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order);
+        int ci_max = std::min(cx - 1, gi / order);
+        int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order);
+        int cj_max = std::min(cy - 1, gj / order);
+        int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order);
+        int ck_max = std::min(cz - 1, gk / order);
+
+        for (int ci = ci_min; ci <= ci_max && all_empty; ci++) {
+          for (int cj = cj_min; cj <= cj_max && all_empty; cj++) {
+            for (int ck = ck_min; ck <= ck_max && all_empty; ck++) {
+              if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) {
+                all_empty = false;
+              }
+            }
+          }
+        }
+
+        if (all_empty) {
+          int idx = gi * ny * nz + gj * nz + gk;
+          pinned[idx] = true;
+        }
+      }
+    }
+  }
+}
+
 
 // make flexcomp object
 bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) {
@@ -548,6 +700,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
         }
       }
 
+      // add two orthogonal sliders (x and y only)
+      else if (doftype == mjFCOMPDOF_2D) {
+        for (int j=0; j < 2; j++) {
+          mjsJoint* jnt = mjs_addJoint(pb, 0);
+          jnt->type = mjJNT_SLIDE;
+          mjuu_setvec(jnt->pos, 0, 0, 0);
+          mjuu_setvec(jnt->axis, 0, 0, 0);
+          jnt->axis[j] = 1;
+        }
+      }
+
       // construct body name, add to vertbody
       char txt[100];
       mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
@@ -588,15 +751,30 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
     int nz = flex->spec.cellcount[2] * flex->spec.order + 1;
     int nnode = nx * ny * nz;
 
+    // mark empty cells and pin nodes exclusively in empty cells
+    MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz);
+
+    // if MarkEmptyCells pinned any nodes, force centered=false
+    // so that pf->node (local positions) is saved to the model
+    if (centered) {
+      for (int i = 0; i < nnode; i++) {
+        if (pinned[i]) {
+          centered = false;
+          break;
+        }
+      }
+    }
+
     std::vector<double> node(3 * nnode, 0);
     int idx = 0;
 
     // Simpson's rule weights for quadratic mass distribution
     double massP2[3] = {1. / 6., 2. / 3., 1. / 6.};
 
-    // compute per-node mass for trilinear:
-    //   mass / nnode (uniform), or use Simpson for quadratic
-    double node_mass_uniform = mass / nnode;
+
+
+    // collect created bodies for mass normalization
+    std::vector<mjsBody*> node_bodies;
 
     for (int gi = 0; gi < nx; gi++) {
       for (int gj = 0; gj < ny; gj++) {
@@ -629,7 +807,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
 
           // mass distribution
           if (doftype == mjFCOMPDOF_TRILINEAR) {
-            pb->mass = node_mass_uniform;
+            pb->mass = 1.0;
           } else {
             // local index within the cell for mass computation
             int li = gi % flex->spec.order;
@@ -639,14 +817,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
             int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1;
             int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1;
             int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1;
-            // use Simpson weights scaled by cell count
+            // use Simpson weights
             double wi = massP2[li == 0 ? 0 : li];
             double wj = massP2[lj == 0 ? 0 : lj];
             double wk = massP2[lk == 0 ? 0 : lk];
-            pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k
-                       / (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]);
+            pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k;
           }
 
+          node_bodies.push_back(pb);
+
           pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
           pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
           pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
@@ -671,6 +850,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
       }
     }
 
+    // normalize masses so total equals prescribed mass
+    double total_mass = 0;
+    for (mjsBody* pb : node_bodies) {
+      total_mass += pb->mass;
+    }
+    if (total_mass > 0) {
+      double scale = mass / total_mass;
+      for (mjsBody* pb : node_bodies) {
+        pb->mass *= scale;
+        pb->inertia[0] *= scale;
+        pb->inertia[1] *= scale;
+        pb->inertia[2] *= scale;
+      }
+    }
+
     if (!centered) {
       mjs_setDouble(pf->node, node.data(), node.size());
     }
@@ -680,20 +874,41 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf
     mjs_setDouble(pf->vert, point.data(), point.size());
   }
 
-  // create edge equality constraint
+  // create equality constraints
   if (equality) {
-    mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
-    mjs_setDefault(pe->element, &model->Default()->spec);
     // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN)
-    if (equality == 1) {
-      pe->type = mjEQ_FLEX;
-    } else if (equality == 2) {
-      pe->type = mjEQ_FLEXVERT;
+    if (equality == 1 || equality == 2) {
+      mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
+      mjs_setDefault(pe->element, &model->Default()->spec);
+      pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT;
+      pe->active = true;
+      mjs_setString(pe->name1, name.c_str());
     } else if (equality == 3) {
-      pe->type = mjEQ_FLEXSTRAIN;
+      // create one strain constraint per cell, storing cell index in eq_data
+      flex->has_strain_eq = true;
+      int cell_cx = flex->spec.cellcount[0];
+      int cell_cy = flex->spec.cellcount[1];
+      int cell_cz = flex->spec.cellcount[2];
+      for (int ci = 0; ci < cell_cx; ci++) {
+        for (int cj = 0; cj < cell_cy; cj++) {
+          for (int ck = 0; ck < cell_cz; ck++) {
+            // skip empty cells
+            if (!flex->cell_empty.empty() &&
+                flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) {
+              continue;
+            }
+            mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
+            mjs_setDefault(pe->element, &model->Default()->spec);
+            pe->type = mjEQ_FLEXSTRAIN;
+            pe->active = true;
+            mjs_setString(pe->name1, name.c_str());
+            pe->data[0] = ci;
+            pe->data[1] = cj;
+            pe->data[2] = ck;
+          }
+        }
+      }
     }
-    pe->active = true;
-    mjs_setString(pe->name1, name.c_str());
   }
 
   return true;

src/user/user_flexcomp.h

@@ -45,6 +45,7 @@ typedef enum _mjtDof {
   mjFCOMPDOF_RADIAL,
   mjFCOMPDOF_TRILINEAR,
   mjFCOMPDOF_QUADRATIC,
+  mjFCOMPDOF_2D,
 
   mjNFCOMPDOFS
 } mjtDof;
@@ -116,6 +117,11 @@ class mjCFlexcomp {
   std::string plugin_name;
   std::string plugin_instance_name;
   mjsPlugin plugin;
+
+ private:
+  // identify empty cells and pin nodes exclusively in empty cells
+  void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt,
+                      const double minmax[6], int nx, int ny, int nz);
 };
 
 #endif  // MUJOCO_SRC_USER_USER_FLEXCOMP_H_

src/user/user_mesh.cc

@@ -300,9 +300,6 @@ void mjCMesh::NameSpace(const mjCModel* m) {
     name = mjuu_stripext(stripped);
   }
   mjCBase::NameSpace(m);
-  if (modelfiledir_.empty()) {
-    modelfiledir_ = FilePath(m->spec_modelfiledir_);
-  }
   if (!plugin_instance_name.empty()) {
     plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
   }
@@ -712,17 +709,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
     mujoco::user::FilePath meshdir_;
     meshdir_ = FilePath(mjs_getString(compiler->meshdir));
 
-    if (modelfiledir_.empty()) {
-      modelfiledir_ = FilePath(model->modelfiledir_);
-    }
-
     // remove path from file if necessary
     if (model->strippath) {
       file_ = mjuu_strippath(file_);
     }
 
+    mjSpec* owning_spec = model->FindSpec(compiler);
     FilePath filename = meshdir_ + FilePath(file_);
-    resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs);
+    resource_ = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs);
 
     // try loading from cache
     if (cache != nullptr && LoadCachedMesh(cache, resource_)) {
@@ -2957,9 +2951,6 @@ void mjCSkin::NameSpace(const mjCModel* m) {
   for (auto& name : spec_bodyname_) {
     name = m->prefix + name + m->suffix;
   }
-  if (modelfiledir_.empty()) {
-    modelfiledir_ = FilePath(m->spec_modelfiledir_);
-  }
 }
 
 
@@ -3046,15 +3037,12 @@ void mjCSkin::Compile(const mjVFS* vfs) {
       throw mjCError(this, "Unknown skin file type: %s", file_.c_str());
     }
 
-    // copy paths from model if not already defined
-    if (modelfiledir_.empty()) {
-      modelfiledir_ = FilePath(model->modelfiledir_);
-    }
     mujoco::user::FilePath meshdir_;
     meshdir_ = FilePath(mjs_getString(compiler->meshdir));
 
     FilePath filename = meshdir_ + FilePath(file_);
-    mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs);
+    mjSpec* owning_spec = model->FindSpec(compiler);
+    mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs);
 
     try {
       LoadSKN(resource);
@@ -3818,6 +3806,48 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
   }
 }
 
+
+// Eigendecompose cell stiffness matrix and store scaled eigenvectors.
+// K_cell is n×n stored (negative convention: K_stored = -K_physical).
+// Output layout in `out`:
+//   [0]: neig (as double)
+//   [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major
+// Returns number of retained eigenmodes.
+static int EigendecomposeStiffness(const double* K_cell_data,
+                                   double* out, int ndof) {
+  // copy K_cell for in-place decomposition
+  std::vector<double> mat(K_cell_data, K_cell_data + ndof * ndof);
+  std::vector<double> eigval(ndof);
+  std::vector<double> eigvec(ndof * ndof);
+
+  mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof);
+
+  // K_stored = -K_physical, so physical eigenvalue = -eigval[i]
+  // retain modes where physical eigenvalue > threshold
+  double max_eigval = 0;
+  for (int i = 0; i < ndof; i++) {
+    max_eigval = std::max(max_eigval, std::abs(eigval[i]));
+  }
+  double threshold = max_eigval * 1e-8;
+
+  int neig = 0;
+  for (int i = 0; i < ndof; i++) {
+    double lambda_phys = -eigval[i];  // negate to get physical eigenvalue
+    if (lambda_phys > threshold) {
+      // store sqrt(λ) * eigenvector (column i of eigvec matrix)
+      double scale = std::sqrt(lambda_phys);
+      for (int j = 0; j < ndof; j++) {
+        out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i];
+      }
+      neig++;
+    }
+  }
+
+  out[0] = static_cast<double>(neig);
+  return neig;
+}
+
+
 //------------------ class mjCFlex implementation --------------------------------------------------
 
 // constructor
@@ -4070,6 +4100,19 @@ void mjCFlex::Compile(const mjVFS* vfs) {
   }
   nelem = (int)elem_.size()/(dim+1);
 
+  // elastic2d checks
+  if (elastic2d) {
+    if (thickness <= 0) {
+      throw mjCError(this, "2d elasticity requires positive thickness");
+    }
+    if (interpolated) {
+      throw mjCError(this, "interpolated flex does not yet support 2d elasticity");
+    }
+    if (dim != 2 && !interpolated) {
+      throw mjCError(this, "2d elasticity requires 2d flex");
+    }
+  }
+
   // set nvert, rigid, centered; check size
   if (vert_.empty()) {
     centered = true;
@@ -4221,6 +4264,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     }
   }
 
+  // compute unrotated node positions for stiffness computation
+  std::vector<double> nodexpos_local = ComputeUnrotatedNodePositions(nodexpos);
+
   // reorder tetrahedra so right-handed face orientation is outside
   // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
   if (dim == 3) {
@@ -4309,9 +4355,6 @@ void mjCFlex::Compile(const mjVFS* vfs) {
 
     // bending stiffness (2D only)
     if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) {
-      if (thickness < 0) {
-        throw mjCError(this, "thickness must be positive for bending stiffness");
-      }
       bending.assign(nedge*17, 0);
 
       for (unsigned int e = 0; e < nedge; e++) {
@@ -4344,7 +4387,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     stiffness_cached = LoadCachedStiffness();
   }
 
-  if (!stiffness_cached && young > 0 && interpolated) {
+  if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) {
+    // use young=1 for strain constraints (eigenvectors are geometry-only)
+    double K_young = has_strain_eq ? 1e1 : young;
+    double K_poisson = has_strain_eq ? 0.3 : poisson;
+
     int npc = pow(spec.order + 1, 3);  // nodes per cell
     int ndof_cell = 3 * npc;
     int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
@@ -4361,6 +4408,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
         for (int ck = 0; ck < cz; ck++) {
           int cell_idx = ci * cy * cz + cj * cz + ck;
 
+          // skip stiffness computation for empty cells (no mesh content)
+          if (!cell_empty.empty() && cell_empty[cell_idx]) {
+            continue;
+          }
+
           // gather cell's local node positions
           std::vector<double> cell_pos(3 * npc);
           int local = 0;
@@ -4371,7 +4423,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
                 int gj = cj * spec.order + lj;
                 int gk = ck * spec.order + lk;
                 int global = gi * ny_global * nz_global + gj * nz_global + gk;
-                mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3);
+                mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3);
                 local++;
               }
             }
@@ -4379,11 +4431,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
 
           // compute per-cell stiffness
           std::vector<double> K_cell(ndof_cell * ndof_cell, 0);
-          ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order);
+          ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order);
+          double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell;
 
-          // copy into global stiffness array
-          mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell,
-                       K_cell.data(), ndof_cell * ndof_cell);
+          if (has_strain_eq) {
+            // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
+            std::fill(out, out + ndof_cell * ndof_cell, 0.0);
+            EigendecomposeStiffness(K_cell.data(), out, ndof_cell);
+          } else {
+            // store raw K for passive forces
+            std::copy(K_cell.begin(), K_cell.end(), out);
+          }
         }
       }
     }
@@ -4408,14 +4466,88 @@ void mjCFlex::Compile(const mjVFS* vfs) {
     }
   }
 
-  // store node cartesian positions
+  // store node positions in unrotated (body-local) frame
+  // this ensures the runtime displacement refpos - R^{-1}*x is zero at rest
   node0_.assign(3*nnode, 0);
   for (int i=0; i < nnode; i++) {
-    mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
+    mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3);
   }
 }
 
 
+// compute unrotated node positions for stiffness computation and node0_
+//
+// the runtime corotational code extracts rotation R from the deformation
+// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0
+// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get
+// zero displacement at rest; additionally, the stiffness eigenvectors must
+// be computed from axis-aligned positions to preserve the diagonal Jacobian
+// assumption in ComputeLinearStiffness.
+std::vector<double> mjCFlex::ComputeUnrotatedNodePositions(
+    const std::vector<double>& nodexpos) const {
+  std::vector<double> nodexpos_local(3*nnode);
+  if (interpolated && nnode > 0) {
+    int ny_global = spec.cellcount[1] * spec.order + 1;
+    int nz_global = spec.cellcount[2] * spec.order + 1;
+
+    // find first non-empty cell
+    int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
+    int ref_ci = 0, ref_cj = 0, ref_ck = 0;
+    bool found = false;
+    for (int ci = 0; ci < cx && !found; ci++) {
+      for (int cj = 0; cj < cy && !found; cj++) {
+        for (int ck = 0; ck < cz && !found; ck++) {
+          int cell_idx = ci * cy * cz + cj * cz + ck;
+          if (cell_empty.empty() || !cell_empty[cell_idx]) {
+            ref_ci = ci; ref_cj = cj; ref_ck = ck;
+            found = true;
+          }
+        }
+      }
+    }
+
+    // corner indices of the reference cell (order=1 corners at local 0,0,0
+    // and at offsets along each parametric axis)
+    int g000 = (ref_ci * spec.order) * ny_global * nz_global +
+               (ref_cj * spec.order) * nz_global +
+               (ref_ck * spec.order);
+    int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global +
+               (ref_cj * spec.order) * nz_global +
+               (ref_ck * spec.order);
+    int g010 = (ref_ci * spec.order) * ny_global * nz_global +
+               ((ref_cj * spec.order) + spec.order) * nz_global +
+               (ref_ck * spec.order);
+    int g001 = (ref_ci * spec.order) * ny_global * nz_global +
+               (ref_cj * spec.order) * nz_global +
+               ((ref_ck * spec.order) + spec.order);
+
+    // edge vectors (columns of the deformation gradient F = R * S)
+    // we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1}
+    double R0[9];
+    for (int d = 0; d < 3; d++) {
+      R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d];
+      R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d];
+      R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d];
+    }
+
+    // normalize to get rotation matrix columns (valid for regular grids)
+    double li = mjuu_normvec(R0+0, 3);
+    double lj = mjuu_normvec(R0+3, 3);
+    double lk = mjuu_normvec(R0+6, 3);
+    (void)li; (void)lj; (void)lk;
+
+    // apply inverse rotation to each nodexpos to get local-frame positions
+    for (int i = 0; i < nnode; i++) {
+      const double* p = nodexpos.data() + 3*i;
+      double* q = nodexpos_local.data() + 3*i;
+      mjuu_mulvecmat(q, p, R0);
+    }
+  } else {
+    nodexpos_local = nodexpos;
+  }
+  return nodexpos_local;
+}
+
 
 // create flex BVH
 void mjCFlex::CreateBVH() {

src/user/user_model.cc

@@ -3657,6 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) {
         int b1 = pfl->vertbodyid[pfl->edge[k].first];
         int b2 = pfl->vertbodyid[pfl->edge[k].second];
         m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
+      } else {
+        m->flexedge_rigid[edge_adr+k] = 0;
       }
     }
 

src/user/user_objects.cc

@@ -4668,9 +4668,6 @@ void mjCHField::NameSpace(const mjCModel* m) {
     name = mjuu_stripext(stripped);
   }
   mjCBase::NameSpace(m);
-  if (modelfiledir_.empty()) {
-    modelfiledir_ = FilePath(m->spec_modelfiledir_);
-  }
 }
 
 
@@ -4798,15 +4795,12 @@ void mjCHField::Compile(const mjVFS* vfs) {
       throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
     }
 
-    // copy paths from model if not already defined
-    if (modelfiledir_.empty()) {
-      modelfiledir_ = FilePath(model->modelfiledir_);
-    }
     mujoco::user::FilePath meshdir_;
     meshdir_ = FilePath(mjs_getString(compiler->meshdir));
 
     FilePath filename = meshdir_ + FilePath(file_);
-    mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs);
+    mjSpec* owning_spec = model->FindSpec(compiler);
+    mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs);
 
     struct CachedHField {
       int nrow, ncol;
@@ -4965,9 +4959,6 @@ void mjCTexture::NameSpace(const mjCModel* m) {
     name = mjuu_stripext(stripped);
   }
   mjCBase::NameSpace(m);
-  if (modelfiledir_.empty()) {
-    modelfiledir_ = FilePath(m->spec_modelfiledir_);
-  }
 }
 
 
@@ -5388,7 +5379,8 @@ void mjCTexture::LoadFlip(std::string filename, const mjVFS* vfs,
   }
 
   // try loading from cache
-  mjResource* resource = LoadResource(modelfiledir_.Str(), filename, vfs);
+  mjSpec* owning_spec = model->FindSpec(compiler);
+  mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename, vfs);
   if (cache && cache->PopulateData(GetCacheId(resource, asset_type), resource, callback)) {
     mju_closeResource(resource);
     return;
@@ -5640,10 +5632,6 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
 void mjCTexture::Compile(const mjVFS* vfs) {
   CopyFromSpec();
 
-  // copy paths from model if not already defined
-  if (modelfiledir_.empty()) {
-    modelfiledir_ = FilePath(model->modelfiledir_);
-  }
   mujoco::user::FilePath texturedir_;
   texturedir_ = FilePath(mjs_getString(compiler->texturedir));
 

src/user/user_objects.h

@@ -983,6 +983,8 @@ class mjCFlex_ : public mjCBase {
   std::vector<int> edgeidx_;              // element edge ids
   std::vector<double> stiffness;          // elasticity stiffness matrix
   std::vector<double> bending;            // bending stiffness matrix
+  bool has_strain_eq = false;             // true if strain constraints reference this flex
+  std::vector<bool> cell_empty;           // true if cell contains no mesh geometry
 
   // variable-size data
   std::vector<std::string> vertbody_;     // vertex body names
@@ -1052,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
   std::vector<double> vert0_;             // vertex positions in [0, 1]^d in the bounding box
   std::vector<double> node0_;             // node Cartesian positions
 
-
+  // compute unrotated node positions for stiffness computation
+  std::vector<double> ComputeUnrotatedNodePositions(const std::vector<double>& nodexpos) const;
 
   // stiffness caching
   std::string ComputeStiffnessCacheKey() const;
@@ -1120,9 +1123,6 @@ class mjCMesh_ : public mjCBase {
 
   // octree
   mjCOctree octree_;                  // octree of the mesh
-
-  // paths stored during model attachment
-  mujoco::user::FilePath modelfiledir_;
 };
 
 class mjCMesh: public mjCMesh_, private mjsMesh {
@@ -1334,9 +1334,6 @@ class mjCSkin_ : public mjCBase {
 
   int matid;                          // material id
   std::vector<int> bodyid;            // body ids
-
-  // paths stored during model attachment
-  mujoco::user::FilePath modelfiledir_;
 };
 
 class mjCSkin: public mjCSkin_, private mjsSkin {
@@ -1389,9 +1386,6 @@ class mjCHField_ : public mjCBase {
   std::string spec_file_;
   std::string spec_content_type_;
   std::vector<float> spec_userdata_;
-
-  // paths stored during model attachment
-  mujoco::user::FilePath modelfiledir_;
 };
 
 class mjCHField : public mjCHField_, private mjsHField {
@@ -1440,9 +1434,6 @@ class mjCTexture_ : public mjCBase {
   std::string spec_file_;
   std::string spec_content_type_;
   std::vector<std::string> spec_cubefiles_;
-
-  // paths stored during model attachment
-  mujoco::user::FilePath modelfiledir_;
 };
 
 class mjCTexture : public mjCTexture_, private mjsTexture {

src/user/user_util.cc

@@ -754,6 +754,81 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
   return iter;
 }
 
+
+// Jacobi eigenvalue decomposition of symmetric n×n matrix.
+// On output, eigenvalues are in eigval and eigenvectors are columns of eigvec.
+// Both arrays must be pre-allocated: eigval[n], eigvec[n*n].
+// The input matrix mat is destroyed.
+int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
+  // initialize eigvec to identity
+  std::fill(eigvec, eigvec + n*n, 0.0);
+  for (int i = 0; i < n; i++) {
+    eigvec[i*n + i] = 1.0;
+  }
+
+  const int max_sweeps = 200;
+  const double tol = 1e-12;
+
+  int sweep;
+  for (sweep = 0; sweep < max_sweeps; sweep++) {
+    // check convergence: sum of squared off-diagonal elements
+    double off_diag = 0;
+    for (int i = 0; i < n; i++) {
+      for (int j = i+1; j < n; j++) {
+        off_diag += mat[i*n + j] * mat[i*n + j];
+      }
+    }
+    if (off_diag < tol * tol) break;
+
+    // sweep over all off-diagonal pairs
+    for (int p = 0; p < n; p++) {
+      for (int q = p+1; q < n; q++) {
+        double apq = mat[p*n + q];
+        if (std::abs(apq) < tol * 1e-3) continue;
+
+        // compute rotation angle
+        double app = mat[p*n + p];
+        double aqq = mat[q*n + q];
+        double tau = (aqq - app) / (2.0 * apq);
+        double t = (tau >= 0 ? 1.0 : -1.0) /
+                   (std::abs(tau) + std::sqrt(1.0 + tau*tau));
+        double c = 1.0 / std::sqrt(1.0 + t*t);
+        double s = t * c;
+
+        // update matrix (Jacobi rotation)
+        mat[p*n + p] -= t * apq;
+        mat[q*n + q] += t * apq;
+        mat[p*n + q] = 0;
+        mat[q*n + p] = 0;
+
+        for (int r = 0; r < n; r++) {
+          if (r == p || r == q) continue;
+          double mrp = mat[r*n + p];
+          double mrq = mat[r*n + q];
+          mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq;
+          mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq;
+        }
+
+        // accumulate eigenvectors
+        for (int r = 0; r < n; r++) {
+          double vrp = eigvec[r*n + p];
+          double vrq = eigvec[r*n + q];
+          eigvec[r*n + p] = c*vrp - s*vrq;
+          eigvec[r*n + q] = s*vrp + c*vrq;
+        }
+      }
+    }
+  }
+
+  // extract eigenvalues from diagonal
+  for (int i = 0; i < n; i++) {
+    eigval[i] = mat[i*n + i];
+  }
+
+  return sweep;
+}
+
+
 // transform vector by pose
 void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4],
                      const double vec[3]) {
@@ -1189,10 +1264,10 @@ template<typename T> std::string VectorToString(const std::vector<T>& v) {
   return s;
 }
 
-template std::string VectorToString(const std::vector<int>& v);
-template std::string VectorToString(const std::vector<float>& v);
-template std::string VectorToString(const std::vector<double>& v);
-template std::string VectorToString(const std::vector<std::string>& v);
+template MJAPI std::string VectorToString(const std::vector<int>& v);
+template MJAPI std::string VectorToString(const std::vector<float>& v);
+template MJAPI std::string VectorToString(const std::vector<double>& v);
+template MJAPI std::string VectorToString(const std::vector<std::string>& v);
 
 namespace {
 
@@ -1258,7 +1333,11 @@ template <typename T> std::vector<T> StringToVector(char* cs) {
   return v;
 }
 
-template<> std::vector<std::string> StringToVector(const std::string& s) {
+template<> MJAPI std::vector<std::string> StringToVector(char* cs) {
+  return StringToVector<std::string>(std::string(cs));
+}
+
+template<> MJAPI std::vector<std::string> StringToVector(const std::string& s) {
   std::vector<std::string> v;
   std::stringstream ss(s);
   std::string word;
@@ -1268,17 +1347,18 @@ template<> std::vector<std::string> StringToVector(const std::string& s) {
   return v;
 }
 
-template std::vector<int>    StringToVector(char* cs);
-template std::vector<float>  StringToVector(char* cs);
-template std::vector<double> StringToVector(char* cs);
+template MJAPI std::vector<int>    StringToVector(char* cs);
+template MJAPI std::vector<float>  StringToVector(char* cs);
+template MJAPI std::vector<double> StringToVector(char* cs);
+template MJAPI std::vector<unsigned char> StringToVector(char* cs);
 
 
 template <typename T> std::vector<T> StringToVector(const std::string& s) {
   return StringToVector<T>(const_cast<char*>(s.c_str()));
 }
-template std::vector<int>    StringToVector(const std::string& s);
-template std::vector<float>  StringToVector(const std::string& s);
-template std::vector<double> StringToVector(const std::string& s);
-template std::vector<unsigned char> StringToVector(const std::string& s);
+template MJAPI std::vector<int>    StringToVector(const std::string& s);
+template MJAPI std::vector<float>  StringToVector(const std::string& s);
+template MJAPI std::vector<double> StringToVector(const std::string& s);
+template MJAPI std::vector<unsigned char> StringToVector(const std::string& s);
 
 }  // namespace mujoco::user

src/user/user_util.h

@@ -26,6 +26,8 @@
 #include <utility>
 #include <vector>
 
+#include <mujoco/mjexport.h>
+
 const double mjEPS = 1E-14;     // minimum value in various calculations
 const double mjMINMASS = 1E-6;  // minimum mass allowed
 
@@ -157,6 +159,12 @@ double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3],
 // eigenvalue decomposition of symmetric 3x3 matrix
 int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]);
 
+// Jacobi eigenvalue decomposition of symmetric n×n matrix
+// eigval[n]: output eigenvalues, eigvec[n*n]: output eigenvectors (columns)
+// mat[n*n]: input matrix (destroyed on output)
+// returns number of sweeps used
+MJAPI int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n);
+
 // transform vector by pose
 void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]);
 
@@ -166,7 +174,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful
 namespace mujoco::user {
 
 // utility class for handling file paths
-class FilePath {
+class MJAPI FilePath {
  public:
   FilePath() = default;
   explicit FilePath(const std::string& str) : path_(PathReduce(str)) {}
@@ -251,11 +259,13 @@ struct Cleanup {
 std::vector<uint8_t> FileToMemory(const char* filename);
 
 // convert vector to string separating elements by whitespace
-template<typename T> std::string VectorToString(const std::vector<T>& v);
+template<typename T> MJAPI std::string VectorToString(const std::vector<T>& v);
 
 // convert string to vector
-template<typename T> std::vector<T> StringToVector(char *cs);
-template<typename T> std::vector<T> StringToVector(const std::string& s);
+template<typename T> MJAPI std::vector<T> StringToVector(char *cs);
+template<typename T> MJAPI std::vector<T> StringToVector(const std::string& s);
+template<> MJAPI std::vector<std::string> StringToVector(char* cs);
+template<> MJAPI std::vector<std::string> StringToVector(const std::string& s);
 
 }  // namespace mujoco::user
 

src/user/user_vfs.cc

@@ -236,6 +236,24 @@ VFS::Status VFS::Unmount(const FilePath& path) {
   return kInvalidResourceProvider;
 }
 
+bool VFS::ContainsBuffer(const char* name) {
+  if (name == nullptr) {
+    return false;
+  }
+  std::lock_guard<std::mutex> lock(mutex_);
+  return mounts_.contains(name);
+}
+
+bool VFS::ContainsFile(const char* directory, const char* filename) {
+  if (filename == nullptr) {
+    return false;
+  }
+  mujoco::user::FilePath path(directory ? directory : "", filename);
+  std::string key = path.StripPath().Lower().Str();
+  std::lock_guard<std::mutex> lock(mutex_);
+  return mounts_.contains(key);
+}
+
 int VFS::Read(mjResource* resource, const void** buffer) {
   if (resource && resource->provider && resource->provider->read) {
     return resource->provider->read(resource, buffer);
@@ -498,3 +516,22 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) {
   }
   return mujoco::user::VFS::kSuccess;
 }
+
+int mj_containsBufferVFS(mjVFS* vfs, const char* name) {
+  mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs);
+  if (impl == nullptr) {
+    mju_error("mjVFS is null.");
+    return -1;
+  }
+  return impl->ContainsBuffer(name);
+}
+
+int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename) {
+  mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs);
+  if (impl == nullptr) {
+    mju_error("mjVFS is null.");
+    return -1;
+  }
+  return impl->ContainsFile(directory, filename);
+}
+

src/user/user_vfs.h

@@ -90,6 +90,12 @@ class VFS {
   // Unmounts the ResourceProvider from the given path.
   Status Unmount(const FilePath& path);
 
+  // Returns true if the VFS contains a buffer with the given name.
+  bool ContainsBuffer(const char* name);
+
+  // Returns true if the VFS contains a file with the given name.
+  bool ContainsFile(const char* directory, const char* filename);
+
   // Sets a destructor to be called when the VFS has no more open resources.
   // Assumes that `destructor` will delete `this`.
   //

src/xml/xml_native_reader.cc

@@ -371,7 +371,7 @@ std::vector<const char*> MJCF[nMJCF] = {
             "active", "solref", "solimp"},
         {"flexvert", "*", "name", "class", "flex",
             "active", "solref", "solimp"},
-        {"flexstrain", "*", "name", "class", "flex",
+        {"flexstrain", "*", "name", "class", "flex", "cell",
             "active", "solref", "solimp"},
     {">"},
 
@@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = {
   {"full",          mjFCOMPDOF_FULL},
   {"radial",        mjFCOMPDOF_RADIAL},
   {"trilinear",     mjFCOMPDOF_TRILINEAR},
-  {"quadratic",     mjFCOMPDOF_QUADRATIC}
+  {"quadratic",     mjFCOMPDOF_QUADRATIC},
+  {"2d",            mjFCOMPDOF_2D}
 };
 
 
@@ -1283,6 +1284,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
     READDSBL("autoreset",    mjDSBL_AUTORESET)
     READDSBL("nativeccd",    mjDSBL_NATIVECCD)
     READDSBL("island",       mjDSBL_ISLAND)
+    READDSBL("multiccd",     mjDSBL_MULTICCD)
 #undef READDSBL
 
 #define READENBL(NAME, MASK) \
@@ -1294,7 +1296,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
     READENBL("energy",      mjENBL_ENERGY)
     READENBL("fwdinv",      mjENBL_FWDINV)
     READENBL("invdiscrete", mjENBL_INVDISCRETE)
-    READENBL("multiccd",    mjENBL_MULTICCD)
     READENBL("sleep",       mjENBL_SLEEP)
 #undef READENBL
   }
@@ -2245,8 +2246,12 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
 
       case mjEQ_FLEX:
       case mjEQ_FLEXVERT:
+        ReadAttrTxt(elem, "flex", name1, true);
+        break;
+
       case mjEQ_FLEXSTRAIN:
         ReadAttrTxt(elem, "flex", name1, true);
+        ReadAttr(elem, "cell", 3, equality->data, text);
         break;
 
       case mjEQ_DISTANCE:

src/xml/xml_native_writer.cc

@@ -728,8 +728,12 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
 
       case mjEQ_FLEX:
       case mjEQ_FLEXVERT:
+        WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
+        break;
+
       case mjEQ_FLEXSTRAIN:
         WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
+        WriteAttr(elem, "cell", 3, equality->data);
         break;
 
       default:
@@ -1089,7 +1093,7 @@ void mjXWriter::Option(XMLElement* root) {
     XMLElement* sub = InsertEnd(section, "flag");
 
 #define WRITEDSBL(NAME, MASK) \
-    if( model->option.disableflags & MASK ) \
+    if (model->option.disableflags & MASK) \
       WriteAttrKey(sub, NAME, enable_map, 2, 0);
     WRITEDSBL("constraint",     mjDSBL_CONSTRAINT)
     WRITEDSBL("equality",       mjDSBL_EQUALITY)
@@ -1110,16 +1114,16 @@ void mjXWriter::Option(XMLElement* root) {
     WRITEDSBL("autoreset",      mjDSBL_AUTORESET)
     WRITEDSBL("nativeccd",      mjDSBL_NATIVECCD)
     WRITEDSBL("island",         mjDSBL_ISLAND)
+    WRITEDSBL("multiccd",       mjDSBL_MULTICCD)
 #undef WRITEDSBL
 
 #define WRITEENBL(NAME, MASK) \
-    if( model->option.enableflags & MASK ) \
+    if (model->option.enableflags & MASK) \
       WriteAttrKey(sub, NAME, enable_map, 2, 1);
     WRITEENBL("override",       mjENBL_OVERRIDE)
     WRITEENBL("energy",         mjENBL_ENERGY)
     WRITEENBL("fwdinv",         mjENBL_FWDINV)
     WRITEENBL("invdiscrete",    mjENBL_INVDISCRETE)
-    WRITEENBL("multiccd",       mjENBL_MULTICCD)
     WRITEENBL("sleep",          mjENBL_SLEEP)
 #undef WRITEENBL
   }

test/benchmark/chol_benchmark_test.cc

@@ -22,7 +22,6 @@
 #include <absl/base/attributes.h>
 #include <mujoco/mjdata.h>
 #include <mujoco/mujoco.h>
-#include "src/engine/engine_memory.h"
 #include "src/engine/engine_support.h"
 #include "src/engine/engine_util_solve.h"
 #include "src/engine/engine_util_sparse.h"
@@ -352,10 +351,6 @@ constexpr int kNumUpdateVectors = 25;
 int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old(
     mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz,
     const int* rowadr, const int* colind, int x_nnz, int* x_ind, mjData* d) {
-  mj_markStack(d);
-  int* buf_ind = mjSTACKALLOC(d, n, int);
-  mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum);
-
   int rank = n, i = x_nnz - 1;
   while (i >= 0) {
     int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]];
@@ -372,10 +367,9 @@ int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old(
     mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c),
                          nnz - 1, i, colind + adr, x_ind);
     int new_x_nnz = mju_combineSparse(x, mat + adr, c, -s, i, nnz - 1, x_ind,
-                                      colind + adr, sparse_buf, buf_ind);
+                                      colind + adr);
     i = i - 1 + (new_x_nnz - i);
   }
-  mj_freeStack(d);
   return rank;
 }
 

test/benchmark/engine_util_sparse_benchmark_test.cc

@@ -14,7 +14,6 @@
 
 // A benchmark for comparing different implementations of mj_solveLD.
 
-#include <cstddef>
 #include <cstring>
 #include <vector>
 
@@ -31,14 +30,179 @@ namespace {
 
 using CombineFuncPtr = decltype(&mju_combineSparse);
 using TransposeFuncPtr = decltype(&mju_transposeSparse);
-using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse);
 
-// number of steps to roll out before benchmarking
-static const int kNumWarmupSteps = 500;
+// ================================ Cached Data ================================
 
-// ----------------------------- old functions --------------------------------
+// ---- MatVecSparse data ----
+struct MatVecData {
+  int nv;
+  int nefc;
+  int nJ;
+  std::vector<mjtNum> efc_J;
+  std::vector<int> efc_J_rownnz, efc_J_rowadr, efc_J_colind, efc_J_rowsuper;
+  std::vector<mjtNum> vec;
+};
 
+MatVecData& GetMatVecData() {
+  static MatVecData data = [] {
+    MatVecData d;
+    mjModel* m = LoadModelFromPath("flex/flag.xml");
+    mjData* dat = mj_makeData(m);
 
+    for (int i = 0; i < 500; i++) {
+      mj_step(m, dat);
+    }
+
+    d.nv = m->nv;
+    d.nefc = dat->nefc;
+    d.nJ = dat->nJ;
+    d.efc_J.assign(dat->efc_J, dat->efc_J + d.nJ);
+    d.efc_J_rownnz.assign(dat->efc_J_rownnz, dat->efc_J_rownnz + d.nefc);
+    d.efc_J_rowadr.assign(dat->efc_J_rowadr, dat->efc_J_rowadr + d.nefc);
+    d.efc_J_colind.assign(dat->efc_J_colind, dat->efc_J_colind + d.nJ);
+    d.efc_J_rowsuper.assign(dat->efc_J_rowsuper, dat->efc_J_rowsuper + d.nefc);
+
+    // compute direction: vec = -M^{-1} * (Ma - qfrc_smooth - qfrc_constraint)
+    mj_markStack(dat);
+    mjtNum* Ma = mj_stackAllocNum(dat, m->nv);
+    mjtNum* grad = mj_stackAllocNum(dat, m->nv);
+    mjtNum* Mgrad = mj_stackAllocNum(dat, m->nv);
+    mj_mulM(m, dat, Ma, dat->qacc);
+    for (int i = 0; i < m->nv; i++) {
+      grad[i] = Ma[i] - dat->qfrc_smooth[i] - dat->qfrc_constraint[i];
+    }
+    mj_solveM(m, dat, Mgrad, grad, 1);
+    d.vec.resize(m->nv);
+    mju_scl(d.vec.data(), Mgrad, -1, m->nv);
+    mj_freeStack(dat);
+
+    mj_deleteData(dat);
+    mj_deleteModel(m);
+    return d;
+  }();
+  return data;
+}
+
+// ---- CombineSparse data ----
+struct CombineData {
+  int nv;
+  std::vector<mjtNum> H;
+  std::vector<int> rownnz, rowadr, colind;
+};
+
+CombineData& GetCombineData() {
+  static CombineData data = [] {
+    CombineData cd;
+    mjModel* m = LoadModelFromPath("humanoid/humanoid.xml");
+    m->opt.jacobian = mjJAC_SPARSE;
+    mjData* d = mj_makeData(m);
+
+    for (int i = 0; i < 500; i++) {
+      mj_step(m, d);
+    }
+
+    cd.nv = m->nv;
+    mj_markStack(d);
+    mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv);
+    int* rownnz = mj_stackAllocInt(d, m->nv);
+    int* rowadr = mj_stackAllocInt(d, m->nv);
+    int* colind = mj_stackAllocInt(d, m->nv*m->nv);
+    int* diagind = mj_stackAllocInt(d, m->nv);
+
+    mjtNum* D = mj_stackAllocNum(d, d->nefc);
+    for (int i = 0; i < d->nefc; i++) {
+      if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
+        D[i] = d->efc_D[i];
+      } else {
+        D[i] = 0;
+      }
+    }
+
+    int* JT_rownnz   = mj_stackAllocInt(d, m->nv);
+    int* JT_rowadr   = mj_stackAllocInt(d, m->nv);
+    int* JT_rowsuper = mj_stackAllocInt(d, m->nv);
+    int* JT_colind   = mj_stackAllocInt(d, d->nJ);
+    mjtNum* JT       = mj_stackAllocNum(d, d->nJ);
+    mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv,
+                        JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper,
+                        d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
+
+    // compute H = J'*D*J, uncompressed layout
+    mju_sqrMatTDUncompressedInit(rowadr, m->nv);
+    mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv,
+                       rownnz, rowadr, colind,
+                       d->efc_J_rownnz, d->efc_J_rowadr,
+                       d->efc_J_colind, d->efc_J_rowsuper,
+                       JT_rownnz, JT_rowadr,
+                       JT_colind, JT_rowsuper, d,
+                       diagind);
+
+    // compute H = M + J'*D*J
+    mj_addM(m, d, H, rownnz, rowadr, colind);
+
+    // copy to persistent storage
+    int nH = rowadr[m->nv-1] + m->nv;  // uncompressed: rowadr[r] = r*nv
+    cd.H.assign(H, H + nH);
+    cd.rownnz.assign(rownnz, rownnz + m->nv);
+    cd.rowadr.assign(rowadr, rowadr + m->nv);
+    cd.colind.assign(colind, colind + nH);
+
+    mj_freeStack(d);
+    mj_deleteData(d);
+    mj_deleteModel(m);
+    return cd;
+  }();
+  return data;
+}
+
+// ---- TransposeSparse data ----
+struct TransposeData {
+  int nv;
+  int nefc;
+  int nJ;
+  std::vector<mjtNum> efc_J;
+  std::vector<int> efc_J_rownnz, efc_J_rowadr, efc_J_colind;
+};
+
+enum class Size { H2_100, H100 };
+
+template <Size S>
+const char* ModelPath() {
+  if constexpr (S == Size::H2_100) {
+    return "../test/benchmark/testdata/2humanoid100_chol.xml";
+  } else {
+    return "../test/benchmark/testdata/100_humanoids_chol.xml";
+  }
+}
+
+template <Size S>
+TransposeData& GetTransposeData() {
+  static TransposeData data = [] {
+    TransposeData td;
+    mjModel* m = LoadModelFromPath(ModelPath<S>());
+    m->opt.jacobian = mjJAC_SPARSE;
+    mjData* d = mj_makeData(m);
+
+    while (d->time < 2) {
+      mj_step(m, d);
+    }
+
+    td.nv = m->nv;
+    td.nefc = d->nefc;
+    td.nJ = d->nJ;
+    td.efc_J.assign(d->efc_J, d->efc_J + d->nJ);
+    td.efc_J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + d->nefc);
+    td.efc_J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + d->nefc);
+    td.efc_J_colind.assign(d->efc_J_colind, d->efc_J_colind + d->nJ);
+
+    mj_deleteData(d);
+    mj_deleteModel(m);
+    return td;
+  }();
+  return data;
+}
+
+// ================================ old functions ==============================
 
 // transpose sparse matrix (uncompressed)
 void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline(
@@ -99,8 +263,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_baseline(mjtNum* dst,
                                                    mjtNum a, mjtNum b,
                                                    int dst_nnz, int src_nnz,
                                                    int* dst_ind,
-                                                   const int* src_ind,
-                                                   mjtNum* buf, int* buf_ind) {
+                                                   const int* src_ind) {
   // check for identical pattern
   if (compare_baseline(dst_ind, src_ind, dst_nnz)) {
     // combine mjtNum data directly
@@ -116,8 +279,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_new(mjtNum* dst,
                                               mjtNum a, mjtNum b,
                                               int dst_nnz, int src_nnz,
                                               int* dst_ind,
-                                              const int* src_ind,
-                                              mjtNum* buf, int* buf_ind) {
+                                              const int* src_ind) {
   // check for identical pattern
   if (compare_memcmp(dst_ind, src_ind, dst_nnz)) {
     // combine mjtNum data directly
@@ -231,61 +393,31 @@ void ABSL_ATTRIBUTE_NOINLINE mulMatVecSparse_8(mjtNum* res,
   }
 }
 
-// ----------------------------- benchmark ------------------------------------
+// ----------------------------- benchmark -------------------------------------
 
 static void BM_MatVecSparse(benchmark::State& state, int unroll) {
-  static mjModel* m = LoadModelFromPath("flex/flag.xml");
-  mjData* d = mj_makeData(m);
+  MatVecData& data = GetMatVecData();
+  std::vector<mjtNum> res(data.nefc);
 
-  // warm-up rollout to get a typical state
-  for (int i=0; i < kNumWarmupSteps; i++) {
-    mj_step(m, d);
-  }
-
-  // allocate gradient
-  mj_markStack(d);
-  mjtNum *Ma = mj_stackAllocNum(d, m->nv);
-  mjtNum *vec = mj_stackAllocNum(d, m->nv);
-  mjtNum *res = mj_stackAllocNum(d, d->nefc);
-  mjtNum *grad = mj_stackAllocNum(d, m->nv);
-  mjtNum *Mgrad  = mj_stackAllocNum(d, m->nv);
-
-  // compute gradient
-  mj_mulM(m, d, Ma, d->qacc);
-  for (int i=0; i < m->nv; i++) {
-    grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i];
-  }
-
-  // compute search direction
-  mj_solveM(m, d, Mgrad, grad, 1);
-  mju_scl(vec, Mgrad, -1, m->nv);
-
-  // save state
-  std::vector<mjtNum> qpos = AsVector(d->qpos, m->nq);
-  std::vector<mjtNum> qvel = AsVector(d->qvel, m->nv);
-  std::vector<mjtNum> act = AsVector(d->act, m->na);
-  std::vector<mjtNum> warmstart = AsVector(d->qacc_warmstart, m->nv);
-
-  // time benchmark
   for (auto s : state) {
     if (unroll == 4) {
-      mju_mulMatVecSparse(res, d->efc_J, vec, d->nefc,
-                          d->efc_J_rownnz, d->efc_J_rowadr,
-                          d->efc_J_colind, d->efc_J_rowsuper);
+      mju_mulMatVecSparse(res.data(), data.efc_J.data(), data.vec.data(),
+                          data.nefc, data.efc_J_rownnz.data(),
+                          data.efc_J_rowadr.data(), data.efc_J_colind.data(),
+                          data.efc_J_rowsuper.data());
     } else if (unroll == 1) {
-      mulMatVecSparse_1(res, d->efc_J, vec, d->nefc,
-                        d->efc_J_rownnz, d->efc_J_rowadr,
-                        d->efc_J_colind, d->efc_J_rowsuper);
+      mulMatVecSparse_1(res.data(), data.efc_J.data(), data.vec.data(),
+                        data.nefc, data.efc_J_rownnz.data(),
+                        data.efc_J_rowadr.data(), data.efc_J_colind.data(),
+                        data.efc_J_rowsuper.data());
     } else if (unroll == 8) {
-      mulMatVecSparse_8(res, d->efc_J, vec, d->nefc,
-                        d->efc_J_rownnz, d->efc_J_rowadr,
-                        d->efc_J_colind, d->efc_J_rowsuper);
+      mulMatVecSparse_8(res.data(), data.efc_J.data(), data.vec.data(),
+                        data.nefc, data.efc_J_rownnz.data(),
+                        data.efc_J_rowadr.data(), data.efc_J_colind.data(),
+                        data.efc_J_rowsuper.data());
     }
   }
 
-  // finalize
-  mj_freeStack(d);
-  mj_deleteData(d);
   state.SetItemsProcessed(state.iterations());
 }
 
@@ -311,75 +443,30 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_MatVecSparse_1(
 BENCHMARK(BM_MatVecSparse_1);
 
 static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) {
-  static mjModel* m = LoadModelFromPath("humanoid/humanoid.xml");
-  m->opt.jacobian = mjJAC_SPARSE;
+  CombineData& data = GetCombineData();
 
-  mjData* d = mj_makeData(m);
-
-  // warm-up rollout to get a typical state
-  for (int i=0; i < kNumWarmupSteps; i++) {
-    mj_step(m, d);
-  }
-
-  // allocate
-  mj_markStack(d);
-  mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv);
-  int* rownnz = mj_stackAllocInt(d, m->nv);
-  int* rowadr = mj_stackAllocInt(d, m->nv);
-  int* colind = mj_stackAllocInt(d, m->nv*m->nv);
-  int* diagind = mj_stackAllocInt(d, m->nv);
-
-  // compute D corresponding to quad states
-  mjtNum* D = mj_stackAllocNum(d, d->nefc);
-  for (int i = 0; i < d->nefc; i++) {
-    if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
-      D[i] = d->efc_D[i];
-    } else {
-      D[i] = 0;
-    }
-  }
-
-  int* JT_rownnz   = mj_stackAllocInt(d, m->nv);
-  int* JT_rowadr   = mj_stackAllocInt(d, m->nv);
-  int* JT_rowsuper = mj_stackAllocInt(d, m->nv);
-  int* JT_colind   = mj_stackAllocInt(d, d->nJ);
-  mjtNum* JT       = mj_stackAllocNum(d, d->nJ);
-  mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv,
-                      JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper,
-                      d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
-
-  // compute H = J'*D*J, uncompressed layout
-  mju_sqrMatTDUncompressedInit(rowadr, m->nv);
-  mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv,
-                     rownnz, rowadr, colind,
-                     d->efc_J_rownnz, d->efc_J_rowadr,
-                     d->efc_J_colind, d->efc_J_rowsuper,
-                     JT_rownnz, JT_rowadr,
-                     JT_colind, JT_rowsuper, d,
-                     diagind);
-
-  // compute H = M + J'*D*J
-  mj_addM(m, d, H, rownnz, rowadr, colind);
+  // make working copies that get modified each iteration
+  std::vector<mjtNum> H = data.H;
+  std::vector<int> rownnz = data.rownnz;
+  std::vector<int> rowadr = data.rowadr;
+  std::vector<int> colind = data.colind;
 
   // time benchmark
   for (auto s : state) {
-    for (int r = m->nv-1; r >= 0; r--) {
+    for (int r = data.nv-1; r >= 0; r--) {
       for (int i = 0; i < rownnz[r]-1; i++) {
         int adr = rowadr[r];
         int c = colind[adr+i];
         // true arguments should be i+1 and colind+rowadr[r]
         // but instead we repeat rownnz[c] and colind+rowadr[c]
         // in order to trigger all if's in combineSparse
-         func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i],
+         func(H.data()+rowadr[c], H.data()+rowadr[r], 1, -H[adr+i],
               rownnz[c], rownnz[c],
-              colind+rowadr[c], colind+rowadr[c], NULL, NULL);
+              colind.data()+rowadr[c], colind.data()+rowadr[c]);
       }
     }
   }
 
-  // finalize
-  mj_freeStack(d);
-  mj_deleteData(d);
   state.SetItemsProcessed(state.iterations());
 }
 
@@ -397,17 +484,6 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_combineSparse_old(
 }
 BENCHMARK(BM_combineSparse_old);
 
-enum class Size { H2_100, H100 };
-
-template <Size S>
-const char* ModelPath() {
-  if constexpr (S == Size::H2_100) {
-    return "../test/benchmark/testdata/2humanoid100_chol.xml";
-  } else {
-    return "../test/benchmark/testdata/100_humanoids_chol.xml";
-  }
-}
-
 enum class Supernode {
   None,
   PostProcess,
@@ -417,44 +493,33 @@ enum class Supernode {
 template <Size S>
 static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func,
                                Supernode super) {
-  static mjModel* m = LoadModelFromPath(ModelPath<S>());
+  TransposeData& data = GetTransposeData<S>();
 
-  // force use of sparse matrices
-  m->opt.jacobian = mjJAC_SPARSE;
-
-  mjData* d = mj_makeData(m);
-
-  // warm-up rollout to get a typical state
-  while (d->time < 2) {
-    mj_step(m, d);
-  }
-
-  mj_markStack(d);
-
-  // need uncompressed layout
-  mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc);
-  int* res_rownnz = mj_stackAllocInt(d, m->nv);
-  int* res_rowadr = mj_stackAllocInt(d, m->nv);
-  int* res_rowsuper = mj_stackAllocInt(d, m->nv);
-  int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc);
+  // allocate output buffers (uncompressed layout)
+  std::vector<mjtNum> res(data.nv * data.nefc);
+  std::vector<int> res_rownnz(data.nv);
+  std::vector<int> res_rowadr(data.nv);
+  std::vector<int> res_rowsuper(data.nv);
+  std::vector<int> res_colind(data.nv * data.nefc);
 
   // time benchmark
   for (auto s : state) {
-    int* rowsuper = (super == Supernode::Inline) ? res_rowsuper : nullptr;
-    func(res, d->efc_J, d->nefc, m->nv,
-         res_rownnz, res_rowadr, res_colind, rowsuper,
-         d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
+    int* rowsuper =
+        (super == Supernode::Inline) ? res_rowsuper.data() : nullptr;
+    func(res.data(), data.efc_J.data(), data.nefc, data.nv,
+         res_rownnz.data(), res_rowadr.data(), res_colind.data(), rowsuper,
+         data.efc_J_rownnz.data(), data.efc_J_rowadr.data(),
+         data.efc_J_colind.data());
     if (super == Supernode::PostProcess) {
-      mju_superSparse(m->nv, res_rowsuper,
-                      res_rownnz, res_rowadr, res_colind);
+      mju_superSparse(data.nv, res_rowsuper.data(),
+                      res_rownnz.data(), res_rowadr.data(), res_colind.data());
     }
   }
 
-  mj_freeStack(d);
-  mj_deleteData(d);
   state.SetItemsProcessed(state.iterations());
 }
 
+
 void ABSL_ATTRIBUTE_NO_TAIL_CALL
 BM_transposeSparse_2H100_old(benchmark::State& state) {
   MujocoErrorTestGuard guard;

test/engine/engine_collision_convex_test.cc

@@ -68,7 +68,7 @@ TEST_F(MjcConvexTest, CylinderBox) {
   EXPECT_EQ(data->ncon, 5);
 
   // with multiCCD disabled, should find 1 contact
-  model->opt.enableflags &= ~mjENBL_MULTICCD;
+  model->opt.disableflags |= mjDSBL_MULTICCD;
   mj_forward(model, data);
   EXPECT_EQ(data->ncon, 1);
 

test/engine/engine_collision_driver_test.cc

@@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) {
   mj_deleteModel(m);
 }
 
+TEST_F(MjCollisionTest, MaxContact) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag multiccd="enable"/>
+    </option>
+    <asset>
+      <mesh name="smallbox"
+        vertex="-1 -1 -1  1 -1 -1   1  1 -1
+                 1  1  1  1 -1  1  -1  1 -1
+                -1  1  1 -1 -1  1"/>
+    </asset>
+    <worldbody>
+      <geom name="mesh" type="mesh" mesh="smallbox"/>
+      <geom name="box" type="box" size="1 1 1"/>
+      <geom name="plane" type="plane" size="1 1 1"/>
+      <geom name="sphere" type="sphere" size="1"/>
+      <geom name="capsule" type="capsule" size="1 1"/>
+      <geom name="ellipsoid" type="ellipsoid" size="1 1 1"/>
+      <geom name="cylinder" type="cylinder" size="1 1"/>
+    </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  mjData* d = mj_makeData(m);
+  ASSERT_THAT(d, NotNull());
+
+  int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh");
+  int box = mj_name2id(m, mjOBJ_GEOM, "box");
+  int plane = mj_name2id(m, mjOBJ_GEOM, "plane");
+  int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere");
+  int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule");
+  int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid");
+  int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder");
+
+  EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4);
+  EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3);
+  EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4);
+  EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4);
+  EXPECT_EQ(mj_maxContact(m, box, box, -1), 8);
+  EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2);
+  EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4);
+  EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2);
+  EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4);
+  EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1);
+  EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5);
+  EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5);
+  EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5);
+  EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5);
+  EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5);
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco

test/engine/engine_collision_gjk_test.cc

@@ -1998,7 +1998,7 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
   <mujoco>
     <statistic meansize="0.15"/>
     <option>
-      <flag gravity="disable"/>
+      <flag gravity="disable" multiccd="disable"/>
     </option>
 
     <worldbody>

test/engine/engine_core_constraint_test.cc

@@ -15,8 +15,6 @@
 // Tests for engine/engine_core_constraint.c.
 
 #include <array>
-#include <cstddef>
-#include <cstring>
 #include <string>
 #include <vector>
 
@@ -622,6 +620,149 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
   mj_deleteModel(m);
 }
 
+// Test flex strain constraint with quadratic interpolation
+TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <option integrator="implicitfast" jacobian="dense"/>
+  <worldbody>
+    <body name="parent">
+      <joint type="free"/>
+      <geom type="box" size=".01 .01 .01" mass=".1"/>
+      <flexcomp name="test" type="box"
+                spacing=".1 .1 .1" radius="0.001"
+                pos="0 0 .5" dof="quadratic" mass="1" dim="3">
+        <contact selfcollide="none"/>
+        <edge equality="strain"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+
+  mj_resetData(m, d);
+  mj_forward(m, d);
+
+  // Check constraints generated
+  EXPECT_GT(d->ne, 0) << "Expected strain constraints";
+
+  // Check that initial strain is ~0
+  mjtNum max_pos = 0;
+  for (int i = 0; i < d->ne; i++) {
+    if (mju_abs(d->efc_pos[i]) > max_pos) {
+      max_pos = mju_abs(d->efc_pos[i]);
+    }
+  }
+  EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0";
+
+  // Check Jacobian for NaN
+  int nv = m->nv;
+  bool has_bad_jacobian = false;
+  for (int i = 0; i < d->ne; i++) {
+    for (int j = 0; j < nv; j++) {
+      if (mju_isBad(d->efc_J[i*nv + j])) {
+        has_bad_jacobian = true;
+      }
+    }
+  }
+  EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN";
+
+  // Run simulation for a few steps
+  for (int i = 0; i < 100; i++) {
+    mj_step(m, d);
+    ASSERT_FALSE(mju_isBad(d->qpos[0]))
+        << "Simulation unstable at step " << i;
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+// Test quadratic passive forces (no constraints) for stability
+TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <option integrator="implicitfast" solver="CG" tolerance="1e-6"/>
+  <worldbody>
+    <geom type="plane" size="10 10 1"/>
+    <flexcomp name="test" type="grid" count="3 3 3"
+              spacing=".05 .05 .05" radius="0.001"
+              pos="0 0 .3" dof="quadratic" mass="1" dim="3">
+      <contact selfcollide="none"/>
+      <elasticity young="1e4" damping="0.01"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+
+  // Run for 500 steps — should stay stable
+  for (int i = 0; i < 500; i++) {
+    mj_step(m, d);
+    ASSERT_FALSE(mju_isBad(d->qpos[0]))
+        << "Passive quadratic unstable at step " << i;
+    for (int j = 0; j < m->nv; j++) {
+      ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
+          << "Velocity exploded at step " << i;
+    }
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+// Test quadratic with anisotropic cells (like what mesh bounding box creates)
+TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <option integrator="implicitfast" solver="CG" tolerance="1e-6"/>
+  <size memory="50M"/>
+  <worldbody>
+    <geom type="plane" size="10 10 1"/>
+    <body name="parent">
+      <joint type="free"/>
+      <geom type="box" size=".01 .01 .01" mass=".1"/>
+      <flexcomp name="test" type="grid" count="3 3 3"
+                spacing=".1 .05 .08" radius="0.001"
+                pos="0 0 .5" dof="quadratic" mass="1" dim="3">
+        <contact selfcollide="none" internal="false"/>
+        <edge equality="strain" damping="0.01"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+
+  mj_forward(m, d);
+  EXPECT_GT(d->ne, 0) << "Expected strain constraints";
+
+  // Run for 200 steps with gravity + contact
+  for (int i = 0; i < 200; i++) {
+    mj_step(m, d);
+    ASSERT_FALSE(mju_isBad(d->qpos[0]))
+        << "Anisotropic quadratic unstable at step " << i;
+    for (int j = 0; j < m->nv; j++) {
+      ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
+          << "Velocity exploded at step " << i
+          << ", qvel[" << j << "]=" << d->qvel[j];
+    }
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
   constexpr char xml[] = R"(
   <mujoco>
@@ -771,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
   }
 }
 
+// --------------------------- strain constraint rotated parent ----------------
+
+struct StrainConstraintTestCase {
+  std::string test_name;
+  std::string body_pos;
+  std::string body_quat;
+  std::string flex_spacing;
+  std::string flex_xyaxes;
+};
+
+class StrainConstraintRotatedTest : public CoreConstraintTest,
+                                    public ::testing::WithParamInterface<
+                                        StrainConstraintTestCase> {
+};
+
+TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
+  auto param = GetParam();
+  std::string xml = R"(
+  <mujoco>
+  <option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
+  <worldbody>
+    <body name="parent" )";
+
+  if (!param.body_pos.empty()) {
+    xml += "pos=\"" + param.body_pos + "\" ";
+  }
+  if (!param.body_quat.empty()) {
+    xml += "quat=\"" + param.body_quat + "\" ";
+  }
+  xml += R"(>
+      <joint type="free"/>
+      <geom type="box" size=".01 .01 .01" mass=".1"/>
+      <flexcomp name="test" type="box" )";
+
+  if (!param.flex_spacing.empty()) {
+    xml += "spacing=\"" + param.flex_spacing + "\" ";
+  }
+  if (!param.flex_xyaxes.empty()) {
+    xml += "xyaxes=\"" + param.flex_xyaxes + "\" ";
+  }
+
+  xml += R"(radius="0.001"
+                pos="0 0 0" dof="trilinear" mass="1" dim="3">
+        <contact selfcollide="none"/>
+        <edge equality="strain"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+
+  mj_forward(m, d);
+
+  // Check we have strain constraints
+  EXPECT_GT(d->ne, 0) << "Expected strain constraints";
+
+  // The critical check: constraint residuals must be ~0 at the initial
+  // (undeformed) configuration, even though the body is rotated.
+  mjtNum max_pos = 0;
+  for (int i = 0; i < d->ne; i++) {
+    max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
+  }
+  EXPECT_LT(max_pos, 1e-6)
+      << "Strain constraint residual should be ~0"
+      << " (max_pos=" << max_pos << ")";
+
+  // Verify stability
+  for (int i = 0; i < 200; i++) {
+    mj_step(m, d);
+    ASSERT_FALSE(mju_isBad(d->qpos[0]))
+        << "Simulation unstable at step " << i;
+    for (int j = 0; j < m->nv; j++) {
+      ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
+          << "Velocity exploded at step " << i
+          << ", qvel[" << j << "]=" << d->qvel[j];
+    }
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  StrainConstraintRotatedTests, StrainConstraintRotatedTest,
+  testing::ValuesIn<StrainConstraintTestCase>({
+      // Test strain constraint with a rotated parent body.
+      // The flexcomp is placed inside a parent body that has a non-identity
+      // initial rotation. This reproduces the "grocery scene" bug where the
+      // stiffness matrix eigenvectors and reference positions were computed
+      // in world frame instead of the unrotated local frame, causing
+      // spurious constraint forces.
+      {
+          "RotatedParent",
+          "1 2 3",
+          "0.707107 0 0.707107 0",
+          ".1 .1 .1",
+          ""
+      },
+      // Same test with an anisotropic box (different spacing per axis) and
+      // arbitrary rotation (combined 45-deg Y + 30-deg X).
+      {
+          "RotatedParentAnisotropic",
+          "0.5 -1 2",
+          "0.8924 0.2392 0.3696 -0.0990",
+          ".15 .08 .05",
+          ""
+      },
+      // Test strain constraint with flexcomp-level xyaxes rotation.
+      // This is the "grocery scene" pattern where the flexcomp grid itself is
+      // rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z).
+      {
+          "FlexcompXyaxes",
+          "",
+          "",
+          ".1 .02 .1",
+          "0 1 0 0 0 1"
+      },
+      // Test combining parent body rotation with flexcomp xyaxes rotation.
+      // The total rotation is the composition of both.
+      {
+          "RotatedParentPlusXyaxes",
+          "1 2 3",
+          "0.707107 0 0.707107 0",
+          ".15 .08 .05",
+          "0 1 0 0 0 1"
+      }
+  }),
+  [](const testing::TestParamInfo<
+      StrainConstraintRotatedTest::ParamType>& info) {
+    return info.param.test_name;
+  }
+);
+
 }  // namespace
 }  // namespace mujoco

test/engine/engine_solver_test.cc

@@ -46,6 +46,7 @@ TEST_F(SolverTest, IslandsEquivalent) {
   model->opt.tolerance = 0;                     // set tolerance to 0
   model->opt.ls_tolerance = 0;                  // set ls_tolerance to 0
   model->opt.ccd_tolerance = 0;                 // set ccd_tolerance to 0
+  model->opt.disableflags |= mjDSBL_MULTICCD;   // disable multiccd
 
   int nv = model->nv;
 

test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc

@@ -179,8 +179,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) {
   EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV);
   EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag,
                                                    mjENBL_INVDISCRETE);
-  EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag,
-                                                   mjENBL_MULTICCD);
 
   mj_deleteModel(default_model);
   mj_deleteSpec(empty_spec);

test/user/CMakeLists.txt

@@ -40,3 +40,5 @@ mujoco_test(user_composite_test)
 mujoco_test(user_resource_test)
 
 mujoco_test(user_vfs_test)
+
+mujoco_test(user_util_test)

test/user/user_api_test.cc

@@ -19,6 +19,7 @@
 #include <cctype>
 #include <cstddef>
 #include <cstdint>
+#include <cstring>
 #include <filesystem>  // NOLINT
 #include <functional>
 #include <map>
@@ -32,6 +33,7 @@
 #include "src/cc/array_safety.h"
 #include <mujoco/mujoco.h>
 #include <mujoco/mjspec.h>
+#include <mujoco/mjplugin.h>
 #include "src/xml/xml_api.h"
 #include "src/xml/xml_numeric_format.h"
 #include "test/fixture.h"
@@ -203,6 +205,103 @@ TEST_F(MujocoTest, AttachAndChildDeletion) {
   mj_deleteSpec(parent_spec);
 }
 
+int open_mock(mjResource* resource) {
+  static const char parent_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="parent_body"/>
+    </worldbody>
+  </mujoco>
+  )";
+  resource->data = mju_malloc(sizeof(parent_xml));
+  std::strcpy((char*)resource->data, parent_xml);
+  return 1;
+}
+
+int read_mock(mjResource* resource, const void** buffer) {
+  *buffer = resource->data;
+  return std::strlen((const char*)resource->data);
+}
+
+void close_mock(mjResource* resource) {
+  mju_free(resource->data);
+  resource->data = nullptr;
+}
+
+TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) {
+  // This test checks that when we attach a child spec to a parent spec that was
+  // loaded from a resource provider, the child spec does not inherit the
+  // resource URI from the parent. This allows the child spec to specify assets
+  // relative to its model file or in the VFS.
+  mjpResourceProvider provider = {
+      .prefix = "fakeprovider",
+      .open = open_mock,
+      .read = read_mock,
+      .close = close_mock,
+  };
+
+  mjp_registerResourceProvider(&provider);
+
+  std::array<char, 1024> err;
+  mjSpec* parent_spec =
+      mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size());
+  mjs_setString(parent_spec->modelname, "parent");
+  ASSERT_THAT(parent_spec, NotNull()) << err.data();
+
+  // Create child spec
+  static constexpr char child_xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body name="child_body">
+        <geom type="mesh" mesh="asset"/>
+      </body>
+    </worldbody>
+    <asset>
+      <mesh name="asset" file="asset.obj"/>
+    </asset>
+  </mujoco>
+  )";
+
+  // Setup VFS with asset
+  mjVFS vfs;
+  mj_defaultVFS(&vfs);
+  static constexpr char asset_data[] = R"(
+  v 0 0 0
+  v 1 0 0
+  v 0 1 0
+  v 0 0 1
+  f 1 2 3
+  f 1 2 4
+  f 2 3 4
+  f 3 1 4
+  )";
+  mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data));
+
+  mjSpec* child_spec =
+      mj_parseXMLString(child_xml, &vfs, err.data(), err.size());
+  mjs_setString(child_spec->modelname, "child");
+  ASSERT_THAT(child_spec, NotNull()) << err.data();
+
+  // Attach child spec to parent spec's world body
+  mjsBody* world = mjs_findBody(parent_spec, "world");
+  ASSERT_THAT(world, NotNull());
+
+  mjsElement* attached =
+      mjs_attach(world->element, child_spec->element, "", "");
+  ASSERT_THAT(attached, NotNull());
+
+  mjModel* model = mj_compile(parent_spec, &vfs);
+  mj_deleteVFS(&vfs);
+
+  EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec);
+
+  if (model) {
+    mj_deleteModel(model);
+  }
+  mj_deleteSpec(parent_spec);
+  mj_deleteSpec(child_spec);
+}
+
 TEST_F(MujocoTest, ActivatePlugin) {
   mjSpec* spec = mj_makeSpec();
   mjs_activatePlugin(spec, "mujoco.elasticity.cable");

test/user/user_flex_test.cc

@@ -1031,5 +1031,325 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
   mj_deleteModel(m);
 }
 
+TEST_F(UserFlexTest, EmptyCellNodePinning) {
+  // A 2x2x2 grid with a box mesh that fills all cells.
+  // No nodes should be pinned.
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="box" spacing=".1 .1 .1" dim="3"
+              dof="trilinear" mass="1" cellcount="2 2 2">
+      <contact selfcollide="none"/>
+      <elasticity young="1"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
+  int nadr = m->flex_nodeadr[0];
+  int nnode = m->flex_nodenum[0];
+  EXPECT_EQ(nnode, 27);
+
+  // All cells are occupied by the box, so no node should be pinned.
+  int pinned = 0;
+  for (int n = nadr; n < nadr + nnode; n++) {
+    int bid = m->flex_nodebodyid[n];
+    if (m->body_jntnum[bid] == 0) {
+      pinned++;
+    }
+  }
+  EXPECT_EQ(pinned, 0);
+
+  // Verify simulation works
+  mjData* d = mj_makeData(m);
+  for (int i = 0; i < 10; i++) {
+    mj_step(m, d);
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
+  // Load bunny_multicell.xml which has a 3x3x3 grid.
+  // The bunny mesh only occupies some cells, so many nodes should be pinned.
+  const std::string xml_path =
+      GetModelPath("flex/bunny_multicell.xml");
+  std::array<char, 1024> error;
+  mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  // 3x3x3 grid, order=1: (3+1)^3 = 64 node positions
+  int nadr = m->flex_nodeadr[0];
+  int nnode = m->flex_nodenum[0];
+  EXPECT_EQ(nnode, 64);
+
+  // Count pinned nodes (no joints)
+  int pinned = 0;
+  int free_nodes = 0;
+  for (int n = nadr; n < nadr + nnode; n++) {
+    int bid = m->flex_nodebodyid[n];
+    if (m->body_jntnum[bid] == 0) {
+      pinned++;
+    } else {
+      free_nodes++;
+    }
+  }
+
+  // At least some nodes should be pinned since the bunny doesn't fill all cells
+  EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells";
+  EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free";
+  EXPECT_EQ(pinned + free_nodes, nnode);
+
+  // Verify the model can simulate
+  mjData* d = mj_makeData(m);
+  mj_forward(m, d);
+  for (int i = 0; i < 10; i++) {
+    mj_step(m, d);
+  }
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
+  // Regression test for ci_min calculation with order=2.
+  // A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis).
+  // We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty.
+  //
+  // Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2).
+  // With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check
+  // cell 0 (non-empty), incorrectly marking gi=3 as non-pinned.
+  // Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid.
+  // Anchor vertex at x=1.0 extends the bounding box to [0,1]^3.
+  // The 3x1x1 quadratic grid splits at x=0.33, 0.67.
+  // Cell 0 has vertices, cells 1 and 2 are empty.
+  // Interior nodes for cells 1,2 should be pinned to the parent body.
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <body name="parent">
+    <freejoint/>
+    <inertial mass="0.01" pos="0 0 0"
+             diaginertia="0.001 0.001 0.001"/>
+    <flexcomp name="test" type="direct" dim="3"
+              dof="quadratic" mass="1" cellcount="3 1 1"
+              point="0.0 0.0 0.0  0.3 0.0 0.0
+                     0.0 1.0 0.0  0.3 1.0 0.0
+                     0.0 0.0 1.0  0.3 0.0 1.0
+                     0.0 1.0 1.0  0.3 1.0 1.0
+                     1.0 0.5 0.5"
+              element="0 1 3 2 4 5 7 6">
+      <contact selfcollide="none"/>
+      <elasticity young="1"/>
+    </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
+  int nadr = m->flex_nodeadr[0];
+  int nnode = m->flex_nodenum[0];
+  EXPECT_EQ(nnode, 63);
+
+  // Count pinned nodes: pinned nodes are assigned to the parent body.
+  int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
+  ASSERT_GT(parent_bid, 0);
+  int pinned = 0;
+  for (int n = nadr; n < nadr + nnode; n++) {
+    if (m->flex_nodebodyid[n] == parent_bid) {
+      pinned++;
+    }
+  }
+
+  // Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned.
+  // Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2.
+  // That's 4 * 3 * 3 = 36 nodes.
+  EXPECT_EQ(pinned, 36);
+
+  mj_deleteData(mj_makeData(m));
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, EmptyCellDetectsElements) {
+  // A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3.
+  // With cellcount="6 6 6" (216 cells), only 8 corner cells contain
+  // mesh vertices.
+  //
+  // Bug: MarkEmptyCells only checked vertices, so 208/216 cells are
+  // marked empty, causing most interior nodes to be incorrectly pinned.
+  // Fix: check element AABBs to correctly identify occupied cells.
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <body name="parent">
+    <freejoint/>
+    <inertial mass="0.01" pos="0.5 0.5 0.5"
+             diaginertia="0.001 0.001 0.001"/>
+    <flexcomp name="test" type="direct" dim="2"
+              dof="trilinear" mass="1" cellcount="6 6 6"
+              point="0 0 0  1 0 0  1 1 0  0 1 0
+                     0 0 1  1 0 1  1 1 1  0 1 1"
+              element="0 1 2  0 2 3  4 6 5  4 7 6
+                       0 5 1  0 4 5  2 7 3  2 6 7
+                       0 3 7  0 7 4  1 5 6  1 6 2">
+      <contact selfcollide="none"/>
+    </flexcomp>
+    </body>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
+  int nadr = m->flex_nodeadr[0];
+  int nnode = m->flex_nodenum[0];
+  ASSERT_EQ(nnode, 343);
+
+  // Count pinned nodes: those assigned to the parent body.
+  int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent");
+  ASSERT_GT(parent_bid, 0);
+  int pinned = 0;
+  for (int n = nadr; n < nadr + nnode; n++) {
+    if (m->flex_nodebodyid[n] == parent_bid) {
+      pinned++;
+    }
+  }
+
+  // The cube surface fills the entire bounding box. The element-AABB
+  // marks all boundary cells as surface cells (152/216). The interior
+  // flood-fill finds no exterior seeds (all boundary cells are surface),
+  // so the remaining 64 cells are classified as interior (non-empty).
+  // No cells are empty → 0 nodes pinned.
+  EXPECT_EQ(pinned, 0);
+
+  mj_deleteData(mj_makeData(m));
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, TotalMassTrilinear) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1"
+              dim="3" dof="trilinear" mass="1.5">
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  double total_mass = 0;
+  for (int i = 1; i < m->nbody; ++i) {
+    total_mass += m->body_mass[i];
+  }
+
+  EXPECT_NEAR(total_mass, 1.5, 1e-5);
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, TotalMassQuadratic) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="3 2 2" spacing="1 1 1"
+              dim="3" dof="quadratic" mass="2.0">
+      <contact selfcollide="none" internal="false"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+
+  double total_mass = 0;
+  for (int i = 1; i < m->nbody; ++i) {
+    total_mass += m->body_mass[i];
+  }
+
+  EXPECT_NEAR(total_mass, 2.0, 1e-5);
+  mj_deleteModel(m);
+}
+
+TEST_F(UserFlexTest, Dof2d) {
+  // 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18
+  static constexpr char xml_2d[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
+              dim="2" radius=".01" dof="2d">
+      <edge equality="true"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+
+  // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27
+  static constexpr char xml_full[] = R"(
+  <mujoco>
+  <worldbody>
+    <flexcomp name="test" type="grid" count="3 3 1" spacing=".1 .1 .1"
+              dim="2" radius=".01">
+      <edge equality="true"/>
+    </flexcomp>
+  </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1024> error;
+
+  // load 2d model
+  mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
+  ASSERT_THAT(m_2d, NotNull()) << error.data();
+  mjData* d_2d = mj_makeData(m_2d);
+
+  // load full model
+  mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size());
+  ASSERT_THAT(m_full, NotNull()) << error.data();
+  mjData* d_full = mj_makeData(m_full);
+
+  // verify DOF counts
+  EXPECT_EQ(m_2d->nv, 18);    // 9 vertices * 2 DOFs
+  EXPECT_EQ(m_full->nv, 27);  // 9 vertices * 3 DOFs
+
+  // same number of vertices and elements
+  EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert);
+  EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem);
+
+  // each body has 2 DOFs in 2d mode, 3 in full mode
+  for (int i = 1; i < m_2d->nbody; i++) {
+    EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i;
+  }
+  for (int i = 1; i < m_full->nbody; i++) {
+    EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i;
+  }
+
+  // simulate a few steps to make sure nothing crashes
+  for (int i = 0; i < 10; i++) {
+    mj_step(m_2d, d_2d);
+    mj_step(m_full, d_full);
+  }
+
+  mj_deleteModel(m_2d);
+  mj_deleteModel(m_full);
+  mj_deleteData(d_2d);
+  mj_deleteData(d_full);
+}
+
 }  // namespace
 }  // namespace mujoco
+

test/user/user_mesh_test.cc

@@ -819,6 +819,58 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <flexcomp name="f" type="grid" count="3 3 1" spacing="1 1 1" dim="2" dof="2d">
+        <elasticity young="1" thickness="0" elastic2d="bend"/>
+      </flexcomp>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(error.data(),
+              HasSubstr("2d elasticity requires positive thickness"));
+}
+
+TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <flexcomp name="f" type="grid" count="3 3 1" spacing="1 1 1" dim="2" dof="trilinear">
+        <contact selfcollide="none"/>
+        <elasticity young="1" thickness="1" elastic2d="bend"/>
+      </flexcomp>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(
+      error.data(),
+      HasSubstr("interpolated flex does not yet support 2d elasticity"));
+}
+
+TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <flexcomp name="f" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="2d">
+        <elasticity young="1" thickness="1" elastic2d="bend"/>
+      </flexcomp>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, testing::IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
+}
+
 TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
   static constexpr char xml[] = R"(
   <mujoco>

test/user/user_util_test.cc

@@ -17,6 +17,8 @@
 #include "src/user/user_util.h"
 
 #include <cerrno>
+#include <cmath>
+#include <random>
 #include <string>
 #include <vector>
 
@@ -180,5 +182,142 @@ TEST_F(UserUtilTest, VectorToStringEmpty) {
   EXPECT_EQ(VectorToString(v), "");
 }
 
+// utility: modified Gram-Schmidt to orthogonalize columns of Q (n x n)
+static void gramSchmidt(double* Q, int n) {
+  for (int j = 0; j < n; j++) {
+    // subtract projections onto previous columns
+    for (int k = 0; k < j; k++) {
+      double dot = 0;
+      for (int i = 0; i < n; i++) {
+        dot += Q[i * n + j] * Q[i * n + k];
+      }
+      for (int i = 0; i < n; i++) {
+        Q[i * n + j] -= dot * Q[i * n + k];
+      }
+    }
+    // normalize
+    double norm = 0;
+    for (int i = 0; i < n; i++) {
+      norm += Q[i * n + j] * Q[i * n + j];
+    }
+    norm = std::sqrt(norm);
+    for (int i = 0; i < n; i++) {
+      Q[i * n + j] /= norm;
+    }
+  }
+}
+
+// utility: compose SPD matrix A = Q * diag(eigvals) * Q^T
+static void composeMatrix(double* A, const double* Q,
+                          const double* eigvals, int n) {
+  for (int i = 0; i < n; i++) {
+    for (int j = 0; j <= i; j++) {
+      double sum = 0;
+      for (int k = 0; k < n; k++) {
+        sum += Q[i * n + k] * eigvals[k] * Q[j * n + k];
+      }
+      A[i * n + j] = sum;
+      A[j * n + i] = sum;
+    }
+  }
+}
+
+TEST_F(UserUtilTest, EigendecomposeConvergence) {
+  // seeded RNG for reproducibility
+  std::mt19937_64 rng;
+  rng.seed(42);
+  std::normal_distribution<double> dist(0, 1);
+
+  // sweep over matrix sizes used by flex stiffness
+  //   order=1: 8 nodes * 3 dof = 24
+  //   order=2: 27 nodes * 3 dof = 81
+  for (int n : {24, 81}) {
+    int total_sweeps = 0;
+    int max_sweeps = 0;
+    int count = 0;
+
+    // generate random orthogonal matrix Q via Gram-Schmidt
+    std::vector<double> Q(n * n);
+    for (int i = 0; i < n * n; i++) {
+      Q[i] = dist(rng);
+    }
+    gramSchmidt(Q.data(), n);
+
+    // sweep eigenvalue spectra of varying difficulty
+    //   well-separated, clustered, wide condition number
+    for (double condition : {1e1, 1e3, 1e6}) {
+      for (double cluster : {0.0, 0.5, 0.9}) {
+        // construct eigenvalues
+        std::vector<double> eigvals(n);
+        for (int i = 0; i < n; i++) {
+          // base: logarithmically spaced from 1 to condition
+          double t = (double)i / (n - 1);
+          double base = std::exp(t * std::log(condition));
+
+          // cluster: push eigenvalues toward geometric mean
+          double mean = std::sqrt(condition);
+          eigvals[i] = (1 - cluster) * base + cluster * mean;
+        }
+
+        // compose A = Q * diag(eigvals) * Q^T
+        std::vector<double> A(n * n);
+        composeMatrix(A.data(), Q.data(), eigvals.data(), n);
+
+        // save copy for verification
+        std::vector<double> A_copy(A);
+
+        // decompose
+        std::vector<double> found_eigval(n);
+        std::vector<double> found_eigvec(n * n);
+        int sweeps = mjuu_eigendecompose(
+            A.data(), found_eigval.data(),
+            found_eigvec.data(), n);
+
+        total_sweeps += sweeps;
+        if (sweeps > max_sweeps) max_sweeps = sweeps;
+        count++;
+
+        // verify convergence
+        EXPECT_LT(sweeps, 200)
+            << "n=" << n
+            << " condition=" << condition
+            << " cluster=" << cluster;
+
+        // verify A*v = lambda*v for each eigenpair
+        for (int i = 0; i < n; i++) {
+          for (int r = 0; r < n; r++) {
+            double Av = 0;
+            for (int c = 0; c < n; c++) {
+              Av += A_copy[r * n + c] * found_eigvec[c * n + i];
+            }
+            double lv = found_eigval[i] * found_eigvec[r * n + i];
+            EXPECT_NEAR(Av, lv,
+                        1e-6 * std::abs(found_eigval[i]))
+                << "n=" << n << " condition=" << condition
+                << " cluster=" << cluster
+                << " eigpair=" << i << " row=" << r;
+          }
+        }
+
+        // verify all eigenvalues are positive
+        for (int i = 0; i < n; i++) {
+          EXPECT_GT(found_eigval[i], 0)
+              << "n=" << n << " eigenvalue " << i;
+        }
+      }
+    }
+
+    double mean_sweeps = (double)total_sweeps / count;
+
+    // assert reasonable average convergence
+    EXPECT_LE(mean_sweeps, 20.0)
+        << "n=" << n << ": mean sweeps too high";
+
+    // assert max sweeps within budget
+    EXPECT_LT(max_sweeps, 200)
+        << "n=" << n << ": max sweeps exceeded 200";
+  }
+}
+
 }  // namespace
 }  // namespace mujoco

test/user/user_vfs_test.cc

@@ -224,6 +224,38 @@ TEST_F(UserVfsTest, DeleteFileRepeat) {
   mj_deleteVFS(&vfs);
 }
 
+TEST_F(UserVfsTest, ContainsBuffer) {
+  mjVFS vfs;
+  mj_defaultVFS(&vfs);
+  std::string buffer = "<mujoco/>";
+  const void* ptr = static_cast<const void*>(buffer.c_str());
+  mj_addBufferVFS(&vfs, "model", ptr, buffer.size());
+
+  EXPECT_TRUE(mj_containsBufferVFS(&vfs, "model"));
+  EXPECT_FALSE(mj_containsBufferVFS(&vfs, "nonexistent"));
+  EXPECT_FALSE(mj_containsBufferVFS(&vfs, "Model"));
+
+  mj_deleteVFS(&vfs);
+}
+
+TEST_F(UserVfsTest, ContainsFile) {
+  mjVFS vfs;
+  mj_defaultVFS(&vfs);
+
+  constexpr char path[] = "engine/testdata/actuation/";
+  const std::string dir = GetTestDataFilePath(path);
+  std::string file = "activation.xml";
+  mj_addFileVFS(&vfs, dir.c_str(), file.c_str());
+
+  EXPECT_TRUE(mj_containsFileVFS(&vfs, dir.c_str(), file.c_str()));
+  EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, (dir + file).c_str()));
+  EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, "Activation.xml"));
+  EXPECT_TRUE(mj_containsFileVFS(&vfs, "some/dir/", "activation.xml"));
+  EXPECT_FALSE(mj_containsFileVFS(&vfs, nullptr, "nonexistent.xml"));
+
+  mj_deleteVFS(&vfs);
+}
+
 
 TEST_F(UserVfsTest, AddBuffer) {
   mjVFS vfs;

unity/Runtime/Bindings/MjBindings.cs

@@ -137,16 +137,16 @@ public enum mjtDisableBit : int{
   mjDSBL_AUTORESET = 65536,
   mjDSBL_NATIVECCD = 131072,
   mjDSBL_ISLAND = 262144,
-  mjNDISABLE = 19,
+  mjDSBL_MULTICCD = 524288,
+  mjNDISABLE = 20,
 }
 public enum mjtEnableBit : int{
   mjENBL_OVERRIDE = 1,
   mjENBL_ENERGY = 2,
   mjENBL_FWDINV = 4,
   mjENBL_INVDISCRETE = 8,
-  mjENBL_MULTICCD = 16,
-  mjENBL_SLEEP = 32,
-  mjNENABLE = 6,
+  mjENBL_SLEEP = 16,
+  mjNENABLE = 5,
 }
 public enum mjtJoint : int{
   mjJNT_FREE = 0,
@@ -6684,6 +6684,12 @@ public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedT
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern int mj_containsBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name);
+
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern int mj_containsFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_deleteVFS(void* vfs);
 
@@ -6951,6 +6957,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern int mj_maxContact(mjModel_* m, int g1, int g2, int has_margin);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_collision(mjModel_* m, mjData_* d);
 

wasm/codegen/generated/bindings.cc

@@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) {
   mj_makeM(m.get(), d.get());
 }
 
+int mj_maxContact_wrapper(const MjModel& m, int g1, int g2, int has_margin) {
+  return mj_maxContact(m.get(), g1, g2, has_margin);
+}
+
 void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) {
   UNPACK_VALUE(mjtNum, res);
   UNPACK_ARRAY(mjtNum, vec);
@@ -10942,6 +10946,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjDSBL_AUTORESET", mjDSBL_AUTORESET)
     .value("mjDSBL_NATIVECCD", mjDSBL_NATIVECCD)
     .value("mjDSBL_ISLAND", mjDSBL_ISLAND)
+    .value("mjDSBL_MULTICCD", mjDSBL_MULTICCD)
     .value("mjNDISABLE", mjNDISABLE);
   enum_<mjtDyn>("mjtDyn")
     .value("mjDYN_NONE", mjDYN_NONE)
@@ -10956,7 +10961,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
     .value("mjENBL_ENERGY", mjENBL_ENERGY)
     .value("mjENBL_FWDINV", mjENBL_FWDINV)
     .value("mjENBL_INVDISCRETE", mjENBL_INVDISCRETE)
-    .value("mjENBL_MULTICCD", mjENBL_MULTICCD)
     .value("mjENBL_SLEEP", mjENBL_SLEEP)
     .value("mjNENABLE", mjNENABLE);
   enum_<mjtEq>("mjtEq")
@@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
   function("mj_local2Global", &mj_local2Global_wrapper);
   function("mj_makeConstraint", &mj_makeConstraint_wrapper);
   function("mj_makeM", &mj_makeM_wrapper);
+  function("mj_maxContact", &mj_maxContact_wrapper);
   function("mj_mulJacTVec", &mj_mulJacTVec_wrapper);
   function("mj_mulJacVec", &mj_mulJacVec_wrapper);
   function("mj_mulM", &mj_mulM_wrapper);

wasm/codegen/generators/constants.py

@@ -124,6 +124,8 @@ _SKIPPED_ASSET_CACHE_FUNCTIONS: tuple[str, ...] = (
 _SKIPPED_VFS_FUNCTIONS: tuple[str, ...] = (
     # go/keep-sorted start
     "mj_addFileVFS",
+    "mj_containsBufferVFS",
+    "mj_containsFileVFS",
     "mj_mountVFS",
     "mj_unmountVFS",
     # go/keep-sorted end

wasm/tests/bindings_test.ts

@@ -684,10 +684,10 @@ describe('MuJoCo WASM Bindings', () => {
   it('should check constants values', () => {
     expect(mujoco.mjNEQDATA).toBe(11);
     expect(mujoco.mjDISABLESTRING).toEqual([
-      'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', 'Spring',
-      'Damper', 'Gravity', 'Clampctrl', 'Warmstart', 'Filterparent',
-      'Actuation', 'Refsafe', 'Sensor', 'Midphase', 'Eulerdamp', 'AutoReset',
-      'NativeCCD', 'Island'
+      'Constraint',   'Equality',  'Frictionloss', 'Limit',     'Contact',
+      'Spring',       'Damper',    'Gravity',      'Clampctrl', 'Warmstart',
+      'Filterparent', 'Actuation', 'Refsafe',      'Sensor',    'Midphase',
+      'Eulerdamp',    'AutoReset', 'NativeCCD',    'Island',    'MultiCCD',
     ]);
     expect(mujoco.mjRNDSTRING).toEqual([
       ['Shadow', '1', 'S'], ['Wireframe', '0', 'W'], ['Reflection', '1', 'R'],