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_DETAIL_COLLISIONGROUP_HPP_
34 #define DART_COLLISION_DETAIL_COLLISIONGROUP_HPP_
35 
37 
40 
41 namespace dart {
42 namespace collision {
43 
44 //==============================================================================
45 template <typename... Others>
47  const dynamics::ShapeFrame* shapeFrame, const Others*... others)
48 {
49  addShapeFrame(shapeFrame);
50 
51  addShapeFramesOf(others...);
52 }
53 
54 //==============================================================================
55 template <typename... Others>
57  const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
58  const Others*... others)
59 {
60  addShapeFrames(shapeFrames);
61 
62  addShapeFramesOf(others...);
63 }
64 
65 //==============================================================================
66 template <typename... Others>
68  const CollisionGroup* otherGroup, const Others*... others)
69 {
70  assert(otherGroup);
71 
72  if (otherGroup && this != otherGroup)
73  {
74  for (const auto& info : otherGroup->mObjectInfoList)
75  addShapeFrame(info->mFrame);
76  }
77 
78  addShapeFramesOf(others...);
79 }
80 
81 //==============================================================================
82 template <typename... Others>
84  const dynamics::BodyNode* bodyNode, const Others*... others)
85 {
86  assert(bodyNode);
87 
88  auto collisionShapeNodes
90 
91  for (auto& shapeNode : collisionShapeNodes)
92  addShapeFrame(shapeNode);
93 
94  addShapeFramesOf(others...);
95 }
96 
97 //==============================================================================
98 template <typename... Others>
100  const dynamics::MetaSkeleton* skel, const Others*... others)
101 {
102  assert(skel);
103 
104  auto numBodyNodes = skel->getNumBodyNodes();
105  for (auto i = 0u; i < numBodyNodes; ++i)
106  addShapeFramesOf(skel->getBodyNode(i));
107 
108  addShapeFramesOf(others...);
109 }
110 
111 //==============================================================================
112 template <typename... Others>
114  const dynamics::ConstBodyNodePtr& bodyNode, const Others&... others)
115 {
116  const auto inserted = mBodyNodeSources.insert(BodyNodeSources::value_type(
117  bodyNode.get(), BodyNodeSource(bodyNode.get(), bodyNode->getVersion())));
118 
119  if (inserted.second)
120  {
121  const BodyNodeSources::iterator& entry = inserted.first;
122 
123  const auto collisionShapeNodes
124  = bodyNode->getShapeNodesWith<dynamics::CollisionAspect>();
125 
126  for (const auto& shapeNode : collisionShapeNodes)
127  {
128  entry->second.mObjects.insert(
129  {shapeNode, addShapeFrameImpl(shapeNode, bodyNode.get())});
130  }
131  }
132 
133  subscribeTo(others...);
134 }
135 
136 //==============================================================================
137 template <typename... Others>
139  const dynamics::ConstSkeletonPtr& skeleton, const Others&... others)
140 {
141  const auto inserted = mSkeletonSources.insert(SkeletonSources::value_type(
142  skeleton.get(), SkeletonSource(skeleton, skeleton->getVersion())));
143 
144  if (inserted.second)
145  {
146  SkeletonSource& entry = inserted.first->second;
147 
148  const std::size_t numBodies = skeleton->getNumBodyNodes();
149  for (std::size_t i = 0u; i < numBodies; ++i)
150  {
151  const dynamics::BodyNode* bn = skeleton->getBodyNode(i);
152 
153  const auto& collisionShapeNodes
155 
156  auto& childInfo
157  = entry.mChildren
158  .insert(std::make_pair(
160  .first->second;
161 
162  for (const auto& shapeNode : collisionShapeNodes)
163  {
164  entry.mObjects.insert(
165  {shapeNode, addShapeFrameImpl(shapeNode, skeleton.get())});
166  childInfo.mFrames.insert(shapeNode);
167  }
168  }
169  }
170 
171  subscribeTo(others...);
172 }
173 
174 //==============================================================================
175 template <typename... Others>
177  const dynamics::ShapeFrame* shapeFrame, const Others*... others)
178 {
179  removeShapeFrame(shapeFrame);
180 
181  removeShapeFramesOf(others...);
182 }
183 
184 //==============================================================================
185 template <typename... Others>
187  const std::vector<const dynamics::ShapeFrame*>& shapeFrames,
188  const Others*... others)
189 {
190  removeShapeFrames(shapeFrames);
191 
192  removeShapeFramesOf(others...);
193 }
194 
195 //==============================================================================
196 template <typename... Others>
198  const CollisionGroup* otherGroup, const Others*... others)
199 {
200  assert(otherGroup);
201 
202  if (otherGroup)
203  {
204  if (this == otherGroup)
205  {
207  return;
208  }
209 
210  for (const auto& info : otherGroup->mObjectInfoList)
211  removeShapeFrame(info->mFrame);
212  }
213 
214  removeShapeFramesOf(others...);
215 }
216 
217 //==============================================================================
218 template <typename... Others>
220  const dynamics::BodyNode* bodyNode, const Others*... others)
221 {
222  assert(bodyNode);
223 
224  auto collisionShapeNodes
226 
227  for (auto& shapeNode : collisionShapeNodes)
228  removeShapeFrame(shapeNode);
229 
230  removeShapeFramesOf(others...);
231 }
232 
233 //==============================================================================
234 template <typename... Others>
236  const dynamics::MetaSkeleton* skel, const Others*... others)
237 {
238  assert(skel);
239 
240  auto numBodyNodes = skel->getNumBodyNodes();
241  for (auto i = 0u; i < numBodyNodes; ++i)
243 
244  removeShapeFramesOf(others...);
245 }
246 
247 //==============================================================================
248 template <typename... Others>
250  const dynamics::BodyNode* bodyNode, const Others*... others)
251 {
252  auto it = mBodyNodeSources.find(bodyNode);
253  if (it != mBodyNodeSources.end())
254  {
255  for (const auto& entry : it->second.mObjects)
256  removeShapeFrameInternal(entry.first, bodyNode);
257 
258  mBodyNodeSources.erase(it);
259  }
260 
261  unsubscribeFrom(others...);
262 }
263 
264 //==============================================================================
265 template <typename... Others>
267  const dynamics::Skeleton* skeleton, const Others*... others)
268 {
269  auto it = mSkeletonSources.find(skeleton);
270  if (it != mSkeletonSources.end())
271  {
272  for (const auto& entry : it->second.mObjects)
273  {
275  entry.first, static_cast<const dynamics::MetaSkeleton*>(skeleton));
276  }
277 
278  mSkeletonSources.erase(it);
279  }
280 
281  unsubscribeFrom(others...);
282 }
283 
284 } // namespace collision
285 } // namespace dart
286 
287 #endif // DART_COLLISION_DETAIL_COLLISIONGROUP_HPP_
Definition: CollisionGroup.hpp:53
void addShapeFramesOf()
Do nothing.
Definition: CollisionGroup.cpp:79
void subscribeTo()
Do nothing.
Definition: CollisionGroup.cpp:85
void removeShapeFrames(const std::vector< const dynamics::ShapeFrame * > &shapeFrames)
Remove ShapeFrames from this CollisionGroup.
Definition: CollisionGroup.cpp:132
void addShapeFrames(const std::vector< const dynamics::ShapeFrame * > &shapeFrames)
Add ShapeFrames to this CollisionGroup.
Definition: CollisionGroup.cpp:71
SkeletonSources mSkeletonSources
Skeleton sources that this group is subscribed to.
Definition: CollisionGroup.hpp:511
void unsubscribeFrom(const dynamics::BodyNode *bodyNode, const Others *... others)
Unsubscribe from bodyNode.
Definition: CollisionGroup.hpp:249
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
void addShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Add a ShapeFrame to this CollisionGroup.
Definition: CollisionGroup.cpp:65
void removeShapeFrame(const dynamics::ShapeFrame *shapeFrame)
Remove a ShapeFrame from this CollisionGroup.
Definition: CollisionGroup.cpp:91
void removeShapeFramesOf()
Do nothing.
Definition: CollisionGroup.cpp:140
void removeShapeFrameInternal(const dynamics::ShapeFrame *shapeFrame, const void *source)
Internal version of removeShapeFrame.
Definition: CollisionGroup.cpp:390
ObjectInfoList mObjectInfoList
Information about ShapeFrames and CollisionObjects that have been added to this CollisionGroup.
Definition: CollisionGroup.hpp:373
BodyNodeSources mBodyNodeSources
BodyNode sources that this group is susbscribed to.
Definition: CollisionGroup.hpp:514
virtual std::size_t getVersion() const
Get the version number of this object.
Definition: VersionCounter.cpp:59
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
const std::vector< ShapeNode * > getShapeNodesWith()
Return the list of ShapeNodes containing given Aspect.
Definition: BodyNode.hpp:221
Definition: ShapeFrame.hpp:117
MetaSkeleton is a pure abstract base class that provides a common interface for obtaining data (such ...
Definition: MetaSkeleton.hpp:62
virtual std::size_t getNumBodyNodes() const =0
Get number of body nodes.
virtual BodyNode * getBodyNode(std::size_t _idx)=0
Get BodyNode whose index is _idx.
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
BodyNodeT * get() const
Get the raw BodyNode pointer.
Definition: BodyNodePtr.hpp:186
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition: SmartPointer.hpp:60
std::multimap< dart::dynamics::Shape *, SimpleFrameShapeDnD * >::iterator iterator
Definition: Viewer.cpp:620
Definition: BulletCollisionDetector.cpp:65
This is information pertaining to a child of the source.
Definition: CollisionGroup.hpp:460
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