DART  6.7.3
Node.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_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> std::size_t getNumNodes() const { return dart::dynamics::BodyNode::getNumNodes<T>(); } \
96  template <class T> T* getNode(std::size_t index) { return dart::dynamics::BodyNode::getNode<T>(index); } \
97  template <class T> const T* getNode(std::size_t index) const { return dart::dynamics::BodyNode::getNode<T>(index); }
98 
99 //==============================================================================
100 #define DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE( NodeName, it ) \
101  mNodeMap[typeid( NodeName )] = std::vector<dart::dynamics::Node*>(); \
102  it = mNodeMap.find(typeid( NodeName ));
103 
104 //==============================================================================
105 #define DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, PluralName ) \
106  DETAIL_DART_INSTANTIATE_SPECIALIZED_NODE( NodeName, m ## PluralName ## Iterator )
107 
108 //==============================================================================
109 #define DART_INSTANTIATE_SPECALIZED_NODE( NodeName ) \
110  DART_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, NodeName ## s )
111 
112 //==============================================================================
113 #define DETAIL_DART_SPECIALIZED_NODE_INLINE( NodeName, PluralName, it ) \
114  private: dart::dynamics::BodyNode::NodeMap::iterator it ; public: \
115  \
116  inline std::size_t getNum ## PluralName () const \
117  { return it->second.size(); } \
118  \
119  inline NodeName * get ## NodeName (std::size_t index) \
120  { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, it->second)); } \
121  \
122  inline const NodeName * get ## NodeName (std::size_t index) const \
123  { return static_cast<const NodeName *>(getVectorObjectIfAvailable(index, it->second)); }
124 
125 //==============================================================================
126 #define DART_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, PluralName ) \
127  DETAIL_DART_SPECIALIZED_NODE_INLINE( NodeName, PluralName, m ## PluralName ## Iterator )
128 
129 //==============================================================================
130 #define DART_SPECIALIZED_NODE_INTERNAL( NodeName ) \
131  DART_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, NodeName ## s )
132 
133 //==============================================================================
134 #define DETAIL_DART_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, it ) \
135  private: dart::dynamics::BodyNode::NodeMap::iterator it ; public:\
136  std::size_t getNum ## PluralName() const;\
137  NodeName * get ## NodeName (std::size_t index);\
138  const NodeName * get ## NodeName(std::size_t index) const;
139 
140 //==============================================================================
141 #define DART_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, PluralName )\
142  DETAIL_DART_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, m ## PluralName ## Iterator )
143 
144 //==============================================================================
145 #define DART_SPECIALIZED_NODE_DECLARE( NodeName )\
146  DART_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, NodeName ## s )
147 
148 //==============================================================================
149 #define DETAIL_DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName, PluralName, it )\
150  std::size_t BodyNodeType :: getNum ## PluralName () const\
151  { return it->second.size(); }\
152  \
153  NodeName * BodyNodeType :: get ## NodeName (std::size_t index)\
154  { return static_cast<NodeName *>(getVectorObjectIfAvailable(index, it->second)); }\
155  \
156  const NodeName * BodyNodeType :: get ## NodeName (std::size_t index) const\
157  { return static_cast<const NodeName *>(getVectorObjectIfAvailable(index, it->second)); }
158 
159 //==============================================================================
160 #define DART_SPECIALIZED_NODE_DEFINE_IRREGULAR( BodyNodeType, NodeName, PluralName )\
161  DETAIL_DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName, PluralName, m ## PluralName ## Iterator )
162 
163 //==============================================================================
164 #define DART_SPECIALIZED_NODE_DEFINE( BodyNodeType, NodeName )\
165  DART_SPECIALIZED_NODE_DEFINE_IRREGULAR( BodyNodeType, NodeName, NodeName ## s )
166 
167 //==============================================================================
168 #define DETAIL_DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName, PluralName ) \
169  template <> inline std::size_t BodyNodeType :: getNumNodes< NodeName >() const { return getNum ## PluralName (); } \
170  template <> inline NodeName * BodyNodeType :: getNode< NodeName >(std::size_t index) { return get ## NodeName (index); } \
171  template <> inline const NodeName * BodyNodeType :: getNode< NodeName >(std::size_t index) const { return get ## NodeName (index); }
172 
173 //==============================================================================
174 #define DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( BodyNodeType, NodeName, PluralName ) \
175  DETAIL_DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName, PluralName )
176 
177 //==============================================================================
178 #define DART_SPECIALIZED_NODE_TEMPLATE( BodyNodeType, NodeName ) \
179  DART_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( BodyNodeType, NodeName, NodeName ## s )
180 
181 //==============================================================================
182 // Macros for specializing Nodes within Skeletons
183 //==============================================================================
184 #define DART_SKEL_ENABLE_NODE_SPECIALIZATION() \
185  public: \
186  template <class T> std::size_t getNumNodes() const { return dart::dynamics::Skeleton::getNumNodes<T>(); } \
187  template <class T> std::size_t getNumNodes(std::size_t treeIndex) const { return dart::dynamics::Skeleton::getNumNodes<T>(treeIndex); } \
188  template <class T> T* getNode(std::size_t index) { return dart::dynamics::Skeleton::getNode<T>(index); } \
189  template <class T> T* getNode(std::size_t nodeIdx, std::size_t treeIndex) { return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex); } \
190  template <class T> const T* getNode(std::size_t index) const { return dart::dynamics::Skeleton::getNode<T>(index); } \
191  template <class T> const T* getNode(std::size_t nodeIdx, std::size_t treeIndex) const { return dart::dynamics::Skeleton::getNode<T>(nodeIdx, treeIndex); } \
192  template <class T> T* getNode(const std::string& name) { return dart::dynamics::Skeleton::getNode<T>(name); } \
193  template <class T> const T* getNode(const std::string& name) const { return dart::dynamics::Skeleton::getNode<T>(name); }
194 
195 //==============================================================================
196 #define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, PluralName ) \
197  mSpecializedTreeNodes[typeid( NodeName )] = &mTree ## PluralName ## Iterators; \
198  mSkelCache.mNodeMap[typeid( NodeName )] = std::vector<dart::dynamics::Node*>(); \
199  m ## PluralName ## Iterator = mSkelCache.mNodeMap.find(typeid( NodeName )); \
200  mNodeNameMgrMap[typeid( NodeName )] = dart::common::NameManager<Node*>( \
201  std::string("Skeleton::") + #NodeName + " | " + mAspectProperties.mName ); \
202  mNameMgrFor ## PluralName = &mNodeNameMgrMap.find(typeid( NodeName) )->second;
203 
204 //==============================================================================
205 #define DART_SKEL_INSTANTIATE_SPECIALIZED_NODE( NodeName ) \
206  DART_SKEL_INSTANTIATE_SPECIALIZED_NODE_IRREGULAR( NodeName, NodeName ## s );
207 
208 //==============================================================================
209 #define DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, func) \
210  if(treeIndex >= treeIts.size()) \
211  { dterr << "[" << #func << "] Requesting an invalid tree (" << treeIndex << "). " \
212  << "The number of trees in this Skeleton is: " << treeIts.size() << "\n"; \
213  assert(false); return 0; }
214 
215 //==============================================================================
216 #define DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName, PluralName, skelIt, treeIts, NameMgr ) \
217  private: \
218  dart::dynamics::Skeleton::NodeMap::iterator skelIt; \
219  std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts; \
220  dart::common::NameManager<dart::dynamics::Node*>* NameMgr; \
221  public: \
222  inline std::size_t getNum ## PluralName () const \
223  { return skelIt->second.size(); } \
224  inline std::size_t getNum ## PluralName (std::size_t treeIndex) const \
225  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum ## PluralName); \
226  treeIts [treeIndex]->second.size(); } \
227  \
228  inline NodeName * get ## NodeName (std::size_t index) \
229  { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \
230  inline NodeName * get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex) \
231  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \
232  return static_cast< NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \
233  \
234  inline const NodeName * get ## NodeName (std::size_t index) const \
235  { return static_cast<const NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \
236  inline const NodeName * get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex) const \
237  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \
238  return static_cast<const NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \
239  \
240  inline NodeName * get ## NodeName (const std::string& name) \
241  { return static_cast< NodeName *>(NameMgr->getObject(name)); } \
242  inline const NodeName * get ## NodeName (const std::string& name) const \
243  { return static_cast<const NodeName *>(NameMgr->getObject(name)); }
244 
245 //==============================================================================
246 #define DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, PluralName ) \
247  DETAIL_DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName );
248 
249 //==============================================================================
250 #define DART_SKEL_SPECIALIZED_NODE_INLINE( NodeName ) \
251  DART_SKEL_SPECIALIZED_NODE_INLINE_IRREGULAR( NodeName, NodeName ## s);
252 
253 //==============================================================================
254 #define DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, skelIt, treeIts, NameMgr ) \
255  private:\
256  dart::dynamics::Skeleton::NodeMap::iterator skelIt; \
257  std::vector<dart::dynamics::Skeleton::NodeMap::iterator> treeIts; \
258  dart::common::NameManager<dart::dynamics::Node*>* NameMgr; \
259  public: \
260  std::size_t getNum ## PluralName () const; \
261  \
262  std::size_t getNum ## PluralName (std::size_t treeIndex) const; \
263  \
264  NodeName * get ## NodeName (std::size_t index); \
265  \
266  NodeName * get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex); \
267  \
268  const NodeName * get ## NodeName (std::size_t index) const; \
269  \
270  const NodeName * get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex) const; \
271  \
272  NodeName * get ## NodeName (const std::string& name); \
273  \
274  const NodeName * get ## NodeName (const std::string& name) const;
275 
276 //==============================================================================
277 #define DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, PluralName )\
278  DETAIL_DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName );
279 
280 //==============================================================================
281 #define DART_SKEL_SPECIALIZED_NODE_DECLARE( NodeName )\
282  DART_SKEL_SPECIALIZED_NODE_DECLARE_IRREGULAR( NodeName, NodeName ## s );
283 
284 //==============================================================================
285 #define DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName, PluralName, skelIt, treeIts, NameMgr )\
286  std::size_t SkeletonType :: getNum ## PluralName () const \
287  { return skelIt->second.size(); } \
288  std::size_t SkeletonType :: getNum ## PluralName (std::size_t treeIndex) const \
289  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, getNum ## PluralName); \
290  return treeIts [treeIndex]->second.size(); } \
291  \
292  NodeName * SkeletonType :: get ## NodeName (std::size_t index) \
293  { return static_cast< NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \
294  NodeName * SkeletonType :: get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex) \
295  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \
296  return static_cast< NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \
297  \
298  const NodeName * SkeletonType :: get ## NodeName (std::size_t index) const \
299  { return static_cast<const NodeName *>(getVectorObjectIfAvailable(index, skelIt ->second)); } \
300  const NodeName * SkeletonType :: get ## NodeName (std::size_t treeIndex, std::size_t nodeIndex) const \
301  { DETAIL_DART_WARN_TREE_INDEX(treeIts, treeIndex, get ## NodeName); \
302  return static_cast<const NodeName *>(getVectorObjectIfAvailable(nodeIndex, treeIts[treeIndex]->second)); } \
303  \
304  NodeName * SkeletonType :: get ## NodeName (const std::string& name) \
305  { return static_cast< NodeName *>(NameMgr->getObject(name)); } \
306  const NodeName * SkeletonType :: get ## NodeName (const std::string& name) const \
307  { return static_cast<const NodeName *>(NameMgr->getObject(name)); }
308 
309 //==============================================================================
310 #define DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( SkeletonType, NodeName, PluralName )\
311  DETAIL_DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName, PluralName, m ## PluralName ## Iterator, mTree ## PluralName ## Iterators, mNameMgrFor ## PluralName );
312 
313 //==============================================================================
314 #define DART_SKEL_SPECIALIZED_NODE_DEFINE( SkeletonType, NodeName )\
315  DART_SKEL_SPECIALIZED_NODE_DEFINED_IRREGULAR( SkeletonType, NodeName, NodeName ## s );
316 
317 //==============================================================================
318 #define DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName, PluralName ) \
319  template <> inline std::size_t SkelType :: getNumNodes< NodeName >() const { return getNum ## PluralName (); } \
320  template <> inline std::size_t SkelType :: getNumNodes< NodeName >(std::size_t index) const { return getNum ## PluralName(index); } \
321  template <> inline NodeName* SkelType :: getNode< NodeName >(std::size_t index) { return get ## NodeName (index); } \
322  template <> inline NodeName* SkelType :: getNode< NodeName >(std::size_t treeIndex, std::size_t nodeIndex) { return get ## NodeName(treeIndex, nodeIndex); } \
323  template <> inline const NodeName* SkelType :: getNode< NodeName >(std::size_t index) const { return get ## NodeName (index); } \
324  template <> inline const NodeName* SkelType :: getNode< NodeName >(std::size_t treeIndex, std::size_t nodeIndex) const { return get ## NodeName(treeIndex, nodeIndex); } \
325  template <> inline NodeName* SkelType::getNode< NodeName >(const std::string& name) { return get ## NodeName (name); } \
326  template <> inline const NodeName* SkelType::getNode< NodeName >(const std::string& name) const { return get ## NodeName (name); }
327 
328 //==============================================================================
329 #define DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( SkelType, NodeName, PluralName ) \
330  DETAIL_DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName, PluralName )
331 
332 //==============================================================================
333 #define DART_SKEL_SPECIALIZED_NODE_TEMPLATE( SkelType, NodeName ) \
334  DART_SKEL_SPECIALIZED_NODE_TEMPLATE_IRREGULAR( SkelType, NodeName, NodeName ## s );
335 
336 
337 #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:63