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

Use body AABB for torque perturbation visualisation, if available.

Browse Source 
PiperOrigin-RevId: 569579715
Change-Id: I62c1083fb868520832746ca1f28fab2bf1d679cc
This commit is contained in:
Yuval Tassa
2023-09-29 13:49:20 -07:00
committed by Copybara-Service
parent ebded4747f
commit e1549376f6
1 changed files with 14 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+14 -3
View File
@@ -774,10 +774,21 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
(pert->active & mjPERT_ROTATE) > 0,
(pert->active2 & mjPERT_ROTATE) > 0);
// construct geom
sz[0] = sz[1] = sz[2] = scl;
// construct geom: if body i has a collision aabb, use that
mjtNum pos[3] = {0};
if (m->body_bvhnum[i]) {
mjtNum* aabb = m->bvh_aabb+6*m->body_bvhadr[i];
mju_copy3(sz, aabb+3);
mju_rotVecMat(pos, aabb, d->ximat+9*i);
}
// otherwise box of size meansize
else {
sz[0] = sz[1] = sz[2] = scl;
}
mju_quat2Mat(mat, pert->refquat);
mjv_initGeom(thisgeom, mjGEOM_BOX, sz, d->xipos+3*i, mat, rgba);
mju_addTo3(pos, d->xipos+3*i);
mjv_initGeom(thisgeom, mjGEOM_BOX, sz, pos, mat, rgba);
FINISH
}