SegmentSelfContact.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
#define SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
#include "SurgSim/Collision/ShapeShapeContactCalculation.h"
#include "SurgSim/DataStructures/AabbTree.h"
#include "SurgSim/Framework/Logger.h"
#include "SurgSim/Math/SegmentMeshShape.h"
namespace SurgSim
{
namespace Collision
{
class CollisionPair;
class SegmentSelfContact : public ShapeShapeContactCalculation<Math::SegmentMeshShape, Math::SegmentMeshShape>
{
public:
SegmentSelfContact();
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;
void setTimeMinPrecisionEpsilon(double precision);
double getTimeMinPrecisionEpsilon();
void setTimeMaxPrecisionEpsilon(double precision);
double getTimeMaxPrecisionEpsilon();
void setDistanceEpsilon(double precision);
double distanceEpsilon();
std::pair<int, int> getShapeTypes() override;
protected:
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;
void getUniqueCandidates(
const std::list<SurgSim::DataStructures::AabbTree::TreeNodePairType>& intersectionList,
std::set<std::pair<size_t, size_t>>* segmentIds) const;
bool removeInvalidCollisions(
const Math::SegmentMeshShape& segmentA,
const Math::SegmentMeshShape& segmentB,
size_t segment1SegID, size_t segment2SegID) const;
bool detectExcessMovement(const SurgSim::Math::Vector3d& pt0,
const SurgSim::Math::Vector3d& pt1,
double threshold) const;
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;
bool isSameSegContactPoint(const Math::SegmentMeshShape& segmentShape,
size_t segId1, double s1, size_t segId2, double s2) const;
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;
private:
double m_distanceEpsilon;
double m_timeMinPrecisionEpsilon;
double m_timeMaxPrecisionEpsilon;
const double m_maxMovementThreshold;
const bool m_useSegmentThickness;
std::shared_ptr<SurgSim::Framework::Logger> m_logger;
};
}; // namespace Collision
}; // namespace SurgSim
#endif // SURGSIM_COLLISION_SEGMENTSELFCONTACT_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Collision::SegmentSelfContact::SegmentSelfContact
SegmentSelfContact()
Constructor.
Definition: SegmentSelfContact.cpp:35
SurgSim::Collision::SegmentSelfContact::distanceEpsilon
double distanceEpsilon()
Definition: SegmentSelfContact.cpp:77
SurgSim::Collision::SegmentSelfContact::setDistanceEpsilon
void setDistanceEpsilon(double precision)
Set the maximum separation for which two points are considered the same.
Definition: SegmentSelfContact.cpp:72
SurgSim::Collision::SegmentSelfContact::getUniqueCandidates
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
SurgSim::Collision::SegmentSelfContact::calculateCcdContact
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
SurgSim::Collision::ShapeShapeContactCalculation
Class that can automate the type conversion and provides a consistent interface to the typed call Tak...
Definition: ShapeShapeContactCalculation.h:31
SurgSim::Collision::SegmentSelfContact::maxTimePrecision
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
SurgSim::Collision::SegmentSelfContact::m_logger
std::shared_ptr< SurgSim::Framework::Logger > m_logger
Logger.
Definition: SegmentSelfContact.h:205
SurgSim::Collision::SegmentSelfContact::m_distanceEpsilon
double m_distanceEpsilon
Minimum distance precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:190
SurgSim::Collision::SegmentSelfContact::detectExcessMovement
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
SurgSim::Collision::SegmentSelfContact::m_timeMinPrecisionEpsilon
double m_timeMinPrecisionEpsilon
Minimum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:193
SurgSim::Collision::SegmentSelfContact::getTimeMinPrecisionEpsilon
double getTimeMinPrecisionEpsilon()
Definition: SegmentSelfContact.cpp:56
SurgSim::Collision::SegmentSelfContact::m_maxMovementThreshold
const double m_maxMovementThreshold
Maximum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:199
SurgSim::Collision::SegmentSelfContact::getShapeTypes
std::pair< int, int > getShapeTypes() override
Function that returns the shapes between which this class performs collision detection.
Definition: SegmentSelfContact.cpp:45
SurgSim::Collision::SegmentSelfContact::removeInvalidCollisions
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
SurgSim::Collision::SegmentSelfContact
SegmentSelfContact computes the self collisions among a SegmentMesh under motion at two time points p...
Definition: SegmentSelfContact.h:46
SurgSim::Collision::SegmentSelfContact::detectCollision
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
SurgSim::Collision::SegmentSelfContact::setTimeMinPrecisionEpsilon
void setTimeMinPrecisionEpsilon(double precision)
Set the minimum time precision allowed when deciding on the depth of recursion.
Definition: SegmentSelfContact.cpp:50
SurgSim::Math::RigidTransform3d
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
SurgSim::Collision::SegmentSelfContact::m_timeMaxPrecisionEpsilon
double m_timeMaxPrecisionEpsilon
Maximum time precision epsilon used in continuous collision detection.
Definition: SegmentSelfContact.h:196
ShapeShapeContactCalculation.h
Logger.h
AabbTree.h
SurgSim::Math::SegmentMeshShape
SegmentMeshShape defines a shape based on a mesh, like MeshShape.
Definition: SegmentMeshShape.h:36
SurgSim::Collision::SegmentSelfContact::getTimeMaxPrecisionEpsilon
double getTimeMaxPrecisionEpsilon()
Definition: SegmentSelfContact.cpp:67
SurgSim::Collision::SegmentSelfContact::isSameSegContactPoint
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
SurgSim::Collision::SegmentSelfContact::m_useSegmentThickness
const bool m_useSegmentThickness
Flag to determine if segment thickness is to be used in contact calculations.
Definition: SegmentSelfContact.h:202
SurgSim::Collision::SegmentSelfContact::findSegSegContact
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
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Collision::SegmentSelfContact::setTimeMaxPrecisionEpsilon
void setTimeMaxPrecisionEpsilon(double precision)
Set the maximum time precision allowed when deciding on the depth of recursion.
Definition: SegmentSelfContact.cpp:61
SegmentMeshShape.h