From 4c8ffb44bf59b29e23ea652e331f7e84ad84bb31 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Thu, 30 Nov 2023 09:42:07 -0800
Subject: [PATCH] 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 ++++++++++++++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc
index 392119ae..a1405eb7 100644
--- a/plugin/sdf/sdflib.cc
+++ b/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