diff --git a/CHANGELOG.md b/CHANGELOG.md index c3a311db6..f3c576aee 100644 --- a/CHANGELOG.md +++ b/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)): diff --git a/doc/python/doxygen_xml_parser.py b/doc/python/doxygen_xml_parser.py index 37f36b1dc..d8f2b8972 100755 --- a/doc/python/doxygen_xml_parser.py +++ b/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(), diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h index 5c458465d..ef99df2e8 100644 --- a/include/coal/collision_object.h +++ b/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 }; diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h index 44a597d26..2b9aac9ee 100644 --- a/include/coal/collision_utility.h +++ b/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]; } diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h index 63739fbf7..578895f67 100644 --- a/include/coal/internal/BV_fitter.h +++ b/include/coal/internal/BV_fitter.h @@ -56,19 +56,19 @@ void fit(Vec3s* ps, unsigned int n, BV& bv) { } template <> -void fit(Vec3s* ps, unsigned int n, OBB& bv); +COAL_DLLAPI void fit(Vec3s* ps, unsigned int n, OBB& bv); template <> -void fit(Vec3s* ps, unsigned int n, RSS& bv); +COAL_DLLAPI void fit(Vec3s* ps, unsigned int n, RSS& bv); template <> -void fit(Vec3s* ps, unsigned int n, kIOS& bv); +COAL_DLLAPI void fit(Vec3s* ps, unsigned int n, kIOS& bv); template <> -void fit(Vec3s* ps, unsigned int n, OBBRSS& bv); +COAL_DLLAPI void fit(Vec3s* ps, unsigned int n, OBBRSS& bv); template <> -void fit(Vec3s* ps, unsigned int n, AABB& bv); +COAL_DLLAPI void fit(Vec3s* ps, unsigned int n, AABB& bv); /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index 8d219a0a7..c3ddb71cf 100644 --- a/include/coal/math/transform.h +++ b/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); } diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h index 919fcc2ac..d25f138a7 100644 --- a/include/coal/narrowphase/minkowski_difference.h +++ b/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 diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h index dd6245cd6..198c0019c 100644 --- a/include/coal/narrowphase/support_functions.h +++ b/include/coal/narrowphase/support_functions.h @@ -63,6 +63,13 @@ namespace details { template 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 +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint, + ShapeSupportData& support_data); + /// @brief Triangle support function. template void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, @@ -108,6 +115,13 @@ template * 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 +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* 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 +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 diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index a4aa2cd9d..0652e6398 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/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 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(&_other); diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h index d3259cfc9..ea570b62e 100644 --- a/include/coal/shape/geometric_shapes_utility.h +++ b/include/coal/shape/geometric_shapes_utility.h @@ -63,6 +63,8 @@ COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, const Transform3s& tf); +COAL_DLLAPI std::vector getBoundVertices(const ShapeBase& s, + const Transform3s& tf); template std::vector getBoundVertices(const ConvexBaseTpl& convex, const Transform3s& tf) { @@ -124,6 +126,13 @@ template <> COAL_DLLAPI void computeBV(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(const ShapeBase& s, + const Transform3s& tf, AABB& bv); + template <> COAL_DLLAPI void computeBV(const Halfspace& s, const Transform3s& tf, AABB& bv); diff --git a/python-nb/collision-geometries.cc b/python-nb/collision-geometries.cc index 64e599d86..fa099cf9a 100644 --- a/python-nb/collision-geometries.cc +++ b/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( diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index c7d1bbcec..0243420c2 100644 --- a/python/collision-geometries.cc +++ b/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(); } diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index a9f086e15..1a16e17d9 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -296,6 +296,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_BOX][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -308,6 +309,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_SPHERE][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide; @@ -320,6 +322,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_ELLIPSOID][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -332,6 +335,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -344,6 +348,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CONE][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide; @@ -356,6 +361,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeCollide; @@ -368,6 +374,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX16][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX16][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeCollide; @@ -380,6 +387,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX32][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CONVEX32][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -392,6 +400,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_PLANE][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -404,6 +413,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeCollide; @@ -416,6 +426,20 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_TRIANGLE][GEOM_CUSTOM] = &ShapeShapeCollide; + + collision_matrix[GEOM_CUSTOM][GEOM_BOX] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_SPHERE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CAPSULE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CONE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CYLINDER] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CONVEX16] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CONVEX32] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_ELLIPSOID] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_TRIANGLE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_PLANE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_HALFSPACE] = &ShapeShapeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_CUSTOM] = &ShapeShapeCollide; collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider::collide; @@ -427,6 +451,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; + collision_matrix[BV_AABB][GEOM_CUSTOM] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider::collide; @@ -438,6 +463,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; + collision_matrix[BV_OBB][GEOM_CUSTOM] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider::collide; @@ -449,6 +475,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; + collision_matrix[BV_RSS][GEOM_CUSTOM] = &BVHShapeCollider::collide; collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; @@ -460,6 +487,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; + collision_matrix[BV_KDOP16][GEOM_CUSTOM] = &BVHShapeCollider, ShapeBase>::collide; collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; @@ -471,6 +499,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; + collision_matrix[BV_KDOP18][GEOM_CUSTOM] = &BVHShapeCollider, ShapeBase>::collide; collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box>::collide; collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere>::collide; @@ -482,6 +511,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane>::collide; collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace>::collide; collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid>::collide; + collision_matrix[BV_KDOP24][GEOM_CUSTOM] = &BVHShapeCollider, ShapeBase>::collide; collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider::collide; @@ -493,6 +523,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; + collision_matrix[BV_kIOS][GEOM_CUSTOM] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider::collide; @@ -504,6 +535,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; + collision_matrix[BV_OBBRSS][GEOM_CUSTOM] = &BVHShapeCollider::collide; collision_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; @@ -515,6 +547,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeCollider::collide; + collision_matrix[HF_AABB][GEOM_CUSTOM] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeCollider::collide; @@ -526,6 +559,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeCollider::collide; collision_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeCollider::collide; + collision_matrix[HF_OBBRSS][GEOM_CUSTOM] = &HeightFieldShapeCollider::collide; collision_matrix[BV_AABB][BV_AABB] = &BVHCollide; collision_matrix[BV_OBB][BV_OBB] = &BVHCollide; @@ -547,6 +581,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CUSTOM] = &OctreeCollide; collision_matrix[GEOM_BOX][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &OctreeCollide; @@ -557,6 +592,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[GEOM_CONVEX32][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CUSTOM][GEOM_OCTREE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OctreeCollide; diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index efc0297a4..4d191bb5e 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -119,6 +119,9 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, case GEOM_CONVEX32: return details::getConvexBaseSupportSetTpl; + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes via ShapeBase overload. + return details::getShapeSupportSetTpl; default: COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index 5c621db05..ae09a6002 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -150,6 +150,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_BOX][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_BOX][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -162,6 +163,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_SPHERE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_SPHERE][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -174,6 +176,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_ELLIPSOID][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_ELLIPSOID][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -186,6 +189,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CAPSULE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CAPSULE][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -198,6 +202,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONE][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -210,6 +215,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CYLINDER][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CYLINDER][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -222,6 +228,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX16][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX16][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -234,6 +241,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_CONVEX32][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CONVEX32][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -246,6 +254,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_PLANE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_PLANE][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -258,6 +267,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_HALFSPACE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_HALFSPACE][GEOM_CUSTOM] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_BOX] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_SPHERE] = &ShapeShapeContactPatch; @@ -270,6 +280,20 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_TRIANGLE][GEOM_HALFSPACE] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; contact_patch_matrix[GEOM_TRIANGLE][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_TRIANGLE][GEOM_CUSTOM] = &ShapeShapeContactPatch; + + contact_patch_matrix[GEOM_CUSTOM][GEOM_BOX] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_SPHERE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CAPSULE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CONE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CYLINDER] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CONVEX16] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CONVEX32] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_ELLIPSOID] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_TRIANGLE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_PLANE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_HALFSPACE] = &ShapeShapeContactPatch; + contact_patch_matrix[GEOM_CUSTOM][GEOM_CUSTOM] = &ShapeShapeContactPatch; // TODO(louis): properly handle non-convex shapes like BVH, Octrees and Hfields. // The following functions work. However apart from the contact frame, these functions don't @@ -284,6 +308,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; + contact_patch_matrix[BV_AABB][GEOM_CUSTOM] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; @@ -295,6 +320,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; + contact_patch_matrix[BV_OBB][GEOM_CUSTOM] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; @@ -306,6 +332,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; + contact_patch_matrix[BV_RSS][GEOM_CUSTOM] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; @@ -317,6 +344,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; + contact_patch_matrix[BV_KDOP16][GEOM_CUSTOM] = &BVHShapeComputeContactPatch, ShapeBase>::run; contact_patch_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; @@ -328,6 +356,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; + contact_patch_matrix[BV_KDOP18][GEOM_CUSTOM] = &BVHShapeComputeContactPatch, ShapeBase>::run; contact_patch_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeComputeContactPatch, Box>::run; contact_patch_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeComputeContactPatch, Sphere>::run; @@ -339,6 +368,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeComputeContactPatch, Plane>::run; contact_patch_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch, Halfspace>::run; contact_patch_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch, Ellipsoid>::run; + contact_patch_matrix[BV_KDOP24][GEOM_CUSTOM] = &BVHShapeComputeContactPatch, ShapeBase>::run; contact_patch_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; @@ -350,6 +380,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; + contact_patch_matrix[BV_kIOS][GEOM_CUSTOM] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeComputeContactPatch::run; @@ -361,6 +392,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeComputeContactPatch::run; + contact_patch_matrix[BV_OBBRSS][GEOM_CUSTOM] = &BVHShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch::run; @@ -372,6 +404,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch::run; + contact_patch_matrix[HF_AABB][GEOM_CUSTOM] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeComputeContactPatch::run; @@ -383,6 +416,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeComputeContactPatch::run; + contact_patch_matrix[HF_OBBRSS][GEOM_CUSTOM] = &HeightFieldShapeComputeContactPatch::run; contact_patch_matrix[BV_AABB][BV_AABB] = &BVHComputeContactPatch::run; contact_patch_matrix[BV_OBB][BV_OBB] = &BVHComputeContactPatch::run; @@ -416,6 +450,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[GEOM_OCTREE][BV_KDOP24] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][HF_AABB] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][HF_OBBRSS] = &contact_patch_function_not_implemented; + contact_patch_matrix[GEOM_OCTREE][GEOM_CUSTOM] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_BOX][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_SPHERE][GEOM_OCTREE] = &contact_patch_function_not_implemented; @@ -437,6 +472,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_KDOP24][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[HF_AABB][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[HF_OBBRSS][GEOM_OCTREE] = &contact_patch_function_not_implemented; + contact_patch_matrix[GEOM_CUSTOM][GEOM_OCTREE] = &contact_patch_function_not_implemented; #endif // clang-format on } diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index ff13e6396..fb7a867eb 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -288,6 +288,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_BOX][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance; @@ -299,6 +300,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_SPHERE][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance; @@ -310,6 +312,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_ELLIPSOID][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance; @@ -321,6 +324,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CAPSULE][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance; @@ -332,6 +336,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CONE][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance; @@ -343,6 +348,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CYLINDER][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_SPHERE] = &ShapeShapeDistance; @@ -354,6 +360,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CONVEX16][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX16][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX16][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_SPHERE] = &ShapeShapeDistance; @@ -365,6 +372,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CONVEX32][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX32][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CONVEX32][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance; @@ -376,6 +384,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_PLANE][GEOM_CUSTOM] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance; @@ -387,6 +396,20 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_HALFSPACE][GEOM_CUSTOM] = &ShapeShapeDistance; + + distance_matrix[GEOM_CUSTOM][GEOM_BOX] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_SPHERE] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CAPSULE] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CONE] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CYLINDER] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CONVEX16] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CONVEX32] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_ELLIPSOID] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_PLANE] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_HALFSPACE] = &ShapeShapeDistance; + distance_matrix[GEOM_CUSTOM][GEOM_CUSTOM] = &ShapeShapeDistance; + // clang-format on /* AABB distance not implemented */ @@ -416,6 +439,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; + distance_matrix[BV_OBB][GEOM_CUSTOM] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; @@ -427,6 +451,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; + distance_matrix[BV_RSS][GEOM_CUSTOM] = &BVHShapeDistancer::distance; // clang-format on /* KDOP distance not implemented */ @@ -482,6 +507,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; + distance_matrix[BV_kIOS][GEOM_CUSTOM] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; @@ -493,6 +519,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; + distance_matrix[BV_OBBRSS][GEOM_CUSTOM] = &BVHShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_BOX] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; @@ -504,6 +531,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[HF_AABB][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_AABB][GEOM_ELLIPSOID] = &HeightFieldShapeDistancer::distance; + distance_matrix[HF_AABB][GEOM_CUSTOM] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_BOX] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_SPHERE] = &HeightFieldShapeDistancer::distance; @@ -515,6 +543,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[HF_OBBRSS][GEOM_PLANE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] = &HeightFieldShapeDistancer::distance; distance_matrix[HF_OBBRSS][GEOM_ELLIPSOID] = &HeightFieldShapeDistancer::distance; + distance_matrix[HF_OBBRSS][GEOM_CUSTOM] = &HeightFieldShapeDistancer::distance; distance_matrix[BV_AABB][BV_AABB] = &BVHDistance; distance_matrix[BV_OBB][BV_OBB] = &BVHDistance; @@ -533,6 +562,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Distance; + distance_matrix[GEOM_OCTREE][GEOM_CUSTOM] = &Distance; distance_matrix[GEOM_BOX][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Distance; @@ -543,6 +573,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[GEOM_CONVEX32][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Distance; + distance_matrix[GEOM_CUSTOM][GEOM_OCTREE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance; diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index caf985870..1dff4f87c 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -173,6 +173,13 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( _SupportOptions>; } } + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes via ShapeBase overload. + // swept_sphere_radius[1] was already set at the top of this function. + if (identity) + return getSupportFuncTpl; + else + return getSupportFuncTpl; default: COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } @@ -255,6 +262,14 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( s1, identity, swept_sphere_radius, data); break; } + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes via ShapeBase overload. + // Shape0 is resolved as ShapeBase; shape1 is still dispatched + // via makeGetSupportFunction1 which resolves known types + // at compile time and falls back to ShapeBase for unknowns. + // swept_sphere_radius[0] was already set at the top of this function. + return makeGetSupportFunction1( + s1, identity, swept_sphere_radius, data); default: COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } @@ -290,6 +305,9 @@ bool getNormalizeSupportDirection(const ShapeBase* shape) { case GEOM_CONVEX32: return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes + return shape->needNesterovNormalizeHeuristic(); default: COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 072002bb2..a02a4ea66 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -48,9 +48,9 @@ namespace details { support, hint, support_data) template -Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint, + ShapeSupportData& support_data) { Vec3s support; - ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: CALL_GET_SHAPE_SUPPORT(TriangleP); @@ -81,6 +81,12 @@ Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { break; case GEOM_PLANE: case GEOM_HALFSPACE: + support.setZero(); + break; + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes + getShapeSupport<_SupportOptions>(shape, dir, support, hint, support_data); + break; default: support.setZero(); ; // nothing @@ -88,13 +94,21 @@ Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { return support; } + +template +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { + ShapeSupportData support_data; + return getSupport<_SupportOptions>(shape, dir, hint, support_data); +} #undef CALL_GET_SHAPE_SUPPORT // Explicit instantiation // clang-format off template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); - template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); + +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&, ShapeSupportData&); +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&, ShapeSupportData&); // clang-format on // ============================================================================ @@ -378,7 +392,7 @@ void getShapeSupportLog(const ConvexBaseTpl* convex, support = pts[static_cast(hint)]; if (_SupportOptions == SupportOptions::WithSweptSphere) { - support += convex->getSweptSphereRadius() * dir.normalized(); + support += convex->getSweptSphereRadius() * dir_normalized; } } @@ -450,6 +464,23 @@ void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, getShapeSupportTplInstantiation(LargeConvex); getShapeSupportTplInstantiation(LargeConvex); +// ============================================================================ +// Generic ShapeBase support function using virtual dispatch. +// This enables custom shapes to participate in GJK/EPA computations +// by overriding ShapeBase::computeShapeSupport(). +template +void getShapeSupport(const ShapeBase* shape, const Vec3s& dir, Vec3s& support, + int& hint, ShapeSupportData& support_data) { + // The direction is forwarded as-is (possibly non-unit-length), exactly as + // for the built-in support functions; see the contract documented on + // ShapeBase::computeShapeSupport(). + shape->computeShapeSupport(dir, support, hint, support_data); + if (_SupportOptions == SupportOptions::WithSweptSphere) { + support += shape->getSweptSphereRadius() * dir.normalized(); + } +} +getShapeSupportTplInstantiation(ShapeBase); + // ============================================================================ #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \ getShapeSupportSet<_SupportOptions>(static_cast(shape), \ @@ -490,10 +521,16 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, break; case GEOM_PLANE: case GEOM_HALFSPACE: + break; + case GEOM_CUSTOM: + // Use virtual dispatch for custom shapes + getShapeSupportSet<_SupportOptions>(shape, support_set, hint, + support_data, max_num_supports, tol); + break; default:; // nothing } } -#undef CALL_GET_SHAPE_SUPPORT +#undef CALL_GET_SHAPE_SUPPORT_SET // Explicit instantiation // clang-format off @@ -962,6 +999,22 @@ void getShapeSupportSet(const LargeConvex* convex, getShapeSupportSetTplInstantiation(LargeConvex); getShapeSupportSetTplInstantiation(LargeConvex); +// ============================================================================ +// Generic ShapeBase support set function using virtual dispatch. +// Default behavior: compute a single support point via computeShapeSupport(). +template +void getShapeSupportSet(const ShapeBase* shape, SupportSet& support_set, + int& hint, ShapeSupportData& support_data, + size_t /*unused*/, Scalar /*unused*/) { + support_set.points().clear(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); + getShapeSupport<_SupportOptions>(shape, support_dir, support, hint, + support_data); + support_set.addPoint(support); +} +getShapeSupportSetTplInstantiation(ShapeBase); + // ============================================================================ COAL_DLLAPI void computeSupportSetConvexHull(const std::vector& cloud, std::vector& cvx_hull) { diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 17ea4f479..2b497506e 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -37,6 +37,7 @@ #include "coal/shape/geometric_shapes.h" #include "coal/shape/geometric_shapes_utility.h" +#include "coal/narrowphase/support_functions.h" #ifdef COAL_HAS_QHULL #include @@ -57,6 +58,21 @@ using orgQhull::QhullVertexSet; namespace coal { +void ShapeBase::computeShapeSupport(const Vec3s& dir, Vec3s& support, int& hint, + details::ShapeSupportData& data) const { + // Guard before delegating: details::getSupport dispatches GEOM_CUSTOM back + // to this virtual, so a custom shape missing its override must stop here + // instead of recursing. + if (getNodeType() == GEOM_CUSTOM) { + COAL_THROW_PRETTY( + "computeShapeSupport not implemented for this custom shape. Override " + "this method to use custom shapes with GJK/EPA.", + std::logic_error); + } + support = details::getSupport( + this, dir, hint, data); +} + template ConvexBaseTpl* ConvexBaseTpl::convexHull( std::shared_ptr>& pts, unsigned int num_points, diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index 5de29349b..d32235156 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -232,6 +232,17 @@ std::vector getBoundVertices(const TriangleP& triangle, return result; } +std::vector getBoundVertices(const ShapeBase& s, const Transform3s& tf) { + AABB aabb; + computeBV(s, tf, aabb); + const Vec3s& lo = aabb.min_; + const Vec3s& hi = aabb.max_; + return {Vec3s(lo[0], lo[1], lo[2]), Vec3s(hi[0], lo[1], lo[2]), + Vec3s(lo[0], hi[1], lo[2]), Vec3s(hi[0], hi[1], lo[2]), + Vec3s(lo[0], lo[1], hi[2]), Vec3s(hi[0], lo[1], hi[2]), + Vec3s(lo[0], hi[1], hi[2]), Vec3s(hi[0], hi[1], hi[2])}; +} + } // namespace details Halfspace transform(const Halfspace& a, const Transform3s& tf) { @@ -381,6 +392,25 @@ void computeBV(const ConvexBase16& s, const Transform3s& tf, computeAABBConvex(s, tf, bv); } +template <> +void computeBV(const ShapeBase& s, const Transform3s& tf, + AABB& bv) { + // Use the precomputed local AABB with the rotated-AABB formula. + // Slightly conservative when aabb_local is not tight to the shape, but this + // is consistent with other computeBV specializations and + // CollisionObject::computeAABB(). + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); + + const Vec3s half = (s.aabb_local.max_ - s.aabb_local.min_) * 0.5; + + const Vec3s new_center = R * s.aabb_local.center() + T; + const Vec3s v_delta(R.cwiseAbs() * half); + + bv.min_ = new_center - v_delta; + bv.max_ = new_center + v_delta; +} + template <> void computeBV(const TriangleP& s, const Transform3s& tf, AABB& bv) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f60f8de31..570fd257a 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -66,6 +66,11 @@ if(NOT BUILD_STANDALONE_PYTHON_INTERFACE) add_coal_test(gjk gjk.cpp) add_coal_test(accelerated_gjk accelerated_gjk.cpp) add_coal_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) + add_coal_test(custom_shape custom_shape.cpp) + add_coal_test( + custom_shape_deformed_cylinder + custom_shape_deformed_cylinder.cpp + ) if(COAL_HAS_OCTOMAP) add_coal_test(octree octree.cpp) endif(COAL_HAS_OCTOMAP) diff --git a/test/custom_shape.cpp b/test/custom_shape.cpp new file mode 100644 index 000000000..e846b55ae --- /dev/null +++ b/test/custom_shape.cpp @@ -0,0 +1,887 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/// @file custom_shape.cpp +/// @brief Tests for custom shape support via ShapeBase virtual dispatch. +/// Demonstrates how to implement a custom shape outside the Coal +/// library, including a cast (swept) sphere for continuous collision +/// detection (Schulman et al., 2014). + +#define BOOST_TEST_MODULE COAL_CUSTOM_SHAPE +#include + +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/narrowphase/minkowski_difference.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "utility.h" +#include "coal/hfield.h" +#include "coal/contact_patch.h" + +#ifdef COAL_HAS_OCTOMAP +#include "coal/octree.h" +#endif + +using namespace coal; + +// ============================================================================ +// CustomSphere: a sphere implemented as a custom ShapeBase subclass. +// This demonstrates the pattern for extending Coal with custom shapes +// without modifying Coal source code. +// ============================================================================ +class CustomSphere : public ShapeBase { + public: + explicit CustomSphere(Scalar radius) : ShapeBase(), radius(radius) {} + + CustomSphere* clone() const override { return new CustomSphere(*this); } + + NODE_TYPE getNodeType() const override { return GEOM_CUSTOM; } + + void computeLocalAABB() override { + const Scalar r = radius + this->getSweptSphereRadius(); + aabb_local.min_ = Vec3s::Constant(-r); + aabb_local.max_ = Vec3s::Constant(r); + aabb_center = Vec3s::Zero(); + aabb_radius = r; + } + + // Support function: radius * normalize(dir). The direction is the raw, + // possibly non-unit-length GJK direction. + // Note: do NOT add swept sphere radius here; Coal handles it separately. + void computeShapeSupport(const Vec3s& dir, Vec3s& support, int& /*hint*/, + details::ShapeSupportData& /*data*/) const override { + support = radius * dir.normalized(); + } + + bool isEqual(const CollisionGeometry& other) const override { + const CustomSphere* other_ = dynamic_cast(&other); + if (other_ == nullptr) return false; + return radius == other_->radius; + } + + Scalar radius; +}; + +// ============================================================================ +// CastSphere: swept volume of a sphere moving from the identity transform to +// a given cast transform. The support function returns the point that maximises +// dot(d, ·) over both poses (Schulman et al., 2014). +// ============================================================================ +class CastSphere : public ShapeBase { + public: + /// @param radius sphere radius + /// @param cast_tf the transform from pose 0 (identity) to pose 1, + /// expressed in the local frame of this shape. + CastSphere(Scalar radius, const Transform3s& cast_tf) + : ShapeBase(), shape_(radius), cast_tf_(cast_tf) { + shape_.computeLocalAABB(); + } + + CastSphere* clone() const override { return new CastSphere(*this); } + + NODE_TYPE getNodeType() const override { return GEOM_CUSTOM; } + + void computeLocalAABB() override { + // Pose 0: shape's local AABB (includes its swept sphere radius). + aabb_local = shape_.aabb_local; + + // Pose 1: shape at cast transform, via Coal's |R|*half-extents formula. + AABB pose1_aabb; + computeBV(shape_, cast_tf_, pose1_aabb); + aabb_local += pose1_aabb; + + aabb_local.expand(getSweptSphereRadius()); + aabb_center = aabb_local.center(); + aabb_radius = (aabb_local.min_ - aabb_center).norm(); + } + + /// @brief Support of the swept volume (convex hull of shape at pose 0 and + /// pose 1). General formula (Schulman et al. 2014): + /// s0 = support_shape(dir) -- pose 0 (identity) + /// s1 = T * support_shape(T.rotation^{-1} * dir) -- pose 1 + /// support_cast(dir) = argmax_{s0, s1} dot(dir, ·) + void computeShapeSupport(const Vec3s& dir, Vec3s& support, int& /*hint*/, + details::ShapeSupportData& /*data*/) const override { + // Pose 0: underlying shape support in its local frame (identity transform). + // WithSweptSphere so shapes with intrinsic radii (Sphere, Capsule) include + // that radius in the support point. + const Vec3s s0 = + details::getSupport( + &shape_, dir, hint0_); + + // Pose 1: rotate dir into pose-1 local frame, get support, transform back. + const Vec3s dir_local1 = cast_tf_.getRotation().transpose() * dir; + const Vec3s s1 = cast_tf_.transform( + details::getSupport( + &shape_, dir_local1, hint1_)); + + // Return the support of the convex hull of both poses (prefer pose 1 on + // tie). + support = (dir.dot(s0) > dir.dot(s1)) ? s0 : s1; + } + + /// @brief Delegate to the underlying shape via shape_traits lookup. + bool needNesterovNormalizeHeuristic() const override { + return details::getNormalizeSupportDirection(&shape_); + } + + bool isEqual(const CollisionGeometry& other) const override { + const CastSphere* other_ = dynamic_cast(&other); + if (other_ == nullptr) return false; + return shape_ == other_->shape_ && cast_tf_ == other_->cast_tf_; + } + + private: + Sphere shape_; + Transform3s cast_tf_; + mutable int hint0_{0}; + mutable int hint1_{0}; +}; + +// ============================================================================ +// Tests +// ============================================================================ + +/// Verify that custom shapes returning GEOM_CUSTOM are recognized correctly. +BOOST_AUTO_TEST_CASE(test_geom_custom_node_type) { + CustomSphere sphere(1.0); + BOOST_CHECK_EQUAL(sphere.getNodeType(), GEOM_CUSTOM); + BOOST_CHECK_EQUAL(sphere.getObjectType(), OT_GEOM); + + CastSphere cast_sphere(1.0, Transform3s()); + BOOST_CHECK_EQUAL(cast_sphere.getNodeType(), GEOM_CUSTOM); +} + +/// Test that a custom sphere gives the same distance result as Coal's +/// built-in Sphere, using both the low-level GJK API and the high-level +/// distance() API. +BOOST_AUTO_TEST_CASE(test_custom_sphere_vs_builtin_sphere_distance) { + const Scalar radius = 1.0; + const Scalar tol = Scalar(1e-6); + + // Built-in sphere + Sphere builtin_sphere(radius); + // Custom sphere (same geometry, implemented via virtual dispatch) + CustomSphere custom_sphere(radius); + + GJKSolver solver; + solver.gjk_tolerance = tol; + solver.epa_tolerance = tol; + + // Test a range of separations + std::vector separations = {0.1, 0.5, 1.0, 2.0, 5.0}; + for (Scalar sep : separations) { + Transform3s tf1; // identity + Transform3s tf2(Quats::Identity(), Vec3s(2 * radius + sep, 0, 0)); + + // Distance between two built-in spheres + Sphere s2_builtin(radius); + DistanceRequest request(true); + DistanceResult result_builtin; + Scalar d_builtin = distance(&builtin_sphere, tf1, &s2_builtin, tf2, request, + result_builtin); + + // Distance between custom sphere and built-in sphere + DistanceResult result_custom; + Scalar d_custom = + distance(&custom_sphere, tf1, &s2_builtin, tf2, request, result_custom); + + BOOST_CHECK_CLOSE(d_custom, d_builtin, Scalar(1e-3)); + + // Also test the symmetric pair (built-in vs. custom) + DistanceResult result_sym; + Scalar d_sym = + distance(&s2_builtin, tf1, &custom_sphere, tf2, request, result_sym); + BOOST_CHECK_CLOSE(d_sym, d_builtin, Scalar(1e-3)); + } +} + +/// Test collision detection between a custom sphere and a built-in Box. +BOOST_AUTO_TEST_CASE(test_custom_sphere_collide_with_box) { + const Scalar radius = 1.0; + CustomSphere custom_sphere(radius); + Box box(2.0, 2.0, 2.0); + + CollisionRequest request; + CollisionResult result; + + // Overlapping: sphere center at distance 0.5 from box surface + Transform3s tf1; // custom sphere at origin + Transform3s tf2(Quats::Identity(), Vec3s(1.5, 0, 0)); + + std::size_t n_contacts = + collide(&custom_sphere, tf1, &box, tf2, request, result); + BOOST_CHECK_GT(n_contacts, 0u); + + // Separated: sphere center at distance 2.0 from box surface + result.clear(); + tf2.setTranslation(Vec3s(4.0, 0, 0)); + n_contacts = collide(&custom_sphere, tf1, &box, tf2, request, result); + BOOST_CHECK_EQUAL(n_contacts, 0u); +} + +/// Test custom-vs-custom collision (both shapes use virtual dispatch). +BOOST_AUTO_TEST_CASE(test_custom_sphere_vs_custom_sphere) { + const Scalar radius = 1.0; + CustomSphere s1(radius); + CustomSphere s2(radius); + + DistanceRequest dist_req(true); + DistanceResult dist_res; + + // Touching: distance should be ~0 + Transform3s tf1; + Transform3s tf2(Quats::Identity(), Vec3s(2 * radius, 0, 0)); + Scalar d = distance(&s1, tf1, &s2, tf2, dist_req, dist_res); + BOOST_CHECK_SMALL(d, Scalar(1e-5)); + + // Separated + dist_res.clear(); + tf2.setTranslation(Vec3s(3 * radius, 0, 0)); + d = distance(&s1, tf1, &s2, tf2, dist_req, dist_res); + BOOST_CHECK_CLOSE(d, Scalar(radius), Scalar(1e-3)); + + // Overlapping + CollisionRequest coll_req; + CollisionResult coll_res; + tf2.setTranslation(Vec3s(radius, 0, 0)); + std::size_t n = collide(&s1, tf1, &s2, tf2, coll_req, coll_res); + BOOST_CHECK_GT(n, 0u); +} + +/// Test a CastSphere (swept volume of a sphere between two poses). +/// +/// The CastSphere represents the convex hull of a sphere moving from position +/// (0,0,0) to (d,0,0). Its support function maximises over both endpoints. +BOOST_AUTO_TEST_CASE(test_cast_sphere_swept_volume) { + const Scalar radius = 1.0; + const Scalar sweep_dist = 3.0; + + // Cast transform: sphere moves sweep_dist along X + Transform3s cast_tf(Quats::Identity(), Vec3s(sweep_dist, 0, 0)); + CastSphere cast_sphere(radius, cast_tf); + cast_sphere.computeLocalAABB(); + + // A box placed just beyond the swept path end + Box box(1.0, 1.0, 1.0); + Transform3s tf_cast; // cast sphere at origin + Transform3s tf_box; + + CollisionRequest coll_req; + CollisionResult coll_res; + + // Box center at sweep_dist + radius + 0.4 = 4.4; box left edge at 3.9. + // End sphere surface at sweep_dist + radius = 4.0 → overlap of 0.1. + tf_box.setTranslation(Vec3s(sweep_dist + radius + Scalar(0.4), 0, 0)); + std::size_t n = + collide(&cast_sphere, tf_cast, &box, tf_box, coll_req, coll_res); + BOOST_CHECK_GT(n, 0u); + + // Box far beyond the swept path: no collision. + coll_res.clear(); + tf_box.setTranslation(Vec3s(sweep_dist + radius + Scalar(2.0), 0, 0)); + n = collide(&cast_sphere, tf_cast, &box, tf_box, coll_req, coll_res); + BOOST_CHECK_EQUAL(n, 0u); + + // Box inside the swept path (mid-point): should collide. + coll_res.clear(); + tf_box.setTranslation(Vec3s(sweep_dist / 2, 0, 0)); + n = collide(&cast_sphere, tf_cast, &box, tf_box, coll_req, coll_res); + BOOST_CHECK_GT(n, 0u); + + // Verify distance API also works for the cast shape + DistanceRequest dist_req(true); + DistanceResult dist_res; + tf_box.setTranslation(Vec3s(sweep_dist + radius + Scalar(2.0), 0, 0)); + Scalar d = distance(&cast_sphere, tf_cast, &box, tf_box, dist_req, dist_res); + BOOST_CHECK_GT(d, Scalar(0)); + // Expected: gap = 2.0 - 0.5 (box half-extent) = 1.5 + BOOST_CHECK_CLOSE(d, Scalar(1.5), Scalar(1)); +} + +/// Verify that getNodeType() returning GEOM_CUSTOM does NOT affect built-in +/// shapes (they must still return their specific GEOM_* type). +BOOST_AUTO_TEST_CASE(test_builtin_shapes_keep_their_node_type) { + BOOST_CHECK_EQUAL(Sphere(1.0).getNodeType(), GEOM_SPHERE); + BOOST_CHECK_EQUAL(Box(1, 1, 1).getNodeType(), GEOM_BOX); + BOOST_CHECK_EQUAL(Capsule(1, 2).getNodeType(), GEOM_CAPSULE); + BOOST_CHECK_EQUAL(Cylinder(1, 2).getNodeType(), GEOM_CYLINDER); + BOOST_CHECK_EQUAL(Cone(1, 2).getNodeType(), GEOM_CONE); + BOOST_CHECK_EQUAL(Ellipsoid(1, 1, 1).getNodeType(), GEOM_ELLIPSOID); +} + +/// Test computeBV produces tight AABBs matching built-in. +BOOST_AUTO_TEST_CASE(test_computeBV_AABB_ShapeBase) { + const Scalar radius = 1.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); // Required: computeBV uses + // aabb_local + Sphere builtin(radius); + + // Identity transform + { + AABB bv_custom, bv_builtin; + computeBV(custom, Transform3s(), bv_custom); + computeBV(builtin, Transform3s(), bv_builtin); + BOOST_CHECK(bv_custom.min_.isApprox(bv_builtin.min_, Scalar(1e-10))); + BOOST_CHECK(bv_custom.max_.isApprox(bv_builtin.max_, Scalar(1e-10))); + } + + // Non-identity transform (rotation + translation). + // computeBV uses the rotated-AABB formula on aabb_local, + // which is slightly conservative when the local AABB is not tight to the + // shape under rotation (e.g. a sphere's cubic AABB). The result must + // contain the tight AABB but may be larger. + { + Transform3s tf; + tf.setTranslation(Vec3s(1.0, 2.0, 3.0)); + Quats q(Eigen::AngleAxis(Scalar(0.7), Vec3s::UnitZ())); + tf.setQuatRotation(q); + + AABB bv_custom, bv_builtin; + computeBV(custom, tf, bv_custom); + computeBV(builtin, tf, bv_builtin); + // Must be a valid enclosure of the tight AABB + BOOST_CHECK( + (bv_custom.min_.array() <= bv_builtin.min_.array() + 1e-10).all()); + BOOST_CHECK( + (bv_custom.max_.array() >= bv_builtin.max_.array() - 1e-10).all()); + // Should not be wildly oversized (sphere conservatism is bounded by + // sqrt(3)) + const Vec3s size_custom = bv_custom.max_ - bv_custom.min_; + const Vec3s size_builtin = bv_builtin.max_ - bv_builtin.min_; + BOOST_CHECK( + (size_custom.array() <= size_builtin.array() * Scalar(1.8)).all()); + } +} + +#ifdef COAL_HAS_OCTOMAP + +/// Helper: create a simple occupied octree centered at the origin. +static OcTree makeSimpleOctree(Scalar resolution = 0.1) { + auto octree_ptr = + coal::shared_ptr(new octomap::OcTree(resolution)); + // Fill a small 1x1x1 cube centered at origin + const Scalar half = Scalar(0.5); + for (Scalar x = -half; x < half; x += resolution) { + for (Scalar y = -half; y < half; y += resolution) { + for (Scalar z = -half; z < half; z += resolution) { + octomap::point3d p(static_cast(x + resolution * Scalar(0.5)), + static_cast(y + resolution * Scalar(0.5)), + static_cast(z + resolution * Scalar(0.5))); + octree_ptr->updateNode(p, true); + } + } + } + octree_ptr->updateInnerOccupancy(); + return OcTree(octree_ptr); +} + +/// Test collision: CustomSphere vs OcTree (both directions). +BOOST_AUTO_TEST_CASE(test_custom_shape_octree_collision) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + OcTree octree = makeSimpleOctree(); + octree.computeLocalAABB(); + + CollisionRequest request; + CollisionResult result; + + // Overlapping: custom sphere at origin overlaps octree at origin + Transform3s tf1; + Transform3s tf2; + std::size_t n = collide(&custom, tf1, &octree, tf2, request, result); + BOOST_CHECK_GT(n, 0u); + + // Reversed order: octree vs custom + result.clear(); + n = collide(&octree, tf2, &custom, tf1, request, result); + BOOST_CHECK_GT(n, 0u); + + // Separated: move custom sphere far away + result.clear(); + tf1.setTranslation(Vec3s(5.0, 0, 0)); + n = collide(&custom, tf1, &octree, tf2, request, result); + BOOST_CHECK_EQUAL(n, 0u); + + // Reversed separated + result.clear(); + n = collide(&octree, tf2, &custom, tf1, request, result); + BOOST_CHECK_EQUAL(n, 0u); +} + +/// Test distance: CustomSphere vs OcTree (both directions). +BOOST_AUTO_TEST_CASE(test_custom_shape_octree_distance) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + OcTree octree = makeSimpleOctree(); + octree.computeLocalAABB(); + + DistanceRequest request(true); + DistanceResult result; + + // Separated: custom sphere well away from octree + Transform3s tf1(Quats::Identity(), Vec3s(3.0, 0, 0)); + Transform3s tf2; + Scalar d = distance(&custom, tf1, &octree, tf2, request, result); + BOOST_CHECK_GT(d, Scalar(0)); + + // Reversed: octree vs custom + result.clear(); + Scalar d_rev = distance(&octree, tf2, &custom, tf1, request, result); + BOOST_CHECK_GT(d_rev, Scalar(0)); + + // Both directions should give similar distances + BOOST_CHECK_CLOSE(d, d_rev, Scalar(1)); +} + +/// Test that ComputeCollision does not throw for GEOM_CUSTOM <-> GEOM_OCTREE. +BOOST_AUTO_TEST_CASE(test_custom_octree_no_throw) { + CustomSphere custom(1.0); + custom.computeLocalAABB(); + + OcTree octree = makeSimpleOctree(); + octree.computeLocalAABB(); + + CollisionRequest request; + CollisionResult result; + + // Should not throw unsupported-pair exception + BOOST_CHECK_NO_THROW( + collide(&custom, Transform3s(), &octree, Transform3s(), request, result)); + result.clear(); + BOOST_CHECK_NO_THROW( + collide(&octree, Transform3s(), &custom, Transform3s(), request, result)); +} + +#endif // COAL_HAS_OCTOMAP + +// ============================================================================ +// BVH mesh tests +// ============================================================================ + +/// Helper: create a simple BVHModel box mesh (unit cube centered at +/// origin). +static coal::shared_ptr> makeBoxMesh() { + auto model = coal::make_shared>(); + generateBVHModel(*model, Box(Vec3s::Ones()), Transform3s()); + return model; +} + +/// Collision: CustomSphere vs BVHModel mesh +BOOST_AUTO_TEST_CASE(test_custom_shape_bvh_collision) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + auto mesh = makeBoxMesh(); + + CollisionRequest request; + CollisionResult result; + + // Overlapping: sphere at origin, mesh at origin + Transform3s tf1, tf2; + std::size_t n = collide(&custom, tf1, mesh.get(), tf2, request, result); + BOOST_CHECK_GT(n, 0u); + + // Reversed order + result.clear(); + n = collide(mesh.get(), tf2, &custom, tf1, request, result); + BOOST_CHECK_GT(n, 0u); + + // Separated: move sphere far away + result.clear(); + tf1.setTranslation(Vec3s(5.0, 0, 0)); + n = collide(&custom, tf1, mesh.get(), tf2, request, result); + BOOST_CHECK_EQUAL(n, 0u); +} + +/// Distance: CustomSphere vs BVHModel mesh +BOOST_AUTO_TEST_CASE(test_custom_shape_bvh_distance) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + auto mesh = makeBoxMesh(); + + DistanceRequest request(true); + DistanceResult result; + + // Separated: sphere at x=3, mesh at origin. Expected distance ~2.0 + // (sphere surface at 2.5, box surface at 0.5) + Transform3s tf1(Quats::Identity(), Vec3s(3.0, 0, 0)); + Transform3s tf2; + Scalar d = distance(&custom, tf1, mesh.get(), tf2, request, result); + BOOST_CHECK_GT(d, Scalar(0)); + BOOST_CHECK_CLOSE(d, Scalar(2.0), Scalar(5)); + + // Symmetry: mesh vs custom should give similar distance + result.clear(); + Scalar d_rev = distance(mesh.get(), tf2, &custom, tf1, request, result); + BOOST_CHECK_GT(d_rev, Scalar(0)); + BOOST_CHECK_CLOSE(d, d_rev, Scalar(1)); +} + +/// Collision: CustomSphere vs HeightField +BOOST_AUTO_TEST_CASE(test_custom_shape_heightfield_collision) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + // Create a simple flat 2x2 heightfield at z=0, spanning [-1,1] x [-1,1] + const Eigen::DenseIndex nx = 3, ny = 3; + MatrixXs heights = MatrixXs::Zero(ny, nx); + HeightField hf(Scalar(2.0), Scalar(2.0), heights, + Scalar(-0.1)); // min_height + hf.computeLocalAABB(); + + CollisionRequest request; + CollisionResult result; + + // Sphere at z=0.3 (overlaps with heightfield surface at z=0) + Transform3s tf1(Quats::Identity(), Vec3s(0, 0, Scalar(0.3))); + Transform3s tf2; + std::size_t n = collide(&custom, tf1, &hf, tf2, request, result); + BOOST_CHECK_GT(n, 0u); + + // Sphere far above: no collision + result.clear(); + tf1.setTranslation(Vec3s(0, 0, 5.0)); + n = collide(&custom, tf1, &hf, tf2, request, result); + BOOST_CHECK_EQUAL(n, 0u); +} + +/// HeightField distance is unimplemented in Coal for ALL shape types (the +/// matrix entry exists but the function throws std::invalid_argument). +/// This test documents that limitation and ensures GEOM_CUSTOM behaves +/// consistently with built-in shapes. +BOOST_AUTO_TEST_CASE(test_custom_shape_heightfield_distance_not_implemented) { + const Scalar radius = 0.5; + CustomSphere custom(radius); + custom.computeLocalAABB(); + + const Eigen::DenseIndex nx = 3, ny = 3; + MatrixXs heights = MatrixXs::Zero(ny, nx); + HeightField hf(Scalar(2.0), Scalar(2.0), heights, Scalar(-0.1)); + hf.computeLocalAABB(); + + DistanceRequest request(true); + DistanceResult result; + + Transform3s tf1(Quats::Identity(), Vec3s(0, 0, 3.0)); + Transform3s tf2; + + // Matches the behaviour of built-in shapes (e.g. Sphere vs HeightField) + BOOST_CHECK_THROW(distance(&custom, tf1, &hf, tf2, request, result), + std::invalid_argument); +} + +/// Verify computeBV produces valid BV containing the shape. +BOOST_AUTO_TEST_CASE(test_computeBV_OBB_ShapeBase) { + const Scalar radius = 1.5; + CustomSphere custom(radius); + + Transform3s tf; + tf.setTranslation(Vec3s(1.0, 2.0, 3.0)); + Quats q(Eigen::AngleAxis(Scalar(0.7), Vec3s::UnitZ())); + tf.setQuatRotation(q); + + // Compute OBB for custom shape + OBB obb; + computeBV(custom, tf, obb); + + // Compute AABB for reference — the OBB should contain the AABB center + AABB aabb; + computeBV(custom, tf, aabb); + + // The OBB should at least contain the AABB center + BOOST_CHECK(obb.contain(aabb.center())); + + // Also test OBBRSS + OBBRSS obbrss; + computeBV(custom, tf, obbrss); + // OBBRSS should contain the AABB center too + BOOST_CHECK(obbrss.contain(aabb.center())); +} + +// ============================================================================ +// Contact patch tests +// ============================================================================ + +/// Contact patch: CustomSphere vs built-in Box. +/// Exercises GEOM_CUSTOM entries in contact_patch_func_matrix and +/// the GEOM_CUSTOM case in ContactPatchSolver::makeSupportSetFunction. +BOOST_AUTO_TEST_CASE(test_custom_shape_contact_patch) { + const Scalar radius = 1.0; + CustomSphere custom(radius); + Box box(2.0, 2.0, 2.0); + + // Sphere at origin, box slightly overlapping along +Z + Transform3s tf1; + const Scalar overlap = Scalar(0.01); + Transform3s tf2(Quats::Identity(), + Vec3s(0, 0, radius + Scalar(1.0) - overlap)); + + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + const ContactPatchRequest patch_req; + + CollisionResult col_res; + coal::collide(&custom, tf1, &box, tf2, col_req, col_res); + BOOST_REQUIRE(col_res.isCollision()); + + { + ContactPatchResult patch_res(patch_req); + coal::computeContactPatch(&custom, tf1, &box, tf2, col_res, patch_req, + patch_res); + BOOST_REQUIRE(patch_res.numContactPatches() > 0); + + const Contact& contact = col_res.getContact(0); + const ContactPatch& patch = patch_res.getContactPatch(0); + + // Sphere is strictly convex => single-point contact patch + BOOST_CHECK_EQUAL(patch.size(), 1u); + + const Scalar tol = Scalar(1e-3); + BOOST_CHECK_SMALL((patch.getNormal() - contact.normal).norm(), tol); + BOOST_CHECK_SMALL( + std::abs(patch.penetration_depth - contact.penetration_depth), tol); + } + + // Reversed order: Box vs CustomSphere + { + CollisionResult col_res2; + coal::collide(&box, tf2, &custom, tf1, col_req, col_res2); + BOOST_REQUIRE(col_res2.isCollision()); + + ContactPatchResult patch_res(patch_req); + coal::computeContactPatch(&box, tf2, &custom, tf1, col_res2, patch_req, + patch_res); + BOOST_CHECK(patch_res.numContactPatches() > 0); + } + + // GEOM_CUSTOM vs GEOM_CUSTOM + { + CustomSphere custom2(radius); + Transform3s tf_c2(Quats::Identity(), Vec3s(0, 0, 2 * radius - overlap)); + + CollisionResult col_res3; + coal::collide(&custom, tf1, &custom2, tf_c2, col_req, col_res3); + BOOST_REQUIRE(col_res3.isCollision()); + + ContactPatchResult patch_res(patch_req); + coal::computeContactPatch(&custom, tf1, &custom2, tf_c2, col_res3, + patch_req, patch_res); + BOOST_REQUIRE(patch_res.numContactPatches() > 0); + + const ContactPatch& patch = patch_res.getContactPatch(0); + BOOST_CHECK_EQUAL(patch.size(), 1u); + } +} + +/// Smoke test: contact patch computation does not throw for various +/// GEOM_CUSTOM pairings. +BOOST_AUTO_TEST_CASE(test_custom_shape_contact_patch_no_throw) { + const Scalar radius = 1.0; + CustomSphere custom(radius); + + Transform3s tf1; + const Scalar overlap = Scalar(0.01); + const size_t num_max_contact = 1; + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, + num_max_contact); + + auto test_pair = [&](CollisionGeometry* o1, const Transform3s& t1, + CollisionGeometry* o2, const Transform3s& t2) { + CollisionResult col_res; + coal::collide(o1, t1, o2, t2, col_req, col_res); + BOOST_REQUIRE(col_res.isCollision()); + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + BOOST_CHECK_NO_THROW(coal::computeContactPatch(o1, t1, o2, t2, col_res, + patch_req, patch_res)); + }; + + Transform3s tf_near(Quats::Identity(), Vec3s(0, 0, 2 * radius - overlap)); + + Sphere sphere(radius); + test_pair(&custom, tf1, &sphere, tf_near); + + Capsule capsule(radius, 2.0); + test_pair(&custom, tf1, &capsule, tf_near); + + Cylinder cylinder(radius, 2.0); + test_pair(&custom, tf1, &cylinder, tf_near); + + Ellipsoid ellipsoid(radius, radius, radius); + test_pair(&custom, tf1, &ellipsoid, tf_near); +} + +/// Support points must be invariant to the magnitude of the support +/// direction: GJK passes raw, possibly non-unit-length directions. +BOOST_AUTO_TEST_CASE(test_support_direction_scale_invariance) { + CustomSphere sphere(0.7); + sphere.setSweptSphereRadius(Scalar(0.2)); + sphere.computeLocalAABB(); + + const Transform3s cast_tf( + Quats(Eigen::AngleAxis(Scalar(0.3), Vec3s(1, 2, 3).normalized())), + Vec3s(Scalar(0.3), Scalar(-0.2), Scalar(1.1))); + CastSphere cast_sphere(0.5, cast_tf); + cast_sphere.computeLocalAABB(); + + const std::vector dirs = { + Vec3s(1, 0, 0), Vec3s(0, -1, 0), + Vec3s(0, 0, 1), Vec3s(1, 1, 1), + Vec3s(-2, 3, 5), Vec3s(Scalar(0.3), Scalar(-1.7), Scalar(2.2))}; + const Scalar scale = Scalar(3.7); + + for (const ShapeBase* shape : {static_cast(&sphere), + static_cast(&cast_sphere)}) { + for (const Vec3s& dir : dirs) { + int hint1 = 0; + int hint2 = 0; + const Vec3s s1 = + details::getSupport( + shape, dir, hint1); + const Vec3s s2 = + details::getSupport( + shape, scale * dir, hint2); + BOOST_CHECK_SMALL((s1 - s2).norm(), Scalar(1e-12)); + + hint1 = 0; + hint2 = 0; + const Vec3s ss1 = + details::getSupport( + shape, dir, hint1); + const Vec3s ss2 = + details::getSupport( + shape, scale * dir, hint2); + BOOST_CHECK_SMALL((ss1 - ss2).norm(), Scalar(1e-12)); + } + } +} + +// ============================================================================ +// A GEOM_CUSTOM shape that does not override computeShapeSupport(). +// ============================================================================ +class NoOverrideCustomShape : public ShapeBase { + public: + NoOverrideCustomShape* clone() const override { + return new NoOverrideCustomShape(*this); + } + + NODE_TYPE getNodeType() const override { return GEOM_CUSTOM; } + + void computeLocalAABB() override { + aabb_local.min_ = Vec3s::Constant(-1); + aabb_local.max_ = Vec3s::Constant(1); + aabb_center = Vec3s::Zero(); + aabb_radius = std::sqrt(Scalar(3)); + } + + bool isEqual(const CollisionGeometry& other) const override { + return dynamic_cast(&other) != nullptr; + } +}; + +/// The default computeShapeSupport() delegates to the built-in support +/// functions: it returns the same core (NoSweptSphere) support point as +/// details::getSupport for every built-in shape. +BOOST_AUTO_TEST_CASE(test_default_compute_shape_support_delegates) { + const Box box(Scalar(1.0), Scalar(1.2), Scalar(0.8)); + const Sphere sphere(Scalar(0.5)); + const Ellipsoid ellipsoid(Scalar(0.5), Scalar(0.7), Scalar(0.9)); + const Capsule capsule(Scalar(0.4), Scalar(1.0)); + const Cone cone(Scalar(0.4), Scalar(1.0)); + const Cylinder cylinder(Scalar(0.4), Scalar(1.0)); + const TriangleP triangle(Vec3s(0, 0, 0), Vec3s(1, 0, 0), Vec3s(0, 1, 0)); + const ConvexTpl convex = + constructPolytopeFromEllipsoid(Ellipsoid(0.6, 0.8, 1.0)); + const Plane plane(Vec3s(0, 0, 1), Scalar(0)); + const Halfspace halfspace(Vec3s(0, 0, 1), Scalar(0)); + + const std::vector shapes = { + &box, &sphere, &ellipsoid, &capsule, &cone, + &cylinder, &triangle, &convex, &plane, &halfspace}; + const std::vector dirs = { + Vec3s(1, 0, 0), Vec3s(0, -1, 0), Vec3s(0, 0, 1), Vec3s(1, 1, 1), + Vec3s(Scalar(0.3), Scalar(-1.7), Scalar(2.2))}; + + for (const ShapeBase* shape : shapes) { + for (const Vec3s& dir : dirs) { + int hint_virtual = 0; + int hint_direct = 0; + details::ShapeSupportData data_virtual; + details::ShapeSupportData data_direct; + Vec3s support; + shape->computeShapeSupport(dir, support, hint_virtual, data_virtual); + const Vec3s expected = + details::getSupport( + shape, dir, hint_direct, data_direct); + BOOST_CHECK(support == expected); + } + } + + // Unbounded shapes have no support point; the dispatch returns zero. + int hint = 0; + details::ShapeSupportData data; + Vec3s support; + plane.computeShapeSupport(Vec3s(0, 0, 1), support, hint, data); + BOOST_CHECK(support.isZero()); +} + +/// A GEOM_CUSTOM shape without a computeShapeSupport() override is a +/// programming error and must throw, not recurse or return garbage. +BOOST_AUTO_TEST_CASE(test_custom_shape_without_override_throws) { + NoOverrideCustomShape shape; + shape.computeLocalAABB(); + Vec3s support; + int hint = 0; + details::ShapeSupportData data; + BOOST_CHECK_THROW( + shape.computeShapeSupport(Vec3s(0, 0, 1), support, hint, data), + std::logic_error); +} diff --git a/test/custom_shape_deformed_cylinder.cpp b/test/custom_shape_deformed_cylinder.cpp new file mode 100644 index 000000000..a49267411 --- /dev/null +++ b/test/custom_shape_deformed_cylinder.cpp @@ -0,0 +1,293 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, INRIA + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/// @file custom_shape_deformed_cylinder.cpp +/// @brief Demonstrates how GEOM_CUSTOM enables the "deformed cylinder" shape +/// from coal-library/coal#792 without any changes to Coal internals. +/// A deformed cylinder is the convex hull of two arbitrarily oriented +/// disks, useful for modelling joints in articulated cylindrical +/// structures (e.g. robotic fingers). + +#define BOOST_TEST_MODULE COAL_CUSTOM_SHAPE_DEFORMED_CYLINDER +#include +#include + +#include "coal/collision.h" +#include "coal/contact_patch.h" +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/support_functions.h" + +using namespace coal; + +// ============================================================================ +// DeformedCylinder: convex hull of two arbitrarily oriented disks. +// +// Each disk is defined by a center (in local frame), a unit normal, and a +// radius. This models the joint piece between two tilted cylindrical segments +// (see coal-library/coal#792). +// +// The support function is closed-form: for each disk, the farthest point in +// direction d is center_i + r_i * normalize(d - (d·n_i)n_i). We return +// whichever disk yields the larger dot(d, support). +// ============================================================================ +class DeformedCylinder : public ShapeBase { + public: + DeformedCylinder(const Vec3s& p1, const Vec3s& n1, Scalar r1, const Vec3s& p2, + const Vec3s& n2, Scalar r2) + : ShapeBase(), + p1(p1), + n1(n1.normalized()), + r1(r1), + p2(p2), + n2(n2.normalized()), + r2(r2) {} + + DeformedCylinder* clone() const override { + return new DeformedCylinder(*this); + } + + NODE_TYPE getNodeType() const override { return GEOM_CUSTOM; } + + void computeLocalAABB() override { + // Closed-form AABB from each disk's per-axis extent: + // p_i ± r*sqrt(1 - n_i²). + for (int i = 0; i < 3; ++i) { + const Scalar extent1 = r1 * std::sqrt(1 - n1[i] * n1[i]); + const Scalar extent2 = r2 * std::sqrt(1 - n2[i] * n2[i]); + aabb_local.max_[i] = std::max(p1[i] + extent1, p2[i] + extent2); + aabb_local.min_[i] = std::min(p1[i] - extent1, p2[i] - extent2); + } + const Scalar ssr = this->getSweptSphereRadius(); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); + aabb_center = (aabb_local.min_ + aabb_local.max_) / 2; + aabb_radius = (aabb_local.max_ - aabb_local.min_).norm() / 2; + } + + void computeShapeSupport(const Vec3s& dir, Vec3s& support, int& /*hint*/, + details::ShapeSupportData& /*data*/) const override { + // When dir is parallel to a disk normal, the projection is zero and + // the support degenerates to the disk center. + auto disk_support = [](const Vec3s& p, const Vec3s& n, Scalar r, + const Vec3s& d) -> Vec3s { + Vec3s proj = d - d.dot(n) * n; + Scalar len = proj.norm(); + if (len > Scalar(1e-12)) return p + r * (proj / len); + return p; + }; + + Vec3s s1 = disk_support(p1, n1, r1, dir); + Vec3s s2 = disk_support(p2, n2, r2, dir); + support = (dir.dot(s1) >= dir.dot(s2)) ? s1 : s2; + } + + bool isEqual(const CollisionGeometry& other) const override { + const auto* o = dynamic_cast(&other); + if (o == nullptr) return false; + return p1 == o->p1 && n1 == o->n1 && r1 == o->r1 && p2 == o->p2 && + n2 == o->n2 && r2 == o->r2; + } + + Vec3s p1; ///< disk 1 center (local frame) + Vec3s n1; ///< disk 1 normal (unit) + Scalar r1; ///< disk 1 radius + Vec3s p2; ///< disk 2 center (local frame) + Vec3s n2; ///< disk 2 normal (unit) + Scalar r2; ///< disk 2 radius +}; + +// ============================================================================ +// Tests +// ============================================================================ + +BOOST_AUTO_TEST_CASE(test_deformed_cylinder_node_type) { + DeformedCylinder dc(Vec3s(0, 0, 0), Vec3s::UnitZ(), 1.0, Vec3s(0, 0, 2), + Vec3s::UnitZ(), 1.0); + BOOST_CHECK_EQUAL(dc.getObjectType(), OT_GEOM); + BOOST_CHECK_EQUAL(dc.getNodeType(), GEOM_CUSTOM); +} + +/// Collision and distance: 30° joint deformed cylinder vs Box. +BOOST_AUTO_TEST_CASE(test_deformed_cylinder_vs_box) { + // Two disks at a 30° joint angle, radius 0.5, separated by 2 units + const Scalar angle = boost::math::constants::pi() / 6; // 30° + const Scalar r = 0.5; + Vec3s n1 = Vec3s::UnitZ(); + Vec3s n2(std::sin(angle), 0, std::cos(angle)); + DeformedCylinder dc(Vec3s(0, 0, 0), n1, r, Vec3s(0, 0, 2), n2, r); + dc.computeLocalAABB(); + + Box box(1.0, 1.0, 1.0); + + // Collision: box overlapping the deformed cylinder + { + CollisionRequest request; + CollisionResult result; + Transform3s tf1; + Transform3s tf2(Quats::Identity(), Vec3s(0, 0, 1)); + std::size_t n = collide(&dc, tf1, &box, tf2, request, result); + BOOST_CHECK_GT(n, 0u); + } + + // No collision: box far away + { + CollisionRequest request; + CollisionResult result; + Transform3s tf1; + Transform3s tf2(Quats::Identity(), Vec3s(5, 0, 1)); + std::size_t n = collide(&dc, tf1, &box, tf2, request, result); + BOOST_CHECK_EQUAL(n, 0u); + } + + // Distance: box separated from deformed cylinder + { + DistanceRequest request(true); + DistanceResult result; + Transform3s tf1; + Transform3s tf2(Quats::Identity(), Vec3s(3, 0, 1)); + Scalar d = distance(&dc, tf1, &box, tf2, request, result); + BOOST_CHECK_GT(d, Scalar(0)); + BOOST_CHECK_CLOSE(d, Scalar(2.0), Scalar(10)); + } +} + +/// When both disks are parallel with the same radius, the deformed cylinder +/// degenerates to a standard cylinder. Verify distance matches Coal's built-in +/// Cylinder. +BOOST_AUTO_TEST_CASE(test_deformed_cylinder_degenerates_to_cylinder) { + const Scalar r = 0.5; + const Scalar half_h = 1.0; + + // DeformedCylinder with parallel disks along Z + DeformedCylinder dc(Vec3s(0, 0, -half_h), Vec3s::UnitZ(), r, + Vec3s(0, 0, half_h), Vec3s::UnitZ(), r); + dc.computeLocalAABB(); + + // Coal built-in Cylinder (centered at origin, total height = 2*half_h) + Cylinder cyl(r, 2 * half_h); + + Sphere probe(0.1); + + // Test distance from several directions + Vec3s offsets[] = { + Vec3s(3, 0, 0), // radial + Vec3s(0, 0, 3), // axial above + Vec3s(0, 0, -3), // axial below + Vec3s(2, 2, 0.5), // diagonal + }; + + for (const auto& offset : offsets) { + DistanceRequest request(true); + Transform3s tf1; + Transform3s tf2(Quats::Identity(), offset); + + DistanceResult res_dc; + Scalar d_dc = distance(&dc, tf1, &probe, tf2, request, res_dc); + + DistanceResult res_cyl; + Scalar d_cyl = distance(&cyl, tf1, &probe, tf2, request, res_cyl); + + // Both should be positive (probe is far enough) + BOOST_CHECK_GT(d_dc, Scalar(0)); + BOOST_CHECK_GT(d_cyl, Scalar(0)); + + // Distances should be very close (not exact due to GJK tolerance) + BOOST_CHECK_CLOSE(d_dc, d_cyl, Scalar(1)); // 1% tolerance + } +} + +/// Contact patch: deformed cylinder vs Box. +BOOST_AUTO_TEST_CASE(test_deformed_cylinder_contact_patch) { + const Scalar r = 0.5; + DeformedCylinder dc(Vec3s(0, 0, 0), Vec3s::UnitZ(), r, Vec3s(0, 0, 2), + Vec3s::UnitZ(), r); + dc.computeLocalAABB(); + + Box box(2.0, 2.0, 2.0); + + Transform3s tf1; + Transform3s tf2(Quats::Identity(), Vec3s(0, 0, 1)); + + const CollisionRequest col_req(CollisionRequestFlag::CONTACT, 1); + CollisionResult col_res; + coal::collide(&dc, tf1, &box, tf2, col_req, col_res); + BOOST_REQUIRE(col_res.isCollision()); + + const ContactPatchRequest patch_req; + ContactPatchResult patch_res(patch_req); + BOOST_CHECK_NO_THROW(coal::computeContactPatch(&dc, tf1, &box, tf2, col_res, + patch_req, patch_res)); + BOOST_CHECK(patch_res.numContactPatches() > 0); +} + +/// Support points must be invariant to the magnitude of the support +/// direction: GJK passes raw, possibly non-unit-length directions. +BOOST_AUTO_TEST_CASE(test_deformed_cylinder_support_scale_invariance) { + const Scalar angle = boost::math::constants::pi() / 6; // 30° + DeformedCylinder dc(Vec3s(0, 0, 0), Vec3s::UnitZ(), Scalar(0.5), + Vec3s(0, 0, 2), + Vec3s(std::sin(angle), 0, std::cos(angle)), Scalar(0.5)); + dc.setSweptSphereRadius(Scalar(0.1)); + dc.computeLocalAABB(); + + const std::vector dirs = { + Vec3s(1, 0, 0), Vec3s(0, -1, 0), + Vec3s(0, 0, 1), Vec3s(1, 1, 1), + Vec3s(-2, 3, 5), Vec3s(Scalar(0.3), Scalar(-1.7), Scalar(2.2))}; + const Scalar scale = Scalar(3.7); + + for (const Vec3s& dir : dirs) { + int hint1 = 0; + int hint2 = 0; + const Vec3s s1 = + details::getSupport(&dc, dir, + hint1); + const Vec3s s2 = + details::getSupport( + &dc, scale * dir, hint2); + BOOST_CHECK_SMALL((s1 - s2).norm(), Scalar(1e-12)); + + hint1 = 0; + hint2 = 0; + const Vec3s ss1 = + details::getSupport(&dc, dir, + hint1); + const Vec3s ss2 = + details::getSupport( + &dc, scale * dir, hint2); + BOOST_CHECK_SMALL((ss1 - ss2).norm(), Scalar(1e-12)); + } +}