chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Expose mjv_flexBodyIdAndPoint as a helper function.

Browse Source 
Use the above in studio rather than a custom implementation.

PiperOrigin-RevId: 928432476
Change-Id: I99d772c4baf7260abbf9ac8f6d8032704f1ba769
This commit is contained in:
Haroon Qureshi
2026-06-08 02:27:55 -07:00
committed by Copybara-Service
parent ea2d785e0a
commit b17b5f7918
3 changed files with 53 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_interact.c
+47 -40
View File
@@ -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
+4
View File
@@ -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
+2 -29
View File
@@ -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;
}