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(const_cast(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