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