Skip to content

Commit 5c1f1f9

Browse files
quaglacopybara-github
quagla
authored and
copybara-github
committed
Add octree to meshes of SDF geoms.
PiperOrigin-RevId: 777634862 Change-Id: I3210c2f8d73fd30ceecb0a11abd1d82e086b3d5d
1 parent 106df98 commit 5c1f1f9

File tree

17 files changed

+368
-11
lines changed

17 files changed

+368
-11
lines changed

‎doc/includes/references.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -959,6 +959,7 @@ struct mjModel_ {
959959
int nbvh; // number of total bounding volumes in all bodies
960960
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
961961
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
962+
int noct; // number of total octree cells in all meshes
962963
int njnt; // number of joints
963964
int ngeom; // number of geoms
964965
int nsite; // number of sites
@@ -1092,6 +1093,11 @@ struct mjModel_ {
10921093
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
10931094
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
10941095

1096+
// octree spatial partitioning
1097+
int* oct_depth; // depth in the octree (noct x 1)
1098+
int* oct_child; // children of octree node (noct x 8)
1099+
mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6)
1100+
10951101
// joints
10961102
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
10971103
int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1)
@@ -1279,6 +1285,8 @@ struct mjModel_ {
12791285
int* mesh_facenum; // number of faces (nmesh x 1)
12801286
int* mesh_bvhadr; // address of bvh root (nmesh x 1)
12811287
int* mesh_bvhnum; // number of bvh (nmesh x 1)
1288+
int* mesh_octadr; // address of octree root (nmesh x 1)
1289+
int* mesh_octnum; // number of octree nodes (nmesh x 1)
12821290
int* mesh_normaladr; // first normal address (nmesh x 1)
12831291
int* mesh_normalnum; // number of normals (nmesh x 1)
12841292
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
@@ -2876,6 +2884,7 @@ struct mjvOption_ { // abstract visualization options
28762884
mjtByte skingroup[mjNGROUP]; // skin visualization by group
28772885
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
28782886
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
2887+
int oct_depth; // depth of the octree to be visualized
28792888
int flex_layer; // element layer to be visualized for 3D flex
28802889
};
28812890
typedef struct mjvOption_ mjvOption;

‎include/mujoco/mjmodel.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -631,6 +631,7 @@ struct mjModel_ {
631631
int nbvh; // number of total bounding volumes in all bodies
632632
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
633633
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
634+
int noct; // number of total octree cells in all meshes
634635
int njnt; // number of joints
635636
int ngeom; // number of geoms
636637
int nsite; // number of sites
@@ -764,6 +765,11 @@ struct mjModel_ {
764765
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
765766
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
766767

768+
// octree spatial partitioning
769+
int* oct_depth; // depth in the octree (noct x 1)
770+
int* oct_child; // children of octree node (noct x 8)
771+
mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6)
772+
767773
// joints
768774
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
769775
int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1)
@@ -951,6 +957,8 @@ struct mjModel_ {
951957
int* mesh_facenum; // number of faces (nmesh x 1)
952958
int* mesh_bvhadr; // address of bvh root (nmesh x 1)
953959
int* mesh_bvhnum; // number of bvh (nmesh x 1)
960+
int* mesh_octadr; // address of octree root (nmesh x 1)
961+
int* mesh_octnum; // number of octree nodes (nmesh x 1)
954962
int* mesh_normaladr; // first normal address (nmesh x 1)
955963
int* mesh_normalnum; // number of normals (nmesh x 1)
956964
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)

‎include/mujoco/mjvisualize.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -292,6 +292,7 @@ struct mjvOption_ { // abstract visualization options
292292
mjtByte skingroup[mjNGROUP]; // skin visualization by group
293293
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
294294
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
295+
int oct_depth; // depth of the octree to be visualized
295296
int flex_layer; // element layer to be visualized for 3D flex
296297
};
297298
typedef struct mjvOption_ mjvOption;

‎include/mujoco/mjxmacro.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,7 @@
7575
X( nbvh ) \
7676
X( nbvhstatic ) \
7777
X( nbvhdynamic ) \
78+
X( noct ) \
7879
X( njnt ) \
7980
X( ngeom ) \
8081
X( nsite ) \
@@ -216,6 +217,9 @@
216217
X ( int, bvh_child, nbvh, 2 ) \
217218
X ( int, bvh_nodeid, nbvh, 1 ) \
218219
X ( mjtNum, bvh_aabb, nbvhstatic, 6 ) \
220+
X ( int, oct_depth, noct, 1 ) \
221+
X ( int, oct_child, noct, 8 ) \
222+
X ( mjtNum, oct_aabb, noct, 6 ) \
219223
X ( int, jnt_type, njnt, 1 ) \
220224
X ( int, jnt_qposadr, njnt, 1 ) \
221225
X ( int, jnt_dofadr, njnt, 1 ) \
@@ -390,6 +394,8 @@
390394
X ( int, mesh_facenum, nmesh, 1 ) \
391395
X ( int, mesh_bvhadr, nmesh, 1 ) \
392396
X ( int, mesh_bvhnum, nmesh, 1 ) \
397+
X ( int, mesh_octadr, nmesh, 1 ) \
398+
X ( int, mesh_octnum, nmesh, 1 ) \
393399
X ( int, mesh_graphadr, nmesh, 1 ) \
394400
X ( mjtNum, mesh_scale, nmesh, 3 ) \
395401
X ( mjtNum, mesh_pos, nmesh, 3 ) \

‎model/plugin/sdf/octree.xml‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<mujoco>
2+
<compiler texturedir="asset"/>
3+
4+
<asset>
5+
<texture name="texspot" type="2d" file="spot.png"/>
6+
<material name="matspot" texture="texspot"/>
7+
<mesh name="spot" file="asset/spot.obj"/>
8+
</asset>
9+
10+
<option sdf_iterations="20" sdf_initpoints="40"/>
11+
12+
<visual>
13+
<map force="1000"/>
14+
</visual>
15+
16+
<default>
17+
<geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.5"/>
18+
</default>
19+
20+
<statistic meansize="0.2"/>
21+
22+
<include file="scene.xml"/>
23+
24+
<worldbody>
25+
<body euler="90 0 0" pos="0 0 .7">
26+
<geom type="sdf" name="cow1" mesh="spot" material="matspot"/>
27+
</body>
28+
<light name="left" pos="0 0 1"/>
29+
<light name="right" pos="1 0 1"/>
30+
</worldbody>
31+
</mujoco>

‎python/mujoco/introspect/structs.py‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -873,6 +873,11 @@
873873
type=ValueType(name='int'),
874874
doc='number of dynamic bounding volumes (aabb stored in mjData)', # pylint: disable=line-too-long
875875
),
876+
StructFieldDecl(
877+
name='noct',
878+
type=ValueType(name='int'),
879+
doc='number of total octree cells in all meshes',
880+
),
876881
StructFieldDecl(
877882
name='njnt',
878883
type=ValueType(name='int'),
@@ -1560,6 +1565,30 @@
15601565
doc='local bounding box (center, size)',
15611566
array_extent=('nbvhstatic', 6),
15621567
),
1568+
StructFieldDecl(
1569+
name='oct_depth',
1570+
type=PointerType(
1571+
inner_type=ValueType(name='int'),
1572+
),
1573+
doc='depth in the octree',
1574+
array_extent=('noct',),
1575+
),
1576+
StructFieldDecl(
1577+
name='oct_child',
1578+
type=PointerType(
1579+
inner_type=ValueType(name='int'),
1580+
),
1581+
doc='children of octree node',
1582+
array_extent=('noct', 8),
1583+
),
1584+
StructFieldDecl(
1585+
name='oct_aabb',
1586+
type=PointerType(
1587+
inner_type=ValueType(name='mjtNum'),
1588+
),
1589+
doc='octree node bounding box (center, size)',
1590+
array_extent=('noct', 6),
1591+
),
15631592
StructFieldDecl(
15641593
name='jnt_type',
15651594
type=PointerType(
@@ -2920,6 +2949,22 @@
29202949
doc='number of bvh',
29212950
array_extent=('nmesh',),
29222951
),
2952+
StructFieldDecl(
2953+
name='mesh_octadr',
2954+
type=PointerType(
2955+
inner_type=ValueType(name='int'),
2956+
),
2957+
doc='address of octree root',
2958+
array_extent=('nmesh',),
2959+
),
2960+
StructFieldDecl(
2961+
name='mesh_octnum',
2962+
type=PointerType(
2963+
inner_type=ValueType(name='int'),
2964+
),
2965+
doc='number of octree nodes',
2966+
array_extent=('nmesh',),
2967+
),
29232968
StructFieldDecl(
29242969
name='mesh_normaladr',
29252970
type=PointerType(
@@ -6873,6 +6918,11 @@
68736918
type=ValueType(name='int'),
68746919
doc='depth of the bounding volume hierarchy to be visualized',
68756920
),
6921+
StructFieldDecl(
6922+
name='oct_depth',
6923+
type=ValueType(name='int'),
6924+
doc='depth of the octree to be visualized',
6925+
),
68766926
StructFieldDecl(
68776927
name='flex_layer',
68786928
type=ValueType(name='int'),

‎python/mujoco/structs.cc‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1110,6 +1110,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
11101110
X(label);
11111111
X(frame);
11121112
X(bvh_depth);
1113+
X(oct_depth);
11131114
X(flex_layer);
11141115
#undef X
11151116

‎src/engine/engine_io.c‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -460,7 +460,7 @@ static void freeModelBuffers(mjModel* m) {
460460
// allocate and initialize mjModel structure
461461
void mj_makeModel(mjModel** dest,
462462
int nq, int nv, int nu, int na, int nbody, int nbvh,
463-
int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
463+
int nbvhstatic, int nbvhdynamic, int noct, int njnt, int ngeom, int nsite, int ncam,
464464
int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem,
465465
int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord,
466466
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
@@ -499,6 +499,7 @@ void mj_makeModel(mjModel** dest,
499499
m->nbvh = nbvh;
500500
m->nbvhstatic = nbvhstatic;
501501
m->nbvhdynamic = nbvhdynamic;
502+
m->noct = noct;
502503
m->njnt = njnt;
503504
m->ngeom = ngeom;
504505
m->nsite = nsite;
@@ -639,7 +640,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
639640
if (!dest) {
640641
mj_makeModel(&dest,
641642
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
642-
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
643+
src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ngeom, src->nsite,
643644
src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge,
644645
src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata,
645646
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
@@ -835,7 +836,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
835836
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
836837
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
837838
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
838-
ints[63], ints[64], ints[65], ints[66], ints[67], ints[68]);
839+
ints[63], ints[64], ints[65], ints[66], ints[67], ints[68], ints[69]);
839840
if (!m || m->nbuffer != sizes[getnsize()-1]) {
840841
mju_warning("Corrupted model, wrong size parameters");
841842
mj_deleteModel(m);

‎src/engine/engine_io.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ void mj_defaultStatistic(mjStatistic* stat);
5252

5353
// allocate mjModel
5454
void mj_makeModel(mjModel** dest,
55-
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
55+
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int noct,
5656
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert,
5757
int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata,
5858
int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal,

‎src/engine/engine_vis_visualize.c‎

Lines changed: 38 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -763,7 +763,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
763763
if (vopt->flags[mjVIS_MESHBVH]) {
764764
for (int geomid = 0; geomid < m->ngeom; geomid++) {
765765
int meshid = m->geom_dataid[geomid];
766-
if (meshid == -1) {
766+
if (m->geom_type[geomid] == mjGEOM_SDF || meshid == -1) {
767767
continue;
768768
}
769769

@@ -806,6 +806,43 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
806806
}
807807
}
808808

809+
// mesh octree
810+
category = mjCAT_DECOR;
811+
objtype = mjOBJ_UNKNOWN;
812+
if (vopt->flags[mjVIS_MESHBVH]) {
813+
for (int geomid = 0; geomid < m->ngeom; geomid++) {
814+
int meshid = m->geom_dataid[geomid];
815+
if (m->geom_type[geomid] != mjGEOM_SDF || meshid == -1) {
816+
continue;
817+
}
818+
819+
for (int b = 0; b < m->mesh_octnum[meshid]; b++) {
820+
int i = b + m->mesh_octadr[meshid];
821+
822+
START
823+
if (m->oct_depth[i] != vopt->bvh_depth) {
824+
continue;
825+
}
826+
827+
// box color
828+
const float* rgba = m->vis.rgba.bv;
829+
830+
// get xpos, xmat, size
831+
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
832+
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
833+
const mjtNum *size = m->oct_aabb + 6*i + 3;
834+
835+
// offset xpos with aabb center (not always at geom origin)
836+
const mjtNum *center = m->oct_aabb + 6*i;
837+
mjtNum pos[3];
838+
mju_mulMatVec3(pos, xmat, center);
839+
mju_addTo3(pos, xpos);
840+
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
841+
FINISH
842+
}
843+
}
844+
}
845+
809846
// inertia
810847
objtype = mjOBJ_BODY;
811848
if (vopt->flags[mjVIS_INERTIA]) {

0 commit comments

Comments
 (0)