DART 6.7.3
Loading...
Searching...
No Matches
CollisionGroup.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_COLLISIONGROUP_HPP_
34#define DART_COLLISION_COLLISIONGROUP_HPP_
35
36#include <unordered_set>
37#include <unordered_map>
38#include <vector>
46
47namespace dart {
48namespace collision {
49
51{
52public:
53
55 CollisionGroup(const CollisionDetectorPtr& collisionDetector);
56 // CollisionGroup also can be created from CollisionDetector::create()
57
59 virtual ~CollisionGroup() = default;
60
63
67
69 void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
70
72 void addShapeFrames(
73 const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
74
83 template <typename... Others>
84 void addShapeFramesOf(const dynamics::ShapeFrame* shapeFrame,
85 const Others*... others);
86
88 template <typename... Others>
90 const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
91 const Others*... others);
92
95 template <typename... Others>
96 void addShapeFramesOf(const CollisionGroup* otherGroup,
97 const Others*... others);
98
101 template <typename... Others>
102 void addShapeFramesOf(const dynamics::BodyNode* bodyNode,
103 const Others*... others);
104
107 template <typename... Others>
108 void addShapeFramesOf(const dynamics::MetaSkeleton* skeleton,
109 const Others*... others);
110
113 void addShapeFramesOf();
114
120 template <typename... Others>
121 void subscribeTo(
122 const dynamics::ConstBodyNodePtr& bodyNode,
123 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,
137 const Others&... others);
138
141 void subscribeTo();
142
147 void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
148
151 const std::vector<const dynamics::ShapeFrame*>& shapeFrames);
152
161 template <typename... Others>
162 void removeShapeFramesOf(const dynamics::ShapeFrame* shapeFrame,
163 const Others*... others);
164
166 template <typename... Others>
168 const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
169 const Others*... others);
170
176 template <typename... Others>
177 void removeShapeFramesOf(const CollisionGroup* otherGroup,
178 const Others*... others);
179
185 template <typename... Others>
186 void removeShapeFramesOf(const dynamics::BodyNode* bodyNode,
187 const Others*... others);
188
194 template <typename... Others>
195 void removeShapeFramesOf(const dynamics::MetaSkeleton* skeleton,
196 const Others*... others);
197
200 void removeShapeFramesOf();
201
204
207 template <typename... Others>
208 void unsubscribeFrom(
209 const dynamics::BodyNode* bodyNode,
210 const Others*... others);
211
214 template <typename... Others>
215 void unsubscribeFrom(
216 const dynamics::Skeleton* skeleton,
217 const Others*... others);
218
220 bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const;
221
223 std::size_t getNumShapeFrames() const;
224
226 const dynamics::ShapeFrame* getShapeFrame(std::size_t index) const;
227
229 bool collide(
230 const CollisionOption& option = CollisionOption(false, 1u, nullptr),
231 CollisionResult* result = nullptr);
232
237 bool collide(
238 CollisionGroup* otherGroup,
239 const CollisionOption& option = CollisionOption(false, 1u, nullptr),
240 CollisionResult* result = nullptr);
241
250 double distance(
251 const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
252 DistanceResult* result = nullptr);
253
266 double distance(
267 CollisionGroup* otherGroup,
268 const DistanceOption& option = DistanceOption(false, 0.0, nullptr),
269 DistanceResult* result = nullptr);
270
272 void setAutomaticUpdate(bool automatic = true);
273
275 bool getAutomaticUpdate() const;
276
286 void update();
287
294
295protected:
296
299 void updateEngineData();
300
304 virtual void initializeEngineData() = 0;
305
308
311 const std::vector<CollisionObject*>& collObjects) = 0;
312
315
318
322
323protected:
324
327 // CollisionGroup shares the ownership of CollisionDetector with other
328 // CollisionGroups created from the same CollisionDetector so that the
329 // CollisionDetector doesn't get destroyed as long as at least one
330 // CollisionGroup is alive.
331
336 struct ObjectInfo final
337 {
340
343
345 std::size_t mLastKnownShapeID;
346
349 std::size_t mLastKnownVersion;
350
357 std::unordered_set<const void*> mSources;
358 };
359
360 using ObjectInfoList = std::vector<std::unique_ptr<ObjectInfo>>;
361
365 // CollisionGroup also shares the ownership of CollisionObjects across other
366 // CollisionGroups for the same reason with above.
367 //
368 // Dev note: This was supposed to be std::map rather than std::vector for
369 // better search performance. The reason we use std::vector is to get
370 // deterministic contact results regardless of the order of CollisionObjects
371 // in this container for FCLCollisionDetector.
372 //
373 // fcl's collision result is dependent on the order of objects in the broad
374 // phase classes. If we use std::map, the orders of element between the
375 // original and copy are not guranteed to be the same as we copy std::map
376 // (e.g., by world cloning).
377
378
379private:
380
392 {
393 public:
394
396 void addShapeFrame(const dynamics::ShapeFrame* shapeFrame);
397
399 void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame);
400
403
407 std::unordered_set<const dynamics::ShapeFrame*> mDeletedFrames;
408
409 protected:
410
412 void handleDestructionNotification(const common::Subject* subject) override;
413
414 private:
415
419 std::unordered_map<const common::Subject*,
421
422 };
423
428 const dynamics::ShapeFrame* shapeFrame,
429 const void* source);
430
434 const dynamics::ShapeFrame* shapeFrame,
435 const void* source);
436
440
444 template <typename Source, typename Child = void>
446 {
448 Source mSource;
449
451 std::size_t mLastKnownVersion;
452
454 std::unordered_map<const dynamics::ShapeFrame*, ObjectInfo*> mObjects;
455
459 {
461 std::size_t mLastKnownVersion;
462
464 std::unordered_set<const dynamics::ShapeFrame*> mFrames;
465
468 explicit ChildInfo(const std::size_t version)
469 : 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),
482 mLastKnownVersion(lastKnownVersion)
483 {
484 // Do nothing
485 }
486 };
487
488 // Convenient typedefs for Skeleton sources
491 using SkeletonSources = std::unordered_map<
493
494 // Convenient typedefs for BodyNode sources
496 using BodyNodeSources = std::unordered_map<
498
501 bool updateSkeletonSource(SkeletonSources::value_type& entry);
502
505 bool updateBodyNodeSource(BodyNodeSources::value_type& entry);
506
509 bool updateShapeFrame(ObjectInfo* object);
510
513
516
519
520};
521
522} // namespace collision
523} // namespace dart
524
526
527#endif // DART_COLLISION_COLLISIONGROUP_HPP_
const CollisionOption & option
Collision option of DART.
Definition FCLCollisionDetector.cpp:154
CollisionResult * result
Collision result of DART.
Definition FCLCollisionDetector.cpp:157
std::size_t index
Definition SkelParser.cpp:1617
This class watches when ShapeFrames get deleted so that they can be safely removes from the Collision...
Definition CollisionGroup.hpp:392
void removeShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Remove a shape frame from this observer.
Definition CollisionGroup.cpp:307
void handleDestructionNotification(const common::Subject *subject) override
This will be called each time an observed shape frame is deleted.
Definition CollisionGroup.cpp:321
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:407
void addShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Add a shape frame to this observer.
Definition CollisionGroup.cpp:298
std::unordered_map< const common::Subject *, const dynamics::ShapeFrame * > mMap
A map from a subject pointer to its corresponding ShapeFrame pointer.
Definition CollisionGroup.hpp:420
void removeAllShapeFrames()
Remove all shape frames from this observer.
Definition CollisionGroup.cpp:315
Definition CollisionGroup.hpp:51
std::unordered_map< const dynamics::MetaSkeleton *, SkeletonSource > SkeletonSources
Definition CollisionGroup.hpp:492
ShapeFrameObserver mObserver
The object that observes the Shape Frames that this group cares about.
Definition CollisionGroup.hpp:518
virtual void addCollisionObjectToEngine(CollisionObject *object)=0
Add CollisionObject to the collision detection engine.
void addShapeFramesOf()
Do nothing.
Definition CollisionGroup.cpp:80
bool collide(const CollisionOption &option=CollisionOption(false, 1u, nullptr), CollisionResult *result=nullptr)
Perform collision check within this CollisionGroup.
Definition CollisionGroup.cpp:180
std::unordered_map< const dynamics::BodyNode *, BodyNodeSource > BodyNodeSources
Definition CollisionGroup.hpp:497
bool mUpdateAutomatically
Set this to true to have this CollisionGroup check for updates automatically.
Definition CollisionGroup.hpp:439
void subscribeTo()
Do nothing.
Definition CollisionGroup.cpp:86
bool updateSkeletonSource(SkeletonSources::value_type &entry)
Internal function called to update a Skeleton source.
Definition CollisionGroup.cpp:395
bool updateBodyNodeSource(BodyNodeSources::value_type &entry)
Internal function called to update a BodyNode source.
Definition CollisionGroup.cpp:526
CollisionDetectorPtr mCollisionDetector
Collision detector.
Definition CollisionGroup.hpp:326
void removeShapeFrames(const std::vector< const dynamics::ShapeFrame * > &shapeFrames)
Remove ShapeFrames from this CollisionGroup.
Definition CollisionGroup.cpp:130
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:72
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:202
SkeletonSources mSkeletonSources
Skeleton sources that this group is subscribed to.
Definition CollisionGroup.hpp:512
bool updateShapeFrame(ObjectInfo *object)
Internal function called to update a ShapeFrame.
Definition CollisionGroup.cpp:591
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:255
void removeDeletedShapeFrames()
Remove any ShapeFrames that have been deleted.
Definition CollisionGroup.cpp:248
ObjectInfo * addShapeFrameImpl(const dynamics::ShapeFrame *shapeFrame, const void *source)
Implementation of addShapeFrame.
Definition CollisionGroup.cpp:329
void removeAllShapeFrames()
Remove all the ShapeFrames in this CollisionGroup.
Definition CollisionGroup.cpp:144
void addShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Add a ShapeFrame to this CollisionGroup.
Definition CollisionGroup.cpp:66
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:92
virtual void addCollisionObjectsToEngine(const std::vector< CollisionObject * > &collObjects)=0
Add CollisionObjects to the collision detection engine.
void updateEngineData()
Update engine data.
Definition CollisionGroup.cpp:289
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:236
bool getAutomaticUpdate() const
Get whether this CollisionGroup is set to automatically check for updates.
Definition CollisionGroup.cpp:230
virtual ~CollisionGroup()=default
Destructor.
bool hasShapeFrame(const dynamics::ShapeFrame *shapeFrame) const
Return true if this CollisionGroup contains shapeFrame.
Definition CollisionGroup.cpp:153
void setAutomaticUpdate(bool automatic=true)
Set whether this CollisionGroup will automatically check for updates.
Definition CollisionGroup.cpp:224
std::size_t getNumShapeFrames() const
Return number of ShapeFrames added to this CollisionGroup.
Definition CollisionGroup.cpp:163
void removeShapeFramesOf()
Do nothing.
Definition CollisionGroup.cpp:138
std::vector< std::unique_ptr< ObjectInfo > > ObjectInfoList
Definition CollisionGroup.hpp:360
void removeShapeFrameInternal(const dynamics::ShapeFrame *shapeFrame, const void *source)
Internal version of removeShapeFrame.
Definition CollisionGroup.cpp:368
CollisionDetectorPtr getCollisionDetector()
Return collision detection engine associated with this CollisionGroup.
Definition CollisionGroup.cpp:54
ObjectInfoList mObjectInfoList
Information about ShapeFrames and CollisionObjects that have been added to this CollisionGroup.
Definition CollisionGroup.hpp:364
const dynamics::ShapeFrame * getShapeFrame(std::size_t index) const
Get the ShapeFrame corresponding to the given index.
Definition CollisionGroup.cpp:169
BodyNodeSources mBodyNodeSources
BodyNode sources that this group is susbscribed to.
Definition CollisionGroup.hpp:515
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:78
MetaSkeleton is a pure abstract base class that provides a common interface for obtaining data (such ...
Definition MetaSkeleton.hpp:62
Definition ShapeFrame.hpp:164
class Skeleton
Definition Skeleton.hpp:59
TemplateBodyNodePtr is a templated class that enables users to create a reference-counting BodyNodePt...
Definition BodyNodePtr.hpp:111
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:63
This is information pertaining to a child of the source.
Definition CollisionGroup.hpp:459
ChildInfo(const std::size_t version)
A constructor that simply accepts a last known version number.
Definition CollisionGroup.hpp:468
std::size_t mLastKnownVersion
Last known version of this child.
Definition CollisionGroup.hpp:461
std::unordered_set< const dynamics::ShapeFrame * > mFrames
Last known set of frames attached to this child.
Definition CollisionGroup.hpp:464
This struct is used to store sources of ShapeFrames that the CollisionGroup is subscribed to,...
Definition CollisionGroup.hpp:446
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:454
Source mSource
The source of ShapeFrames.
Definition CollisionGroup.hpp:448
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:451
Information on a collision object belonging to this group.
Definition CollisionGroup.hpp:337
std::size_t mLastKnownVersion
The last known version of the last known shape that was held by the shape frame.
Definition CollisionGroup.hpp:349
std::unordered_set< const void * > mSources
The set of all sources that indicate that this object should be in this group.
Definition CollisionGroup.hpp:357
CollisionObjectPtr mObject
The CollisionObject that was generated by the CollisionDetector.
Definition CollisionGroup.hpp:342
const dynamics::ShapeFrame * mFrame
The ShapeFrame for this object.
Definition CollisionGroup.hpp:339
std::size_t mLastKnownShapeID
The ID of that last known shape that was held by the shape frame.
Definition CollisionGroup.hpp:345
Definition CollisionOption.hpp:45
Definition DistanceOption.hpp:45
Definition DistanceResult.hpp:47