diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index fba730a6..d3f8f597 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -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); } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 616154f9..0da63e06 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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 diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 964ff6ed..23f7cae1 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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& vert, const std::vector& face) { for (int i = 0; i < face.size(); i += 3) { std::array 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& 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 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& 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; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 16d70763..4b2a403d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -215,15 +215,23 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { //------------------------- class mjCOctree -------------------------------------------------------- -typedef std::array, 3> Triangle; +typedef std::array Point; +typedef std::array Triangle; + +struct OctNode { + int level = 0; // level of the node + std::array child = {-1}; // children nodes + std::array vertid = {-1}; // vertex id's + std::array aabb = {0}; // bounding box + std::array coeff = {0}; // interpolation coefficients +}; struct mjCOctree_ { int nnode_ = 0; - std::vector child_; // children of each node (nnode x 8) - std::vector node_; // bounding boxes (nnode x 6) - std::vector level_; // levels of each node (nnode x 1) - std::vector coeff_; // interpo coefficients (nnode x 8) - std::vector face_; // mesh faces (nface x 3) + int nvert_ = 0; + std::vector node_; + std::vector face_; // mesh faces (nmeshface x 3) + std::vector 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& Child() const { return child_; } - const std::vector& Nodes() const { return node_; } - const std::vector& 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& vert, const std::vector& 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& Coeff() const { return coeff_; } + void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; } private: void Make(std::vector& elements); diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 3cc8fbc4..bfefa0e3 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -14,6 +14,7 @@ // Tests for user/user_objects.cc. +#include #include #include #include @@ -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"( + + + + + + + + + )"; + std::array 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"(