DART 6.10.1
Loading...
Searching...
No Matches
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>
48
49namespace dart {
50namespace collision {
51
53{
54public:
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>
86 const dynamics::ShapeFrame* shapeFrame, const Others*... others);
87
89 template <typename... Others>
91 const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
92 const Others*... others);
93
96 template <typename... Others>
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
150 const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
151
160 template <typename... Others>
162 const dynamics::ShapeFrame* shapeFrame, const Others*... others);
163
165 template <typename... Others>
167 const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
168 const Others*... others);
169
175 template <typename... Others>
177 const CollisionGroup* otherGroup, const Others*... others);
178
184 template <typename... Others>
186 const dynamics::BodyNode* bodyNode, const Others*... others);
187
193 template <typename... Others>
195 const dynamics::MetaSkeleton* skeleton, const Others*... others);
196
199 void removeShapeFramesOf();
200
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
305protected:
308 void updateEngineData();
309
313 virtual void initializeEngineData() = 0;
314
317
320 const std::vector<CollisionObject*>& collObjects)
321 = 0;
322
325
328
332
333protected:
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
387private:
399 {
400 public:
402 void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
403
405 void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
406
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
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
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