DART 6.12.2
Loading...
Searching...
No Matches
Node.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_DYNAMICS_DETAIL_NODE_HPP_
34#define DART_DYNAMICS_DETAIL_NODE_HPP_
35
36#include <cassert>
37
38#include "dart/common/StlHelpers.hpp"
39#include "dart/dynamics/Node.hpp"
40
41namespace dart {
42namespace dynamics {
43
44//==============================================================================
45template <class NodeType>
46std::size_t AccessoryNode<NodeType>::getIndexInBodyNode() const
47{
48 return static_cast<const NodeType*>(this)->mIndexInBodyNode;
49}
50
51//==============================================================================
52template <class NodeType>
53std::size_t AccessoryNode<NodeType>::getIndexInSkeleton() const
54{
55 return static_cast<const NodeType*>(this)->mIndexInSkeleton;
56}
57
58//==============================================================================
59template <class NodeType>
60std::size_t AccessoryNode<NodeType>::getIndexInTree() const
61{
62 return static_cast<const NodeType*>(this)->mIndexInTree;
63}
64
65//==============================================================================
66template <class NodeType>
67std::size_t AccessoryNode<NodeType>::getTreeIndex() const
68{
69 return static_cast<const NodeType*>(this)->getBodyNodePtr()->getTreeIndex();
70}
71
72//==============================================================================
73template <class NodeType>
74void AccessoryNode<NodeType>::remove()
75{
76 return static_cast<NodeType*>(this)->stageForRemoval();
77}
78
79//==============================================================================
80template <class NodeType>
81void AccessoryNode<NodeType>::reattach()
82{
83 static_cast<NodeType*>(this)->attach();
84}
85
86} // namespace dynamics
87} // namespace dart
88
89//==============================================================================
90// Macros for specializing Nodes within BodyNodes
91//==============================================================================
92#define DART_ENABLE_NODE_SPECIALIZATION() \
93public: \
94 template <class T> \
95 std::size_t getNumNodes() const \
96 { \
97 return dart::dynamics::BodyNode::getNumNodes<T>(); \
98 } \
99 template <class T> \
100 T* getNode(std::size_t index) \
101 { \
102 return dart::dynamics::BodyNode::getNode<T>(index); \
103 } \
104 template <class T> \
105 const T* getNode(std::size_t index) const \
106 { \
107 return dart::dynamics::BodyNode::getNode<T>(index); \
108 }
109
110//==============================================================================
111#define DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE(NodeName, it) \
112 mNodeMap[typeid(NodeName)] = std::vector<dart::dynamics::Node*>(); \
113 it = mNodeMap.find(typeid(NodeName));
114
115//==============================================================================
116#define DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, PluralName) \
117 DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE(NodeName, m##PluralName##Iterator)
118
119//==============================================================================
120#define DART_INSTANTIATE_SPECALIZED_NODE(NodeName) \
121 DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, NodeName##s)
122
123//==============================================================================
124#define DETAIL_DART_SPECIALIZED_NODE_INLINE(NodeName, PluralName, it) \
125private: \
126 dart::dynamics::BodyNode::NodeMap::iterator it; \
127 \
128public: \
129 inline std::size_t getNum##PluralName() const \
130 { \
131 return it->second.size(); \
132 } \
133 \
134 inline NodeName* get##NodeName(std::size_t index) \
135 { \
136 return static_cast<NodeName*>( \
137 getVectorObjectIfAvailable(index, it->second)); \
138 } \
139 \
140 inline const NodeName* get##NodeName(std::size_t index) const \
141 { \
142 return static_cast<const NodeName*>( \
143 getVectorObjectIfAvailable(index, it->second)); \
144 }
145
146//==============================================================================
147#define DART_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, PluralName) \
148 DETAIL_DART_SPECIALIZED_NODE_INLINE( \
149 NodeName, PluralName, m##PluralName##Iterator)
150
151//==============================================================================
152#define DART_SPECIALIZED_NODE_INTERNAL(NodeName) \
153 DART_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, NodeName##s)
154
155//==============================================================================
156#define DETAIL_DART_SPECIALIZED_NODE_DECLARE(NodeName, PluralName, it) \
157private: \
158 dart::dynamics::BodyNode::NodeMap::iterator it; \
159 \
160public: \
161 std::size_t getNum##PluralName() const; \
162 NodeName* get##NodeName(std::size_t index); \
163 const NodeName* get##NodeName(std::size_t index) const;
164
165//==============================================================================
166#define DART_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, PluralName) \
167 DETAIL_DART_SPECIALIZED_NODE_DECLARE( \
168 NodeName, PluralName, m##PluralName##Iterator)
169
170//==============================================================================
171#define DART_SPECIALIZED_NODE_DECLARE(NodeName) \
172 DART_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, NodeName##s)
173
174//==============================================================================
175#define DETAIL_DART_SPECIALIZED_NODE_DEFINE( \
176 BodyNodeType, NodeName, PluralName, it) \
177 std::size_t BodyNodeType ::getNum##PluralName() const \
178 { \
179 return it->second.size(); \
180 } \
181 \
182 NodeName* BodyNodeType ::get##NodeName(std::size_t index) \
183 { \
184 return static_cast<NodeName*>( \
185 getVectorObjectIfAvailable(index, it->second)); \
186 } \
187 \
188 const NodeName* BodyNodeType ::get##NodeName(std::size_t index) const \
189 { \
190 return static_cast<const NodeName*>( \
191 getVectorObjectIfAvailable(index, it->second)); \
192 }
193
194//==============================================================================
195#define DART_SPECIALIZED_NODE_DEFINE_IRREGULAR( \
196 BodyNodeType, NodeName, PluralName) \
197 DETAIL_DART_SPECIALIZED_NODE_DEFINE( \
198 BodyNodeType, NodeName, PluralName, m##PluralName##Iterator)
199
200//==============================================================================
201#define DART_SPECIALIZED_NODE_DEFINE(BodyNodeType, NodeName) \
202 DART_SPECIALIZED_NODE_DEFINE_IRREGULAR(BodyNodeType, NodeName, NodeName##s)
203
204//==============================================================================
205#define DETAIL_DART_SPECIALIZED_NODE_TEMPLATE( \
206 BodyNodeType, NodeName, PluralName) \
207 template <> \
208 inline std::size_t BodyNodeType ::getNumNodes<NodeName>() const \
209 { \
210 return getNum##PluralName(); \
211 } \
212 template <> \
213 inline NodeName* BodyNodeType ::getNode<NodeName>(std::size_t index) \
214 { \
215 return get##NodeName(index); \
216 } \
217 template <> \
218 inline const NodeName* BodyNodeType ::getNode<NodeName>(std::size_t index) \
219 const \
220 { \
221 return get##NodeName(index); \
222 }
223
224//==============================================================================
225#define DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( \
226 BodyNodeType, NodeName, PluralName) \
227 DETAIL_DART_SPECIALIZED_NODE_TEMPLATE(BodyNodeType, NodeName, PluralName)
228
229//==============================================================================
230#define DART_SPECIALIZED_NODE_TEMPLATE(BodyNodeType, NodeName) \
231 DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR(BodyNodeType, NodeName, NodeName##s)
232
233//==============================================================================
234// Macros for specializing Nodes within Skeletons
235//==============================================================================
236#define DART_SKEL_ENABLE_NODE_SPECIALIZATION() \
237public: \
238 template <class T> \
239 std::size_t getNumNodes() const \
240 { \
241 return dart::dynamics::Skeleton::getNumNodes<T>(); \
242 } \
243 template <class T> \
244 std::size_t getNumNodes(std::size_t treeIndex) const \
245 { \
246 return dart::dynamics::Skeleton::getNumNodes<T>(treeIndex); \
247 } \
248 template <class T> \
249 T* getNode(std::size_t index) \
250 { \
251 return dart::dynamics::Skeleton::getNode<T>(index); \
252 } \
253 template <class T> \
254 T* getNode(std::size_t nodeIdx, std::size_t treeIndex) \
255 { \
256 return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex); \
257 } \
258 template <class T> \
259 const T* getNode(std::size_t index) const \
260 { \
261 return dart::dynamics::Skeleton::getNode<T>(index); \
262 } \
263 template <class T> \
264 const T* getNode(std::size_t nodeIdx, std::size_t treeIndex) const \
265 { \
266 return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex); \
267 } \
268 template <class T> \
269 T* getNode(const std::string& name) \
270 { \
271 return dart::dynamics::Skeleton::getNode<T>(name); \
272 } \
273 template <class T> \
274 const T* getNode(const std::string& name) const \
275 { \
276 return dart::dynamics::Skeleton::getNode<T>(name); \
277 }
278
279//==============================================================================
280#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, PluralName) \
281 mSpecializedTreeNodes[typeid(NodeName)] = &mTree##PluralName##Iterators; \
282 mSkelCache.mNodeMap[typeid(NodeName)] \
283 = std::vector<dart::dynamics::Node*>(); \
284 m##PluralName##Iterator = mSkelCache.mNodeMap.find(typeid(NodeName)); \
285 mNodeNameMgrMap[typeid(NodeName)] = dart::common::NameManager<Node*>( \
286 std::string("Skeleton::") + #NodeName + " | " \
287 + mAspectProperties.mName); \
288 mNameMgrFor##PluralName = &mNodeNameMgrMap.find(typeid(NodeName))->second;
289
290//==============================================================================
291#define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE(NodeName) \
292 DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR(NodeName, NodeName##s);
293
294//==============================================================================
295#define DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, func) \
296 if (treeIndex >= treeIts.size()) \
297 { \
298 dterr << "[" << #func << "] Requesting an invalid tree (" << treeIndex \
299 << "). " \
300 << "The number of trees in this Skeleton is: " << treeIts.size() \
301 << "\n"; \
302 assert(false); \
303 return 0; \
304 }
305
306//==============================================================================
307#define DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( \
308 NodeName, PluralName, skelIt, treeIts, NameMgr) \
309private: \
310 dart::dynamics::Skeleton::NodeMap::iterator skelIt; \
311 std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts; \
312 dart::common::NameManager<dart::dynamics::Node*>* NameMgr; \
313 \
314public: \
315 inline std::size_t getNum##PluralName() const \
316 { \
317 return skelIt->second.size(); \
318 } \
319 inline std::size_t getNum##PluralName(std::size_t treeIndex) const \
320 { \
321 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum##PluralName); \
322 treeIts[treeIndex]->second.size(); \
323 } \
324 \
325 inline NodeName* get##NodeName(std::size_t index) \
326 { \
327 return static_cast<NodeName*>( \
328 getVectorObjectIfAvailable(index, skelIt->second)); \
329 } \
330 inline NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex) \
331 { \
332 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName); \
333 return static_cast<NodeName*>( \
334 getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); \
335 } \
336 \
337 inline const NodeName* get##NodeName(std::size_t index) const \
338 { \
339 return static_cast<const NodeName*>( \
340 getVectorObjectIfAvailable(index, skelIt->second)); \
341 } \
342 inline const NodeName* get##NodeName( \
343 std::size_t treeIndex, std::size_t nodeIndex) const \
344 { \
345 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName); \
346 return static_cast<const NodeName*>( \
347 getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); \
348 } \
349 \
350 inline NodeName* get##NodeName(const std::string& name) \
351 { \
352 return static_cast<NodeName*>(NameMgr->getObject(name)); \
353 } \
354 inline const NodeName* get##NodeName(const std::string& name) const \
355 { \
356 return static_cast<const NodeName*>(NameMgr->getObject(name)); \
357 }
358
359//==============================================================================
360#define DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, PluralName) \
361 DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( \
362 NodeName, \
363 PluralName, \
364 m##PluralName##Iterator, \
365 mTree##PluralName##Iterators, \
366 mNameMgrFor##PluralName);
367
368//==============================================================================
369#define DART_SKEL_SPECIALIZED_NODE_INLINE(NodeName) \
370 DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR(NodeName, NodeName##s);
371
372//==============================================================================
373#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( \
374 NodeName, PluralName, skelIt, treeIts, NameMgr) \
375private: \
376 dart::dynamics::Skeleton::NodeMap::iterator skelIt; \
377 std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts; \
378 dart::common::NameManager<dart::dynamics::Node*>* NameMgr; \
379 \
380public: \
381 std::size_t getNum##PluralName() const; \
382 \
383 std::size_t getNum##PluralName(std::size_t treeIndex) const; \
384 \
385 NodeName* get##NodeName(std::size_t index); \
386 \
387 NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex); \
388 \
389 const NodeName* get##NodeName(std::size_t index) const; \
390 \
391 const NodeName* get##NodeName(std::size_t treeIndex, std::size_t nodeIndex) \
392 const; \
393 \
394 NodeName* get##NodeName(const std::string& name); \
395 \
396 const NodeName* get##NodeName(const std::string& name) const;
397
398//==============================================================================
399#define DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, PluralName) \
400 DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( \
401 NodeName, \
402 PluralName, \
403 m##PluralName##Iterator, \
404 mTree##PluralName##Iterators, \
405 mNameMgrFor##PluralName);
406
407//==============================================================================
408#define DART_SKEL_SPECIALIZED_NODE_DECLARE(NodeName) \
409 DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR(NodeName, NodeName##s);
410
411//==============================================================================
412#define DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( \
413 SkeletonType, NodeName, PluralName, skelIt, treeIts, NameMgr) \
414 std::size_t SkeletonType ::getNum##PluralName() const \
415 { \
416 return skelIt->second.size(); \
417 } \
418 std::size_t SkeletonType ::getNum##PluralName(std::size_t treeIndex) const \
419 { \
420 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum##PluralName); \
421 return treeIts[treeIndex]->second.size(); \
422 } \
423 \
424 NodeName* SkeletonType ::get##NodeName(std::size_t index) \
425 { \
426 return static_cast<NodeName*>( \
427 getVectorObjectIfAvailable(index, skelIt->second)); \
428 } \
429 NodeName* SkeletonType ::get##NodeName( \
430 std::size_t treeIndex, std::size_t nodeIndex) \
431 { \
432 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName); \
433 return static_cast<NodeName*>( \
434 getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); \
435 } \
436 \
437 const NodeName* SkeletonType ::get##NodeName(std::size_t index) const \
438 { \
439 return static_cast<const NodeName*>( \
440 getVectorObjectIfAvailable(index, skelIt->second)); \
441 } \
442 const NodeName* SkeletonType ::get##NodeName( \
443 std::size_t treeIndex, std::size_t nodeIndex) const \
444 { \
445 DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get##NodeName); \
446 return static_cast<const NodeName*>( \
447 getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); \
448 } \
449 \
450 NodeName* SkeletonType ::get##NodeName(const std::string& name) \
451 { \
452 return static_cast<NodeName*>(NameMgr->getObject(name)); \
453 } \
454 const NodeName* SkeletonType ::get##NodeName(const std::string& name) const \
455 { \
456 return static_cast<const NodeName*>(NameMgr->getObject(name)); \
457 }
458
459//==============================================================================
460#define DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( \
461 SkeletonType, NodeName, PluralName) \
462 DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( \
463 SkeletonType, \
464 NodeName, \
465 PluralName, \
466 m##PluralName##Iterator, \
467 mTree##PluralName##Iterators, \
468 mNameMgrFor##PluralName);
469
470//==============================================================================
471#define DART_SKEL_SPECIALIZED_NODE_DEFINE(SkeletonType, NodeName) \
472 DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( \
473 SkeletonType, NodeName, NodeName##s);
474
475//==============================================================================
476#define DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE( \
477 SkelType, NodeName, PluralName) \
478 template <> \
479 inline std::size_t SkelType ::getNumNodes<NodeName>() const \
480 { \
481 return getNum##PluralName(); \
482 } \
483 template <> \
484 inline std::size_t SkelType ::getNumNodes<NodeName>(std::size_t index) const \
485 { \
486 return getNum##PluralName(index); \
487 } \
488 template <> \
489 inline NodeName* SkelType ::getNode<NodeName>(std::size_t index) \
490 { \
491 return get##NodeName(index); \
492 } \
493 template <> \
494 inline NodeName* SkelType ::getNode<NodeName>( \
495 std::size_t treeIndex, std::size_t nodeIndex) \
496 { \
497 return get##NodeName(treeIndex, nodeIndex); \
498 } \
499 template <> \
500 inline const NodeName* SkelType ::getNode<NodeName>(std::size_t index) const \
501 { \
502 return get##NodeName(index); \
503 } \
504 template <> \
505 inline const NodeName* SkelType ::getNode<NodeName>( \
506 std::size_t treeIndex, std::size_t nodeIndex) const \
507 { \
508 return get##NodeName(treeIndex, nodeIndex); \
509 } \
510 template <> \
511 inline NodeName* SkelType::getNode<NodeName>(const std::string& name) \
512 { \
513 return get##NodeName(name); \
514 } \
515 template <> \
516 inline const NodeName* SkelType::getNode<NodeName>(const std::string& name) \
517 const \
518 { \
519 return get##NodeName(name); \
520 }
521
522//==============================================================================
523#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( \
524 SkelType, NodeName, PluralName) \
525 DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE(SkelType, NodeName, PluralName)
526
527//==============================================================================
528#define DART_SKEL_SPECIALIZED_NODE_TEMPLATE(SkelType, NodeName) \
529 DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( \
530 SkelType, NodeName, NodeName##s);
531
532#endif // DART_DYNAMICS_DETAIL_NODE_HPP_
dart::dynamics::AccessoryNode::getTreeIndex
std::size_t getTreeIndex() const
Get the index of this Node's tree within its Skeleton.
Definition Node.hpp:67
dart::dynamics::AccessoryNode::getIndexInBodyNode
std::size_t getIndexInBodyNode() const
Get the index of this Node within its BodyNode.
Definition Node.hpp:46
dart::dynamics::AccessoryNode::getIndexInSkeleton
std::size_t getIndexInSkeleton() const
Get the index of this Node within its Skeleton.
Definition Node.hpp:53
dart::dynamics::AccessoryNode::remove
void remove()
Stage the Node for removal.
Definition Node.hpp:74
dart::dynamics::AccessoryNode::reattach
void reattach()
Undo the effectos of calling remove().
Definition Node.hpp:81
dart::dynamics::AccessoryNode::getIndexInTree
std::size_t getIndexInTree() const
Get the index of this Node within its tree.
Definition Node.hpp:60
dart
Definition BulletCollisionDetector.cpp:60