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_SPECIALIZEDNODEMANAGER_HPP_
34 #define DART_DYNAMICS_DETAIL_SPECIALIZEDNODEMANAGER_HPP_
35
36 #include "dart/dynamics/SpecializedNodeManager.hpp"
37
38 namespace dart {
39 namespace dynamics {
40
41 // This preprocessor token should only be used by the unittest that is
42 // responsible for checking that the specialized routines are being used to
43 // access specialized Aspects
44 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
45 bool usedSpecializedNodeAccess;
46 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
47
48 //==============================================================================
49 template <class SpecNode>
BodyNodeSpecializedFor()50 BodyNodeSpecializedFor<SpecNode>::BodyNodeSpecializedFor()
51 {
52 mNodeMap[typeid(SpecNode)] = std::vector<Node*>();
53 mSpecNodeIterator = mNodeMap.find(typeid(SpecNode));
54 }
55
56 //==============================================================================
57 template <class SpecNode>
58 template <class NodeType>
getNumNodes() const59 std::size_t BodyNodeSpecializedFor<SpecNode>::getNumNodes() const
60 {
61 return _getNumNodes(type<NodeType>());
62 }
63
64 //==============================================================================
65 template <class SpecNode>
66 template <class NodeType>
getNode(std::size_t index)67 NodeType* BodyNodeSpecializedFor<SpecNode>::getNode(std::size_t index)
68 {
69 return _getNode(type<NodeType>(), index);
70 }
71
72 //==============================================================================
73 template <class SpecNode>
74 template <class NodeType>
getNode(std::size_t index) const75 const NodeType* BodyNodeSpecializedFor<SpecNode>::getNode(
76 std::size_t index) const
77 {
78 return const_cast<BodyNodeSpecializedFor<SpecNode>*>(this)->_getNode(
79 type<NodeType>(), index);
80 }
81
82 //==============================================================================
83 template <class SpecNode>
84 template <class NodeType>
isSpecializedForNode()85 constexpr bool BodyNodeSpecializedFor<SpecNode>::isSpecializedForNode()
86 {
87 return _isSpecializedForNode(type<NodeType>());
88 }
89
90 //==============================================================================
91 template <class SpecNode>
92 template <class NodeType>
_getNumNodes(type<NodeType>) const93 std::size_t BodyNodeSpecializedFor<SpecNode>::_getNumNodes(type<NodeType>) const
94 {
95 return detail::BasicNodeManagerForBodyNode::getNumNodes<NodeType>();
96 }
97
98 //==============================================================================
99 template <class SpecNode>
_getNumNodes(type<SpecNode>) const100 std::size_t BodyNodeSpecializedFor<SpecNode>::_getNumNodes(type<SpecNode>) const
101 {
102 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
103 usedSpecializedNodeAccess = true;
104 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
105
106 return mSpecNodeIterator->second.size();
107 }
108
109 //==============================================================================
110 template <class SpecNode>
111 template <class NodeType>
_getNode(type<NodeType>,std::size_t index)112 NodeType* BodyNodeSpecializedFor<SpecNode>::_getNode(
113 type<NodeType>, std::size_t index)
114 {
115 return detail::BasicNodeManagerForBodyNode::getNode<NodeType>(index);
116 }
117
118 //==============================================================================
119 template <class SpecNode>
_getNode(type<SpecNode>,std::size_t index)120 SpecNode* BodyNodeSpecializedFor<SpecNode>::_getNode(
121 type<SpecNode>, std::size_t index)
122 {
123 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
124 usedSpecializedNodeAccess = true;
125 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
126
127 return static_cast<SpecNode*>(
128 getVectorObjectIfAvailable(index, mSpecNodeIterator->second));
129 }
130
131 //==============================================================================
132 template <class SpecNode>
133 template <class NodeType>
_isSpecializedForNode(type<NodeType>)134 constexpr bool BodyNodeSpecializedFor<SpecNode>::_isSpecializedForNode(
135 type<NodeType>)
136 {
137 return false;
138 }
139
140 //==============================================================================
141 template <class SpecNode>
_isSpecializedForNode(type<SpecNode>)142 constexpr bool BodyNodeSpecializedFor<SpecNode>::_isSpecializedForNode(
143 type<SpecNode>)
144 {
145 return true;
146 }
147
148 //==============================================================================
149 template <class SpecNode>
SkeletonSpecializedFor()150 SkeletonSpecializedFor<SpecNode>::SkeletonSpecializedFor()
151 {
152 mSpecializedTreeNodes[typeid(SpecNode)] = &mTreeSpecNodeIterators;
153
154 mNodeNameMgrMap[typeid(SpecNode)] = common::NameManager<Node*>();
155 mSpecNodeNameMgrIterator = mNodeNameMgrMap.find(typeid(SpecNode));
156 }
157
158 //==============================================================================
159 template <class SpecNode>
160 template <class NodeType>
getNumNodes(std::size_t treeIndex) const161 std::size_t SkeletonSpecializedFor<SpecNode>::getNumNodes(
162 std::size_t treeIndex) const
163 {
164 return _getNumNodes(type<NodeType>(), treeIndex);
165 }
166
167 //==============================================================================
168 template <class SpecNode>
169 template <class NodeType>
getNode(std::size_t treeIndex,std::size_t nodeIndex)170 NodeType* SkeletonSpecializedFor<SpecNode>::getNode(
171 std::size_t treeIndex, std::size_t nodeIndex)
172 {
173 return _getNode(type<NodeType>(), treeIndex, nodeIndex);
174 }
175
176 //==============================================================================
177 template <class SpecNode>
178 template <class NodeType>
getNode(std::size_t treeIndex,std::size_t nodeIndex) const179 const NodeType* SkeletonSpecializedFor<SpecNode>::getNode(
180 std::size_t treeIndex, std::size_t nodeIndex) const
181 {
182 return const_cast<SkeletonSpecializedFor<SpecNode>*>(this)->_getNode(
183 type<NodeType>(), treeIndex, nodeIndex);
184 }
185
186 //==============================================================================
187 template <class SpecNode>
188 template <class NodeType>
getNode(const std::string & name)189 NodeType* SkeletonSpecializedFor<SpecNode>::getNode(const std::string& name)
190 {
191 return _getNode(type<NodeType>(), name);
192 }
193
194 //==============================================================================
195 template <class SpecNode>
196 template <class NodeType>
getNode(const std::string & name) const197 const NodeType* SkeletonSpecializedFor<SpecNode>::getNode(
198 const std::string& name) const
199 {
200 return const_cast<SkeletonSpecializedFor<SpecNode>*>(this)->_getNode(
201 type<NodeType>(), name);
202 }
203
204 //==============================================================================
205 template <class SpecNode>
206 template <class NodeType>
isSpecializedForNode()207 constexpr bool SkeletonSpecializedFor<SpecNode>::isSpecializedForNode()
208 {
209 return _isSpecializedForNode(type<NodeType>());
210 }
211
212 //==============================================================================
213 template <class SpecNode>
214 template <class NodeType>
_getNumNodes(type<NodeType>,std::size_t treeIndex) const215 std::size_t SkeletonSpecializedFor<SpecNode>::_getNumNodes(
216 type<NodeType>, std::size_t treeIndex) const
217 {
218 return detail::BasicNodeManagerForSkeleton::getNumNodes<NodeType>(treeIndex);
219 }
220
221 //==============================================================================
222 template <class SpecNode>
_getNumNodes(type<SpecNode>,std::size_t treeIndex) const223 std::size_t SkeletonSpecializedFor<SpecNode>::_getNumNodes(
224 type<SpecNode>, std::size_t treeIndex) const
225 {
226 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
227 usedSpecializedNodeAccess = true;
228 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
229
230 if (treeIndex >= mTreeNodeMaps.size())
231 {
232 dterr << "[Skeleton::getNumNodes<" << typeid(SpecNode).name() << ">] "
233 << "Requested tree index (" << treeIndex << "), but there are only ("
234 << mTreeNodeMaps.size() << ") trees available\n";
235 assert(false);
236 return 0;
237 }
238
239 return mTreeSpecNodeIterators[treeIndex]->second.size();
240 }
241
242 //==============================================================================
243 template <class SpecNode>
244 template <class NodeType>
_getNode(type<NodeType>,std::size_t treeIndex,std::size_t nodeIndex)245 NodeType* SkeletonSpecializedFor<SpecNode>::_getNode(
246 type<NodeType>, std::size_t treeIndex, std::size_t nodeIndex)
247 {
248 return detail::BasicNodeManagerForSkeleton::getNode<NodeType>(
249 treeIndex, nodeIndex);
250 }
251
252 //==============================================================================
253 template <class SpecNode>
_getNode(type<SpecNode>,std::size_t treeIndex,std::size_t nodeIndex)254 SpecNode* SkeletonSpecializedFor<SpecNode>::_getNode(
255 type<SpecNode>, std::size_t treeIndex, std::size_t nodeIndex)
256 {
257 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
258 usedSpecializedNodeAccess = true;
259 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
260
261 if (treeIndex >= mTreeNodeMaps.size())
262 {
263 dterr << "[Skeleton::getNode<" << typeid(SpecNode).name() << ">] "
264 << "Requested tree index (" << treeIndex << "), but there are only ("
265 << mTreeNodeMaps.size() << ") trees available\n";
266 assert(false);
267 return nullptr;
268 }
269
270 NodeMap::iterator& it = mTreeSpecNodeIterators[treeIndex];
271
272 if (nodeIndex >= it->second.size())
273 {
274 dterr << "[Skeleton::getNode<" << typeid(SpecNode).name() << ">] "
275 << "Requested index (" << nodeIndex << ") within tree (" << treeIndex
276 << "), but there are only (" << it->second.size() << ") Nodes of the "
277 << "requested type within that tree\n";
278 assert(false);
279 return nullptr;
280 }
281
282 return static_cast<SpecNode*>(it->second[nodeIndex]);
283 }
284
285 //==============================================================================
286 template <class SpecNode>
287 template <class NodeType>
_getNode(type<NodeType>,const std::string & name)288 NodeType* SkeletonSpecializedFor<SpecNode>::_getNode(
289 type<NodeType>, const std::string& name)
290 {
291 return detail::BasicNodeManagerForSkeleton::getNode<NodeType>(name);
292 }
293
294 //==============================================================================
295 template <class SpecNode>
_getNode(type<SpecNode>,const std::string & name)296 SpecNode* SkeletonSpecializedFor<SpecNode>::_getNode(
297 type<SpecNode>, const std::string& name)
298 {
299 #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS
300 usedSpecializedNodeAccess = true;
301 #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS
302
303 return static_cast<SpecNode*>(
304 mSpecNodeNameMgrIterator->second.getObject(name));
305 }
306
307 //==============================================================================
308 template <class SpecNode>
309 template <class NodeType>
_isSpecializedForNode(type<NodeType>)310 constexpr bool SkeletonSpecializedFor<SpecNode>::_isSpecializedForNode(
311 type<NodeType>)
312 {
313 return false;
314 }
315
316 //==============================================================================
317 template <class SpecNode>
_isSpecializedForNode(type<SpecNode>)318 constexpr bool SkeletonSpecializedFor<SpecNode>::_isSpecializedForNode(
319 type<SpecNode>)
320 {
321 return true;
322 }
323
324 } // namespace dynamics
325 } // namespace dart
326
327 #endif // DART_DYNAMICS_DETAIL_SPECIALIZEDNODEMANAGER_HPP_
328