ShapeShapeContactCalculation.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_SHAPESHAPECONTACTCALCULATION_H
17 #define SURGSIM_COLLISION_SHAPESHAPECONTACTCALCULATION_H
18 
20 #include "SurgSim/Math/Shape.h"
21 
22 namespace SurgSim
23 {
24 
25 namespace Collision
26 {
27 
30 template <class Shape1, class Shape2>
32 {
37  virtual std::list<std::shared_ptr<Contact>> calculateDcdContact(
38  const Shape1& shape1, const Math::RigidTransform3d& pose1,
39  const Shape2& shape2, const Math::RigidTransform3d& pose2) const
40  {
41  SURGSIM_FAILURE() << "Not implemented";
42  return std::list<std::shared_ptr<Contact>>();
43  }
44 
45  virtual std::list<std::shared_ptr<Contact>> calculateCcdContact(
46  const Shape1& shape1AtTime0, const Math::RigidTransform3d& pose1AtTime0,
47  const Shape1& shape1AtTime1, const Math::RigidTransform3d& pose1AtTime1,
48  const Shape2& shape2AtTime0, const Math::RigidTransform3d& pose2AtTime0,
49  const Shape2& shape2AtTime1, const Math::RigidTransform3d& pose2AtTime1) const
50  {
51  SURGSIM_FAILURE() << "Not implemented";
52  return std::list<std::shared_ptr<Contact>>();
53  }
54 
56  std::list<std::shared_ptr<Contact>> doCalculateDcdContact(
57  const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape1,
58  const Math::PosedShape<std::shared_ptr<Math::Shape>>& posedShape2) override
59  {
60  auto one = std::static_pointer_cast<Shape1>(posedShape1.getShape());
61  auto two = std::static_pointer_cast<Shape2>(posedShape2.getShape());
62 
63  SURGSIM_ASSERT(one->getType() == posedShape1.getShape()->getType()) << "Invalid Shape 1";
64  SURGSIM_ASSERT(two->getType() == posedShape2.getShape()->getType()) << "Invalid Shape 2";
65 
66  return calculateDcdContact(*one, posedShape1.getPose(), *two, posedShape2.getPose());
67  }
68 
70  std::list<std::shared_ptr<Contact>> doCalculateCcdContact(
71  const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion1,
72  const Math::PosedShapeMotion<std::shared_ptr<Math::Shape>>& posedShapeMotion2) override
73  {
74  auto oneAtTime0 = std::static_pointer_cast<Shape1>(posedShapeMotion1.first.getShape());
75  auto oneAtTime1 = std::static_pointer_cast<Shape1>(posedShapeMotion1.second.getShape());
76  auto twoAtTime0 = std::static_pointer_cast<Shape2>(posedShapeMotion2.first.getShape());
77  auto twoAtTime1 = std::static_pointer_cast<Shape2>(posedShapeMotion2.second.getShape());
78 
79  SURGSIM_ASSERT(oneAtTime0->getType() == posedShapeMotion1.first.getShape()->getType()) <<
80  "Invalid Shape 1 detected at time 0";
81  SURGSIM_ASSERT(oneAtTime1->getType() == posedShapeMotion1.second.getShape()->getType()) <<
82  "Invalid Shape 1 detected at time 1";
83  SURGSIM_ASSERT(twoAtTime0->getType() == posedShapeMotion2.first.getShape()->getType()) <<
84  "Invalid Shape 2 detected at time 0";
85  SURGSIM_ASSERT(twoAtTime1->getType() == posedShapeMotion2.second.getShape()->getType()) <<
86  "Invalid Shape 2 detected at time 1";
87 
88  return calculateCcdContact(
89  *oneAtTime0, posedShapeMotion1.first.getPose(), *oneAtTime1, posedShapeMotion1.second.getPose(),
90  *twoAtTime0, posedShapeMotion2.first.getPose(), *twoAtTime1, posedShapeMotion2.second.getPose());
91  }
92 };
93 
94 }
95 }
96 
97 #endif
Shape.h
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Collision::ContactCalculation
Base class responsible for calculating contact data between two objects.
Definition: ContactCalculation.h:41
SurgSim::Collision::ShapeShapeContactCalculation::calculateDcdContact
virtual std::list< std::shared_ptr< Contact > > calculateDcdContact(const Shape1 &shape1, const Math::RigidTransform3d &pose1, const Shape2 &shape2, const Math::RigidTransform3d &pose2) const
Virtual function to be overridden, this provides the typed contact calculation between two shapes it ...
Definition: ShapeShapeContactCalculation.h:37
SurgSim::Math::PosedShapeMotion
PosedShapeMotion is embedding the motion of a PosedShape, providing a posed shape at 2 different inst...
Definition: Shape.h:145
SurgSim::Math::RigidTransform3d
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
SurgSim::Math::PosedShape
PosedShape is a transformed shape with a record of the pose used to transform it.
Definition: Shape.h:117
SURGSIM_FAILURE
#define SURGSIM_FAILURE()
Report that something very bad has happened and abort program execution.
Definition: Assert.h:95
SurgSim::Collision::ShapeShapeContactCalculation::doCalculateDcdContact
std::list< std::shared_ptr< Contact > > doCalculateDcdContact(const Math::PosedShape< std::shared_ptr< Math::Shape >> &posedShape1, const Math::PosedShape< std::shared_ptr< Math::Shape >> &posedShape2) override
Overrides the dcd contact calculation to go from untyped shapes to the typed shapes.
Definition: ShapeShapeContactCalculation.h:56
ContactCalculation.h
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Collision::ShapeShapeContactCalculation::doCalculateCcdContact
std::list< std::shared_ptr< Contact > > doCalculateCcdContact(const Math::PosedShapeMotion< std::shared_ptr< Math::Shape >> &posedShapeMotion1, const Math::PosedShapeMotion< std::shared_ptr< Math::Shape >> &posedShapeMotion2) override
Overrides the ccd contact calculation to go from untyped shapes to the typed shapes.
Definition: ShapeShapeContactCalculation.h:70
SurgSim::Collision::ShapeShapeContactCalculation::calculateCcdContact
virtual std::list< std::shared_ptr< Contact > > calculateCcdContact(const Shape1 &shape1AtTime0, const Math::RigidTransform3d &pose1AtTime0, const Shape1 &shape1AtTime1, const Math::RigidTransform3d &pose1AtTime1, const Shape2 &shape2AtTime0, const Math::RigidTransform3d &pose2AtTime0, const Shape2 &shape2AtTime1, const Math::RigidTransform3d &pose2AtTime1) const
Definition: ShapeShapeContactCalculation.h:45
SurgSim::Collision::ShapeShapeContactCalculation
Class that can automate the type conversion and provides a consistent interface to the typed call Tak...
Definition: ShapeShapeContactCalculation.h:31