DART  6.7.3
FCLCollisionDetector.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_HPP_
34 #define DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_HPP_
35 
36 #include <vector>
39 
40 namespace dart {
41 namespace collision {
42 
43 class FCLCollisionObject;
44 
46 {
47 public:
49 
50  static std::shared_ptr<FCLCollisionDetector> create();
51 
64  {
65  PRIMITIVE = 0,
66  MESH
67  };
68 
78  {
79  FCL = 0,
80  DART
81  };
82 
84  virtual ~FCLCollisionDetector();
85 
86  // Documentation inherited
87  std::shared_ptr<CollisionDetector> cloneWithoutCollisionObjects() override;
88 
89  // Documentation inherited
90  const std::string& getType() const override;
91 
93  static const std::string& getStaticType();
94 
95  // Documentation inherited
96  std::unique_ptr<CollisionGroup> createCollisionGroup() override;
97 
98  // Documentation inherited
99  bool collide(
100  CollisionGroup* group,
101  const CollisionOption& option = CollisionOption(false, 1u, nullptr),
102  CollisionResult* result = nullptr) override;
103 
104  // Documentation inherited
105  bool collide(
106  CollisionGroup* group1,
107  CollisionGroup* group2,
108  const CollisionOption& option = CollisionOption(false, 1u, nullptr),
109  CollisionResult* result = nullptr) override;
110 
111  // Documentation inherited
112  double distance(
113  CollisionGroup* group,
114  const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
115  DistanceResult* result = nullptr) override;
116 
117  // Documentation inherited
118  double distance(
119  CollisionGroup* group1,
120  CollisionGroup* group2,
121  const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
122  DistanceResult* result = nullptr) override;
123 
126 
129 
132 
135 
136 protected:
137 
140 
141  // Documentation inherited
142  std::unique_ptr<CollisionObject> createCollisionObject(
143  const dynamics::ShapeFrame* shapeFrame) override;
144 
145  void refreshCollisionObject(CollisionObject* object) override;
146 
150  const dynamics::ConstShapePtr& shape);
151 
152 protected:
153 
155 
157 
158 private:
159 
163  {
164  public:
165 
167  const dynamics::ConstShapePtr& shape);
168 
170 
171  private:
172 
174 
176 
177  };
178 
180  struct ShapeInfo final
181  {
184 
186  std::size_t mLastKnownVersion;
187  };
188 
192  const dynamics::ConstShapePtr& shape,
194  const FCLCollisionGeometryDeleter& deleter);
195 
196 private:
197 
198  using ShapeMap = std::map<dynamics::ConstShapePtr, ShapeInfo>;
199 
201 
203 };
204 
205 } // namespace collision
206 } // namespace dart
207 
208 #endif // DART_COLLISION_FCL_FCLCOLLISIONDETECTOR_HPP_
boost::weak_ptr< T > fcl_weak_ptr
Definition: BackwardCompatibility.hpp:101
boost::shared_ptr< T > fcl_shared_ptr
Definition: BackwardCompatibility.hpp:100
const CollisionOption & option
Collision option of DART.
Definition: FCLCollisionDetector.cpp:154
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:157
std::string type
Definition: SdfParser.cpp:82
Definition: CollisionDetector.hpp:56
virtual std::unique_ptr< CollisionGroup > createCollisionGroup()=0
Create a collision group.
Definition: CollisionGroup.hpp:51
Definition: CollisionObject.hpp:45
Definition: CollisionResult.hpp:52
This deleter is responsible for deleting fcl::CollisionGeometry and removing it from mShapeMap when i...
Definition: FCLCollisionDetector.hpp:163
FCLCollisionGeometryDeleter(FCLCollisionDetector *cd, const dynamics::ConstShapePtr &shape)
Definition: FCLCollisionDetector.cpp:1093
void operator()(dart::collision::fcl::CollisionGeometry *geom) const
Definition: FCLCollisionDetector.cpp:1104
dynamics::ConstShapePtr mShape
Definition: FCLCollisionDetector.hpp:175
FCLCollisionDetector * mFCLCollisionDetector
Definition: FCLCollisionDetector.hpp:173
Definition: FCLCollisionDetector.hpp:46
const std::string & getType() const override
Return collision detection engine type as a std::string.
Definition: FCLCollisionDetector.cpp:676
ContactPointComputationMethod mContactPointComputationMethod
Definition: FCLCollisionDetector.hpp:156
FCLCollisionDetector()
Constructor.
Definition: FCLCollisionDetector.cpp:876
void setContactPointComputationMethod(ContactPointComputationMethod method)
Set contact point computation method.
Definition: FCLCollisionDetector.cpp:851
ShapeMap mShapeMap
Definition: FCLCollisionDetector.hpp:200
void setPrimitiveShapeType(PrimitiveShape type)
Set primitive shape type.
Definition: FCLCollisionDetector.cpp:827
virtual ~FCLCollisionDetector()
Constructor.
Definition: FCLCollisionDetector.cpp:663
std::shared_ptr< CollisionDetector > cloneWithoutCollisionObjects() override
Create a clone of this CollisionDetector.
Definition: FCLCollisionDetector.cpp:670
static const std::string & getStaticType()
Get collision detector type for this class.
Definition: FCLCollisionDetector.cpp:682
std::unique_ptr< CollisionObject > createCollisionObject(const dynamics::ShapeFrame *shapeFrame) override
Create CollisionObject.
Definition: FCLCollisionDetector.cpp:885
fcl_shared_ptr< dart::collision::fcl::CollisionGeometry > createFCLCollisionGeometry(const dynamics::ConstShapePtr &shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter &deleter)
Create fcl::CollisionGeometry with the custom deleter FCLCollisionGeometryDeleter.
Definition: FCLCollisionDetector.cpp:934
static std::shared_ptr< FCLCollisionDetector > create()
Definition: FCLCollisionDetector.cpp:657
std::map< dynamics::ConstShapePtr, ShapeInfo > ShapeMap
Definition: FCLCollisionDetector.hpp:198
bool collide(CollisionGroup *group, const CollisionOption &option=CollisionOption(false, 1u, nullptr), CollisionResult *result=nullptr) override
Perform collision check for a single group.
Definition: FCLCollisionDetector.cpp:711
PrimitiveShape
Whether to use analytic collision checking for primitive shapes.
Definition: FCLCollisionDetector.hpp:64
@ PRIMITIVE
Definition: FCLCollisionDetector.hpp:65
@ MESH
Definition: FCLCollisionDetector.hpp:66
ContactPointComputationMethod
Whether to use FCL's contact point computation.
Definition: FCLCollisionDetector.hpp:78
@ FCL
Definition: FCLCollisionDetector.hpp:79
@ DART
Definition: FCLCollisionDetector.hpp:80
PrimitiveShape mPrimitiveShapeType
Definition: FCLCollisionDetector.hpp:154
void refreshCollisionObject(CollisionObject *object) override
Update the collision geometry of a ShapeFrame.
Definition: FCLCollisionDetector.cpp:895
double distance(CollisionGroup *group, const DistanceOption &option=DistanceOption(false, 0.0, nullptr), DistanceResult *result=nullptr) override
Get the minimum signed distance between the Shape pairs in the given CollisionGroup.
Definition: FCLCollisionDetector.cpp:774
static Registrar< FCLCollisionDetector > mRegistrar
Definition: FCLCollisionDetector.hpp:202
std::unique_ptr< CollisionGroup > createCollisionGroup() override
Create a collision group.
Definition: FCLCollisionDetector.cpp:690
PrimitiveShape getPrimitiveShapeType() const
Get primitive shape type.
Definition: FCLCollisionDetector.cpp:845
ContactPointComputationMethod getContactPointComputationMethod() const
Get contact point computation method.
Definition: FCLCollisionDetector.cpp:870
fcl_shared_ptr< dart::collision::fcl::CollisionGeometry > claimFCLCollisionGeometry(const dynamics::ConstShapePtr &shape)
Return fcl::CollisionGeometry associated with give Shape.
Definition: FCLCollisionDetector.cpp:906
Helper class to register a object creator function to the Singleton.
Definition: Factory.hpp:121
Definition: ShapeFrame.hpp:164
::fcl::CollisionGeometry CollisionGeometry
Definition: BackwardCompatibility.hpp:153
std::shared_ptr< const Shape > ConstShapePtr
Definition: SmartPointer.hpp:81
Definition: BulletCollisionDetector.cpp:63
Definition: CollisionOption.hpp:45
Definition: DistanceOption.hpp:45
Definition: DistanceResult.hpp:47
Information for a shape that was generated by this collision detector.
Definition: FCLCollisionDetector.hpp:181
fcl_weak_ptr< dart::collision::fcl::CollisionGeometry > mShape
A weak reference to the shape.
Definition: FCLCollisionDetector.hpp:183
std::size_t mLastKnownVersion
The last version of the shape, as known by this collision detector.
Definition: FCLCollisionDetector.hpp:186