SegmentSelfContact.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013-2015, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
17 #define SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
18 
23 
24 namespace SurgSim
25 {
26 namespace Collision
27 {
28 
29 class CollisionPair;
30 
46 class SegmentSelfContact : public ShapeShapeContactCalculation<Math::SegmentMeshShape, Math::SegmentMeshShape>
47 {
48 public:
51 
52  std::list<std::shared_ptr<Contact>> calculateCcdContact(
53  const Math::SegmentMeshShape& segmentShape1AtTime0, const Math::RigidTransform3d& segmentPose1AtTime0,
54  const Math::SegmentMeshShape& segmentShape1AtTime1, const Math::RigidTransform3d& segmentPose1AtTime1,
55  const Math::SegmentMeshShape& segmentShape2AtTime0, const Math::RigidTransform3d& segmentPose2AtTime0,
56  const Math::SegmentMeshShape& segmentShape2AtTime1, const Math::RigidTransform3d& segmentPose2AtTime1)
57  const override;
58 
61  void setTimeMinPrecisionEpsilon(double precision);
62 
65 
68  void setTimeMaxPrecisionEpsilon(double precision);
69 
72 
75  void setDistanceEpsilon(double precision);
76 
78  double distanceEpsilon();
79 
82  std::pair<int, int> getShapeTypes() override;
83 
84 protected:
100  bool detectCollision(
101  const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
102  const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
103  const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
104  const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
105  double segmentRadius1, double segmentRadius2,
106  double timePrecision,
107  double* r, double* s, double* t,
108  SurgSim::Math::Vector3d* pToQDir,
109  SurgSim::Math::Vector3d* contactPtP,
110  SurgSim::Math::Vector3d* contactPtQ) const;
111 
116  void getUniqueCandidates(
117  const std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType>& intersectionList,
118  std::set<std::pair<size_t, size_t>>* segmentIds) const;
119 
131  const Math::SegmentMeshShape& segmentA,
132  const Math::SegmentMeshShape& segmentB,
133  size_t segment1SegID, size_t segment2SegID) const;
134 
142  const SurgSim::Math::Vector3d& pt1,
143  double threshold) const;
144 
156  bool findSegSegContact(const Math::SegmentMeshShape& segmentShape,
157  const std::list<std::shared_ptr<Contact>>& contacts,
158  double t, Collision::CollisionDetectionType collisionType,
159  size_t segId1, double s1, size_t segId2, double s2, double timeEpsilon) const;
160 
168  bool isSameSegContactPoint(const Math::SegmentMeshShape& segmentShape,
169  size_t segId1, double s1, size_t segId2, double s2) const;
170 
181  double maxTimePrecision(
182  const std::array<SurgSim::Math::Vector3d, 2>& pt0Positions,
183  const std::array<SurgSim::Math::Vector3d, 2>& pt1Positions,
184  const std::array<SurgSim::Math::Vector3d, 2>& qt0Positions,
185  const std::array<SurgSim::Math::Vector3d, 2>& qt1Positions,
186  double effectiveThickness) const;
187 
188 private:
191 
194 
197 
200 
203 
205  std::shared_ptr<SurgSim::Framework::Logger> m_logger;
206 };
207 
208 }; // namespace Collision
209 }; // namespace SurgSim
210 
211 #endif // SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
Definition: CompoundShapeToGraphics.cpp:29
SegmentSelfContact()
Constructor.
Definition: SegmentSelfContact.cpp:35
double distanceEpsilon()
Definition: SegmentSelfContact.cpp:77
void setDistanceEpsilon(double precision)
Set the maximum separation for which two points are considered the same.
Definition: SegmentSelfContact.cpp:72
void getUniqueCandidates(const std::list< SurgSim::DataStructures::AabbTree::TreeNodePairType > &intersectionList, std::set< std::pair< size_t, size_t >> *segmentIds) const
Given a list of potentially intersecting AABB nodes, cull the list of any duplicates and return the u...
Definition: SegmentSelfContact.cpp:364
std::list< std::shared_ptr< Contact > > calculateCcdContact(const Math::SegmentMeshShape &segmentShape1AtTime0, const Math::RigidTransform3d &segmentPose1AtTime0, const Math::SegmentMeshShape &segmentShape1AtTime1, const Math::RigidTransform3d &segmentPose1AtTime1, const Math::SegmentMeshShape &segmentShape2AtTime0, const Math::RigidTransform3d &segmentPose2AtTime0, const Math::SegmentMeshShape &segmentShape2AtTime1, const Math::RigidTransform3d &segmentPose2AtTime1) const override
Definition: SegmentSelfContact.cpp:82
Class that can automate the type conversion and provides a consistent interface to the typed call Tak...
Definition: ShapeShapeContactCalculation.h:31
double maxTimePrecision(const std::array< SurgSim::Math::Vector3d, 2 > &pt0Positions, const std::array< SurgSim::Math::Vector3d, 2 > &pt1Positions, const std::array< SurgSim::Math::Vector3d, 2 > &qt0Positions, const std::array< SurgSim::Math::Vector3d, 2 > &qt1Positions, double effectiveThickness) const
Calculate the maximum time interval that guarantees that all collisions can be detected given the der...
Definition: SegmentSelfContact.cpp:523
std::shared_ptr< SurgSim::Framework::Logger > m_logger
Logger.
Definition: SegmentSelfContact.h:205
double m_distanceEpsilon
Minimum distance precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:190
bool detectExcessMovement(const SurgSim::Math::Vector3d &pt0, const SurgSim::Math::Vector3d &pt1, double threshold) const
Verify the a given point at times t0 and t1 have remained within a reasonable neighborhood.
Definition: SegmentSelfContact.cpp:441
double m_timeMinPrecisionEpsilon
Minimum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:193
double getTimeMinPrecisionEpsilon()
Definition: SegmentSelfContact.cpp:56
const double m_maxMovementThreshold
Maximum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:199
std::pair< int, int > getShapeTypes() override
Function that returns the shapes between which this class performs collision detection.
Definition: SegmentSelfContact.cpp:45
bool removeInvalidCollisions(const Math::SegmentMeshShape &segmentA, const Math::SegmentMeshShape &segmentB, size_t segment1SegID, size_t segment2SegID) const
From the initial AABB tree collisions, there are some very simple filtering operations that we can do...
Definition: SegmentSelfContact.cpp:396
SegmentSelfContact computes the self collisions among a SegmentMesh under motion at two time points p...
Definition: SegmentSelfContact.h:46
bool detectCollision(const std::array< SurgSim::Math::Vector3d, 2 > &pt0Positions, const std::array< SurgSim::Math::Vector3d, 2 > &pt1Positions, const std::array< SurgSim::Math::Vector3d, 2 > &qt0Positions, const std::array< SurgSim::Math::Vector3d, 2 > &qt1Positions, double segmentRadius1, double segmentRadius2, double timePrecision, double *r, double *s, double *t, SurgSim::Math::Vector3d *pToQDir, SurgSim::Math::Vector3d *contactPtP, SurgSim::Math::Vector3d *contactPtQ) const
Detect if two segments actually collide either at time t=0 (Fixed case) or within a movement phase...
Definition: SegmentSelfContact.cpp:272
void setTimeMinPrecisionEpsilon(double precision)
Set the minimum time precision allowed when deciding on the depth of recursion.
Definition: SegmentSelfContact.cpp:50
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
double m_timeMaxPrecisionEpsilon
Maximum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:196
SegmentMeshShape defines a shape based on a mesh, like MeshShape.
Definition: SegmentMeshShape.h:36
double getTimeMaxPrecisionEpsilon()
Definition: SegmentSelfContact.cpp:67
bool isSameSegContactPoint(const Math::SegmentMeshShape &segmentShape, size_t segId1, double s1, size_t segId2, double s2) const
Check for the same location among two parametric location specifications.
Definition: SegmentSelfContact.cpp:486
const bool m_useSegmentThickness
Flag to determine if segment thickness is to be used in contact calculations.
Definition: SegmentSelfContact.h:202
bool findSegSegContact(const Math::SegmentMeshShape &segmentShape, const std::list< std::shared_ptr< Contact >> &contacts, double t, Collision::CollisionDetectionType collisionType, size_t segId1, double s1, size_t segId2, double s2, double timeEpsilon) const
Search the list of contacts for a match to the current contact.
Definition: SegmentSelfContact.cpp:451
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
void setTimeMaxPrecisionEpsilon(double precision)
Set the maximum time precision allowed when deciding on the depth of recursion.
Definition: SegmentSelfContact.cpp:61