[Collision] Add ccd collision detection based on TightInclusion method

Sofa/Component/Collision/Detection/Algorithm/src/sofa/component/collision/detection/algorithm/CollisionPipeline.cpp

@@ -149,6 +149,7 @@ void CollisionPipeline::doCollisionDetection(const type::vector<core::CollisionM
         simulation::Visitor::printNode("ComputeBoundingTree");
 #endif
         const bool continuous = intersectionMethod->useContinuous();
+        const auto continuousIntersectionType = intersectionMethod->continuousIntersectionType();
         const SReal dt       = getContext()->getDt();
 
         type::vector<CollisionModel*>::const_iterator it;
@@ -171,7 +172,7 @@ void CollisionPipeline::doCollisionDetection(const type::vector<core::CollisionM
             {
                 const std::string msg = "Compute Continuous BoundingTree: " + (*it)->getName();
                 ScopedAdvancedTimer continuousBoundingTreeTimer(msg.c_str());
-                (*it)->computeContinuousBoundingTree(dt, used_depth);
+                (*it)->computeContinuousBoundingTree(dt, continuousIntersectionType, used_depth);
             }
             else
             {

Sofa/Component/Collision/Detection/Intersection/CMakeLists.txt

@@ -7,6 +7,7 @@ set(HEADER_FILES
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/config.h.in
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/init.h
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/BaseProximityIntersection.h
+    ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/CCDTightInclusionIntersection.h
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/DiscreteIntersection.h
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/LocalMinDistance.h
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/MeshDiscreteIntersection.h
@@ -26,6 +27,7 @@ set(HEADER_FILES
 set(SOURCE_FILES
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/init.cpp
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/BaseProximityIntersection.cpp
+    ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/CCDTightInclusionIntersection.cpp
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/DiscreteIntersection.cpp
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/LocalMinDistance.cpp
     ${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/MeshDiscreteIntersection.cpp
@@ -42,8 +44,23 @@ sofa_find_package(Sofa.Simulation.Core REQUIRED)
 sofa_find_package(Sofa.Component.Collision.Geometry REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${WRAPPER_FILES})
+
+sofa_find_package(tight_inclusion QUIET)
+
+if(NOT tight_inclusion_FOUND AND SOFA_ALLOW_FETCH_DEPENDENCIES)
+
+    sofa_fetch_dependency(tight_inclusion
+            GIT_REPOSITORY https://github.com/sofa-framework/Tight-Inclusion
+            GIT_TAG        v1.0.6-export-target
+    )
+
+elseif(NOT tight_inclusion_FOUND )
+    message(FATAL_ERROR "Sofa.Component.Collision.Detection.Intersection: tight_inclusion is not found. SOFA_ALLOW_FETCH_DEPENDENCIES is OFF and thus cannot be fetched. Provide a tight_inclusion library installation, or enable SOFA_ALLOW_FETCH_DEPENDENCIES to fix this issue.")
+endif ()
+
 target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Simulation.Core)
 target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.Collision.Geometry)
+target_link_libraries(${PROJECT_NAME} PRIVATE tight_inclusion)
 
 sofa_create_package_with_targets(
     PACKAGE_NAME ${PROJECT_NAME}

Sofa/Component/Collision/Detection/Intersection/src/sofa/component/collision/detection/intersection/CCDTightInclusionIntersection.h

@@ -0,0 +1,124 @@
+/******************************************************************************
+*                 SOFA, Simulation Open-Framework Architecture                *
+*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
+*                                                                             *
+* This program is free software; you can redistribute it and/or modify it     *
+* under the terms of the GNU Lesser General Public License as published by    *
+* the Free Software Foundation; either version 2.1 of the License, or (at     *
+* your option) any later version.                                             *
+*                                                                             *
+* This program is distributed in the hope that it will be useful, but WITHOUT *
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
+* for more details.                                                           *
+*                                                                             *
+* You should have received a copy of the GNU Lesser General Public License    *
+* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
+*******************************************************************************
+* Authors: The SOFA Team and external contributors (see Authors.txt)          *
+*                                                                             *
+* Contact information: [email protected]                             *
+******************************************************************************/
+#pragma once
+#include <sofa/component/collision/detection/intersection/config.h>
+
+#include <sofa/component/collision/detection/intersection/BaseProximityIntersection.h>
+
+#include <sofa/component/collision/geometry/SphereModel.h>
+#include <sofa/component/collision/geometry/TriangleModel.h>
+#include <sofa/component/collision/geometry/LineModel.h>
+#include <sofa/component/collision/geometry/PointModel.h>
+#include <sofa/component/collision/geometry/CubeModel.h>
+#include <sofa/component/collision/geometry/RayModel.h>
+
+namespace sofa::component::collision::detection::intersection
+{
+
+/**
+ * Intersection methods using proximities. Filters are added to limit the number of contacts.
+ * The following pairs of collision models are supported:
+ * - Cube/Cube
+ * - Line/Line
+ * - Triangle/Point
+ * The following pairs of collision models are ignored:
+ * - Sphere/Sphere
+ * - Sphere/Point
+ * - Point/Point
+ * - Line/Point
+ * - Line/Sphere
+ * - Triangle/Line
+ * - Triangle/Triangle
+ * - Triangle/Sphere
+ * - Ray/Triangle
+ * - Ray/Sphere
+ * - Ray/Point
+ * - Ray/Line
+ */
+class SOFA_COMPONENT_COLLISION_DETECTION_INTERSECTION_API CCDTightInclusionIntersection
+    : public BaseProximityIntersection
+{
+   public:
+    SOFA_CLASS(CCDTightInclusionIntersection, BaseProximityIntersection);
+
+    typedef core::collision::IntersectorFactory<CCDTightInclusionIntersection> IntersectorFactory;
+
+   protected:
+    CCDTightInclusionIntersection();
+
+   public:
+    void init() override;
+
+    virtual bool useContinuous() const override;
+    virtual core::CollisionModel::ContinuousIntersectionTypeFlag continuousIntersectionType() const;
+
+
+    bool testIntersection(collision::geometry::Cube&, collision::geometry::Cube&,
+                          const core::collision::Intersection* currentIntersection) override;
+
+    // bool testIntersection(collision::geometry::Point&, collision::geometry::Point&, const
+    // core::collision::Intersection* currentIntersection); bool
+    // testIntersection(collision::geometry::Sphere&, collision::geometry::Point&, const
+    // core::collision::Intersection* currentIntersection); bool
+    // testIntersection(collision::geometry::Sphere&, collision::geometry::Sphere&, const
+    // core::collision::Intersection* currentIntersection) override; bool
+    // testIntersection(collision::geometry::Line&, collision::geometry::Point&, const
+    // core::collision::Intersection* currentIntersection); bool
+    // testIntersection(collision::geometry::Line&, collision::geometry::Sphere&, const
+    // core::collision::Intersection* currentIntersection);
+    bool testIntersection(collision::geometry::Line&, collision::geometry::Line&,
+                          const core::collision::Intersection* currentIntersection);
+    bool testIntersection(collision::geometry::Triangle&, collision::geometry::Point&,
+                          const core::collision::Intersection* currentIntersection);
+    // int testIntersection(collision::geometry::Triangle&, collision::geometry::Sphere&, const
+    // core::collision::Intersection* currentIntersection);
+
+    int computeIntersection(collision::geometry::Cube&, collision::geometry::Cube&, OutputVector*,
+                            const core::collision::Intersection* currentIntersection) override;
+    // int computeIntersection(collision::geometry::Point&, collision::geometry::Point&,
+    // OutputVector*, const core::collision::Intersection* currentIntersection); int
+    // computeIntersection(collision::geometry::Sphere&, collision::geometry::Point&, OutputVector*,
+    // const core::collision::Intersection* currentIntersection); int
+    // computeIntersection(collision::geometry::Sphere&, collision::geometry::Sphere&,
+    // OutputVector*, const core::collision::Intersection* currentIntersection) override; int
+    // computeIntersection(collision::geometry::Line&, collision::geometry::Point&, OutputVector*,
+    // const core::collision::Intersection* currentIntersection); int
+    // computeIntersection(collision::geometry::Line&, collision::geometry::Sphere&, OutputVector*,
+    // const core::collision::Intersection* currentIntersection);
+    int computeIntersection(collision::geometry::Line&, collision::geometry::Line&, OutputVector*,
+                            const core::collision::Intersection* currentIntersection);
+    int computeIntersection(collision::geometry::Triangle&, collision::geometry::Point&,
+                            OutputVector*,
+                            const core::collision::Intersection* currentIntersection);
+    // int computeIntersection(collision::geometry::Triangle&, collision::geometry::Sphere&,
+    // OutputVector*, const core::collision::Intersection* currentIntersection); int
+    // computeIntersection(collision::geometry::Ray&, collision::geometry::Sphere&, OutputVector*,
+    // const core::collision::Intersection* currentIntersection); int
+    // computeIntersection(collision::geometry::Ray&, collision::geometry::Triangle&, OutputVector*,
+    // const core::collision::Intersection* currentIntersection);
+
+
+    Data<sofa::helper::OptionsGroup> d_continuousCollisionType;
+    Data<SReal> d_tolerance;
+    Data<long> d_maxIterations;
+};
+} // namespace sofa::component::collision::detection::intersection

Sofa/Component/Collision/Detection/Intersection/src/sofa/component/collision/detection/intersection/init.cpp

@@ -30,6 +30,7 @@ extern void registerDiscreteIntersection(sofa::core::ObjectFactory* factory);
 extern void registerLocalMinDistance(sofa::core::ObjectFactory* factory);
 extern void registerMinProximityIntersection(sofa::core::ObjectFactory* factory);
 extern void registerNewProximityIntersection(sofa::core::ObjectFactory* factory);
+extern void registerCCDTightInclusionIntersection(sofa::core::ObjectFactory* factory);
 
 extern "C" {
     SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule();
@@ -59,6 +60,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
     registerLocalMinDistance(factory);
     registerMinProximityIntersection(factory);
     registerNewProximityIntersection(factory);
+    registerCCDTightInclusionIntersection(factory);
 }
 
 void init()

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/LineCollisionModel.h

@@ -25,7 +25,8 @@
 #include <sofa/core/CollisionModel.h>
 #include <sofa/core/objectmodel/BaseObject.h>
 #include <sofa/core/topology/BaseMeshTopology.h>
-#include <sofa/defaulttype/VecTypes.h>
+#include <sofa/core/collision/Intersection.h>
+
 
 namespace sofa::component::collision::geometry
 {
@@ -123,7 +124,7 @@ public :
 
     void computeBoundingTree(int maxDepth=0) override;
 
-    void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
+    void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
 
     void handleTopologyChange() override;
 

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/LineCollisionModel.inl

@@ -495,7 +495,7 @@ void LineCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
 }
 
 template<class DataTypes>
-void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
+void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag, int maxDepth)
 {
     CubeCollisionModel* cubeModel = createPrevious<CubeCollisionModel>();
     updateFromTopology();
@@ -512,10 +512,10 @@ void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int
         for (sofa::Size i=0; i<size; i++)
         {
             TLine<DataTypes> t(this,i);
-            const type::Vec3& pt1 = t.p1();
-            const type::Vec3& pt2 = t.p2();
-            const type::Vec3 pt1v = pt1 + t.v1()*dt;
-            const type::Vec3 pt2v = pt2 + t.v2()*dt;
+            const auto& pt1 = t.p1();
+            const auto& pt2 = t.p2();
+            const auto pt1v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt1 + t.v1()*dt : t.p1Free());
+            const auto pt2v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt2 + t.v2()*dt : t.p2Free());
 
             for (int c = 0; c < 3; c++)
             {

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/PointCollisionModel.h

@@ -91,7 +91,7 @@ class PointCollisionModel : public core::CollisionModel
 
     void computeBoundingTree(int maxDepth=0) override;
 
-    void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
+    void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
 
     void draw(const core::visual::VisualParams*, sofa::Index index) override;
 

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/PointCollisionModel.inl

@@ -143,7 +143,7 @@ void PointCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
 }
 
 template<class DataTypes>
-void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
+void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag , int maxDepth)
 {
     CubeCollisionModel* cubeModel = createPrevious<CubeCollisionModel>();
     const auto npoints = mstate->getSize();
@@ -169,7 +169,8 @@ void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int
         {
             TPoint<DataTypes> p(this,i);
             const type::Vec3& pt = p.p();
-            const type::Vec3 ptv = pt + p.v()*dt;
+            const type::Vec3 ptv = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt + p.v()*dt : p.pFree());
+
 
             for (int c = 0; c < 3; c++)
             {

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/SphereCollisionModel.h

@@ -110,7 +110,7 @@ class SphereCollisionModel : public core::CollisionModel
 
     void computeBoundingTree(int maxDepth=0) override;
 
-    void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
+    void computeContinuousBoundingTree(SReal dt,  ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia,  int maxDepth=0) override;
 
     void draw(const core::visual::VisualParams*, sofa::Index index) override;
 
@@ -195,7 +195,7 @@ template<class DataTypes>
 inline const typename TSphere<DataTypes>::Coord& TSphere<DataTypes>::p() const { return DataTypes::getCPos(this->model->mstate->read(core::vec_id::read_access::position)->getValue()[this->index]);}
 
 template<class DataTypes>
-inline const typename TSphere<DataTypes>::Coord& TSphere<DataTypes>::pFree() const { return (*this->model->mstate->read(core::vec_id::read_access::freePosition)).getValue()[this->index]; }
+inline const typename TSphere<DataTypes>::Coord& TSphere<DataTypes>::pFree() const { return DataTypes::getCPos((*this->model->mstate->read(core::vec_id::read_access::freePosition)).getValue()[this->index]); }
 
 template<class DataTypes>
 inline const typename SphereCollisionModel<DataTypes>::Coord& SphereCollisionModel<DataTypes>::velocity(sofa::Index index) const { return DataTypes::getDPos(mstate->read(core::vec_id::read_access::velocity)->getValue()[index]);}

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/SphereCollisionModel.inl

@@ -194,7 +194,7 @@ void SphereCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
 
 
 template <class DataTypes>
-void SphereCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
+void SphereCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag  continuousIntersectionFlag, int maxDepth)
 {
     using sofa::type::Vec3 ;
 
@@ -223,8 +223,8 @@ void SphereCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, in
         for (sofa::Size i=0; i<size; i++)
         {
             TSphere<DataTypes> p(this,i);
-            const Vec3& pt = p.p();
-            const Vec3 ptv = pt + p.v()*dt;
+            const auto& pt = p.p();
+            const auto ptv = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia) ? pt + p.v()*dt : p.pFree();
 
             for (int c = 0; c < 3; c++)
             {

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/TriangleCollisionModel.h

@@ -179,7 +179,7 @@ class TriangleCollisionModel : public core::CollisionModel
 
     void computeBoundingTree(int maxDepth=0) override;
 
-    void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
+    void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
 
     void draw(const core::visual::VisualParams*, sofa::Index index) override;
 

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/TriangleCollisionModel.inl

@@ -276,7 +276,7 @@ void TriangleCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
 }
 
 template<class DataTypes>
-void TriangleCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
+void TriangleCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag, int maxDepth)
 {
     CubeCollisionModel* cubeModel = createPrevious<CubeCollisionModel>();
 
@@ -297,12 +297,12 @@ void TriangleCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt,
         for (sofa::Size i=0; i<size; i++)
         {
             Element t(this,i);
-            const type::Vec3& pt1 = t.p1();
-            const type::Vec3& pt2 = t.p2();
-            const type::Vec3& pt3 = t.p3();
-            const type::Vec3 pt1v = pt1 + t.v1()*dt;
-            const type::Vec3 pt2v = pt2 + t.v2()*dt;
-            const type::Vec3 pt3v = pt3 + t.v3()*dt;
+            const auto& pt1 = t.p1();
+            const auto& pt2 = t.p2();
+            const auto& pt3 = t.p3();
+            const auto pt1v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt1 + t.v1()*dt : t.p1Free());
+            const auto pt2v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt2 + t.v2()*dt : t.p2Free());
+            const auto pt3v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt3 + t.v3()*dt : t.p3Free());
 
             for (int c = 0; c < 3; c++)
             {

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/TriangleOctreeCollisionModel.cpp

@@ -111,8 +111,6 @@ void TriangleOctreeCollisionModel::computeBoundingTree(int maxDepth)
 
         for (auto& p : pt)
         {
-
-
             for(int c=0; c<3; c++)
             {
                 if ((*p)[c] > maxElem[c]) maxElem[c] = (*p)[c];
@@ -131,7 +129,7 @@ void TriangleOctreeCollisionModel::computeBoundingTree(int maxDepth)
     }
 }
 
-void TriangleOctreeCollisionModel::computeContinuousBoundingTree(SReal/* dt*/, int maxDepth)
+void TriangleOctreeCollisionModel::computeContinuousBoundingTree(SReal/* dt*/, ContinuousIntersectionTypeFlag continuousIntersectionFlag, int maxDepth)
 {
     computeBoundingTree(maxDepth);
 }

Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/TriangleOctreeCollisionModel.h

@@ -46,7 +46,7 @@ class SOFA_COMPONENT_COLLISION_GEOMETRY_API TriangleOctreeCollisionModel : publi
     /// the normals for each point
     type::vector<type::Vec3> pNorms;
     void computeBoundingTree(int maxDepth=0) override;
-    void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
+    void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
     /// init the octree creation
     void buildOctree ();
 };