diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 300db93e..9c18bba6 100644 --- a/src/engine/engine_vis_interact.c +++ b/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); } } } diff --git a/src/engine/engine_vis_interact.h b/src/engine/engine_vis_interact.h index 0135f086..88d018d2 100644 --- a/src/engine/engine_vis_interact.h +++ b/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 diff --git a/src/experimental/platform/ux/interaction.cc b/src/experimental/platform/ux/interaction.cc index 646684b2..bf3cfc08 100644 --- a/src/experimental/platform/ux/interaction.cc +++ b/src/experimental/platform/ux/interaction.cc @@ -21,6 +21,7 @@ #include #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; }