DART 6.13.2
Loading...
Searching...
No Matches
BasicNodeManager.hpp
Go to the documentation of this file.
1/*
2 * Copyright (c) 2011-2022, 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"
44
45namespace dart {
46namespace dynamics {
47namespace detail {
48
50{
51public:
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
82protected:
83 template <class T>
84 struct type
85 {
86 };
87
90
93};
94
97{
98public:
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
125protected:
128
130 std::vector<NodeMap> mTreeNodeMaps;
131
139};
141
142//==============================================================================
143template <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//==============================================================================
154template <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//==============================================================================
165template <class NodeType>
166const NodeType* BasicNodeManagerForBodyNode::getNode(std::size_t index) const
167{
168 return const_cast<BasicNodeManagerForBodyNode*>(this)->getNode<NodeType>(
169 index);
170}
171
172//==============================================================================
173template <class NodeType>
175{
176 // When invoked through a BasicNodeManager, this should always return false.
177 return false;
178}
179
180//==============================================================================
181template <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//==============================================================================
203template <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//==============================================================================
241template <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//==============================================================================
250template <class NodeType>
251NodeType* 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//==============================================================================
262template <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 template <typename Func> \
286 void each##AspectName(Func func) const \
287 { \
288 if constexpr (std::is_same_v< \
289 std::invoke_result_t<Func, const AspectName*>, \
290 bool>) \
291 { \
292 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
293 { \
294 if (!func(get##AspectName(i))) \
295 return; \
296 } \
297 } \
298 else \
299 { \
300 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
301 { \
302 func(get##AspectName(i)); \
303 } \
304 } \
305 } \
306 template <typename Func> \
307 void each##AspectName(Func func) \
308 { \
309 if constexpr (std::is_same_v< \
310 std::invoke_result_t<Func, AspectName*>, \
311 bool>) \
312 { \
313 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
314 { \
315 if (!func(get##AspectName(i))) \
316 return; \
317 } \
318 } \
319 else \
320 { \
321 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
322 { \
323 func(get##AspectName(i)); \
324 } \
325 } \
326 }
327
328//==============================================================================
329#define DART_BAKE_SPECIALIZED_NODE(AspectName) \
330 DART_BAKE_SPECIALIZED_NODE_IRREGULAR(AspectName, AspectName, AspectName##s)
331
332//==============================================================================
333#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( \
334 TypeName, AspectName, PluralAspectName) \
335 DART_BAKE_SPECIALIZED_NODE_IRREGULAR(TypeName, AspectName, PluralAspectName) \
336 inline std::size_t getNum##PluralAspectName(std::size_t treeIndex) const \
337 { \
338 return getNumNodes<TypeName>(treeIndex); \
339 } \
340 inline TypeName* get##AspectName( \
341 std::size_t treeIndex, std::size_t nodeIndex) \
342 { \
343 return getNode<TypeName>(treeIndex, nodeIndex); \
344 } \
345 inline const TypeName* get##AspectName( \
346 std::size_t treeIndex, std::size_t nodeIndex) const \
347 { \
348 return getNode<TypeName>(treeIndex, nodeIndex); \
349 } \
350 \
351 inline TypeName* get##AspectName(const std::string& name) \
352 { \
353 return getNode<TypeName>(name); \
354 } \
355 inline const TypeName* get##AspectName(const std::string& name) const \
356 { \
357 return getNode<TypeName>(name); \
358 } \
359 template <typename Func> \
360 void each##AspectName(std::size_t treeIndex, Func func) const \
361 { \
362 if constexpr (std::is_same_v< \
363 std::invoke_result_t<Func, const AspectName*>, \
364 bool>) \
365 { \
366 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
367 { \
368 if (!func(get##AspectName(treeIndex, i))) \
369 return; \
370 } \
371 } \
372 else \
373 { \
374 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
375 { \
376 func(get##AspectName(treeIndex, i)); \
377 } \
378 } \
379 } \
380 template <typename Func> \
381 void each##AspectName(std::size_t treeIndex, Func func) \
382 { \
383 if constexpr (std::is_same_v< \
384 std::invoke_result_t<Func, AspectName*>, \
385 bool>) \
386 { \
387 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
388 { \
389 if (!func(get##AspectName(treeIndex, i))) \
390 return; \
391 } \
392 } \
393 else \
394 { \
395 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
396 { \
397 func(get##AspectName(treeIndex, i)); \
398 } \
399 } \
400 }
401
402//==============================================================================
403#define DART_BAKE_SPECIALIZED_NODE_SKEL(AspectName) \
404 DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( \
405 AspectName, AspectName, AspectName##s)
406
407//==============================================================================
408#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
409 TypeName, AspectName, PluralAspectName) \
410 std::size_t getNum##PluralAspectName() const; \
411 TypeName* get##AspectName(std::size_t index); \
412 const TypeName* get##AspectName(std::size_t index) const; \
413 template <typename Func> \
414 void each##AspectName(Func func) const \
415 { \
416 if constexpr (std::is_same_v< \
417 std::invoke_result_t<Func, const AspectName*>, \
418 bool>) \
419 { \
420 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
421 { \
422 if (!func(get##AspectName(i))) \
423 return; \
424 } \
425 } \
426 else \
427 { \
428 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
429 { \
430 func(get##AspectName(i)); \
431 } \
432 } \
433 } \
434 template <typename Func> \
435 void each##AspectName(Func func) \
436 { \
437 if constexpr (std::is_same_v< \
438 std::invoke_result_t<Func, AspectName*>, \
439 bool>) \
440 { \
441 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
442 { \
443 if (!func(get##AspectName(i))) \
444 return; \
445 } \
446 } \
447 else \
448 { \
449 for (auto i = 0u; i < getNum##PluralAspectName(); ++i) \
450 { \
451 func(get##AspectName(i)); \
452 } \
453 } \
454 }
455
456//==============================================================================
457#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS(AspectName) \
458 DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
459 AspectName, AspectName, AspectName##s)
460
461//==============================================================================
462#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( \
463 TypeName, AspectName, PluralAspectName) \
464 DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( \
465 TypeName, AspectName, PluralAspectName) \
466 std::size_t getNum##PluralAspectName(std::size_t treeIndex) const; \
467 TypeName* get##AspectName(std::size_t treeIndex, std::size_t nodeIndex); \
468 const TypeName* get##AspectName( \
469 std::size_t treeIndex, std::size_t nodeIndex) const; \
470 \
471 TypeName* get##AspectName(const std::string& name); \
472 const TypeName* get##AspectName(const std::string& name) const; \
473 \
474 template <typename Func> \
475 void each##AspectName(std::size_t treeIndex, Func func) const \
476 { \
477 if constexpr (std::is_same_v< \
478 std::invoke_result_t<Func, const AspectName*>, \
479 bool>) \
480 { \
481 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
482 { \
483 if (!func(get##AspectName(treeIndex, i))) \
484 return; \
485 } \
486 } \
487 else \
488 { \
489 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
490 { \
491 func(get##AspectName(treeIndex, i)); \
492 } \
493 } \
494 } \
495 template <typename Func> \
496 void each##AspectName(std::size_t treeIndex, Func func) \
497 { \
498 if constexpr (std::is_same_v< \
499 std::invoke_result_t<Func, AspectName*>, \
500 bool>) \
501 { \
502 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
503 { \
504 if (!func(get##AspectName(treeIndex, i))) \
505 return; \
506 } \
507 } \
508 else \
509 { \
510 for (auto i = 0u; i < getNum##PluralAspectName(treeIndex); ++i) \
511 { \
512 func(get##AspectName(treeIndex, i)); \
513 } \
514 } \
515 }
516
517//==============================================================================
518#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(AspectName) \
519 DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( \
520 AspectName, AspectName, AspectName##s)
521
522//==============================================================================
523#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
524 ClassName, TypeName, AspectName, PluralAspectName) \
525 std::size_t ClassName ::getNum##PluralAspectName() const \
526 { \
527 return getNumNodes<TypeName>(); \
528 } \
529 TypeName* ClassName ::get##AspectName(std::size_t index) \
530 { \
531 return getNode<TypeName>(index); \
532 } \
533 const TypeName* ClassName ::get##AspectName(std::size_t index) const \
534 { \
535 return getNode<TypeName>(index); \
536 }
537
538//==============================================================================
539#define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(ClassName, AspectName) \
540 DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
541 ClassName, AspectName, AspectName, AspectName##s)
542
543//==============================================================================
544#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( \
545 ClassName, TypeName, AspectName, PluralAspectName) \
546 DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( \
547 ClassName, TypeName, AspectName, PluralAspectName) \
548 std::size_t ClassName ::getNum##PluralAspectName(std::size_t treeIndex) \
549 const \
550 { \
551 return getNumNodes<TypeName>(treeIndex); \
552 } \
553 TypeName* ClassName ::get##AspectName( \
554 std::size_t treeIndex, std::size_t nodeIndex) \
555 { \
556 return getNode<TypeName>(treeIndex, nodeIndex); \
557 } \
558 const TypeName* ClassName ::get##AspectName( \
559 std::size_t treeIndex, std::size_t nodeIndex) const \
560 { \
561 return getNode<TypeName>(treeIndex, nodeIndex); \
562 } \
563 \
564 TypeName* ClassName ::get##AspectName(const std::string& name) \
565 { \
566 return getNode<TypeName>(name); \
567 } \
568 const TypeName* ClassName ::get##AspectName(const std::string& name) const \
569 { \
570 return getNode<TypeName>(name); \
571 }
572
573//==============================================================================
574#define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS(ClassName, AspectName) \
575 DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( \
576 ClassName, AspectName, AspectName, AspectName##s)
577
578} // namespace detail
579} // namespace dynamics
580} // namespace dart
581
582#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:1698
std::size_t index
Definition SkelParser.cpp:1673
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
std::size_t getNumNodes() const
Get the number of Nodes corresponding to the specified type.
Definition BasicNodeManager.hpp:144
NodeNameMgrMap mNodeNameMgrMap
NameManager for tracking Nodes.
Definition BasicNodeManager.hpp:127
Definition BulletCollisionDetector.cpp:60