Extend SDF outside the bounding box for SdfLib plugin.

SdfLib returns the sum of the distance from a given point to the bounding box and a saved minimum distance from the surface over the whole box boundary. This underestimates the true distance and can cause spurious contacts and jittering. This CL performs a box projection of points outside the bounding box and adds the distance of the original point from the box to the distance of the projected point to the surface. This overestimates the true distance and avoids spurious collisions.

PiperOrigin-RevId: 586701933
Change-Id: I22eecb9757fa22359440156ef6433135648fa3ff

plugin/sdf/sdflib.cc

@@ -31,6 +31,34 @@ inline unsigned int* MakeNonConstUnsigned(const int* ptr) {
   return reinterpret_cast<unsigned int*>(const_cast<int*>(ptr));
 }
 
+mjtNum boxProjection(glm::vec3& point, const sdflib::BoundingBox& box) {
+  glm::vec3 r = point - box.getCenter();
+  glm::vec3 q = glm::abs(r) - 0.5f * box.getSize();
+  mjtNum dist_sqr = 0;
+  mjtNum eps = 1e-6;
+
+  // skip the projection if inside
+  if (q.x <= 0 && q.y <= 0 && q.z <= 0) {
+    return glm::max(q.x, glm::max(q.y, q.z));
+  }
+
+  // in-place projection inside the box if outside
+  if ( q.x >= 0 ) {
+    dist_sqr += q.x * q.x;
+    point.x -= r.x > 0 ? (q.x+eps) : -(q.x+eps);
+  }
+  if ( q.y >= 0 ) {
+    dist_sqr += q.y * q.y;
+    point.y -= r.y > 0 ? (q.y+eps) : -(q.y+eps);
+  }
+  if ( q.z >= 0 ) {
+    dist_sqr += q.z * q.z;
+    point.z -= r.z > 0 ? (q.z+eps) : -(q.z+eps);
+  }
+
+  return mju_sqrt(dist_sqr);
+}
+
 }  // namespace
 
 // factory function
@@ -89,7 +117,8 @@ void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
 // sdf
 mjtNum SdfLib::Distance(const mjtNum p[3]) const {
   glm::vec3 point(p[0], p[1], p[2]);
-  return sdf_func_.getDistance(point);
+  mjtNum boxDist = boxProjection(point, sdf_func_.getGridBoundingBox());
+  return sdf_func_.getDistance(point) + (boxDist <= 0 ? 0 : boxDist);
 }
 
 // gradient of sdf