DART  6.10.1
BasicNodeManager.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_BASICNODEMANAGER_HPP_
34 #define DART_DYNAMICS_DETAIL_BASICNODEMANAGER_HPP_
35 
36 #include <map>
37 #include <typeindex>
38 #include <unordered_set>
39 
41 #include "dart/common/Empty.hpp"
43 #include "dart/dynamics/Node.hpp"
44 
45 namespace dart {
46 namespace dynamics {
47 namespace detail {
48 
50 {
51 public:
52  using NodeMap = std::map<std::type_index, std::vector<Node*> >;
53  using NodeDestructorSet = std::unordered_set<NodeDestructorPtr>;
54  using NodeNameMgrMap = std::map<std::type_index, common::NameManager<Node*> >;
56  = std::map<std::type_index, std::vector<NodeMap::iterator>*>;
57 
60 
64  = delete;
65 
67  template <class NodeType>
68  std::size_t getNumNodes() const;
69 
71  template <class NodeType>
72  NodeType* getNode(std::size_t index);
73 
75  template <class NodeType>
76  const NodeType* getNode(std::size_t index) const;
77 
79  template <class NodeType>
80  static constexpr bool isSpecializedForNode();
81 
82 protected:
83  template <class T>
84  struct type
85  {
86  };
87 
90 
93 };
94 
97 {
98 public:
101 
104  template <class NodeType>
105  std::size_t getNumNodes(std::size_t treeIndex) const;
106 
109  template <class NodeType>
110  NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex);
111 
114  template <class NodeType>
115  const NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex) const;
116 
118  template <class NodeType>
119  NodeType* getNode(const std::string& name);
120 
122  template <class NodeType>
123  const NodeType* getNode(const std::string& name) const;
124 
125 protected:
128 
130  std::vector<NodeMap> mTreeNodeMaps;
131 
139 };
141 
142 //==============================================================================
143 template <class NodeType>
145 {
146  NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType));
147  if (mNodeMap.end() == it)
148  return 0;
149 
150  return it->second.size();
151 }
152 
153 //==============================================================================
154 template <class NodeType>
156 {
157  NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType));
158  if (mNodeMap.end() == it)
159  return nullptr;
160 
161  return static_cast<NodeType*>(getVectorObjectIfAvailable(index, it->second));
162 }
163 
164 //==============================================================================
165 template <class NodeType>
166 const NodeType* BasicNodeManagerForBodyNode::getNode(std::size_t index) const
167 {
168  return const_cast<BasicNodeManagerForBodyNode*>(this)->getNode<NodeType>(
169  index);
170 }
171 
172 //==============================================================================
173 template <class NodeType>
175 {
176  // When invoked through a BasicNodeManager, this should always return false.
177  return false;
178 }
179 
180 //==============================================================================
181 template <class NodeType>
183  std::size_t treeIndex) const
184 {
185  if (treeIndex >= mTreeNodeMaps.size())
186  {
187  dterr << "[Skeleton::getNumNodes<" << typeid(NodeType).name() << ">] "
188  << "Requested tree index (" << treeIndex << "), but there are only ("
189  << mTreeNodeMaps.size() << ") trees available\n";
190  assert(false);
191  return 0;
192  }
193 
194  const NodeMap& nodeMap = mTreeNodeMaps[treeIndex];
195  NodeMap::const_iterator it = nodeMap.find(typeid(NodeType));
196  if (nodeMap.end() == it)
197  return 0;
198 
199  return it->second.size();
200 }
201 
202 //==============================================================================
203 template <class NodeType>
205  std::size_t treeIndex, std::size_t nodeIndex)
206 {
207  if (treeIndex >= mTreeNodeMaps.size())
208  {
209  dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
210  << "Requested tree index (" << treeIndex << "), but there are only ("
211  << mTreeNodeMaps.size() << ") trees available\n";
212  assert(false);
213  return nullptr;
214  }
215 
216  const NodeMap& nodeMap = mTreeNodeMaps[treeIndex];
217  NodeMap::const_iterator it = nodeMap.find(typeid(NodeType));
218  if (nodeMap.end() == it)
219  {
220  dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
221  << "Requested index (" << nodeIndex << ") within tree (" << treeIndex
222  << "), but there are no Nodes of the requested type in this tree\n";
223  assert(false);
224  return nullptr;
225  }
226 
227  if (nodeIndex >= it->second.size())
228  {
229  dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
230  << "Requested index (" << nodeIndex << ") within tree (" << treeIndex
231  << "), but there are only (" << it->second.size() << ") Nodes of the "
232  << "requested type within that tree\n";
233  assert(false);
234  return nullptr;
235  }
236 
237  return static_cast<NodeType*>(it->second[nodeIndex]);
238 }
239 
240 //==============================================================================
241 template <class NodeType>
243  std::size_t treeIndex, std::size_t nodeIndex) const
244 {
245  return const_cast<BasicNodeManagerForSkeleton*>(this)->getNode<NodeType>(
246  treeIndex, nodeIndex);
247 }
248 
249 //==============================================================================
250 template <class NodeType>
251 NodeType* BasicNodeManagerForSkeleton::getNode(const std::string& name)
252 {
253  NodeNameMgrMap::const_iterator it = mNodeNameMgrMap.find(typeid(NodeType));
254 
255  if (mNodeNameMgrMap.end() == it)
256  return nullptr;
257 
258  return static_cast<NodeType*>(it->second.getObject(name));
259 }
260 
261 //==============================================================================
262 template <class NodeType>
264  const std::string& name) const
265 {
266  return const_cast<BasicNodeManagerForSkeleton*>(this)->getNode<NodeType>(
267  name);
268 }
269 
270 //==============================================================================
271 #define DART_BAKE_SPECIALIZED_NODE_IRREGULAR( \
272  TypeName, AspectName, PluralAspectName) \
273  inline std::size_t getNum##PluralAspectName() const \
274  { \
275  return getNumNodes<TypeName>(); \
276  } \
277  inline TypeName* get##AspectName(std::size_t index) \
278  { \
279  return getNode<TypeName>(index); \
280  } \
281  inline const TypeName* get##AspectName(std::size_t index) const \
282  { \
283  return getNode<TypeName>(index); \
284  }
285 
286 //==============================================================================
287 #define DART_BAKE_SPECIALIZED_NODE(AspectName) \
288  DART_BAKE_SPECIALIZED_NODE_IRREGULAR(AspectName, AspectName, AspectName##s)
289 
290 //==============================================================================
291 #define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( \
292  TypeName, AspectName, PluralAspectName) \
293  DART_BAKE_SPECIALIZED_NODE_IRREGULAR(TypeName, AspectName, PluralAspectName) \
294  inline std::size_t getNum##PluralAspectName(std::size_t treeIndex) const \
295  { \
296  return getNumNodes<TypeName>(treeIndex); \
297  } \
298  inline TypeName* get##AspectName( \
299  std::size_t treeIndex, std::size_t nodeIndex) \
300  { \
301  return getNode<TypeName>(treeIndex, nodeIndex); \
302  } \
303  inline const TypeName* get##AspectName( \
304  std::size_t treeIndex, std::size_t nodeIndex) const \
305  { \
306  return getNode<TypeName>(treeIndex, nodeIndex); \
307  } \
308  \
309  inline TypeName* get##AspectName(const std::string& name) \
310  { \
311  return getNode<TypeName>(name); \
312  } \
313  inline const TypeName* get##AspectName(const std::string& name) const \
314  { \
315  return getNode<TypeName>(name); \
316  }
317 
318 //==============================================================================
319 #define DART_BAKE_SPECIALIZED_NODE_SKEL(AspectName) \
320  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( \
321  AspectName, AspectName, AspectName##s)
322 
323 //==============================================================================
324 #define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
325  TypeName, AspectName, PluralAspectName) \
326  std::size_t getNum##PluralAspectName() const; \
327  TypeName* get##AspectName(std::size_t index); \
328  const TypeName* get##AspectName(std::size_t index) const;
329 
330 //==============================================================================
331 #define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS(AspectName) \
332  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
333  AspectName, AspectName, AspectName##s)
334 
335 //==============================================================================
336 #define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( \
337  TypeName, AspectName, PluralAspectName) \
338  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
339  TypeName, AspectName, PluralAspectName) \
340  std::size_t getNum##PluralAspectName(std::size_t treeIndex) const; \
341  TypeName* get##AspectName(std::size_t treeIndex, std::size_t nodeIndex); \
342  const TypeName* get##AspectName( \
343  std::size_t treeIndex, std::size_t nodeIndex) const; \
344  \
345  TypeName* get##AspectName(const std::string& name); \
346  const TypeName* get##AspectName(const std::string& name) const;
347 
348 //==============================================================================
349 #define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(AspectName) \
350  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( \
351  AspectName, AspectName, AspectName##s)
352 
353 //==============================================================================
354 #define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
355  ClassName, TypeName, AspectName, PluralAspectName) \
356  std::size_t ClassName ::getNum##PluralAspectName() const \
357  { \
358  return getNumNodes<TypeName>(); \
359  } \
360  TypeName* ClassName ::get##AspectName(std::size_t index) \
361  { \
362  return getNode<TypeName>(index); \
363  } \
364  const TypeName* ClassName ::get##AspectName(std::size_t index) const \
365  { \
366  return getNode<TypeName>(index); \
367  }
368 
369 //==============================================================================
370 #define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(ClassName, AspectName) \
371  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
372  ClassName, AspectName, AspectName, AspectName##s)
373 
374 //==============================================================================
375 #define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( \
376  ClassName, TypeName, AspectName, PluralAspectName) \
377  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
378  ClassName, TypeName, AspectName, PluralAspectName) \
379  std::size_t ClassName ::getNum##PluralAspectName(std::size_t treeIndex) \
380  const \
381  { \
382  return getNumNodes<TypeName>(treeIndex); \
383  } \
384  TypeName* ClassName ::get##AspectName( \
385  std::size_t treeIndex, std::size_t nodeIndex) \
386  { \
387  return getNode<TypeName>(treeIndex, nodeIndex); \
388  } \
389  const TypeName* ClassName ::get##AspectName( \
390  std::size_t treeIndex, std::size_t nodeIndex) const \
391  { \
392  return getNode<TypeName>(treeIndex, nodeIndex); \
393  } \
394  \
395  TypeName* ClassName ::get##AspectName(const std::string& name) \
396  { \
397  return getNode<TypeName>(name); \
398  } \
399  const TypeName* ClassName ::get##AspectName(const std::string& name) const \
400  { \
401  return getNode<TypeName>(name); \
402  }
403 
404 //==============================================================================
405 #define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS(ClassName, AspectName) \
406  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( \
407  ClassName, AspectName, AspectName, AspectName##s)
408 
409 } // namespace detail
410 } // namespace dynamics
411 } // namespace dart
412 
413 #endif // DART_DYNAMICS_DETAIL_BASICNODEMANAGER_HPP_
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
Definition: ClassWithVirtualBase.hpp:44
#define DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
Definition: ClassWithVirtualBase.hpp:43
#define dterr
Output an error message.
Definition: Console.hpp:49
std::string * name
Definition: SkelParser.cpp:1697
std::size_t index
Definition: SkelParser.cpp:1672
Definition: BasicNodeManager.hpp:50
std::map< std::type_index, common::NameManager< Node * > > NodeNameMgrMap
Definition: BasicNodeManager.hpp:54
BasicNodeManagerForBodyNode()=default
Default constructor.
std::unordered_set< NodeDestructorPtr > NodeDestructorSet
Definition: BasicNodeManager.hpp:53
NodeDestructorSet mNodeDestructors
A set for storing the Node destructors.
Definition: BasicNodeManager.hpp:92
NodeType * getNode(std::size_t index)
Get the Node of the specified type and the specified index.
Definition: BasicNodeManager.hpp:155
std::map< std::type_index, std::vector< NodeMap::iterator > * > SpecializedTreeNodes
Definition: BasicNodeManager.hpp:56
static constexpr bool isSpecializedForNode()
Check if this Manager is specialized for a specific type of Node.
Definition: BasicNodeManager.hpp:174
NodeMap mNodeMap
Map that retrieves the Nodes of a specified type.
Definition: BasicNodeManager.hpp:89
std::map< std::type_index, std::vector< Node * > > NodeMap
Definition: BasicNodeManager.hpp:52
std::size_t getNumNodes() const
Get the number of Nodes corresponding to the specified type.
Definition: BasicNodeManager.hpp:144
BasicNodeManagerForBodyNode(const BasicNodeManagerForBodyNode &)=delete
Delete copy constructors and assignment operators.
BasicNodeManagerForBodyNode & operator=(const BasicNodeManagerForBodyNode &)=delete
Definition: BasicNodeManager.hpp:97
NodeType * getNode(std::size_t treeIndex, std::size_t nodeIndex)
Get the nodeIndexth Node of the specified type within the tree of treeIndex.
Definition: BasicNodeManager.hpp:204
std::vector< NodeMap > mTreeNodeMaps
A NodeMap for each tree to allow tree Nodes to be accessed independently.
Definition: BasicNodeManager.hpp:130
SpecializedTreeNodes mSpecializedTreeNodes
A map that allows SpecializedNodeManagers to have a direct iterator to the tree-wise storage of its s...
Definition: BasicNodeManager.hpp:138
NodeNameMgrMap mNodeNameMgrMap
NameManager for tracking Nodes.
Definition: BasicNodeManager.hpp:127
static T getVectorObjectIfAvailable(std::size_t index, const std::vector< T > &vec)
Definition: StlHelpers.hpp:55
Definition: BulletCollisionDetector.cpp:65