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

Add octree to meshes of SDF geoms.

Browse Source 
PiperOrigin-RevId: 777634862
Change-Id: I3210c2f8d73fd30ceecb0a11abd1d82e086b3d5d
This commit is contained in:
Alessio Quaglino
2025-06-30 10:49:08 -07:00
committed by Copybara-Service
parent 106df98946
commit 5c1f1f9dba
17 changed files with 369 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+9
View File
@@ -959,6 +959,7 @@ struct mjModel_ {
int nbvh; // number of total bounding volumes in all bodies
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
int noct; // number of total octree cells in all meshes
int njnt; // number of joints
int ngeom; // number of geoms
int nsite; // number of sites
@@ -1092,6 +1093,11 @@ struct mjModel_ {
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
// octree spatial partitioning
int* oct_depth; // depth in the octree (noct x 1)
int* oct_child; // children of octree node (noct x 8)
mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1)
@@ -1279,6 +1285,8 @@ struct mjModel_ {
int* mesh_facenum; // number of faces (nmesh x 1)
int* mesh_bvhadr; // address of bvh root (nmesh x 1)
int* mesh_bvhnum; // number of bvh (nmesh x 1)
int* mesh_octadr; // address of octree root (nmesh x 1)
int* mesh_octnum; // number of octree nodes (nmesh x 1)
int* mesh_normaladr; // first normal address (nmesh x 1)
int* mesh_normalnum; // number of normals (nmesh x 1)
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
@@ -2876,6 +2884,7 @@ struct mjvOption_ { // abstract visualization options
mjtByte skingroup[mjNGROUP]; // skin visualization by group
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int oct_depth; // depth of the octree to be visualized
int flex_layer; // element layer to be visualized for 3D flex
};
typedef struct mjvOption_ mjvOption;
include/mujoco/mjmodel.h
+8
View File
@@ -631,6 +631,7 @@ struct mjModel_ {
int nbvh; // number of total bounding volumes in all bodies
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
int noct; // number of total octree cells in all meshes
int njnt; // number of joints
int ngeom; // number of geoms
int nsite; // number of sites
@@ -764,6 +765,11 @@ struct mjModel_ {
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
// octree spatial partitioning
int* oct_depth; // depth in the octree (noct x 1)
int* oct_child; // children of octree node (noct x 8)
mjtNum* oct_aabb; // octree node bounding box (center, size) (noct x 6)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1)
@@ -951,6 +957,8 @@ struct mjModel_ {
int* mesh_facenum; // number of faces (nmesh x 1)
int* mesh_bvhadr; // address of bvh root (nmesh x 1)
int* mesh_bvhnum; // number of bvh (nmesh x 1)
int* mesh_octadr; // address of octree root (nmesh x 1)
int* mesh_octnum; // number of octree nodes (nmesh x 1)
int* mesh_normaladr; // first normal address (nmesh x 1)
int* mesh_normalnum; // number of normals (nmesh x 1)
int* mesh_texcoordadr; // texcoord data address; -1: no texcoord (nmesh x 1)
include/mujoco/mjvisualize.h
+1
View File
@@ -292,6 +292,7 @@ struct mjvOption_ { // abstract visualization options
mjtByte skingroup[mjNGROUP]; // skin visualization by group
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int oct_depth; // depth of the octree to be visualized
int flex_layer; // element layer to be visualized for 3D flex
};
typedef struct mjvOption_ mjvOption;
include/mujoco/mjxmacro.h
+6
View File
@@ -75,6 +75,7 @@
X( nbvh ) \
X( nbvhstatic ) \
X( nbvhdynamic ) \
X( noct ) \
X( njnt ) \
X( ngeom ) \
X( nsite ) \
@@ -216,6 +217,9 @@
X ( int, bvh_child, nbvh, 2 ) \
X ( int, bvh_nodeid, nbvh, 1 ) \
X ( mjtNum, bvh_aabb, nbvhstatic, 6 ) \
X ( int, oct_depth, noct, 1 ) \
X ( int, oct_child, noct, 8 ) \
X ( mjtNum, oct_aabb, noct, 6 ) \
X ( int, jnt_type, njnt, 1 ) \
X ( int, jnt_qposadr, njnt, 1 ) \
X ( int, jnt_dofadr, njnt, 1 ) \
@@ -390,6 +394,8 @@
X ( int, mesh_facenum, nmesh, 1 ) \
X ( int, mesh_bvhadr, nmesh, 1 ) \
X ( int, mesh_bvhnum, nmesh, 1 ) \
X ( int, mesh_octadr, nmesh, 1 ) \
X ( int, mesh_octnum, nmesh, 1 ) \
X ( int, mesh_graphadr, nmesh, 1 ) \
X ( mjtNum, mesh_scale, nmesh, 3 ) \
X ( mjtNum, mesh_pos, nmesh, 3 ) \
model/plugin/sdf/octree.xml
+31
View File
@@ -0,0 +1,31 @@
<mujoco>
<compiler texturedir="asset"/>
<asset>
<texture name="texspot" type="2d" file="spot.png"/>
<material name="matspot" texture="texspot"/>
<mesh name="spot" file="asset/spot.obj"/>
</asset>
<option sdf_iterations="20" sdf_initpoints="40"/>
<visual>
<map force="1000"/>
</visual>
<default>
<geom solref="0.01 1" solimp=".95 .99 .0001" friction="0.5"/>
</default>
<statistic meansize="0.2"/>
<include file="scene.xml"/>
<worldbody>
<body euler="90 0 0" pos="0 0 .7">
<geom type="sdf" name="cow1" mesh="spot" material="matspot"/>
</body>
<light name="left" pos="0 0 1"/>
<light name="right" pos="1 0 1"/>
</worldbody>
</mujoco>
python/mujoco/introspect/structs.py
+50
View File
@@ -873,6 +873,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='number of dynamic bounding volumes (aabb stored in mjData)', # pylint: disable=line-too-long
),
StructFieldDecl(
name='noct',
type=ValueType(name='int'),
doc='number of total octree cells in all meshes',
),
StructFieldDecl(
name='njnt',
type=ValueType(name='int'),
@@ -1560,6 +1565,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='local bounding box (center, size)',
array_extent=('nbvhstatic', 6),
),
StructFieldDecl(
name='oct_depth',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='depth in the octree',
array_extent=('noct',),
),
StructFieldDecl(
name='oct_child',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='children of octree node',
array_extent=('noct', 8),
),
StructFieldDecl(
name='oct_aabb',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='octree node bounding box (center, size)',
array_extent=('noct', 6),
),
StructFieldDecl(
name='jnt_type',
type=PointerType(
@@ -2920,6 +2949,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='number of bvh',
array_extent=('nmesh',),
),
StructFieldDecl(
name='mesh_octadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address of octree root',
array_extent=('nmesh',),
),
StructFieldDecl(
name='mesh_octnum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of octree nodes',
array_extent=('nmesh',),
),
StructFieldDecl(
name='mesh_normaladr',
type=PointerType(
@@ -6873,6 +6918,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='depth of the bounding volume hierarchy to be visualized',
),
StructFieldDecl(
name='oct_depth',
type=ValueType(name='int'),
doc='depth of the octree to be visualized',
),
StructFieldDecl(
name='flex_layer',
type=ValueType(name='int'),
python/mujoco/structs.cc
+1
View File
@@ -1110,6 +1110,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(label);
X(frame);
X(bvh_depth);
X(oct_depth);
X(flex_layer);
#undef X
src/engine/engine_io.c
+4 -3
View File
@@ -460,7 +460,7 @@ static void freeModelBuffers(mjModel* m) {
// allocate and initialize mjModel structure
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh,
int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
int nbvhstatic, int nbvhdynamic, int noct, int njnt, int ngeom, int nsite, int ncam,
int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem,
int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
@@ -499,6 +499,7 @@ void mj_makeModel(mjModel** dest,
m->nbvh = nbvh;
m->nbvhstatic = nbvhstatic;
m->nbvhdynamic = nbvhdynamic;
m->noct = noct;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
@@ -639,7 +640,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
if (!dest) {
mj_makeModel(&dest,
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
@@ -835,7 +836,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63], ints[64], ints[65], ints[66], ints[67], ints[68]);
ints[63], ints[64], ints[65], ints[66], ints[67], ints[68], ints[69]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_warning("Corrupted model, wrong size parameters");
mj_deleteModel(m);
src/engine/engine_io.h
+1 -1
View File
@@ -52,7 +52,7 @@ void mj_defaultStatistic(mjStatistic* stat);
// allocate mjModel
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic, int noct,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata,
int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal,
src/engine/engine_vis_visualize.c
+38 -1
View File
@@ -763,7 +763,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_MESHBVH]) {
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
if (meshid == -1) {
if (m->geom_type[geomid] == mjGEOM_SDF || meshid == -1) {
continue;
}
@@ -806,6 +806,43 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
}
// mesh octree
category = mjCAT_DECOR;
objtype = mjOBJ_UNKNOWN;
if (vopt->flags[mjVIS_MESHBVH]) {
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
if (m->geom_type[geomid] != mjGEOM_SDF || meshid == -1) {
continue;
}
for (int b = 0; b < m->mesh_octnum[meshid]; b++) {
int i = b + m->mesh_octadr[meshid];
START
if (m->oct_depth[i] != vopt->bvh_depth) {
continue;
}
// box color
const float* rgba = m->vis.rgba.bv;
// get xpos, xmat, size
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
const mjtNum *size = m->oct_aabb + 6*i + 3;
// offset xpos with aabb center (not always at geom origin)
const mjtNum *center = m->oct_aabb + 6*i;
mjtNum pos[3];
mju_mulMatVec3(pos, xmat, center);
mju_addTo3(pos, xpos);
mjv_initGeom(thisgeom, mjGEOM_LINEBOX, size, pos, xmat, rgba);
FINISH
}
}
}
// inertia
objtype = mjOBJ_BODY;
if (vopt->flags[mjVIS_INERTIA]) {
src/user/user_mesh.cc
+12
View File
@@ -139,6 +139,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
maxhullvert_ = -1;
processed_ = false;
visual_ = true;
needoct_ = false;
// reset to default if given
if (_def) {
@@ -406,6 +407,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
}
mesh->tree_ = tree_;
mesh->face_aabb_ = face_aabb_;
mesh->octree_ = octree_;
// calculate estimated size of mesh
std::size_t size = sizeof(mjCMesh)
@@ -423,6 +425,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
+ (sizeof(double) * 18)
+ (sizeof(int) * ncenter)
+ tree_.Size()
+ octree_.Size()
+ (sizeof(double) * face_aabb_.size());
std::shared_ptr<const void> cached_data(mesh, +[] (const void* data) {
@@ -682,6 +685,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
}
}
// make octree
if (!needoct_) {
octree_.Clear();
} else if (octree_.Nodes().empty()) {
octree_.SetFace(vert_, face_);
octree_.CreateOctree(aamm_);
}
// close resource
if (resource_ != nullptr) {
mju_closeResource(resource_);
@@ -1073,6 +1084,7 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) {
}
tree_ = mesh->tree_;
face_aabb_ = mesh->face_aabb_;
octree_ = mesh->octree_;
return true;
};
src/user/user_model.cc
+17 -2
View File
@@ -963,6 +963,7 @@ void mjCModel::Clear() {
nbvh = 0;
nbvhstatic = 0;
nbvhdynamic = 0;
noct = 0;
njnt = 0;
ngeom = 0;
nsite = 0;
@@ -1726,6 +1727,7 @@ void mjCModel::IndexAssets(bool discard) {
((mjCMesh*)mesh)->SetNotVisual(); // reset to true by mesh->Compile()
}
geom->mesh = (discard && geom->visual_) ? nullptr : (mjCMesh*)mesh;
static_cast<mjCMesh*>(mesh)->needoct_ |= geom->spec.type == mjGEOM_SDF;
} else {
throw mjCError(geom, "mesh '%s' not found in geom %d", geom->get_meshname().c_str(), i);
}
@@ -1936,6 +1938,7 @@ void mjCModel::SetSizes() {
}
for (int i=0; i < nmesh; i++) {
nbvhstatic += meshes_[i]->tree().Nbvh();
noct += meshes_[i]->octree().NumNodes();
}
for (int i=0; i < nflex; i++) {
nbvhdynamic += flexes_[i]->tree.Nbvh();
@@ -2943,7 +2946,7 @@ int mjCModel::CountNJmom(const mjModel* m) {
// copy objects outside kinematic tree
void mjCModel::CopyObjects(mjModel* m) {
int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr, oct_adr;
int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr;
int poly_adr, polymap_adr, polyvert_adr;
@@ -2963,6 +2966,7 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// meshes
oct_adr = 0;
vert_adr = 0;
normal_adr = 0;
texcoord_adr = 0;
@@ -2989,6 +2993,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
m->mesh_bvhnum[i] = pme->tree().Nbvh();
m->mesh_bvhadr[i] = pme->tree().Nbvh() ? bvh_adr : -1;
m->mesh_octnum[i] = pme->octree().NumNodes();
m->mesh_octadr[i] = pme->octree().NumNodes() ? oct_adr : -1;
mjuu_copyvec(&m->mesh_scale[3 * i], pme->Scale(), 3);
mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetPosPtr(), 3);
mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetQuatPtr(), 4);
@@ -3023,6 +3029,14 @@ void mjCModel::CopyObjects(mjModel* m) {
}
}
// copy octree data
if (pme->octree().NumNodes()) {
int n_oct = pme->octree().NumNodes();
memcpy(m->oct_aabb + 6*oct_adr, pme->octree().Nodes().data(), 6*n_oct*sizeof(mjtNum));
memcpy(m->oct_child + 8*oct_adr, pme->octree().Child().data(), 8*n_oct*sizeof(int));
memcpy(m->oct_depth + oct_adr, pme->octree().Level().data(), n_oct*sizeof(int));
}
// advance counters
poly_adr += pme->npolygon();
polyvert_adr += pme->npolygonvert();
@@ -3033,6 +3047,7 @@ void mjCModel::CopyObjects(mjModel* m) {
face_adr += pme->nface();
graph_adr += pme->szgraph();
bvh_adr += pme->tree().Nbvh();
oct_adr += pme->octree().NumNodes();
}
// flexes
@@ -4523,7 +4538,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// create low-level model
mj_makeModel(&m,
nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ngeom, nsite,
ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly,
src/user/user_model.h
+1
View File
@@ -87,6 +87,7 @@ class mjCModel_ : public mjsElement {
int nbvh; // number of total boundary volume hierarchies
int nbvhstatic; // number of static boundary volume hierarchies
int nbvhdynamic; // number of dynamic boundary volume hierarchies
int noct; // number of total octree cells
int nflexnode; // number of nodes in all flexes
int nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
src/user/user_objects.cc
+109 -4
View File
@@ -404,9 +404,15 @@ mjCBoundingVolumeHierarchy::AddBoundingVolume(const int* id, int contype, int co
// create bounding volume hierarchy
void mjCBoundingVolumeHierarchy::CreateBVH() {
std::vector<BVElement> elements;
Make(elements);
MakeBVH(elements.begin(), elements.end());
}
void mjCBoundingVolumeHierarchy::Make(std::vector<BVElement>& elements) {
// precompute the positions of each element in the hierarchy's axes, and drop
// visual-only elements.
std::vector<BVElement> elements;
elements.reserve(bvleaf_.size());
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
for (int i = 0; i < bvleaf_.size(); i++) {
@@ -420,9 +426,9 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
elements.push_back(std::move(element));
}
}
MakeBVH(elements.begin(), elements.end());
}
// compute bounding volume hierarchy
int mjCBoundingVolumeHierarchy::MakeBVH(
std::vector<BVElement>::iterator elements_begin,
@@ -546,10 +552,109 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
return index;
}
//------------------------- class mjCOctree implementation --------------------------------------------
void mjCOctree::SetFace(const std::vector<double>& vert, const std::vector<int>& face) {
for (int i = 0; i < face.size(); i += 3) {
std::array<double, 3> v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]};
std::array<double, 3> v1 = {vert[3*face[i+1]], vert[3*face[i+1]+1], vert[3*face[i+1]+2]};
std::array<double, 3> v2 = {vert[3*face[i+2]], vert[3*face[i+2]+1], vert[3*face[i+2]+2]};
face_.push_back({v0, v1, v2});
}
}
// TODO: use the same code as mjCBoundingVolumeHierarchy::Make()
void mjCOctree::Make(std::vector<Triangle>& elements) {
// rotate triangles to the body inertial frame
elements.assign(face_.size(), {{{0}}});
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
for (int i = 0; i < face_.size(); i++) {
for (int j = 0; j < 3; j++) {
double vert[3] = {face_[i][j][0] - ipos_[0],
face_[i][j][1] - ipos_[1],
face_[i][j][2] - ipos_[2]};
mjuu_rotVecQuat(elements[i][j].data(), vert, qinv);
}
}
}
void mjCOctree::CreateOctree(const double aamm[6]) {
std::vector<Triangle> elements;
Make(elements);
std::vector<Triangle*> elements_ptrs(elements.size());
std::transform(elements.begin(), elements.end(), elements_ptrs.begin(),
[](Triangle& triangle) { return &triangle; });
MakeOctree(elements_ptrs, aamm);
}
static bool boxTriangle(const Triangle& element, const double aamm[6]) {
for (int i = 0; i < 3; i++) {
if (element[0][i] < aamm[i] && element[1][i] < aamm[i] && element[2][i] < aamm[i]) {
return false;
}
int j = i + 3;
if (element[0][i] > aamm[j] && element[1][i] > aamm[j] && element[2][i] > aamm[j]) {
return false;
}
}
// TODO: add additionally separating axis tests
return true;
}
int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev) {
level_.push_back(lev);
// create a new node
int index = nnode_++;
double aabb[6] = {(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, (aamm[2] + aamm[5]) / 2,
(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2};
for (int i = 0; i < 6; i++) {
node_.push_back(aabb[i]);
}
for (int i = 0; i < 8; i++) {
child_.push_back(-1);
}
// find all triangles that intersect the current box
std::vector<Triangle*> colliding;
for (auto* element : elements) {
if (boxTriangle(*element, aamm)) {
colliding.push_back(element);
}
}
// return if the box is empty
if (colliding.empty() || lev >= 6) {
return index;
}
// split the box into 8 sub-boxes
double new_aamm[8][6];
for (int i = 0; i < 8; i++) {
new_aamm[i][0] = aabb[0] + aabb[3] * (i & 1 ? -1 : 0);
new_aamm[i][1] = aabb[1] + aabb[4] * (i & 2 ? -1 : 0);
new_aamm[i][2] = aabb[2] + aabb[5] * (i & 4 ? -1 : 0);
new_aamm[i][3] = aabb[0] + aabb[3] * (i & 1 ? 0 : 1);
new_aamm[i][4] = aabb[1] + aabb[4] * (i & 2 ? 0 : 1);
new_aamm[i][5] = aabb[2] + aabb[5] * (i & 4 ? 0 : 1);
}
// recursive calls to create sub-boxes
for (int i = 0; i < 8; i++) {
child_[8*index + i] = MakeOctree(colliding, new_aamm[i], lev + 1);
}
return index;
}
//------------------------- class mjCDef implementation --------------------------------------------
// constructor
mjCDef::mjCDef() {
name.clear();
src/user/user_objects.h
+50 -1
View File
@@ -206,13 +206,54 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
// position of the element in the BVH axes
double lpos[3];
};
void Make(std::vector<BVElement>& elements);
int MakeBVH(std::vector<BVElement>::iterator elements_begin,
std::vector<BVElement>::iterator elements_end, int lev = 0);
};
//------------------------- class mjCOctree --------------------------------------------------------
typedef std::array<std::array<double, 3>, 3> Triangle;
struct mjCOctree_ {
int nnode_ = 0;
std::vector<int> child_; // children of each node (nnode x 8)
std::vector<double> node_; // bounding boxes (nnode x 6)
std::vector<int> level_; // levels of each node (nnode x 1)
std::vector<Triangle> face_; // mesh faces (nface x 3)
double ipos_[3] = {0, 0, 0};
double iquat_[4] = {1, 0, 0, 0};
};
class mjCOctree : public mjCOctree_ {
public:
void CreateOctree(const double aamm[6]);
int NumNodes() const { return nnode_; }
const std::vector<int>& Child() const { return child_; }
const std::vector<double>& Nodes() const { return node_; }
const std::vector<int>& Level() const { return level_; }
void SetFace(const std::vector<double>& vert, const std::vector<int>& face);
int Size() const {
return sizeof(int) * child_.size() + sizeof(double) * node_.size() +
sizeof(int) * level_.size() + sizeof(Triangle) * face_.size();
}
void Clear() {
child_.clear();
node_.clear();
level_.clear();
face_.clear();
}
private:
void Make(std::vector<Triangle>& elements);
int MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev = 0);
};
//------------------------- class mjCBase ----------------------------------------------------------
// Generic functionality for all derived classes
@@ -965,6 +1006,7 @@ class mjCMesh_ : public mjCBase {
std::vector<int> spec_facetexcoord_;
// used by the compiler
bool needoct_; // needs octree
bool visual_; // true: the mesh is only visual
std::vector< std::pair<int, int> > halfedge_; // half-edge data
@@ -985,9 +1027,13 @@ class mjCMesh_ : public mjCBase {
bool needhull_; // needs convex hull for collisions
int maxhullvert_; // max vertex count of convex hull
// bounding volume hierarchy tree
mjCBoundingVolumeHierarchy tree_; // bounding volume hierarchy
std::vector<double> face_aabb_; // bounding boxes of all faces
// octree
mjCOctree octree_; // octree of the mesh
// paths stored during model attachment
mujoco::user::FilePath modelfiledir_;
mujoco::user::FilePath meshdir_;
@@ -1061,6 +1107,9 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
// bounding volume hierarchy tree
const mjCBoundingVolumeHierarchy& tree() { return tree_; }
// octree
const mjCOctree& octree() { return octree_; }
void Compile(const mjVFS* vfs); // compiler
double* GetPosPtr(); // get position
double* GetQuatPtr(); // get orientation
test/user/user_mesh_test.cc
+24
View File
@@ -1264,6 +1264,30 @@ TEST_F(MjCMeshTest, LoadSkin) {
mj_deleteSpec(spec);
}
// ------------- test octree ---------------------------------------------------
TEST_F(MjCMeshTest, Octree) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->noct, 0);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
unity/Runtime/Bindings/MjBindings.cs
+7
View File
@@ -5242,6 +5242,7 @@ public unsafe struct mjModel_ {
public int nbvh;
public int nbvhstatic;
public int nbvhdynamic;
public int noct;
public int njnt;
public int ngeom;
public int nsite;
@@ -5358,6 +5359,9 @@ public unsafe struct mjModel_ {
public int* bvh_child;
public int* bvh_nodeid;
public double* bvh_aabb;
public int* oct_depth;
public int* oct_child;
public double* oct_aabb;
public int* jnt_type;
public int* jnt_qposadr;
public int* jnt_dofadr;
@@ -5528,6 +5532,8 @@ public unsafe struct mjModel_ {
public int* mesh_facenum;
public int* mesh_bvhadr;
public int* mesh_bvhnum;
public int* mesh_octadr;
public int* mesh_octnum;
public int* mesh_normaladr;
public int* mesh_normalnum;
public int* mesh_texcoordadr;
@@ -6111,6 +6117,7 @@ public unsafe struct mjvOption_ {
public fixed byte skingroup[6];
public fixed byte flags[32];
public int bvh_depth;
public int oct_depth;
public int flex_layer;
}