From 4ea8218466b6260ead4f885ab0c6171c2459886b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 16 May 2023 07:54:34 -0700 Subject: [PATCH] If inertia is being visualised, draw body frames at the inertial frame. PiperOrigin-RevId: 532458388 Change-Id: Ie58cea8dee4215ad9e40ef827361bc3793c8f449 --- src/engine/engine_vis_visualize.c | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index a5439de0..13335327 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -776,6 +776,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, continue; } + mjtNum* xmat = vopt->flags[mjVIS_INERTIA] ? d->ximat+9*i : d->xmat+9*i; + mjtNum* xpos = vopt->flags[mjVIS_INERTIA] ? d->xipos+3*i : d->xpos+3*i; + // draw the three axes (separate geoms) for (int j=0; j<3; j++) { START @@ -784,16 +787,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int k=0; k<3; k++) { axis[k] = (j==k ? sz[1] : 0); } - mju_mulMatVec(vec, d->xmat+9*i, axis, 3, 3); + mju_mulMatVec(vec, xmat, axis, 3, 3); // create a cylinder - mjv_makeConnector(thisgeom, mjGEOM_CYLINDER, sz[0], - d->xpos[3*i+0], - d->xpos[3*i+1], - d->xpos[3*i+2], - d->xpos[3*i+0] + vec[0], - d->xpos[3*i+1] + vec[1], - d->xpos[3*i+2] + vec[2]); + mjv_makeConnector(thisgeom, mjGEOM_CYLINDER, sz[0], xpos[0], xpos[1], xpos[2], + xpos[0] + vec[0], xpos[1] + vec[1], xpos[2] + vec[2]); // set color: R, G or B depending on axis for (int k=0; k<3; k++) {