Skip to content

Commit 0b57eb7

Browse files
bakpaulhugtalbotfredroy
authored
[Collision] Add ccd collision detection based on TightInclusion method (#5792)
* Add tight inclusion as dependency in cmake * Compiling with edge edge and point triangle * Little cleaning * WIP modify cube collision and pipeline to enable CCD * Revert changes on cube models * Add a way to specify the type of continuous collision wanted * Code wortking ! * Rename CCDIntersection into CCDTightInclusionIntersection * Fix bad diff * Fix continuous position computationf or freemotion * Fix Cuda compilation * Fix barycentric coordinates for Edge Edge collision + remove debug output * Remove unwanted change * Update Sofa/Component/Collision/Geometry/src/sofa/component/collision/geometry/SphereCollisionModel.inl Co-authored-by: Hugo <[email protected]> --------- Co-authored-by: Hugo <[email protected]> Co-authored-by: Frederick Roy <[email protected]>
1 parent b90b636 commit 0b57eb7

File tree

19 files changed

+572
-77
lines changed

19 files changed

+572
-77
lines changed

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,7 @@ void CollisionPipeline::doCollisionDetection(const type::vector<core::CollisionM
149149
simulation::Visitor::printNode("ComputeBoundingTree");
150150
#endif
151151
const bool continuous = intersectionMethod->useContinuous();
152+
const auto continuousIntersectionType = intersectionMethod->continuousIntersectionType();
152153
const SReal dt = getContext()->getDt();
153154

154155
type::vector<CollisionModel*>::const_iterator it;
@@ -171,7 +172,7 @@ void CollisionPipeline::doCollisionDetection(const type::vector<core::CollisionM
171172
{
172173
const std::string msg = "Compute Continuous BoundingTree: " + (*it)->getName();
173174
ScopedAdvancedTimer continuousBoundingTreeTimer(msg.c_str());
174-
(*it)->computeContinuousBoundingTree(dt, used_depth);
175+
(*it)->computeContinuousBoundingTree(dt, continuousIntersectionType, used_depth);
175176
}
176177
else
177178
{

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

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ set(HEADER_FILES
77
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/config.h.in
88
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/init.h
99
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/BaseProximityIntersection.h
10+
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/CCDTightInclusionIntersection.h
1011
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/DiscreteIntersection.h
1112
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/LocalMinDistance.h
1213
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/MeshDiscreteIntersection.h
@@ -26,6 +27,7 @@ set(HEADER_FILES
2627
set(SOURCE_FILES
2728
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/init.cpp
2829
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/BaseProximityIntersection.cpp
30+
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/CCDTightInclusionIntersection.cpp
2931
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/DiscreteIntersection.cpp
3032
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/LocalMinDistance.cpp
3133
${SOFACOMPONENTCOLLISIONDETECTIONINTERSECTION_SOURCE_DIR}/MeshDiscreteIntersection.cpp
@@ -42,8 +44,23 @@ sofa_find_package(Sofa.Simulation.Core REQUIRED)
4244
sofa_find_package(Sofa.Component.Collision.Geometry REQUIRED)
4345

4446
add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${WRAPPER_FILES})
47+
48+
sofa_find_package(tight_inclusion QUIET)
49+
50+
if(NOT tight_inclusion_FOUND AND SOFA_ALLOW_FETCH_DEPENDENCIES)
51+
52+
sofa_fetch_dependency(tight_inclusion
53+
GIT_REPOSITORY https://github.com/sofa-framework/Tight-Inclusion
54+
GIT_TAG v1.0.6-export-target
55+
)
56+
57+
elseif(NOT tight_inclusion_FOUND )
58+
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.")
59+
endif ()
60+
4561
target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Simulation.Core)
4662
target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.Collision.Geometry)
63+
target_link_libraries(${PROJECT_NAME} PRIVATE tight_inclusion)
4764

4865
sofa_create_package_with_targets(
4966
PACKAGE_NAME ${PROJECT_NAME}

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

Lines changed: 305 additions & 0 deletions
Large diffs are not rendered by default.

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

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
/******************************************************************************
2+
* SOFA, Simulation Open-Framework Architecture *
3+
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
4+
* *
5+
* This program is free software; you can redistribute it and/or modify it *
6+
* under the terms of the GNU Lesser General Public License as published by *
7+
* the Free Software Foundation; either version 2.1 of the License, or (at *
8+
* your option) any later version. *
9+
* *
10+
* This program is distributed in the hope that it will be useful, but WITHOUT *
11+
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
12+
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
13+
* for more details. *
14+
* *
15+
* You should have received a copy of the GNU Lesser General Public License *
16+
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
17+
*******************************************************************************
18+
* Authors: The SOFA Team and external contributors (see Authors.txt) *
19+
* *
20+
* Contact information: [email protected] *
21+
******************************************************************************/
22+
#pragma once
23+
#include <sofa/component/collision/detection/intersection/config.h>
24+
25+
#include <sofa/component/collision/detection/intersection/BaseProximityIntersection.h>
26+
27+
#include <sofa/component/collision/geometry/SphereModel.h>
28+
#include <sofa/component/collision/geometry/TriangleModel.h>
29+
#include <sofa/component/collision/geometry/LineModel.h>
30+
#include <sofa/component/collision/geometry/PointModel.h>
31+
#include <sofa/component/collision/geometry/CubeModel.h>
32+
#include <sofa/component/collision/geometry/RayModel.h>
33+
34+
namespace sofa::component::collision::detection::intersection
35+
{
36+
37+
/**
38+
* Intersection methods using proximities. Filters are added to limit the number of contacts.
39+
* The following pairs of collision models are supported:
40+
* - Cube/Cube
41+
* - Line/Line
42+
* - Triangle/Point
43+
* The following pairs of collision models are ignored:
44+
* - Sphere/Sphere
45+
* - Sphere/Point
46+
* - Point/Point
47+
* - Line/Point
48+
* - Line/Sphere
49+
* - Triangle/Line
50+
* - Triangle/Triangle
51+
* - Triangle/Sphere
52+
* - Ray/Triangle
53+
* - Ray/Sphere
54+
* - Ray/Point
55+
* - Ray/Line
56+
*/
57+
class SOFA_COMPONENT_COLLISION_DETECTION_INTERSECTION_API CCDTightInclusionIntersection
58+
: public BaseProximityIntersection
59+
{
60+
public:
61+
SOFA_CLASS(CCDTightInclusionIntersection, BaseProximityIntersection);
62+
63+
typedef core::collision::IntersectorFactory<CCDTightInclusionIntersection> IntersectorFactory;
64+
65+
protected:
66+
CCDTightInclusionIntersection();
67+
68+
public:
69+
void init() override;
70+
71+
virtual bool useContinuous() const override;
72+
virtual core::CollisionModel::ContinuousIntersectionTypeFlag continuousIntersectionType() const;
73+
74+
75+
bool testIntersection(collision::geometry::Cube&, collision::geometry::Cube&,
76+
const core::collision::Intersection* currentIntersection) override;
77+
78+
// bool testIntersection(collision::geometry::Point&, collision::geometry::Point&, const
79+
// core::collision::Intersection* currentIntersection); bool
80+
// testIntersection(collision::geometry::Sphere&, collision::geometry::Point&, const
81+
// core::collision::Intersection* currentIntersection); bool
82+
// testIntersection(collision::geometry::Sphere&, collision::geometry::Sphere&, const
83+
// core::collision::Intersection* currentIntersection) override; bool
84+
// testIntersection(collision::geometry::Line&, collision::geometry::Point&, const
85+
// core::collision::Intersection* currentIntersection); bool
86+
// testIntersection(collision::geometry::Line&, collision::geometry::Sphere&, const
87+
// core::collision::Intersection* currentIntersection);
88+
bool testIntersection(collision::geometry::Line&, collision::geometry::Line&,
89+
const core::collision::Intersection* currentIntersection);
90+
bool testIntersection(collision::geometry::Triangle&, collision::geometry::Point&,
91+
const core::collision::Intersection* currentIntersection);
92+
// int testIntersection(collision::geometry::Triangle&, collision::geometry::Sphere&, const
93+
// core::collision::Intersection* currentIntersection);
94+
95+
int computeIntersection(collision::geometry::Cube&, collision::geometry::Cube&, OutputVector*,
96+
const core::collision::Intersection* currentIntersection) override;
97+
// int computeIntersection(collision::geometry::Point&, collision::geometry::Point&,
98+
// OutputVector*, const core::collision::Intersection* currentIntersection); int
99+
// computeIntersection(collision::geometry::Sphere&, collision::geometry::Point&, OutputVector*,
100+
// const core::collision::Intersection* currentIntersection); int
101+
// computeIntersection(collision::geometry::Sphere&, collision::geometry::Sphere&,
102+
// OutputVector*, const core::collision::Intersection* currentIntersection) override; int
103+
// computeIntersection(collision::geometry::Line&, collision::geometry::Point&, OutputVector*,
104+
// const core::collision::Intersection* currentIntersection); int
105+
// computeIntersection(collision::geometry::Line&, collision::geometry::Sphere&, OutputVector*,
106+
// const core::collision::Intersection* currentIntersection);
107+
int computeIntersection(collision::geometry::Line&, collision::geometry::Line&, OutputVector*,
108+
const core::collision::Intersection* currentIntersection);
109+
int computeIntersection(collision::geometry::Triangle&, collision::geometry::Point&,
110+
OutputVector*,
111+
const core::collision::Intersection* currentIntersection);
112+
// int computeIntersection(collision::geometry::Triangle&, collision::geometry::Sphere&,
113+
// OutputVector*, const core::collision::Intersection* currentIntersection); int
114+
// computeIntersection(collision::geometry::Ray&, collision::geometry::Sphere&, OutputVector*,
115+
// const core::collision::Intersection* currentIntersection); int
116+
// computeIntersection(collision::geometry::Ray&, collision::geometry::Triangle&, OutputVector*,
117+
// const core::collision::Intersection* currentIntersection);
118+
119+
120+
Data<sofa::helper::OptionsGroup> d_continuousCollisionType;
121+
Data<SReal> d_tolerance;
122+
Data<long> d_maxIterations;
123+
};
124+
} // namespace sofa::component::collision::detection::intersection

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ extern void registerDiscreteIntersection(sofa::core::ObjectFactory* factory);
3030
extern void registerLocalMinDistance(sofa::core::ObjectFactory* factory);
3131
extern void registerMinProximityIntersection(sofa::core::ObjectFactory* factory);
3232
extern void registerNewProximityIntersection(sofa::core::ObjectFactory* factory);
33+
extern void registerCCDTightInclusionIntersection(sofa::core::ObjectFactory* factory);
3334

3435
extern "C" {
3536
SOFA_EXPORT_DYNAMIC_LIBRARY void initExternalModule();
@@ -59,6 +60,7 @@ void registerObjects(sofa::core::ObjectFactory* factory)
5960
registerLocalMinDistance(factory);
6061
registerMinProximityIntersection(factory);
6162
registerNewProximityIntersection(factory);
63+
registerCCDTightInclusionIntersection(factory);
6264
}
6365

6466
void init()

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,8 @@
2525
#include <sofa/core/CollisionModel.h>
2626
#include <sofa/core/objectmodel/BaseObject.h>
2727
#include <sofa/core/topology/BaseMeshTopology.h>
28-
#include <sofa/defaulttype/VecTypes.h>
28+
#include <sofa/core/collision/Intersection.h>
29+
2930

3031
namespace sofa::component::collision::geometry
3132
{
@@ -123,7 +124,7 @@ public :
123124

124125
void computeBoundingTree(int maxDepth=0) override;
125126

126-
void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
127+
void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
127128

128129
void handleTopologyChange() override;
129130

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -495,7 +495,7 @@ void LineCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
495495
}
496496

497497
template<class DataTypes>
498-
void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
498+
void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag, int maxDepth)
499499
{
500500
CubeCollisionModel* cubeModel = createPrevious<CubeCollisionModel>();
501501
updateFromTopology();
@@ -512,10 +512,10 @@ void LineCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int
512512
for (sofa::Size i=0; i<size; i++)
513513
{
514514
TLine<DataTypes> t(this,i);
515-
const type::Vec3& pt1 = t.p1();
516-
const type::Vec3& pt2 = t.p2();
517-
const type::Vec3 pt1v = pt1 + t.v1()*dt;
518-
const type::Vec3 pt2v = pt2 + t.v2()*dt;
515+
const auto& pt1 = t.p1();
516+
const auto& pt2 = t.p2();
517+
const auto pt1v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt1 + t.v1()*dt : t.p1Free());
518+
const auto pt2v = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt2 + t.v2()*dt : t.p2Free());
519519

520520
for (int c = 0; c < 3; c++)
521521
{

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,7 @@ class PointCollisionModel : public core::CollisionModel
9191

9292
void computeBoundingTree(int maxDepth=0) override;
9393

94-
void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
94+
void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
9595

9696
void draw(const core::visual::VisualParams*, sofa::Index index) override;
9797

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,7 +143,7 @@ void PointCollisionModel<DataTypes>::computeBoundingTree(int maxDepth)
143143
}
144144

145145
template<class DataTypes>
146-
void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int maxDepth)
146+
void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag , int maxDepth)
147147
{
148148
CubeCollisionModel* cubeModel = createPrevious<CubeCollisionModel>();
149149
const auto npoints = mstate->getSize();
@@ -169,7 +169,8 @@ void PointCollisionModel<DataTypes>::computeContinuousBoundingTree(SReal dt, int
169169
{
170170
TPoint<DataTypes> p(this,i);
171171
const type::Vec3& pt = p.p();
172-
const type::Vec3 ptv = pt + p.v()*dt;
172+
const type::Vec3 ptv = (continuousIntersectionFlag == ContinuousIntersectionTypeFlag::Inertia ? pt + p.v()*dt : p.pFree());
173+
173174

174175
for (int c = 0; c < 3; c++)
175176
{

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -110,7 +110,7 @@ class SphereCollisionModel : public core::CollisionModel
110110

111111
void computeBoundingTree(int maxDepth=0) override;
112112

113-
void computeContinuousBoundingTree(SReal dt, int maxDepth=0) override;
113+
void computeContinuousBoundingTree(SReal dt, ContinuousIntersectionTypeFlag continuousIntersectionFlag = ContinuousIntersectionTypeFlag::Inertia, int maxDepth=0) override;
114114

115115
void draw(const core::visual::VisualParams*, sofa::Index index) override;
116116

@@ -195,7 +195,7 @@ template<class DataTypes>
195195
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]);}
196196

197197
template<class DataTypes>
198-
inline const typename TSphere<DataTypes>::Coord& TSphere<DataTypes>::pFree() const { return (*this->model->mstate->read(core::vec_id::read_access::freePosition)).getValue()[this->index]; }
198+
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]); }
199199

200200
template<class DataTypes>
201201
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]);}

0 commit comments

Comments
 (0)