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
40 #include "dart/common/ClassWithVirtualBase.hpp"
41 #include "dart/common/Empty.hpp"
42 #include "dart/common/NameManager.hpp"
43 #include "dart/dynamics/Node.hpp"
44
45 namespace dart {
46 namespace dynamics {
47 namespace detail {
48
49 class BasicNodeManagerForBodyNode
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*> >;
55 using SpecializedTreeNodes
56 = std::map<std::type_index, std::vector<NodeMap::iterator>*>;
57
58 /// Default constructor
59 BasicNodeManagerForBodyNode() = default;
60
61 /// Delete copy constructors and assignment operators
62 BasicNodeManagerForBodyNode(const BasicNodeManagerForBodyNode&) = delete;
63 BasicNodeManagerForBodyNode& operator=(const BasicNodeManagerForBodyNode&)
64 = delete;
65
66 /// Get the number of Nodes corresponding to the specified type
67 template <class NodeType>
68 std::size_t getNumNodes() const;
69
70 /// Get the Node of the specified type and the specified index
71 template <class NodeType>
72 NodeType* getNode(std::size_t index);
73
74 /// Get the Node of the specified type and the specified index
75 template <class NodeType>
76 const NodeType* getNode(std::size_t index) const;
77
78 /// Check if this Manager is specialized for a specific type of Node
79 template <class NodeType>
80 static constexpr bool isSpecializedForNode();
81
82 protected:
83 template <class T>
84 struct type
85 {
86 };
87
88 /// Map that retrieves the Nodes of a specified type
89 NodeMap mNodeMap;
90
91 /// A set for storing the Node destructors
92 NodeDestructorSet mNodeDestructors;
93 };
94
95 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
96 class BasicNodeManagerForSkeleton : public virtual BasicNodeManagerForBodyNode
97 {
98 public:
99 using BasicNodeManagerForBodyNode::getNode;
100 using BasicNodeManagerForBodyNode::getNumNodes;
101
102 /// Get the number of Nodes of the specified type that are in the treeIndexth
103 /// tree of this Skeleton
104 template <class NodeType>
105 std::size_t getNumNodes(std::size_t treeIndex) const;
106
107 /// Get the nodeIndexth Node of the specified type within the tree of
108 /// treeIndex.
109 template <class NodeType>
110 NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex);
111
112 /// Get the nodeIndexth Node of the specified type within the tree of
113 /// treeIndex.
114 template <class NodeType>
115 const NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex) const;
116
117 /// Get the Node of the specified type with the given name.
118 template <class NodeType>
119 NodeType* getNode(const std::string& name);
120
121 /// Get the Node of the specified type with the given name.
122 template <class NodeType>
123 const NodeType* getNode(const std::string& name) const;
124
125 protected:
126 /// NameManager for tracking Nodes
127 NodeNameMgrMap mNodeNameMgrMap;
128
129 /// A NodeMap for each tree to allow tree Nodes to be accessed independently
130 std::vector<NodeMap> mTreeNodeMaps;
131
132 /// A map that allows SpecializedNodeManagers to have a direct iterator to
133 /// the tree-wise storage of its specialized Node. Each entry in this map
134 /// contains a pointer to a vector of iterators. Each vector of iterators is
135 /// stored in its corresponding SpecializedNodeManager. This system allows
136 /// Node specialization to be extensible, enabling custom derived Skeleton
137 /// types that are specialized for more than the default specialized Nodes.
138 SpecializedTreeNodes mSpecializedTreeNodes;
139 };
140 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END
141
142 //==============================================================================
143 template <class NodeType>
getNumNodes() const144 std::size_t BasicNodeManagerForBodyNode::getNumNodes() const
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>
getNode(std::size_t index)155 NodeType* BasicNodeManagerForBodyNode::getNode(std::size_t index)
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>
getNode(std::size_t index) const166 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>
isSpecializedForNode()174 constexpr bool BasicNodeManagerForBodyNode::isSpecializedForNode()
175 {
176 // When invoked through a BasicNodeManager, this should always return false.
177 return false;
178 }
179
180 //==============================================================================
181 template <class NodeType>
getNumNodes(std::size_t treeIndex) const182 std::size_t BasicNodeManagerForSkeleton::getNumNodes(
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>
getNode(std::size_t treeIndex,std::size_t nodeIndex)204 NodeType* BasicNodeManagerForSkeleton::getNode(
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>
getNode(std::size_t treeIndex,std::size_t nodeIndex) const242 const NodeType* BasicNodeManagerForSkeleton::getNode(
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>
getNode(const std::string & name)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>
getNode(const std::string & name) const263 const NodeType* BasicNodeManagerForSkeleton::getNode(
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_
414