CollisionPair.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_COLLISIONPAIR_H
17 #define SURGSIM_COLLISION_COLLISIONPAIR_H
18 
19 #include <list>
20 #include <memory>
21 
24 #include "SurgSim/Math/Vector.h"
25 
26 
27 namespace SurgSim
28 {
29 namespace Collision
30 {
31 
37 struct Contact
38 {
39  Contact(const CollisionDetectionType& newType,
40  const double& newDepth,
41  const double& newTime,
42  const SurgSim::Math::Vector3d& newContact,
43  const SurgSim::Math::Vector3d& newNormal,
44  const std::pair<SurgSim::DataStructures::Location,
45  SurgSim::DataStructures::Location>& newPenetrationPoints) :
46  type(newType), depth(newDepth), time(newTime), contact(newContact),
47  normal(newNormal), penetrationPoints(newPenetrationPoints), force(SurgSim::Math::Vector3d::Zero())
48  {
49  }
50  std::shared_ptr<Contact> makeComplimentary()
51  {
52  auto complimentary = std::make_shared<Contact>(type, depth, time, contact,
53  -normal, std::make_pair(penetrationPoints.second, penetrationPoints.first));
54  complimentary->force = -force;
55  return complimentary;
56  }
57  bool operator==(const Contact& contact) const
58  {
59  return type == contact.type &&
60  std::abs(time - contact.time) < 1e-8 &&
61  penetrationPoints.first.isApprox(contact.penetrationPoints.first) &&
62  penetrationPoints.second.isApprox(contact.penetrationPoints.second) &&
63  normal.isApprox(contact.normal);
64  }
65  CollisionDetectionType type;
66  double depth;
67  double time;
73 };
74 
79 {
80 public:
82  CollisionPair();
83 
85  CollisionPair(const std::shared_ptr<Representation>& first,
86  const std::shared_ptr<Representation>& second);
87 
90 
94  void setRepresentations(const std::shared_ptr<Representation>& first,
95  const std::shared_ptr<Representation>& second);
96 
99  const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
100  getRepresentations() const;
101 
104  CollisionDetectionType getType() const;
105 
107  std::shared_ptr<Representation> getFirst() const;
108 
110  std::shared_ptr<Representation> getSecond() const;
111 
113  bool hasContacts() const;
114 
121  void addCcdContact(const double& depth,
122  const double& time,
123  const SurgSim::Math::Vector3d& contactPoint,
124  const SurgSim::Math::Vector3d& normal,
125  const std::pair<SurgSim::DataStructures::Location,
126  SurgSim::DataStructures::Location>& penetrationPoints);
127 
132  void addDcdContact(const double& depth,
133  const SurgSim::Math::Vector3d& normal,
134  const std::pair<SurgSim::DataStructures::Location,
135  SurgSim::DataStructures::Location>& penetrationPoints);
136 
139  void addContact(const std::shared_ptr<Contact>& contact);
140 
142  void updateRepresentations();
143 
145  std::list<std::shared_ptr<Contact>>& getContacts();
146 
148  void clearContacts();
149 
151  void swapRepresentations();
152 
155  bool isSwapped() const;
156 
159  bool mayIntersect() const;
160 
161 private:
163  std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>> m_representations;
164 
166  CollisionDetectionType m_type;
167 
169  std::list<std::shared_ptr<Contact>> m_contacts;
170 
172 };
173 
174 
175 }; // namespace Collision
176 }; // namespace SurgSim
177 
178 template <typename charT, typename traits>
179 std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
180  const SurgSim::Collision::Contact& contact)
181 {
182  out << "Normal: " << contact.normal.transpose() << std::endl;
183  out << "Depth: " << contact.depth << std::endl;
184  out << "Time: " << contact.time << std::endl;
185  out << "Penetration Point 1 :" << contact.penetrationPoints.first << std::endl;
186  out << "Penetration Point 2 :" << contact.penetrationPoints.second << std::endl;
187 
188  return out;
189 }
190 #endif
SurgSim::Collision::CollisionPair::m_isSwapped
bool m_isSwapped
Definition: CollisionPair.h:171
SurgSim::Collision::CollisionPair::getContacts
std::list< std::shared_ptr< Contact > > & getContacts()
Definition: CollisionPair.cpp:134
SurgSim::Collision::CollisionPair::m_type
CollisionDetectionType m_type
Collision detection type for this pair.
Definition: CollisionPair.h:166
Vector.h
Definitions of small fixed-size vector types.
SurgSim::Collision::CollisionPair
Collision Pair class, it signifies a pair of items that should be checked with the collision algorith...
Definition: CollisionPair.h:79
SurgSim::Collision::Contact::force
SurgSim::Math::Vector3d force
The reaction force to correct this contact.
Definition: CollisionPair.h:72
SurgSim::Collision::Contact::depth
double depth
What is the penetration depth for the representation.
Definition: CollisionPair.h:66
SurgSim::Collision::CollisionPair::getRepresentations
const std::pair< std::shared_ptr< Representation >, std::shared_ptr< Representation > > & getRepresentations() const
Function that returns the pair of representations of the objects that are colliding.
Definition: CollisionPair.cpp:75
SurgSim::Collision::Contact
Contact data structure used when two representations touch each other The convention is that if body ...
Definition: CollisionPair.h:38
SurgSim::Collision::Contact::contact
SurgSim::Math::Vector3d contact
The actual contact point, only used for CCD.
Definition: CollisionPair.h:68
SurgSim::Collision::CollisionPair::isSwapped
bool isSwapped() const
Query if this the pair has been swapped from when it was constructed.
Definition: CollisionPair.cpp:151
SurgSim::Collision::CollisionPair::m_contacts
std::list< std::shared_ptr< Contact > > m_contacts
List of current contacts.
Definition: CollisionPair.h:169
SurgSim::Collision::CollisionPair::getType
CollisionDetectionType getType() const
Get the collision detection type for this pair.
Definition: CollisionPair.cpp:80
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Collision::Contact::normal
SurgSim::Math::Vector3d normal
The normal on the contact point (normalized)
Definition: CollisionPair.h:69
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Collision::Contact::makeComplimentary
std::shared_ptr< Contact > makeComplimentary()
Definition: CollisionPair.h:50
SurgSim::Collision::CollisionPair::clearContacts
void clearContacts()
Reset clear the list of contacts, invalidating all the contacts.
Definition: CollisionPair.cpp:139
SurgSim::Collision::Contact::operator==
bool operator==(const Contact &contact) const
Definition: CollisionPair.h:57
SurgSim::Collision::Contact::Contact
Contact(const CollisionDetectionType &newType, const double &newDepth, const double &newTime, const SurgSim::Math::Vector3d &newContact, const SurgSim::Math::Vector3d &newNormal, const std::pair< SurgSim::DataStructures::Location, SurgSim::DataStructures::Location > &newPenetrationPoints)
Definition: CollisionPair.h:39
SurgSim::Collision::CollisionPair::setRepresentations
void setRepresentations(const std::shared_ptr< Representation > &first, const std::shared_ptr< Representation > &second)
Sets the representations in this pair, representations cannot be the same instance and neither can be...
Definition: CollisionPair.cpp:43
SurgSim::Collision::CollisionPair::addDcdContact
void addDcdContact(const double &depth, const SurgSim::Math::Vector3d &normal, const std::pair< SurgSim::DataStructures::Location, SurgSim::DataStructures::Location > &penetrationPoints)
Adds a DCD contact to the collision pair.
Definition: CollisionPair.cpp:109
SurgSim::Collision::CollisionPair::getFirst
std::shared_ptr< Representation > getFirst() const
Definition: CollisionPair.cpp:85
SurgSim::Collision::CollisionPair::addContact
void addContact(const std::shared_ptr< Contact > &contact)
Adds a contact.
Definition: CollisionPair.cpp:118
SurgSim::Collision::CollisionPair::CollisionPair
CollisionPair()
Default Constructor.
Definition: CollisionPair.cpp:28
SurgSim::Collision::CollisionPair::hasContacts
bool hasContacts() const
Definition: CollisionPair.cpp:95
SurgSim::Collision::CollisionPair::addCcdContact
void addCcdContact(const double &depth, const double &time, const SurgSim::Math::Vector3d &contactPoint, const SurgSim::Math::Vector3d &normal, const std::pair< SurgSim::DataStructures::Location, SurgSim::DataStructures::Location > &penetrationPoints)
Adds a CCD contact to the collision pair.
Definition: CollisionPair.cpp:100
SurgSim::Collision::CollisionPair::swapRepresentations
void swapRepresentations()
Swap the representation pair so that first becomes second and second becomes first.
Definition: CollisionPair.cpp:144
SurgSim::DataStructures::Location
A Location defines a local position w.r.t.
Definition: Location.h:40
Location.h
operator<<
std::basic_ostream< charT, traits > & operator<<(std::basic_ostream< charT, traits > &out, const SurgSim::Collision::Contact &contact)
Definition: CollisionPair.h:179
SurgSim::Collision::Contact::type
CollisionDetectionType type
What collision algorithm class was used to get the contact.
Definition: CollisionPair.h:65
Representation.h
SurgSim::Collision::CollisionPair::mayIntersect
bool mayIntersect() const
Definition: CollisionPair.cpp:156
SurgSim::Collision::Contact::time
double time
What is the time of the collision, CCD only.
Definition: CollisionPair.h:67
SurgSim::Collision::CollisionPair::getSecond
std::shared_ptr< Representation > getSecond() const
Definition: CollisionPair.cpp:90
SurgSim::Collision::CollisionPair::~CollisionPair
~CollisionPair()
Destructor.
Definition: CollisionPair.cpp:38
SurgSim::Collision::Contact::penetrationPoints
std::pair< SurgSim::DataStructures::Location, SurgSim::DataStructures::Location > penetrationPoints
The deepest point inside the opposing object.
Definition: CollisionPair.h:71
SurgSim::Collision::CollisionPair::updateRepresentations
void updateRepresentations()
Update the representations by adding the contacts to them.
Definition: CollisionPair.cpp:125
SurgSim::Collision::CollisionPair::m_representations
std::pair< std::shared_ptr< Representation >, std::shared_ptr< Representation > > m_representations
Pair of objects that are colliding.
Definition: CollisionPair.h:163