Add GEOM_CUSTOM node type for user-defined shapes

CHANGELOG.md

@@ -22,6 +22,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Added `resolveReferences` method to `Contact` and `DistanceResult` to remap the `o1/o2` pointers (typically after serialization/deserialization) ([855](https://github.com/coal-library/coal/pull/855)).
 - Added copy constructors to `Contact::Contact(const Contact& other, const CollisionGeometry* new_o1, const CollisionGeometry* new_o2)` and `DistanceResult::DistanceResult(const DistanceResult& other, const CollisionGeometry* new_o1, const CollisionGeometry* new_o2)` to allow copying a `Contact` or `DistanceResult` while remapping the `o1/o2` pointers to new geometries. This is typically useful in the context of deep-copying ([#856](https://github.com/coal-library/coal/pull/820)).
 - Added the `COAL_EQUAL_OPERATOR_CHECK` macro. This macro can be overridden at compile time, extremely practial for debugging serialization for example. ([#859](https://github.com/coal-library/coal/pull/859))
+- Add `GEOM_CUSTOM` node type for user-defined shapes ([#822](https://github.com/coal-library/coal/pull/822))
+  - Custom shapes override `ShapeBase::computeShapeSupport()` (and `getNodeType()`) to participate in GJK/EPA queries; the default implementation delegates to the built-in support functions for built-in shapes
 
 ### Removed
 - Remove direct dependency to ([#744](https://github.com/coal-library/coal/pull/744)):

doc/python/doxygen_xml_parser.py

@@ -305,6 +305,20 @@ def s_rettype(self):
     def s_name(self):
         return self.xml.find("name").text.strip()
 
+    def s_name_without_template_args(self):
+        # Strip any trailing <...> specialization from s_name(). Needed because
+        # the generated static_cast lives inside namespace doxygen, where the
+        # bare type names appearing in the template-argument list are not
+        # visible. Dropping them lets the static_cast pick the right
+        # specialization from the target function-pointer type.
+        # operator<, operator<<, operator<=> etc. embed '<' in the name itself
+        # — leave those untouched.
+        name = self.s_name()
+        if name.startswith("operator"):
+            return name
+        idx = name.find("<")
+        return name[:idx].rstrip() if idx >= 0 else name
+
     def s_docstring(self):
         return self.index.xml_docstring.getDocString(
             self.xml.find("briefdescription"),
@@ -756,7 +770,7 @@ def write(self):
                 [
                     template_static_func_doc_body.format(
                         namespace=member.parent.innerNamespace(),
-                        membername=member.s_name(),
+                        membername=member.s_name_without_template_args(),
                         docstring=docstring,
                         rettype=member.s_rettype(),
                         argsstring=member.s_prototypeArgs(),

include/coal/collision_object.h

@@ -87,6 +87,9 @@ enum NODE_TYPE {
   GEOM_ELLIPSOID,
   HF_AABB,
   HF_OBBRSS,
+  /// @brief Custom shape type, used for user-defined shapes that extend
+  /// ShapeBase outside the Coal library. See ShapeBase::computeShapeSupport().
+  GEOM_CUSTOM,
   NODE_COUNT
 };
 

include/coal/collision_utility.h

@@ -31,12 +31,13 @@ COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model,
  */
 inline const char* get_node_type_name(NODE_TYPE node_type) {
   static const char* node_type_name_all[] = {
-      "BV_UNKNOWN",     "BV_AABB",       "BV_OBB",      "BV_RSS",
-      "BV_kIOS",        "BV_OBBRSS",     "BV_KDOP16",   "BV_KDOP18",
-      "BV_KDOP24",      "GEOM_BOX",      "GEOM_SPHERE", "GEOM_CAPSULE",
-      "GEOM_CONE",      "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE",
-      "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID",
-      "HF_AABB",        "HF_OBBRSS",     "NODE_COUNT"};
+      "BV_UNKNOWN",     "BV_AABB",        "BV_OBB",        "BV_RSS",
+      "BV_kIOS",        "BV_OBBRSS",      "BV_KDOP16",     "BV_KDOP18",
+      "BV_KDOP24",      "GEOM_BOX",       "GEOM_SPHERE",   "GEOM_CAPSULE",
+      "GEOM_CONE",      "GEOM_CYLINDER",  "GEOM_CONVEX16", "GEOM_CONVEX32",
+      "GEOM_PLANE",     "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE",
+      "GEOM_ELLIPSOID", "HF_AABB",        "HF_OBBRSS",     "GEOM_CUSTOM",
+      "NODE_COUNT"};
 
   return node_type_name_all[node_type];
 }

include/coal/internal/BV_fitter.h

@@ -56,19 +56,19 @@ void fit(Vec3s* ps, unsigned int n, BV& bv) {
 }
 
 template <>
-void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
+COAL_DLLAPI void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
 
 template <>
-void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
+COAL_DLLAPI void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
 
 template <>
-void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
+COAL_DLLAPI void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
 
 template <>
-void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
+COAL_DLLAPI void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
 
 template <>
-void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
+COAL_DLLAPI void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
 
 /// @brief The class for the default algorithm fitting a bounding volume to a
 /// set of points

include/coal/math/transform.h

@@ -167,7 +167,7 @@ class COAL_DLLAPI Transform3s {
   }
 
   /// @brief inverse transform
-  inline Transform3s inverse() {
+  inline Transform3s inverse() const {
     return Transform3s(R.transpose(), -R.transpose() * T);
   }
 

include/coal/narrowphase/minkowski_difference.h

@@ -179,6 +179,10 @@ struct COAL_DLLAPI MinkowskiDiff {
   }
 };
 
+/// @brief Runtime query for the NeedNesterovNormalizeHeuristic shape trait.
+/// Uses shape_traits for built-in shapes, virtual dispatch for GEOM_CUSTOM.
+COAL_DLLAPI bool getNormalizeSupportDirection(const ShapeBase* shape);
+
 }  // namespace details
 
 }  // namespace coal

include/coal/narrowphase/support_functions.h

@@ -63,6 +63,13 @@ namespace details {
 template <int _SupportOptions = SupportOptions::NoSweptSphere>
 Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint);
 
+/// @brief Same as getSupport, but reuses caller-provided ShapeSupportData
+/// to avoid per-call allocation of the visited vector for ConvexBase shapes.
+/// Useful for custom shapes that delegate to an inner shape repeatedly.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint,
+                 ShapeSupportData& support_data);
+
 /// @brief Triangle support function.
 template <int _SupportOptions = SupportOptions::NoSweptSphere>
 void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
@@ -108,6 +115,13 @@ template <int _SupportOptions = SupportOptions::NoSweptSphere,
 void getShapeSupport(const ConvexBaseTpl<IndexType>* convex, const Vec3s& dir,
                      Vec3s& support, int& hint, ShapeSupportData& /*unused*/);
 
+/// @brief Generic ShapeBase support function.
+/// This overload uses virtual dispatch via ShapeBase::computeShapeSupport(),
+/// allowing custom shapes to participate in GJK/EPA computations.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupport(const ShapeBase* shape, const Vec3s& dir, Vec3s& support,
+                     int& hint, ShapeSupportData& support_data);
+
 /// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of
 /// `getShapeSupport`. This is **much** faster than the linear version of
 /// `getShapeSupport` when a `ConvexBase` has more than a few dozen of vertices.
@@ -284,6 +298,15 @@ void getShapeSupportSet(const LargeConvex<IndexType>* convex,
                         size_t /*unused*/ num_sampled_supports = 6,
                         Scalar tol = Scalar(1e-3));
 
+/// @brief Generic ShapeBase support set function.
+/// This overload uses virtual dispatch for custom shapes.
+/// The default behavior computes a single support point.
+template <int _SupportOptions = SupportOptions::NoSweptSphere>
+void getShapeSupportSet(const ShapeBase* shape, SupportSet& support_set,
+                        int& hint, ShapeSupportData& support_data,
+                        size_t num_sampled_supports = 6,
+                        Scalar tol = Scalar(1e-3));
+
 /// @brief Computes the convex-hull of support_set. For now, this function is
 /// only needed for Box and ConvexBase.
 /// @param[in] cloud data which contains the 2d points of the support set which

include/coal/shape/geometric_shapes.h

@@ -46,6 +46,7 @@
 #include "coal/collision_object.h"
 #include "coal/data_types.h"
 #include "coal/shared_ptr_comparison.h"
+#include "coal/narrowphase/support_data.h"
 
 #ifdef COAL_HAS_QHULL
 namespace orgQhull {
@@ -86,6 +87,39 @@ class COAL_DLLAPI ShapeBase : public CollisionGeometry {
   /// This radius is always >= 0.
   Scalar getSweptSphereRadius() const { return this->m_swept_sphere_radius; }
 
+  /// @brief Compute the support point of this shape in the given direction,
+  /// i.e. a point of the shape which maximizes the dot product with dir.
+  /// The output support point is expressed in the local frame of the shape
+  /// and is the "core" support: it does not account for the swept-sphere
+  /// radius (e.g. Sphere returns the origin, Capsule returns a segment
+  /// endpoint — their radii live in Coal's swept-sphere accounting). Callers
+  /// who want the inflated surface point should use
+  /// details::getSupport<details::SupportOptions::WithSweptSphere> instead.
+  /// The default implementation delegates to the built-in support functions
+  /// for all built-in node types (unbounded shapes — Plane, Halfspace —
+  /// return zero) and throws std::logic_error for a shape reporting
+  /// GEOM_CUSTOM that did not override this method.
+  /// Override getNodeType() to return GEOM_CUSTOM and this method to enable
+  /// custom shapes to participate in GJK/EPA collision and distance
+  /// computations.
+  /// @param[in] dir support direction; may be non-unit-length. The support
+  /// point of a convex shape is invariant to the magnitude of dir; normalize
+  /// internally if your formula requires a unit direction (as the built-in
+  /// Cylinder does for its radial component).
+  /// @param[out] support the computed support point.
+  /// @param[in,out] hint warm-start hint (used mainly for convex shapes).
+  /// @param[in,out] data temporary data for support computation.
+  virtual void computeShapeSupport(const Vec3s& dir, Vec3s& support, int& hint,
+                                   details::ShapeSupportData& data) const;
+
+  /// @brief Whether the Nesterov normalize heuristic should be used
+  /// for this shape in GJK. Override for custom shapes if needed.
+  /// When wrapping another shape, delegate via
+  /// details::getNormalizeSupportDirection(&inner); the call resolves through
+  /// the inner shape's node type, so it terminates for any acyclic
+  /// delegation.
+  virtual bool needNesterovNormalizeHeuristic() const { return false; }
+
  protected:
   bool isEqual(const CollisionGeometry& _other) const override {
     const ShapeBase* other_ptr = dynamic_cast<const ShapeBase*>(&_other);

include/coal/shape/geometric_shapes_utility.h

@@ -63,6 +63,8 @@ COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
                                                 const Transform3s& tf);
 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
                                                 const Transform3s& tf);
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const ShapeBase& s,
+                                                const Transform3s& tf);
 template <typename IndexType>
 std::vector<Vec3s> getBoundVertices(const ConvexBaseTpl<IndexType>& convex,
                                     const Transform3s& tf) {
@@ -124,6 +126,13 @@ template <>
 COAL_DLLAPI void computeBV<AABB, TriangleP>(const TriangleP& s,
                                             const Transform3s& tf, AABB& bv);
 
+/// @pre s.computeLocalAABB() must have been called (reads s.aabb_local).
+/// Do not call from within computeLocalAABB() — compute the AABB from the
+/// shape's geometric parameters directly.
+template <>
+COAL_DLLAPI void computeBV<AABB, ShapeBase>(const ShapeBase& s,
+                                            const Transform3s& tf, AABB& bv);
+
 template <>
 COAL_DLLAPI void computeBV<AABB, Halfspace>(const Halfspace& s,
                                             const Transform3s& tf, AABB& bv);

python-nb/collision-geometries.cc

@@ -68,6 +68,7 @@ void exposeCollisionGeometries(nb::module_& m) {
       .value("GEOM_OCTREE", GEOM_OCTREE)
       .value("HF_AABB", HF_AABB)
       .value("HF_OBBRSS", HF_OBBRSS)
+      .value("GEOM_CUSTOM", GEOM_CUSTOM)
       .export_values();
 
   m.def(

python/collision-geometries.cc

@@ -566,6 +566,7 @@ void exposeCollisionGeometries() {
         .value("GEOM_OCTREE", GEOM_OCTREE)
         .value("HF_AABB", HF_AABB)
         .value("HF_OBBRSS", HF_OBBRSS)
+        .value("GEOM_CUSTOM", GEOM_CUSTOM)
         .export_values();
   }