DART  6.10.1
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 
42 namespace dart {
43 namespace dynamics {
44 
45 //==============================================================================
46 template <class NodeType>
47 std::size_t AccessoryNode<NodeType>::getIndexInBodyNode() const
48 {
49  return static_cast<const NodeType*>(this)->mIndexInBodyNode;
50 }
51 
52 //==============================================================================
53 template <class NodeType>
54 std::size_t AccessoryNode<NodeType>::getIndexInSkeleton() const
55 {
56  return static_cast<const NodeType*>(this)->mIndexInSkeleton;
57 }
58 
59 //==============================================================================
60 template <class NodeType>
61 std::size_t AccessoryNode<NodeType>::getIndexInTree() const
62 {
63  return static_cast<const NodeType*>(this)->mIndexInTree;
64 }
65 
66 //==============================================================================
67 template <class NodeType>
68 std::size_t AccessoryNode<NodeType>::getTreeIndex() const
69 {
70  return static_cast<const NodeType*>(this)->getBodyNodePtr()->getTreeIndex();
71 }
72 
73 //==============================================================================
74 template <class NodeType>
75 void AccessoryNode<NodeType>::remove()
76 {
77  return static_cast<NodeType*>(this)->stageForRemoval();
78 }
79 
80 //==============================================================================
81 template <class NodeType>
82 void 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() \
94 public: \
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) \
126 private: \
127  dart::dynamics::BodyNode::NodeMap::iterator it; \
128  \
129 public: \
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) \
158 private: \
159  dart::dynamics::BodyNode::NodeMap::iterator it; \
160  \
161 public: \
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() \
238 public: \
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) \
310 private: \
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  \
315 public: \
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) \
376 private: \
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  \
381 public: \
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