Replace SdfLib with linear octree interpolation of TriangleMeshDistance.

Temporary changes to the octree:
- Changed frame from mesh to geom

Before change (tolerance 1e-3):
```
 Simulation time      : 2.34 s
 Steps per second     : 4275
 Realtime factor      : 8.55 x
 Time per step        : 233.9 µs

 Newton iters / step  : 2.26
 Contacts / step      : 3.69
 Constraints / step   : 14.76
 Degrees of freedom   : 12
 Dynamic memory usage : 0.7% of 14M
```

After change (6 octree levels):
```
 Simulation time      : 2.24 s
 Steps per second     : 4472
 Realtime factor      : 8.94 x
 Time per step        : 223.6 µs

 Newton iters / step  : 2.47
 Contacts / step      : 3.37
 Constraints / step   : 13.49
 Degrees of freedom   : 12
 Dynamic memory usage : 0.2% of 14M
```

PiperOrigin-RevId: 781019754
Change-Id: Ib15581244dfe9e571e1c6c2cac4a101cf3d8ba3d

src/user/user_mesh.cc

@@ -628,7 +628,8 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
     resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs);
 
     // try loading from cache
-    if (cache != nullptr && LoadCachedMesh(cache, resource_)) {
+    // TODO: move octree to mesh frame so it can be cached
+    if (cache != nullptr && !needoct_ && LoadCachedMesh(cache, resource_)) {
       mju_closeResource(resource_);
       resource_ = nullptr;
       fromCache = true;
@@ -684,14 +685,6 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
     }
   }
 
-  // make octree
-  if (!needoct_) {
-    octree_.Clear();
-  } else if (octree_.Nodes().empty()) {
-    octree_.SetFace(vert_, face_);
-    octree_.CreateOctree(aamm_);
-  }
-
   // close resource
   if (resource_ != nullptr) {
     mju_closeResource(resource_);
@@ -1539,6 +1532,24 @@ void mjCMesh::Process() {
   boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume);
   boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume);
 
+  // make octree in the geom frame
+  // TODO: make octree in the mesh frame, update engine_collision_sdf
+  if (!needoct_) {
+    octree_.Clear();
+  } else if (octree_.Nodes().empty()) {
+    double aamm[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
+    for (int i = 0; i < nvert(); i++) {
+      aamm[0] = std::min(aamm[0], vert_[3*i + 0]);
+      aamm[3] = std::max(aamm[3], vert_[3*i + 0]);
+      aamm[1] = std::min(aamm[1], vert_[3*i + 1]);
+      aamm[4] = std::max(aamm[4], vert_[3*i + 1]);
+      aamm[2] = std::min(aamm[2], vert_[3*i + 2]);
+      aamm[5] = std::max(aamm[5], vert_[3*i + 2]);
+    }
+    octree_.SetFace(vert_, face_);
+    octree_.CreateOctree(aamm);
+  }
+
   // transform CoM to origin
   for (int i=0; i < nvert(); i++) {
     vert_[3*i + 0] -= CoM[0];

src/user/user_objects.cc

@@ -583,12 +583,16 @@ void mjCOctree::Make(std::vector<Triangle>& elements) {
 
 
 void mjCOctree::CreateOctree(const double aamm[6]) {
+  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};
+  double box[6] = {aabb[0] - 1.1 * aabb[3], aabb[1] - 1.1 * aabb[4], aabb[2] - 1.1 * aabb[5],
+                   aabb[0] + 1.1 * aabb[3], aabb[1] + 1.1 * aabb[4], aabb[2] + 1.1 * aabb[5]};
   std::vector<Triangle> elements;
   Make(elements);
   std::vector<Triangle*> elements_ptrs(elements.size());
   std::transform(elements.begin(), elements.end(), elements_ptrs.begin(),
                  [](Triangle& triangle) { return &triangle; });
-  MakeOctree(elements_ptrs, aamm);
+  MakeOctree(elements_ptrs, box);
 }