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

Added a collision midphase on geom pairs using rotated AABBs static trees.

Browse Source 
PiperOrigin-RevId: 517357695
Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b
This commit is contained in:
Alessio Quaglino
2023-03-17 02:36:38 -07:00
committed by Copybara-Service
parent 47b58f5ccb
commit 70959c1a2c
30 changed files with 2228 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+327 -55
View File
@@ -19,7 +19,6 @@
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjxmacro.h>
#include "engine/engine_callback.h"
#include "engine/engine_collision_convex.h"
#include "engine/engine_collision_primitive.h"
@@ -48,8 +47,319 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
//------------------------------------ static functions --------------------------------------------
// plane to geom_center squared distance, g1 is a plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// bounding-sphere collision
static int mj_collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) {
// neither geom is a plane
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return 0;
}
}
// one geom is a plane
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return 0;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return 0;
}
return 1;
}
//------------------------------------ binary tree search ------------------------------------------
// checks if the proposed collision pair is already present in pair_geom and calls narrow phase
void mj_collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
found = 1;
break;
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// oriented bounding boxes collision (see Gottschalk et al.)
int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize) {
// get infinite dimensions (planes only)
mjtByte inf1[3] = {aabb1[3] >= mjMAXVAL, aabb1[4] >= mjMAXVAL, aabb1[5] >= mjMAXVAL};
mjtByte inf2[3] = {aabb2[3] >= mjMAXVAL, aabb2[4] >= mjMAXVAL, aabb2[5] >= mjMAXVAL};
// if a bounding box is infinite, there must be a collision
if ((inf1[0] && inf1[1] && inf1[2]) || (inf2[0] && inf2[1] && inf2[2])) {
return 1;
}
const mjtNum* aabb[2] = {aabb1, aabb2};
const mjtNum *xmat[2] = {xmat1, xmat2};
const mjtNum *xpos[2] = {xpos1, xpos2};
mjtNum xcenter[2][3], normal[2][3][3];
mjtNum proj[2], radius[2];
mjtByte infinite[2] = {inf1[0] || inf1[1] || inf1[2], inf2[0] || inf2[1] || inf2[2]};
// compute centers in local coordinates
if (product==NULL) {
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // axes
mju_rotVecMat(xcenter[i], aabb[i], xmat[i]);
mju_addTo3(xcenter[i], xpos[i]);
}
}
}
// compute normals in global coordinates
for (int i=0; i<2; i++) { // bounding boxes
for (int j=0; j<3; j++) { // faces
for (int k=0; k<3; k++) { // world axes
normal[i][j][k] = xmat[i][3*k+j];
}
}
}
// precompute dot products
if (product && offset && *initialize) {
for (int i=0; i<2; i++) { // bodies
for (int j=0; j<2; j++) { // bodies
for (int k=0; k<3; k++) { // axes
for (int l=0; l<3; l++) { // axes
product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]);
}
offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]);
}
}
}
*initialize = 0;
}
// check intersections
for (int j=0; j<2; j++) { // bounding boxes
if (infinite[1-j]) {
continue; // skip test against an infinite body
}
for (int k=0; k<3; k++) { // face
for (int i=0; i<2; i++) { // bounding boxes
if (product==NULL) {
proj[i] = mju_dot3(xcenter[i], normal[j][k]);
radius[i] = fabs(aabb[i][3]*mju_dot3(normal[i][0], normal[j][k])) +
fabs(aabb[i][4]*mju_dot3(normal[i][1], normal[j][k])) +
fabs(aabb[i][5]*mju_dot3(normal[i][2], normal[j][k]));
} else {
int adr = 18*i + 9*j + 3*k;
proj[i] = aabb[i][0] * product[adr + 0] +
aabb[i][1] * product[adr + 1] +
aabb[i][2] * product[adr + 2] +
offset[6*i + 3*j + k];
radius[i] = fabs(aabb[i][3]*product[adr + 0]) +
fabs(aabb[i][4]*product[adr + 1]) +
fabs(aabb[i][5]*product[adr + 2]);
}
}
if (radius[0]+radius[1] < fabs(proj[1]-proj[0])) {
return 0;
}
}
}
return 1;
}
static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) {
// check that the quotient is an integer
_Static_assert(sizeof(mjCollisionTree*) % sizeof(mjtNum) == 0,
"mjCollisionTree has a different size from mjtNum");
return (mjCollisionTree*)mj_stackAlloc(
d, max_stack * sizeof(mjCollisionTree*) / sizeof(mjtNum));
}
// binary search between two body trees
void mj_collideTree(const mjModel* m, mjData* d, int b1, int b2,
int merged, int startadr, int pairadr) {
const int bvhadr1 = m->body_bvhadr[b1];
const int bvhadr2 = m->body_bvhadr[b2];
const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1;
const mjtNum* bvh2 = m->bvh_aabb + 6 * bvhadr2;
const int* child1 = m->bvh_child + 2 * bvhadr1;
const int* child2 = m->bvh_child + 2 * bvhadr2;
mjtNum product[36] = {}; // 2 bb x 2 bb x 3 axes (body) x 3 axes (world)
mjtNum offset[12] = {}; // 2 bb x 2 bb x 3 axes (world)
mjtByte initialize = 1;
mjMARKSTACK;
// TODO(b/273737633): Store bvh max depths to make this bound tighter.
const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2];
mjCollisionTree* stack = mj_stackAllocTree(d, max_stack);
int nstack = 1;
stack[0].node1 = stack[0].node2 = 0;
while (nstack) {
// pop from stack
nstack--;
int node1 = stack[nstack].node1;
int node2 = stack[nstack].node2;
mjtByte isleaf1 = (child1[2*node1] == -1) && (child1[2*node1+1] == -1);
mjtByte isleaf2 = (child2[2*node2] == -1) && (child2[2*node2+1] == -1);
int nodeid1 = m->bvh_geomid[bvhadr1 + node1];
int nodeid2 = m->bvh_geomid[bvhadr2 + node2];
// both are leaves
if (isleaf1 && isleaf2 && nodeid1!=-1 && nodeid2!=-1) {
if (mj_collideSphere(m, d, nodeid1, nodeid2, /*margin=*/ 0)) {
if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2,
d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1,
d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2,
NULL, NULL, &initialize)) {
mj_collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
}
}
continue;
}
// if no intersection at intermediate levels, stop
if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2,
d->xipos + 3*b1, d->ximat + 9*b1,
d->xipos + 3*b2, d->ximat + 9*b2,
product, offset, &initialize)) {
continue;
}
d->bvh_active[node1 + bvhadr1] = 1;
d->bvh_active[node2 + bvhadr2] = 1;
// keep traversing the tree
if (!isleaf1 && isleaf2) {
for (int i=0; i<2; i++) {
if (child1[2*node1+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2*node1+i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else if (isleaf1 && !isleaf2) {
for (int i=0; i<2; i++) {
if (child2[2*node2+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
} else {
// compute surface areas of bounding boxes
mjtNum x1 = bvh1[6*node1+3]-bvh1[6*node1+0];
mjtNum y1 = bvh1[6*node1+4]-bvh1[6*node1+1];
mjtNum z1 = bvh1[6*node1+5]-bvh1[6*node1+2];
mjtNum x2 = bvh2[6*node2+3]-bvh2[6*node2+0];
mjtNum y2 = bvh2[6*node2+4]-bvh2[6*node2+1];
mjtNum z2 = bvh2[6*node2+5]-bvh2[6*node2+2];
mjtNum surface1 = x1*y1 + y1*z1 + z1*x1;
mjtNum surface2 = x2*y2 + y2*z2 + z2*x2;
// traverse the hierarchy whose bounding box has the larger surface area
if (surface1 > surface2) {
for (int i = 0; i < 2; i++) {
if (child1[2 * node1 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2 * node1 + i];
stack[nstack].node2 = node2;
nstack++;
}
}
} else {
for (int i = 0; i < 2; i++) {
if (child2[2 * node2 + i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
}
}
}
mjFREESTACK;
}
//----------------------------- collision detection entry point ------------------------------------
// compare contact pairs by their geom IDs
quicksortfunc(contactcompare, context, el1, el2) {
const mjModel* m = (const mjModel*) context;
mjContact* con1 = (mjContact*)el1;
mjContact* con2 = (mjContact*)el2;
// reproduce the order contacts without mj_collideTree
// normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type.
// here we undo this swapping for the purpose of sorting - needs to be done for each mjContact
int con1_g1 = con1->geom1;
int con1_g2 = con1->geom2;
if (m->geom_type[con1_g1] > m->geom_type[con1_g2]) {
int tmp = con1_g1;
con1_g1 = con1_g2;
con1_g2 = tmp;
}
int con2_g1 = con2->geom1;
int con2_g2 = con2->geom2;
if (m->geom_type[con2_g1] > m->geom_type[con2_g2]) {
int tmp = con2_g1;
con2_g1 = con2_g2;
con2_g2 = tmp;
}
if (con1_g1 < con2_g1) return -1;
if (con1_g1 > con2_g1) return 1;
if (con1_g2 < con2_g2) return -1;
if (con1_g2 > con2_g2) return 1;
return 0;
}
void mj_collision(const mjModel* m, mjData* d) {
int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
int nexclude = m->nexclude, npair = m->npair, nbodypair = ((m->nbody-1)*m->nbody)/2;
@@ -59,6 +369,9 @@ void mj_collision(const mjModel* m, mjData* d) {
// reset the size of the contact array
d->ncon = 0;
// reset the visualization flags
memset(d->bvh_active, 0, m->nbvh);
// return if disabled
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT)
|| m->nconmax==0 || m->nbody < 2) {
@@ -122,29 +435,17 @@ void mj_collision(const mjModel* m, mjData* d) {
// test all geom pairs within this body pair
if (m->body_geomnum[b1] && m->body_geomnum[b2]) {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
// merged: make sure geom pair is not repeated
if (merged) {
// find matching pair
int found = 0;
for (int k=startadr; k<pairadr; k++) {
if ((m->pair_geom1[k]==g1 && m->pair_geom2[k]==g2) ||
(m->pair_geom1[k]==g2 && m->pair_geom2[k]==g1)) {
found = 1;
break;
}
}
// not found: test
if (!found) {
mj_collideGeoms(m, d, g1, g2, 0, 0);
}
}
// not merged: always test
else {
mj_collideGeoms(m, d, g1, g2, 0, 0);
if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) {
int ncon_before = d->ncon;
mj_collideTree(m, d, b1, b2, merged, startadr, pairadr);
int ncon_after = d->ncon;
void* context = (void*) m;
mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
sizeof(mjContact), contactcompare, context);
} else {
for (g1=m->body_geomadr[b1]; g1<m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) {
for (g2=m->body_geomadr[b2]; g2<m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) {
mj_collidePair(m, d, g1, g2, merged, startadr, pairadr);
}
}
}
@@ -522,22 +823,6 @@ endbroad:
//----------------------------- narrow-phase collision detection -----------------------------------
// plane : geom_center distance, assuming g1 is plane
static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) {
mjtNum* mat1 = d->geom_xmat + 9*g1;
mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
mjtNum dif[3];
mju_sub3(dif, d->geom_xpos + 3*g2, d->geom_xpos + 3*g1);
return mju_dot3(dif, norm);
}
// squared Euclidean distance between 3D vectors
static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]};
return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2];
}
// test two geoms for collision, apply filters, add to contact list
// flg_user disables filters and uses usermargin
@@ -615,21 +900,8 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user,
}
// bounding sphere filter
if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) {
mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin;
if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) {
return;
}
}
// plane : bounding sphere filter
if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0
&& plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) {
return;
}
if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0
&& plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) {
return;
if (!mj_collideSphere(m, d, g1, g2, margin)) {
return;
}
// call collision detector to generate contacts
src/engine/engine_collision_driver.h
+14
View File
@@ -21,14 +21,28 @@
#ifdef __cplusplus
extern "C" {
#else
#include <stdalign.h>
#endif
struct mjCollisionTree_ {
alignas(mjtNum) int node1;
int node2;
};
typedef struct mjCollisionTree_ mjCollisionTree;
// collision function pointers and max contact pairs
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
// collision detection entry point
MJAPI void mj_collision(const mjModel* m, mjData* d);
// applies Separating Axis Theorem for rotated AABBs
MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
mjtNum product[36], mjtNum offset[12], mjtByte* initialize);
// broad phase collistion detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
src/engine/engine_io.c
+5 -3
View File
@@ -144,6 +144,7 @@ void mj_defaultVisual(mjVisual* vis) {
vis->global.offwidth = 640;
vis->global.offheight = 480;
vis->global.realtime = 1.0;
vis->global.treedepth = 1;
// rendering quality
vis->quality.shadowsize = 4096;
@@ -375,7 +376,7 @@ static int safeAddToBufferSize(intptr_t* offset, int* nbuffer, size_t type_size,
// allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -402,6 +403,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
m->nu = nu;
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
@@ -521,7 +523,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->njnt,
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->njnt,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
@@ -702,7 +704,7 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
info[28], info[29], info[30], info[31], info[32], info[33], info[34],
info[35], info[36], info[37], info[38], info[39], info[40], info[41],
info[42], info[43], info[44], info[45], info[46], info[47], info[48],
info[49], info[50], info[51]);
info[49], info[50], info[51], info[52]);
if (!m || m->nbuffer!=info[getnint()-1]) {
if (fp) {
fclose(fp);
src/engine/engine_io.h
+1 -1
View File
@@ -47,7 +47,7 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int njnt,
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
src/engine/engine_support.c
+2 -1
View File
@@ -53,7 +53,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = {
"Filterparent",
"Actuation",
"Refsafe",
"Sensor"
"Sensor",
"Midphase"
};
src/engine/engine_vis_init.c
+2 -1
View File
@@ -88,7 +88,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Center of &Mass", "0", "M"},
{"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"}
{"Skin", "1", ";"},
{"Body Tree", "0", "`"}
};
src/engine/engine_vis_visualize.c
+65
View File
@@ -520,6 +520,71 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// bounding volume hierarchy
if (vopt->flags[mjVIS_MIDPHASE]) {
int bodyid = 0;
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvh; i++) {
int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != m->vis.global.treedepth) {
if (!isleaf || m->bvh_depth[i] > m->vis.global.treedepth) {
continue;
}
}
// find geom number
int geomid = m->bvh_geomid[i];
while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) {
bodyid++;
if (bodyid >= m->nbody) {
mju_error("nbvh outside body range.");
}
}
// compute transformation
mjtNum *aabb = isleaf ? m->geom_aabb + 6*geomid : m->bvh_aabb + 6*i;
mjtNum x[3];
const mjtNum* xpos = isleaf ? d->geom_xpos + 3 * geomid : d->xipos + 3 * bodyid;
const mjtNum* xmat = isleaf ? d->geom_xmat + 9 * geomid : d->ximat + 9 * bodyid;
mju_rotVecMat(x, aabb, xmat);
mju_addTo3(x, xpos);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
mjtNum dist[3][3];
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
int split[3] = {1, 2, 4};
for (int v=0; v<8; v++) {
mjtNum from[3] = {x[0], x[1], x[2]};
for (int k=0; k<3; k++) {
mju_addToScl3(from, dist[k], v&split[k] ? 1 : -1);
}
mjtNum to[3];
for (int k=0; k<3; k++) {
mju_addScl3(to, from, dist[k], 2);
if (!(v&split[k])) {
START
mjv_makeConnector(thisgeom, mjGEOM_LINE, 2,
from[0], from[1], from[2],
to[0], to[1], to[2]);
f2f(thisgeom->rgba, rgba, 4);
FINISH
}
}
}
}
}
// inertia
objtype = mjOBJ_BODY;
if (vopt->flags[mjVIS_INERTIA]) {