Avoid duplicated vertices in the SDF octree.

Before: nnode: 42777, nvert: 342216
After: nnode: 42777, nvert: 77588
PiperOrigin-RevId: 792115571
Change-Id: I17913ed3d22f22a0370dc4cbd2b833a0f5983cda

src/user/user_mesh.cc

@@ -780,7 +780,6 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
     if (!plugin.active) {
       tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface());
 
-      // TODO: do not evaluate the SDF multiple times at the same vertex
       // TODO: the value at hanging vertices should be computed from the parent
       for (int i = 0; i < octree_.NumNodes(); ++i) {
         for (int j = 0; j < 8; j++) {

src/user/user_objects.cc

@@ -23,7 +23,6 @@
 #include <cstring>
 #include <functional>
 #include <limits>
-#include <map>
 #include <memory>
 #include <new>
 #include <optional>
@@ -31,6 +30,7 @@
 #include <sstream>
 #include <string>
 #include <string_view>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -623,7 +623,8 @@ void mjCOctree::CreateOctree(const double aamm[6]) {
   std::vector<Triangle*> elements_ptrs(elements.size());
   std::transform(elements.begin(), elements.end(), elements_ptrs.begin(),
                  [](Triangle& triangle) { return &triangle; });
-  MakeOctree(elements_ptrs, box);
+  std::unordered_map<Point, int> vert_map;
+  MakeOctree(elements_ptrs, box, 0, vert_map);
 }
 
 
@@ -684,7 +685,8 @@ static bool boxTriangle(const Triangle& v, const double aamm[6]) {
 }
 
 
-int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev) {
+int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev,
+                          std::unordered_map<Point, int>& vert_map) {
   node_.push_back(OctNode());
   OctNode& node = node_.back();
   node.level = lev;
@@ -697,11 +699,17 @@ int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double a
       (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2};
   node.aabb = aabb;
   for (int i = 0; i < 8; i++) {
-    Point v = {(i & 1) ? aamm[3] : aamm[0],
+    Point v = {{(i & 1) ? aamm[3] : aamm[0],
-               (i & 2) ? aamm[4] : aamm[1],
+                (i & 2) ? aamm[4] : aamm[1],
-               (i & 4) ? aamm[5] : aamm[2]};
+                (i & 4) ? aamm[5] : aamm[2]}};
-    vert_.push_back(v);
+    auto it = vert_map.find(v);
-    node.vertid[i] = nvert_++;
+    if (it != vert_map.end()) {
+      node.vertid[i] = it->second;
+    } else {
+      node.vertid[i] = nvert_;
+      vert_map[v] = nvert_++;
+      vert_.push_back(v);
+    }
     node.child[i] = -1;
   }
 
@@ -731,7 +739,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++) {
-    node_[index].child[i] = MakeOctree(colliding, new_aamm[i], lev + 1);
+    node_[index].child[i] = MakeOctree(colliding, new_aamm[i], lev + 1, vert_map);
   }
 
   return index;

src/user/user_objects.h

@@ -17,10 +17,10 @@
 
 #include <stdbool.h>
 #include <algorithm>
+#include <cmath>
 #include <cstddef>
 #include <array>
 #include <functional>
-#include <map>
 #include <string>
 #include <string_view>
 #include <unordered_map>
@@ -215,8 +215,37 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
 
 //------------------------- class mjCOctree --------------------------------------------------------
 
-typedef std::array<double, 3> Point;
+struct Point {
-typedef std::array<Point, 3> Triangle;
+  std::array<double, 3> p;
+
+  double& operator[](size_t i) { return p[i]; }
+  const double& operator[](size_t i) const { return p[i]; }
+
+  bool operator==(const Point& other) const {
+    constexpr double kEpsilon = 1e-9;
+    return std::abs(this->p[0] - other.p[0]) < kEpsilon &&
+           std::abs(this->p[1] - other.p[1]) < kEpsilon &&
+           std::abs(this->p[2] - other.p[2]) < kEpsilon;
+  }
+};
+
+namespace std {
+template <>
+struct hash<Point> {
+  size_t operator()(const Point& pt) const {
+    size_t h1 = hash<double>()(pt.p[0]);
+    size_t h2 = hash<double>()(pt.p[1]);
+    size_t h3 = hash<double>()(pt.p[2]);
+    // combine hashes
+    size_t seed = h1;
+    seed ^= h2 + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+    seed ^= h3 + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+    return seed;
+  }
+};
+}  // namespace std
+
+typedef std::array<std::array<double, 3>, 3> Triangle;
 
 struct OctNode {
   int level = 0;                       // level of the node
@@ -246,7 +275,7 @@ class mjCOctree : public mjCOctree_ {
   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(); }
+  const double* Vert(int n, int v) const { return vert_[node_[n].vertid[v]].p.data(); }
   void SetFace(const std::vector<double>& vert, const std::vector<int>& face);
   int Size() const {
     return sizeof(OctNode) * node_.size() + sizeof(Triangle) * face_.size() +
@@ -260,7 +289,8 @@ class mjCOctree : public mjCOctree_ {
 
  private:
   void Make(std::vector<Triangle>& elements);
-  int MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev = 0);
+  int MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev,
+                 std::unordered_map<Point, int>& vert_map);
 };