DART  6.10.1
CollisionGroup.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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_COLLISIONGROUP_HPP_
34 #define DART_COLLISION_COLLISIONGROUP_HPP_
35 
36 #include <unordered_map>
37 #include <unordered_set>
38 #include <vector>
46 #include "dart/common/Observer.hpp"
48 
49 namespace dart {
50 namespace collision {
51 
53 {
54 public:
56  CollisionGroup(const CollisionDetectorPtr& collisionDetector);
57  // CollisionGroup also can be created from CollisionDetector::create()
58 
60  virtual ~CollisionGroup() = default;
61 
64 
68 
70  void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
71 
73  void addShapeFrames(
74  const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
75 
84  template <typename... Others>
85  void addShapeFramesOf(
86  const dynamics::ShapeFrame* shapeFrame, const Others*... others);
87 
89  template <typename... Others>
90  void addShapeFramesOf(
91  const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
92  const Others*... others);
93 
96  template <typename... Others>
97  void addShapeFramesOf(
98  const CollisionGroup* otherGroup, const Others*... others);
99 
102  template <typename... Others>
103  void addShapeFramesOf(
104  const dynamics::BodyNode* bodyNode, const Others*... others);
105 
108  template <typename... Others>
109  void addShapeFramesOf(
110  const dynamics::MetaSkeleton* skeleton, const Others*... others);
111 
114  void addShapeFramesOf();
115 
121  template <typename... Others>
122  void subscribeTo(
123  const dynamics::ConstBodyNodePtr& bodyNode, const Others&... others);
124 
130  //
131  // TODO(MXG): Figure out how to determine a version number for MetaSkeletons
132  // so that this function can accept a ConstMetaSkeletonPtr instead of only a
133  // ConstSkeletonPtr.
134  template <typename... Others>
135  void subscribeTo(
136  const dynamics::ConstSkeletonPtr& skeleton, const Others&... others);
137 
140  void subscribeTo();
141 
146  void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
147 
149  void removeShapeFrames(
150  const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
151 
160  template <typename... Others>
161  void removeShapeFramesOf(
162  const dynamics::ShapeFrame* shapeFrame, const Others*... others);
163 
165  template <typename... Others>
166  void removeShapeFramesOf(
167  const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
168  const Others*... others);
169 
175  template <typename... Others>
176  void removeShapeFramesOf(
177  const CollisionGroup* otherGroup, const Others*... others);
178 
184  template <typename... Others>
185  void removeShapeFramesOf(
186  const dynamics::BodyNode* bodyNode, const Others*... others);
187 
193  template <typename... Others>
194  void removeShapeFramesOf(
195  const dynamics::MetaSkeleton* skeleton, const Others*... others);
196 
199  void removeShapeFramesOf();
200 
202  void removeAllShapeFrames();
203 
206  template <typename... Others>
207  void unsubscribeFrom(
208  const dynamics::BodyNode* bodyNode, const Others*... others);
209 
212  template <typename... Others>
213  void unsubscribeFrom(
214  const dynamics::Skeleton* skeleton, const Others*... others);
215 
217  bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const;
218 
220  std::size_t getNumShapeFrames() const;
221 
223  const dynamics::ShapeFrame* getShapeFrame(std::size_t index) const;
224 
226  bool collide(
227  const CollisionOption& option = CollisionOption(false, 1u, nullptr),
228  CollisionResult* result = nullptr);
229 
234  bool collide(
235  CollisionGroup* otherGroup,
236  const CollisionOption& option = CollisionOption(false, 1u, nullptr),
237  CollisionResult* result = nullptr);
238 
247  double distance(
248  const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
249  DistanceResult* result = nullptr);
250 
263  double distance(
264  CollisionGroup* otherGroup,
265  const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
266  DistanceResult* result = nullptr);
267 
275  bool raycast(
276  const Eigen::Vector3d& from,
277  const Eigen::Vector3d& to,
279  RaycastResult* result = nullptr);
280 
282  void setAutomaticUpdate(bool automatic = true);
283 
285  bool getAutomaticUpdate() const;
286 
296  void update();
297 
304 
305 protected:
308  void updateEngineData();
309 
313  virtual void initializeEngineData() = 0;
314 
316  virtual void addCollisionObjectToEngine(CollisionObject* object) = 0;
317 
320  const std::vector<CollisionObject*>& collObjects)
321  = 0;
322 
325 
328 
331  virtual void updateCollisionGroupEngineData() = 0;
332 
333 protected:
336  // CollisionGroup shares the ownership of CollisionDetector with other
337  // CollisionGroups created from the same CollisionDetector so that the
338  // CollisionDetector doesn't get destroyed as long as at least one
339  // CollisionGroup is alive.
340 
345  struct ObjectInfo final
346  {
349 
352 
354  std::size_t mLastKnownShapeID;
355 
358  std::size_t mLastKnownVersion;
359 
366  std::unordered_set<const void*> mSources;
367  };
368 
369  using ObjectInfoList = std::vector<std::unique_ptr<ObjectInfo>>;
370 
374  // CollisionGroup also shares the ownership of CollisionObjects across other
375  // CollisionGroups for the same reason with above.
376  //
377  // Dev note: This was supposed to be std::map rather than std::vector for
378  // better search performance. The reason we use std::vector is to get
379  // deterministic contact results regardless of the order of CollisionObjects
380  // in this container for FCLCollisionDetector.
381  //
382  // fcl's collision result is dependent on the order of objects in the broad
383  // phase classes. If we use std::map, the orders of element between the
384  // original and copy are not guranteed to be the same as we copy std::map
385  // (e.g., by world cloning).
386 
387 private:
399  {
400  public:
402  void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
403 
405  void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
406 
408  void removeAllShapeFrames();
409 
413  std::unordered_set<const dynamics::ShapeFrame*> mDeletedFrames;
414 
415  protected:
417  void handleDestructionNotification(const common::Subject* subject) override;
418 
419  private:
423  std::unordered_map<const common::Subject*, const dynamics::ShapeFrame*>
425  };
426 
431  const dynamics::ShapeFrame* shapeFrame, const void* source);
432 
436  const dynamics::ShapeFrame* shapeFrame, const void* source);
437 
441 
445  template <typename Source, typename Child = void>
447  {
449  Source mSource;
450 
452  std::size_t mLastKnownVersion;
453 
455  std::unordered_map<const dynamics::ShapeFrame*, ObjectInfo*> mObjects;
456 
459  struct ChildInfo
460  {
462  std::size_t mLastKnownVersion;
463 
465  std::unordered_set<const dynamics::ShapeFrame*> mFrames;
466 
469  explicit ChildInfo(const std::size_t version) : mLastKnownVersion(version)
470  {
471  // Do nothing
472  }
473  };
474 
477  std::unordered_map<const Child*, ChildInfo> mChildren;
478 
480  CollisionSource(const Source& source, std::size_t lastKnownVersion)
481  : mSource(source), mLastKnownVersion(lastKnownVersion)
482  {
483  // Do nothing
484  }
485  };
486 
487  // Convenient typedefs for Skeleton sources
491  = std::unordered_map<const dynamics::MetaSkeleton*, SkeletonSource>;
492 
493  // Convenient typedefs for BodyNode sources
496  = std::unordered_map<const dynamics::BodyNode*, BodyNodeSource>;
497 
500  bool updateSkeletonSource(SkeletonSources::value_type& entry);
501 
504  bool updateBodyNodeSource(BodyNodeSources::value_type& entry);
505 
508  bool updateShapeFrame(ObjectInfo* object);
509 
512 
515 
518 };
519 
520 } // namespace collision
521 } // namespace dart
522 
524 
525 #endif // DART_COLLISION_COLLISIONGROUP_HPP_
const CollisionOption & option
Collision option of DART.
Definition: FCLCollisionDetector.cpp:157
CollisionResult * result
Collision result of DART.
Definition: FCLCollisionDetector.cpp:160
std::size_t index
Definition: SkelParser.cpp:1672
This class watches when ShapeFrames get deleted so that they can be safely removes from the Collision...
Definition: CollisionGroup.hpp:399
void removeShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Remove a shape frame from this observer.
Definition: CollisionGroup.cpp:326
void handleDestructionNotification(const common::Subject *subject) override
This will be called each time an observed shape frame is deleted.
Definition: CollisionGroup.cpp:340
std::unordered_set< const dynamics::ShapeFrame * > mDeletedFrames
Whenever an observed shape frame gets deleted, its pointer will be added to this set.
Definition: CollisionGroup.hpp:413
void addShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Add a shape frame to this observer.
Definition: CollisionGroup.cpp:317
std::unordered_map< const common::Subject *, const dynamics::ShapeFrame * > mMap
A map from a subject pointer to its corresponding ShapeFrame pointer.
Definition: CollisionGroup.hpp:424
void removeAllShapeFrames()
Remove all shape frames from this observer.
Definition: CollisionGroup.cpp:334
Definition: CollisionGroup.hpp:53
ShapeFrameObserver mObserver
The object that observes the Shape Frames that this group cares about.
Definition: CollisionGroup.hpp:517
virtual void addCollisionObjectToEngine(CollisionObject *object)=0
Add CollisionObject to the collision detection engine.
void addShapeFramesOf()
Do nothing.
Definition: CollisionGroup.cpp:79
bool collide(const CollisionOption &option=CollisionOption(false, 1u, nullptr), CollisionResult *result=nullptr)
Perform collision check within this CollisionGroup.
Definition: CollisionGroup.cpp:184
bool mUpdateAutomatically
Set this to true to have this CollisionGroup check for updates automatically.
Definition: CollisionGroup.hpp:440
void subscribeTo()
Do nothing.
Definition: CollisionGroup.cpp:85
bool updateSkeletonSource(SkeletonSources::value_type &entry)
Internal function called to update a Skeleton source.
Definition: CollisionGroup.cpp:418
bool updateBodyNodeSource(BodyNodeSources::value_type &entry)
Internal function called to update a BodyNode source.
Definition: CollisionGroup.cpp:549
CollisionDetectorPtr mCollisionDetector
Collision detector.
Definition: CollisionGroup.hpp:335
void removeShapeFrames(const std::vector< const dynamics::ShapeFrame * > &shapeFrames)
Remove ShapeFrames from this CollisionGroup.
Definition: CollisionGroup.cpp:132
virtual void initializeEngineData()=0
Initialize the collision detection engine data such as broadphase algorithm.
void addShapeFrames(const std::vector< const dynamics::ShapeFrame * > &shapeFrames)
Add ShapeFrames to this CollisionGroup.
Definition: CollisionGroup.cpp:71
double distance(const DistanceOption &option=DistanceOption(false, 0.0, nullptr), DistanceResult *result=nullptr)
Get the minimum signed distance between the Shape pairs in this CollisionGroup.
Definition: CollisionGroup.cpp:206
SkeletonSources mSkeletonSources
Skeleton sources that this group is subscribed to.
Definition: CollisionGroup.hpp:511
bool updateShapeFrame(ObjectInfo *object)
Internal function called to update a ShapeFrame.
Definition: CollisionGroup.cpp:614
std::unordered_map< const dynamics::BodyNode *, BodyNodeSource > BodyNodeSources
Definition: CollisionGroup.hpp:496
virtual void removeAllCollisionObjectsFromEngine()=0
Remove all the CollisionObjects from the collision detection engine.
void unsubscribeFrom(const dynamics::BodyNode *bodyNode, const Others *... others)
Unsubscribe from bodyNode.
Definition: CollisionGroup.hpp:249
void removeDeletedShapeFrames()
Remove any ShapeFrames that have been deleted.
Definition: CollisionGroup.cpp:265
ObjectInfo * addShapeFrameImpl(const dynamics::ShapeFrame *shapeFrame, const void *source)
Implementation of addShapeFrame.
Definition: CollisionGroup.cpp:348
void removeAllShapeFrames()
Remove all the ShapeFrames in this CollisionGroup.
Definition: CollisionGroup.cpp:146
bool raycast(const Eigen::Vector3d &from, const Eigen::Vector3d &to, const RaycastOption &option=RaycastOption(), RaycastResult *result=nullptr)
Performs raycast to this collision group.
Definition: CollisionGroup.cpp:228
void addShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Add a ShapeFrame to this CollisionGroup.
Definition: CollisionGroup.cpp:65
virtual void updateCollisionGroupEngineData()=0
Update the collision detection engine data such as broadphase algorithm.
void removeShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Remove a ShapeFrame from this CollisionGroup.
Definition: CollisionGroup.cpp:91
virtual void addCollisionObjectsToEngine(const std::vector< CollisionObject * > &collObjects)=0
Add CollisionObjects to the collision detection engine.
void updateEngineData()
Update engine data.
Definition: CollisionGroup.cpp:308
virtual void removeCollisionObjectFromEngine(CollisionObject *object)=0
Remove CollisionObject from the collision detection engine.
void update()
Check whether this CollisionGroup's subscriptions or any of its objects need an update,...
Definition: CollisionGroup.cpp:253
bool getAutomaticUpdate() const
Get whether this CollisionGroup is set to automatically check for updates.
Definition: CollisionGroup.cpp:247
virtual ~CollisionGroup()=default
Destructor.
bool hasShapeFrame(const dynamics::ShapeFrame *shapeFrame) const
Return true if this CollisionGroup contains shapeFrame.
Definition: CollisionGroup.cpp:155
void setAutomaticUpdate(bool automatic=true)
Set whether this CollisionGroup will automatically check for updates.
Definition: CollisionGroup.cpp:241
CollisionGroup(const CollisionDetectorPtr &collisionDetector)
Constructor.
Definition: CollisionGroup.cpp:46
std::size_t getNumShapeFrames() const
Return number of ShapeFrames added to this CollisionGroup.
Definition: CollisionGroup.cpp:167
void removeShapeFramesOf()
Do nothing.
Definition: CollisionGroup.cpp:140
std::vector< std::unique_ptr< ObjectInfo > > ObjectInfoList
Definition: CollisionGroup.hpp:369
void removeShapeFrameInternal(const dynamics::ShapeFrame *shapeFrame, const void *source)
Internal version of removeShapeFrame.
Definition: CollisionGroup.cpp:390
CollisionDetectorPtr getCollisionDetector()
Return collision detection engine associated with this CollisionGroup.
Definition: CollisionGroup.cpp:53
ObjectInfoList mObjectInfoList
Information about ShapeFrames and CollisionObjects that have been added to this CollisionGroup.
Definition: CollisionGroup.hpp:373
const dynamics::ShapeFrame * getShapeFrame(std::size_t index) const
Get the ShapeFrame corresponding to the given index.
Definition: CollisionGroup.cpp:173
BodyNodeSources mBodyNodeSources
BodyNode sources that this group is susbscribed to.
Definition: CollisionGroup.hpp:514
std::unordered_map< const dynamics::MetaSkeleton *, SkeletonSource > SkeletonSources
Definition: CollisionGroup.hpp:491
Definition: CollisionObject.hpp:45
Definition: CollisionResult.hpp:52
The Observer class should be inherited by any class that wants to respond in a customized way to the ...
Definition: Observer.hpp:52
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
MetaSkeleton is a pure abstract base class that provides a common interface for obtaining data (such ...
Definition: MetaSkeleton.hpp:62
Definition: ShapeFrame.hpp:192
class Skeleton
Definition: Skeleton.hpp:59
TemplateBodyNodePtr is a templated class that enables users to create a reference-counting BodyNodePt...
Definition: BodyNodePtr.hpp:110
std::shared_ptr< const CollisionDetector > ConstCollisionDetectorPtr
Definition: SmartPointer.hpp:41
std::shared_ptr< CollisionDetector > CollisionDetectorPtr
Definition: SmartPointer.hpp:41
std::shared_ptr< CollisionObject > CollisionObjectPtr
Definition: SmartPointer.hpp:45
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition: SmartPointer.hpp:60
Definition: BulletCollisionDetector.cpp:65
This is information pertaining to a child of the source.
Definition: CollisionGroup.hpp:460
ChildInfo(const std::size_t version)
A constructor that simply accepts a last known version number.
Definition: CollisionGroup.hpp:469
std::size_t mLastKnownVersion
Last known version of this child.
Definition: CollisionGroup.hpp:462
std::unordered_set< const dynamics::ShapeFrame * > mFrames
Last known set of frames attached to this child.
Definition: CollisionGroup.hpp:465
This struct is used to store sources of ShapeFrames that the CollisionGroup is subscribed to,...
Definition: CollisionGroup.hpp:447
std::unordered_map< const Child *, ChildInfo > mChildren
The last known versions of the children related to this source.
Definition: CollisionGroup.hpp:477
std::unordered_map< const dynamics::ShapeFrame *, ObjectInfo * > mObjects
The set of objects that pertain to this source.
Definition: CollisionGroup.hpp:455
Source mSource
The source of ShapeFrames.
Definition: CollisionGroup.hpp:449
CollisionSource(const Source &source, std::size_t lastKnownVersion)
Constructor.
Definition: CollisionGroup.hpp:480
std::size_t mLastKnownVersion
The last known version of that source.
Definition: CollisionGroup.hpp:452
Information on a collision object belonging to this group.
Definition: CollisionGroup.hpp:346
std::size_t mLastKnownVersion
The last known version of the last known shape that was held by the shape frame.
Definition: CollisionGroup.hpp:358
std::unordered_set< const void * > mSources
The set of all sources that indicate that this object should be in this group.
Definition: CollisionGroup.hpp:366
CollisionObjectPtr mObject
The CollisionObject that was generated by the CollisionDetector.
Definition: CollisionGroup.hpp:351
const dynamics::ShapeFrame * mFrame
The ShapeFrame for this object.
Definition: CollisionGroup.hpp:348
std::size_t mLastKnownShapeID
The ID of that last known shape that was held by the shape frame.
Definition: CollisionGroup.hpp:354
Definition: CollisionOption.hpp:45
Definition: DistanceOption.hpp:45
Definition: DistanceResult.hpp:47
Definition: RaycastOption.hpp:43
Definition: RaycastResult.hpp:63