chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
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
};