Expose mjv_flexBodyIdAndPoint as a helper function.

Use the above in studio rather than a custom implementation.

PiperOrigin-RevId: 928432476
Change-Id: I99d772c4baf7260abbf9ac8f6d8032704f1ba769

src/engine/engine_vis_interact.c

@@ -799,6 +799,52 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
 }
 
 
+// return body id, compute position of a vertex in a flex
+int mjv_flexBodyId(const mjModel* m, const mjData* d, int flexid, int vertid, mjtNum flexpnt[3]) {
+  int flexbodyid = -1;
+  if (m->flex_interp[flexid]) {
+    mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[flexid] + vertid);
+    int order = m->flex_interp[flexid];
+    order = order < 0 ? -order : order;
+    int npc = (order+1)*(order+1)*(order+1);
+
+    // cell lookup: get local coords and node indices
+    mjtNum loc[3];
+    int nodeindices[27];  // max npc for quadratic: 3^3 = 27
+    mju_cellLookup(coord, m->flex_cellnum+3*flexid, order, loc, nodeindices);
+
+    // find node with largest weight in this cell
+    // in shell mode, skip interior nodes (pinned to worldbody)
+    int nodeid = -1;
+    int nstart = m->flex_nodeadr[flexid];
+    mjtNum w = 0;
+    int shell_mode = m->flex_interp[flexid] < 0;
+    for (int j = 0; j < npc; j++) {
+      mjtNum ww = mju_evalBasis(loc, j, order);
+      int nid = nodeindices[j];
+      // skip interior nodes in shell mode (they map to worldbody)
+      if (shell_mode && m->body_dofnum[m->flex_nodebodyid[nstart + nid]] == 0) {
+        continue;
+      }
+      if (ww > w) {
+        w = ww;
+        nodeid = nid;
+      }
+    }
+    flexbodyid = m->flex_nodebodyid[nstart + nodeid];
+    if (m->flex_centered[flexid]) {
+      mju_copy3(flexpnt, d->xpos + 3*flexbodyid);
+    } else {
+      mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*(nstart + nodeid));
+      mju_addTo3(flexpnt, d->xpos + 3*flexbodyid);
+    }
+  } else {
+    flexbodyid = m->flex_vertbodyid[m->flex_vertadr[flexid] + vertid];
+    mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + vertid));
+  }
+  return flexbodyid;
+}
+
 // Select geom, flex or skin with mouse, return bodyid; -1: none selected.
 int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
                mjtNum aspectratio, mjtNum relx, mjtNum rely,
@@ -861,47 +907,8 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
       // update if closer intersection found
       if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
         flexdist = newdist;
-        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
-          mjtNum loc[3];
-          int nodeindices[27];  // max npc for quadratic: 3^3 = 27
-          mju_cellLookup(coord, m->flex_cellnum+3*i, order, loc, nodeindices);
-
-          // find node with largest weight in this cell
-          // in shell mode, skip interior nodes (pinned to worldbody)
-          int nodeid = -1;
-          int nstart = m->flex_nodeadr[i];
-          mjtNum w = 0;
-          int shell_mode = m->flex_interp[i] < 0;
-          for (int j = 0; j < npc; j++) {
-            mjtNum ww = mju_evalBasis(loc, j, order);
-            int nid = nodeindices[j];
-            // skip interior nodes in shell mode (they map to worldbody)
-            if (shell_mode && m->body_dofnum[m->flex_nodebodyid[nstart + nid]] == 0) {
-              continue;
-            }
-            if (ww > w) {
-              w = ww;
-              nodeid = nid;
-            }
-          }
-          flexbodyid = m->flex_nodebodyid[nstart + nodeid];
-          if (m->flex_centered[i]) {
-            mju_copy3(flexpnt, d->xpos + 3*flexbodyid);
-          } else {
-            mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*(nstart + nodeid));
-            mju_addTo3(flexpnt, d->xpos + 3*flexbodyid);
-          }
-        } else {
-          flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
-          mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
-        }
         *flexid = i;
+        flexbodyid = mjv_flexBodyId(m, d, *flexid, vertid, flexpnt);
       }
     }
   }

src/engine/engine_vis_interact.h

@@ -78,6 +78,10 @@ MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
                      const mjvScene* scn, mjtNum selpnt[3],
                      int geomid[1], int flexid[1], int skinid[1]);
 
+// return body id, compute position of a vertex in a flex
+MJAPI int mjv_flexBodyId(const mjModel* m, const mjData* d, int flexid, int vertid,
+                         mjtNum flexpnt[3]);
+
 #ifdef __cplusplus
 }
 #endif

src/experimental/platform/ux/interaction.cc

@@ -21,6 +21,7 @@
 #include <mujoco/mujoco.h>
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"
+#include "engine/engine_vis_interact.h"
 #include "engine/engine_vis_visualize.h"
 
 namespace mujoco::platform {
@@ -376,36 +377,8 @@ static PickResult PickFlex(const mjModel* m, const mjData* d,
     }
 
     result.dist = test_dist;
-    if (m->flex_interp[i]) {
-      const mjtNum* coord = m->flex_vert0 + 3 * (m->flex_vertadr[i] + vertid);
-      mjtNum w = 0;
-      int nodeid = -1;
-      int nstart = m->flex_nodeadr[i];
-      int nend = nstart + m->flex_nodenum[i];
-      for (int j = nstart; j < nend; j++) {
-        if (mju_evalBasis(coord, j - nstart, m->flex_interp[i]) > w) {
-          w = mju_evalBasis(coord, j - nstart, m->flex_interp[i]);
-          nodeid = j;
-        }
-      }
-      if (nodeid < 0) {
-        mjERROR("flex %d: node closest to vertex %d not found", i, vertid);
-      }
-      result.body = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid];
-
-      if (m->flex_centered[i]) {
-        mju_copy3(result.point, d->xpos + 3 * result.body);
-      } else {
-        mju_mulMatVec3(result.point, d->xmat + 9 * result.body,
-                       m->flex_node + 3 * nodeid);
-        mju_addTo3(result.point, d->xpos + 3 * result.body);
-      }
-    } else {
-      result.body = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
-      mju_copy3(result.point,
-                d->flexvert_xpos + 3 * (m->flex_vertadr[i] + vertid));
-    }
     result.flex = i;
+    result.body = mjv_flexBodyId(m, d, i, vertid, result.point);
   }
   return result;
 }