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

Refactor octree in preparation for continuous interpolation.

Browse Source 
PiperOrigin-RevId: 792092644
Change-Id: I096ca0989210a00ae0b4556498d96212755d23c7
This commit is contained in:
Alessio Quaglino
2025-08-07 04:04:59 -07:00
committed by Copybara-Service
parent 4e67fe79f3
commit 840108cf7c
5 changed files with 123 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_mesh.cc
+3 -7
View File
@@ -772,7 +772,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// make octree
if (!needsdf) {
octree_.Clear(); // this occurs when a non-SDF mesh is loaded from a cached SDF mesh
} else if (octree_.Nodes().empty()) {
} else if (octree_.NumNodes() == 0) {
octree_.SetFace(vert_, face_);
octree_.CreateOctree(aamm_);
@@ -782,14 +782,10 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// TODO: do not evaluate the SDF multiple times at the same vertex
// TODO: the value at hanging vertices should be computed from the parent
const double* nodes = octree_.Nodes().data();
for (int i = 0; i < octree_.NumNodes(); ++i) {
for (int j = 0; j < 8; j++) {
mjtNum v[3];
v[0] = nodes[6*i+0] + (j&1 ? 1 : -1) * nodes[6*i+3];
v[1] = nodes[6*i+1] + (j&2 ? 1 : -1) * nodes[6*i+4];
v[2] = nodes[6*i+2] + (j&4 ? 1 : -1) * nodes[6*i+5];
octree_.AddCoeff(sdf.signed_distance(v).distance);
const double* v = octree_.Vert(i, j);
octree_.AddCoeff(i, j, sdf.signed_distance(v).distance);
}
}
}
src/user/user_model.cc
+4 -9
View File
@@ -3001,15 +3001,10 @@ 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));
if (!pme->octree().Coeff().empty()) {
memcpy(m->oct_coeff + 8*oct_adr, pme->octree().Coeff().data(), 8*n_oct*sizeof(mjtNum));
} else {
mjuu_zerovec(m->oct_coeff + 8*oct_adr, 8*n_oct);
}
pme->octree().CopyAabb(m->oct_aabb + 6*oct_adr);
pme->octree().CopyChild(m->oct_child + 8*oct_adr);
pme->octree().CopyLevel(m->oct_depth + oct_adr);
pme->octree().CopyCoeff(m->oct_coeff + 8*oct_adr);
}
// advance counters
src/user/user_objects.cc
+45 -8
View File
@@ -557,6 +557,36 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
//------------------------- class mjCOctree implementation --------------------------------------------
void mjCOctree::CopyLevel(int* level) const {
for (int i = 0; i < node_.size(); ++i) {
level[i] = node_[i].level;
}
}
void mjCOctree::CopyChild(int* child) const {
for (int i = 0; i < node_.size(); ++i) {
for (int j = 0; j < 8; ++j) {
child[i * 8 + j] = node_[i].child[j];
}
}
}
void mjCOctree::CopyAabb(mjtNum* aabb) const {
for (int i = 0; i < node_.size(); ++i) {
for (int j = 0; j < 6; ++j) {
aabb[i * 6 + j] = node_[i].aabb[j];
}
}
}
void mjCOctree::CopyCoeff(mjtNum* coeff) const {
for (int i = 0; i < node_.size(); ++i) {
for (int j = 0; j < 8; ++j) {
coeff[i * 8 + j] = node_[i].coeff[j];
}
}
}
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]};
@@ -655,17 +685,24 @@ static bool boxTriangle(const Triangle& v, const double aamm[6]) {
int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev) {
level_.push_back(lev);
node_.push_back(OctNode());
OctNode& node = node_.back();
node.level = 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]);
}
std::array<double, 6> aabb = {
(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};
node.aabb = aabb;
for (int i = 0; i < 8; i++) {
child_.push_back(-1);
Point v = {(i & 1) ? aamm[3] : aamm[0],
(i & 2) ? aamm[4] : aamm[1],
(i & 4) ? aamm[5] : aamm[2]};
vert_.push_back(v);
node.vertid[i] = nvert_++;
node.child[i] = -1;
}
// find all triangles that intersect the current box
@@ -694,7 +731,7 @@ int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double a
// recursive calls to create sub-boxes
for (int i = 0; i < 8; i++) {
child_[8*index + i] = MakeOctree(colliding, new_aamm[i], lev + 1);
node_[index].child[i] = MakeOctree(colliding, new_aamm[i], lev + 1);
}
return index;
src/user/user_objects.h
+23 -15
View File
@@ -215,15 +215,23 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
//------------------------- class mjCOctree --------------------------------------------------------
typedef std::array<std::array<double, 3>, 3> Triangle;
typedef std::array<double, 3> Point;
typedef std::array<Point, 3> Triangle;
struct OctNode {
int level = 0; // level of the node
std::array<int, 8> child = {-1}; // children nodes
std::array<int, 8> vertid = {-1}; // vertex id's
std::array<double, 6> aabb = {0}; // bounding box
std::array<double, 8> coeff = {0}; // interpolation coefficients
};
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<double> coeff_; // interpo coefficients (nnode x 8)
std::vector<Triangle> face_; // mesh faces (nface x 3)
int nvert_ = 0;
std::vector<OctNode> node_;
std::vector<Triangle> face_; // mesh faces (nmeshface x 3)
std::vector<Point> vert_; // octree vertices (nvert x 3)
double ipos_[3] = {0, 0, 0};
double iquat_[4] = {1, 0, 0, 0};
};
@@ -233,22 +241,22 @@ class mjCOctree : public mjCOctree_ {
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_; }
int NumVerts() const { return nvert_; }
void CopyLevel(int* level) const;
void CopyChild(int* child) const;
void CopyAabb(mjtNum* aabb) const;
void CopyCoeff(mjtNum* coeff) const;
const double* Vert(int n, int v) const { return vert_[node_[n].vertid[v]].data(); }
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();
return sizeof(OctNode) * node_.size() + sizeof(Triangle) * face_.size() +
sizeof(Point) * vert_.size();
}
void Clear() {
child_.clear();
node_.clear();
level_.clear();
face_.clear();
}
void AddCoeff(double coeff) { coeff_.push_back(coeff); }
const std::vector<double>& Coeff() const { return coeff_; }
void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; }
private:
void Make(std::vector<Triangle>& elements);
test/user/user_mesh_test.cc
+48
View File
@@ -14,6 +14,7 @@
// Tests for user/user_objects.cc.
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
@@ -1287,6 +1288,53 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
mj_deleteModel(model);
}
double CubeSDF(double p[3], double b[3]) {
double q[3] = {std::abs(p[0]) - b[0],
std::abs(p[1]) - b[1],
std::abs(p[2]) - b[2]};
return std::sqrt(std::pow(std::max(q[0], 0.0), 2) +
std::pow(std::max(q[1], 0.0), 2) +
std::pow(std::max(q[2], 0.0), 2)) +
std::min(std::max(q[0], std::max(q[1], q[2])), 0.0);
}
TEST_F(MjCMeshTest, OctreeCube) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="-1 -1 -1 1 -1 -1 -1 1 -1 1 1 -1
-1 -1 1 1 -1 1 -1 1 1 1 1 1"/>
</asset>
<worldbody>
<geom mesh="mesh" type="sdf"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->noct, 54089);
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mjSDF sdf;
int instance = 0;
mjtGeom geomtype = mjGEOM_SDF;
const mjpPlugin* sdf_ptr = NULL;
sdf.id = &instance;
sdf.type = mjSDFTYPE_SINGLE;
sdf.plugin = &sdf_ptr;
sdf.geomtype = &geomtype;
mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
mjtNum size[3] = {1, 1, 1};
for (int i = 0; i < 3; ++i) {
EXPECT_NEAR(mjc_distance(m, d, &sdf, pnt[i]), CubeSDF(pnt[i], size), 1e-1)
<< "i = " << i;
}
mj_deleteModel(m);
mj_deleteData(d);
}
TEST_F(MjCMeshTest, HemisphereSizes) {
static constexpr char xml[] = R"(
<mujoco model="makemesh">