CollisionPair.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_COLLISIONPAIR_H
#define SURGSIM_COLLISION_COLLISIONPAIR_H
#include <list>
#include <memory>
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/DataStructures/Location.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
namespace Collision
{
struct 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) :
type(newType), depth(newDepth), time(newTime), contact(newContact),
normal(newNormal), penetrationPoints(newPenetrationPoints), force(SurgSim::Math::Vector3d::Zero())
{
}
std::shared_ptr<Contact> makeComplimentary()
{
auto complimentary = std::make_shared<Contact>(type, depth, time, contact,
-normal, std::make_pair(penetrationPoints.second, penetrationPoints.first));
complimentary->force = -force;
return complimentary;
}
bool operator==(const Contact& contact) const
{
return type == contact.type &&
std::abs(time - contact.time) < 1e-8 &&
penetrationPoints.first.isApprox(contact.penetrationPoints.first) &&
penetrationPoints.second.isApprox(contact.penetrationPoints.second) &&
normal.isApprox(contact.normal);
}
CollisionDetectionType type;
double depth;
double time;
SurgSim::Math::Vector3d contact;
SurgSim::Math::Vector3d normal;
std::pair<SurgSim::DataStructures::Location,
SurgSim::DataStructures::Location> penetrationPoints;
SurgSim::Math::Vector3d force;
};
class CollisionPair
{
public:
CollisionPair();
CollisionPair(const std::shared_ptr<Representation>& first,
const std::shared_ptr<Representation>& second);
~CollisionPair();
void setRepresentations(const std::shared_ptr<Representation>& first,
const std::shared_ptr<Representation>& second);
const std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>>&
getRepresentations() const;
CollisionDetectionType getType() const;
std::shared_ptr<Representation> getFirst() const;
std::shared_ptr<Representation> getSecond() const;
bool hasContacts() const;
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);
void addDcdContact(const double& depth,
const SurgSim::Math::Vector3d& normal,
const std::pair<SurgSim::DataStructures::Location,
SurgSim::DataStructures::Location>& penetrationPoints);
void addContact(const std::shared_ptr<Contact>& contact);
void updateRepresentations();
std::list<std::shared_ptr<Contact>>& getContacts();
void clearContacts();
void swapRepresentations();
bool isSwapped() const;
bool mayIntersect() const;
private:
std::pair<std::shared_ptr<Representation>, std::shared_ptr<Representation>> m_representations;
CollisionDetectionType m_type;
std::list<std::shared_ptr<Contact>> m_contacts;
bool m_isSwapped;
};
}; // namespace Collision
}; // namespace SurgSim
template <typename charT, typename traits>
std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& out,
const SurgSim::Collision::Contact& contact)
{
out << "Normal: " << contact.normal.transpose() << std::endl;
out << "Depth: " << contact.depth << std::endl;
out << "Time: " << contact.time << std::endl;
out << "Penetration Point 1 :" << contact.penetrationPoints.first << std::endl;
out << "Penetration Point 2 :" << contact.penetrationPoints.second << std::endl;
return out;
}
#endif
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
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
SurgSim::DataStructures::Location
A Location defines a local position w.r.t.
Definition: Location.h:39
SurgSim::Collision::Contact::force
SurgSim::Math::Vector3d force
The reaction force to correct this contact.
Definition: CollisionPair.h:72
SurgSim::Collision::Contact::normal
SurgSim::Math::Vector3d normal
The normal on the contact point (normalized)
Definition: CollisionPair.h:69
SurgSim::Collision::CollisionPair
Collision Pair class, it signifies a pair of items that should be checked with the collision algorith...
Definition: CollisionPair.h:78
SurgSim::Collision::CollisionPair::m_isSwapped
bool m_isSwapped
Definition: CollisionPair.h:171
SurgSim::Collision::Contact::depth
double depth
What is the penetration depth for the representation.
Definition: CollisionPair.h:66
SurgSim::Collision::CollisionPair::m_type
CollisionDetectionType m_type
Collision detection type for this pair.
Definition: CollisionPair.h:166
Representation.h
SurgSim::Collision::Contact::time
double time
What is the time of the collision, CCD only.
Definition: CollisionPair.h:67
SurgSim::Collision::Contact::penetrationPoints
std::pair< SurgSim::DataStructures::Location, SurgSim::DataStructures::Location > penetrationPoints
The deepest point inside the opposing object.
Definition: CollisionPair.h:71
Vector.h
Definitions of small fixed-size vector types.
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::Contact::type
CollisionDetectionType type
What collision algorithm class was used to get the contact.
Definition: CollisionPair.h:65
SurgSim::Collision::Contact::operator==
bool operator==(const Contact &contact) const
Definition: CollisionPair.h:57
SurgSim::Collision::Contact::makeComplimentary
std::shared_ptr< Contact > makeComplimentary()
Definition: CollisionPair.h:50
Location.h
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Collision::CollisionPair::m_contacts
std::list< std::shared_ptr< Contact > > m_contacts
List of current contacts.
Definition: CollisionPair.h:169
SurgSim::Collision::Contact::contact
SurgSim::Math::Vector3d contact
The actual contact point, only used for CCD.
Definition: CollisionPair.h:68
SurgSim::Collision::Contact
Contact data structure used when two representations touch each other The convention is that if body ...
Definition: CollisionPair.h:37