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

Add mesh BVH.

Browse Source 
PiperOrigin-RevId: 530559807
Change-Id: Ibdafae22c7c74d15d707f205be73e4a571a6ae10
This commit is contained in:
Alessio Quaglino
2023-05-09 03:30:25 -07:00
committed by Copybara-Service
parent 49066e1794
commit 4c487e07ed
13 changed files with 242 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+3
View File
@@ -989,6 +989,8 @@ struct mjModel_ {
int* mesh_vertnum; // number of vertices (nmesh x 1)
int* mesh_faceadr; // first face address (nmesh x 1)
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_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)
@@ -1666,6 +1668,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjVIS_MESHBVH, // mesh bounding volume hierarchy
mjNVISFLAG // number of visualization flags
} mjtVisFlag;
include/mujoco/mjmodel.h
+2
View File
@@ -771,6 +771,8 @@ struct mjModel_ {
int* mesh_vertnum; // number of vertices (nmesh x 1)
int* mesh_faceadr; // first face address (nmesh x 1)
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_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
@@ -123,6 +123,7 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjVIS_MESHBVH, // mesh bounding volume hierarchy
mjNVISFLAG // number of visualization flags
} mjtVisFlag;
include/mujoco/mjxmacro.h
+2
View File
@@ -281,6 +281,8 @@
X ( int, mesh_texcoordnum, nmesh, 1 ) \
X ( int, mesh_faceadr, nmesh, 1 ) \
X ( int, mesh_facenum, nmesh, 1 ) \
X ( int, mesh_bvhadr, nmesh, 1 ) \
X ( int, mesh_bvhnum, nmesh, 1 ) \
XMJV( int, mesh_graphadr, nmesh, 1 ) \
X ( float, mesh_vert, nmeshvert, 3 ) \
X ( float, mesh_normal, nmeshnormal, 3 ) \
introspect/enums.py
+2 -1
View File
@@ -523,7 +523,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjVIS_STATIC', 21),
('mjVIS_SKIN', 22),
('mjVIS_MIDPHASE', 23),
('mjNVISFLAG', 24),
('mjVIS_MESHBVH', 24),
('mjNVISFLAG', 25),
]),
)),
('mjtRndFlag',
introspect/structs.py
+15 -1
View File
@@ -1993,6 +1993,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='number of faces (nmesh x 1)',
),
StructFieldDecl(
name='mesh_bvhadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address of bvh root (nmesh x 1)',
),
StructFieldDecl(
name='mesh_bvhnum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of bvh (nmesh x 1)',
),
StructFieldDecl(
name='mesh_normaladr',
type=PointerType(
@@ -4758,7 +4772,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
name='flags',
type=ArrayType(
inner_type=ValueType(name='mjtByte'),
extents=(24,),
extents=(25,),
),
doc='visualization flags (indexed by mjtVisFlag)',
),
src/engine/engine_vis_init.c
+2 -1
View File
@@ -89,7 +89,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"},
{"Body Tree", "0", "`"}
{"Body Tree", "0", "`"},
{"Mesh Tree", "0", "1"}
};
src/engine/engine_vis_visualize.c
+86 -29
View File
@@ -458,6 +458,57 @@ static int bodycategory(const mjModel* m, int bodyid) {
}
}
// draw bounding box
static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn,
const mjtNum aabb[6], const mjtNum xpos[3],
const mjtNum xmat[9], const float rgba[4],
int i, int objtype, int category) {
mjtNum x[3];
mjtNum dist[3][3];
if (xmat != NULL) {
mju_rotVecMat(x, aabb, xmat);
mju_addTo3(x, xpos);
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
dist[k][j] = aabb[k+3] * xmat[3*j+k];
}
}
} else {
mju_copy3(x, aabb);
mju_addTo3(x, xpos);
for (int j=0; j<3; j++) {
mju_zero3(dist[j]);
dist[j][j] = aabb[j+3];
}
}
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
}
}
}
}
// add abstract geoms
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
@@ -536,51 +587,57 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
// 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.");
if (++bodyid >= m->nbody) {
break;
}
}
// stop after body bvh are finished
if (bodyid >= m->nbody) {
break;
}
// 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];
}
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
}
}
// mesh bounding volume hierarchy
if (vopt->flags[mjVIS_MESHBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int geomid = 0; geomid < m->ngeom; geomid++) {
int meshid = m->geom_dataid[geomid];
if (meshid == -1) {
continue;
}
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
for (int b = 0; b < m->mesh_bvhnum[meshid]; b++) {
int i = b + m->mesh_bvhadr[meshid];
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] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// compute transformation
const mjtNum *aabb = m->bvh_aabb + 6*i;
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
}
}
}
src/user/user_mesh.cc
+92
View File
@@ -113,6 +113,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
szgraph = 0;
vert = NULL;
normal = NULL;
center = NULL;
texcoord = NULL;
face = NULL;
facenormal = NULL;
@@ -153,6 +154,7 @@ mjCMesh::~mjCMesh() {
if (vert) mju_free(vert);
if (normal) mju_free(normal);
if (texcoord) mju_free(texcoord);
if (center) mju_free(center);
if (face) mju_free(face);
if (facenormal) mju_free(facenormal);
if (facetexcoord) mju_free(facetexcoord);
@@ -370,6 +372,50 @@ void mjCMesh::Compile(int vfs_provider) {
// scale, center, orient, compute mass and inertia
Process();
processed = true;
// no radii: make
if (!center) {
MakeCenter();
}
// make bounding volume hierarchy
if (tree.bvh.empty()) {
face_aabb.assign(6*nface, 0);
for (int i=0; i<nface; i++) {
tree.AddBundingVolume(GetBoundingVolume(i));
}
tree.CreateBVH();
}
}
// get bounding volume
mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
mjCBoundingVolume node;
node.id = faceid;
node.conaffinity = 1;
node.contype = 1;
node.pos = center + 3*faceid;
node.quat = NULL;
mjtNum AABB[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) {
int vertid = face[3*faceid+j];
AABB[0] = mjMIN(AABB[0], vert[3*vertid+0]);
AABB[1] = mjMIN(AABB[1], vert[3*vertid+1]);
AABB[2] = mjMIN(AABB[2], vert[3*vertid+2]);
AABB[3] = mjMAX(AABB[3], vert[3*vertid+0]);
AABB[4] = mjMAX(AABB[4], vert[3*vertid+1]);
AABB[5] = mjMAX(AABB[5], vert[3*vertid+2]);
}
face_aabb[6*faceid+0] = .5 * (AABB[0] + AABB[3]);
face_aabb[6*faceid+1] = .5 * (AABB[1] + AABB[4]);
face_aabb[6*faceid+2] = .5 * (AABB[2] + AABB[5]);
face_aabb[6*faceid+3] = .5 * (AABB[3] - AABB[0]);
face_aabb[6*faceid+4] = .5 * (AABB[4] - AABB[1]);
face_aabb[6*faceid+5] = .5 * (AABB[5] - AABB[2]);
node.aabb = face_aabb.data() + 6*faceid;
return node;
}
@@ -1488,6 +1534,52 @@ void mjCMesh::MakeNormal(void) {
// compute face circumradii
void mjCMesh::MakeCenter(void) {
if (center) {
return;
}
// allocate and clear
center = (double*) mju_malloc(3*nface*sizeof(double));
memset(center, 0, 3*nface*sizeof(double));
for (int i=0; i<nface; i++) {
// get vertex ids
int* vertid = face + 3*i;
// get triangle edges
mjtNum a[3], b[3], c[3];
for (int j=0; j<3; j++) {
a[j] = vert[3*vertid[0]+j] - vert[3*vertid[2]+j];
b[j] = vert[3*vertid[1]+j] - vert[3*vertid[2]+j];
c[j] = vert[3*vertid[0]+j] - vert[3*vertid[1]+j];
}
// compute face normal
mjtNum nrm[3];
mju_cross(nrm, a, b);
// compute circumradius
mjtNum norm_a_2 = mju_dot3(a, a);
mjtNum norm_b_2 = mju_dot3(b, b);
mjtNum area = mju_norm3(nrm);
// compute circumcenter
mjtNum res[3], vec[3] = {
norm_a_2 * b[0] - norm_b_2 * a[0],
norm_a_2 * b[1] - norm_b_2 * a[1],
norm_a_2 * b[2] - norm_b_2 * a[2]
};
mju_cross(res, vec, nrm);
center[3*i+0] = res[0]/(2*area*area) + vert[3*vertid[2]+0];
center[3*i+1] = res[1]/(2*area*area) + vert[3*vertid[2]+1];
center[3*i+2] = res[2]/(2*area*area) + vert[3*vertid[2]+2];
}
}
//------------------ class mjCSkin implementation --------------------------------------------------
// constructor
src/user/user_model.cc
+12 -1
View File
@@ -939,6 +939,7 @@ void mjCModel::SetSizes(void) {
nmeshface += meshes[i]->nface;
nmeshtexcoord += (meshes[i]->texcoord ? meshes[i]->ntexcoord : 0);
nmeshgraph += meshes[i]->szgraph;
nbvh += meshes[i]->tree.nbvh;
}
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
@@ -1685,7 +1686,7 @@ void mjCModel::CopyTree(mjModel* m) {
// copy objects outside kinematic tree
void mjCModel::CopyObjects(mjModel* m) {
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
int bonevert_adr, graph_adr, data_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr=0;
// sizes outside call to mj_makeModel
m->nemax = nemax;
@@ -1700,6 +1701,9 @@ void mjCModel::CopyObjects(mjModel* m) {
texcoord_adr = 0;
face_adr = 0;
graph_adr = 0;
for (int i=0; i<nbody; i++) {
bvh_adr = mju_max(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
}
for (int i=0; i<nmesh; i++) {
// get pointer
mjCMesh* pme = meshes[i];
@@ -1714,6 +1718,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_faceadr[i] = face_adr;
m->mesh_facenum[i] = pme->nface;
m->mesh_graphadr[i] = (pme->szgraph ? graph_adr : -1);
m->mesh_bvhadr[i] = bvh_adr;
m->mesh_bvhnum[i] = pme->tree.nbvh;
// copy vertices, normals, faces, texcoords, aux data
memcpy(m->mesh_vert + 3*vert_adr, pme->vert, 3*pme->nvert*sizeof(float));
@@ -1729,6 +1735,10 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pme->szgraph) {
memcpy(m->mesh_graph + graph_adr, pme->graph, pme->szgraph*sizeof(int));
}
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree.bvh.data(), 6*pme->tree.nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pme->tree.child.data(), 2*pme->tree.nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree.level.data(), pme->tree.nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pme->tree.nodeid.data(), pme->tree.nbvh*sizeof(int));
// advance counters
vert_adr += pme->nvert;
@@ -1736,6 +1746,7 @@ void mjCModel::CopyObjects(mjModel* m) {
texcoord_adr += (pme->texcoord ? pme->ntexcoord : 0);
face_adr += pme->nface;
graph_adr += pme->szgraph;
bvh_adr += pme->tree.nbvh;
}
// skins
src/user/user_objects.cc
+10 -13
View File
@@ -321,11 +321,14 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
vert[2] = (v&4 ? aabb[5] : aabb[2]);
// rotate to the body inertial frame
mju_rotVecQuat(box, vert, elements[i].quat);
box[0] += elements[i].pos[0] - ipos_[0];
box[1] += elements[i].pos[1] - ipos_[1];
box[2] += elements[i].pos[2] - ipos_[2];
mju_rotVecQuat(vert, box, qinv);
if (elements[i].quat) {
mju_rotVecQuat(box, vert, elements[i].quat);
box[0] += elements[i].pos[0] - ipos_[0];
box[1] += elements[i].pos[1] - ipos_[1];
box[2] += elements[i].pos[2] - ipos_[2];
mju_rotVecQuat(vert, box, qinv);
}
AABB[0] = mjMIN(AABB[0], vert[0]);
AABB[1] = mjMIN(AABB[1], vert[1]);
AABB[2] = mjMIN(AABB[2], vert[2]);
@@ -342,18 +345,12 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
nodeid.push_back(-1);
level.push_back(lev);
// transform representation
mjtNum center[] = {(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2,
(AABB[5] + AABB[2]) / 2};
mjtNum size[] = {(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2,
(AABB[5] - AABB[2]) / 2};
// store bounding box of the current node
for (int i=0; i<3; i++) {
bvh.push_back(center[i]);
bvh.push_back((AABB[3+i] + AABB[i]) / 2);
}
for (int i=0; i<3; i++) {
bvh.push_back(size[i]);
bvh.push_back((AABB[3+i] - AABB[i]) / 2);
}
// leaf node, return
src/user/user_objects.h
+10 -2
View File
@@ -120,6 +120,7 @@ class mjCAlternative {
//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
// bounding volume
class mjCBoundingVolume {
public:
@@ -334,7 +335,6 @@ class mjCGeom : public mjCBase {
friend class mjCModel;
friend class mjXWriter;
friend class mjXURDF;
friend class mjCBoundingVolumeHierarchy;
public:
double GetVolume(void); // compute geom volume
@@ -524,7 +524,10 @@ class mjCMesh: public mjCBase {
double* GetQuatPtr(mjtMeshType type); // get orientation
double* GetInertiaBoxPtr(mjtMeshType type); // get inertia box
double& GetVolumeRef(mjtMeshType type); // get volume
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
// returns a bounding volume given a face
mjCBoundingVolume GetBoundingVolume(int faceid);
std::string file; // mesh file
double refpos[3]; // reference position (translate)
@@ -550,6 +553,7 @@ class mjCMesh: public mjCBase {
void MakeGraph(void); // make graph of convex hull
void CopyGraph(void); // copy graph into face data
void MakeNormal(void); // compute vertex normals
void MakeCenter(void); // compute face circumcircle data
void Process(); // apply transformations
void RemoveRepeated(void); // remove repeated vertices
void CheckMesh(void); // check if the mesh is valid
@@ -585,6 +589,7 @@ class mjCMesh: public mjCBase {
int szgraph; // size of graph data in ints
float* vert; // vertex data (3*nvert), relative to (pos, quat)
float* normal; // vertex normal data (3*nnormal)
double* center; // face circumcenter data (3*nface)
float* texcoord; // vertex texcoord data (2*ntexcoord or NULL)
int* face; // face vertex indices (3*nface)
int* facenormal; // face normal indices (3*nface)
@@ -592,6 +597,9 @@ class mjCMesh: public mjCBase {
int* graph; // convex graph data
bool needhull; // needs convex hull for collisions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
std::vector<double> face_aabb; // bounding boxes of all faces
};
unity/Runtime/Bindings/MjBindings.cs
+5 -2
View File
@@ -487,7 +487,8 @@ public enum mjtVisFlag : int{
mjVIS_STATIC = 21,
mjVIS_SKIN = 22,
mjVIS_MIDPHASE = 23,
mjNVISFLAG = 24,
mjVIS_MESHBVH = 24,
mjNVISFLAG = 25,
}
public enum mjtRndFlag : int{
mjRND_SHADOW = 0,
@@ -2067,6 +2068,8 @@ public unsafe struct mjModel_ {
public int* mesh_vertnum;
public int* mesh_faceadr;
public int* mesh_facenum;
public int* mesh_bvhadr;
public int* mesh_bvhnum;
public int* mesh_normaladr;
public int* mesh_normalnum;
public int* mesh_texcoordadr;
@@ -2574,7 +2577,7 @@ public unsafe struct mjvOption_ {
public fixed byte tendongroup[6];
public fixed byte actuatorgroup[6];
public fixed byte skingroup[6];
public fixed byte flags[24];
public fixed byte flags[25];
public int bvh_depth;
}