33
44#include < math/Vector.h>
55
6+ // TODO: should split this header into a separate helpers, to avoid including string
7+ #include < string>
68#include < vector>
79
810#include < cassert>
@@ -36,9 +38,21 @@ struct NodeTree
3638{
3739 std::size_t size () const ;
3840
41+ bool hasChild (Node::Index aNode) const ;
42+ bool hasParent (Node::Index aNode) const ;
43+ bool hasName (Node::Index aNode) const ;
44+
3945 // / @param aParent if set to gInvalidIndex, the call adds a root node.
4046 Node::Index addNode (Node::Index aParent, T_pose aLocalPose);
4147
48+ void setLocalPose (Node::Index aNode, T_pose aLocalPose);
49+ void recurseGlobalPose (Node::Index aNode, const T_pose & aParentGlobalPose);
50+
51+ // / @brief Return the name of the node if available, or a generated name between angle brackets.
52+ // / @return A reference to the string that contains the name, which **might not** be provided aClientStorage.
53+ [[nodiscard]] const std::string & getSafeName (Node::Index aNode,
54+ std::string & aClientStorage) const ;
55+
4256 // / @brief Insert provided subtree into this NodeTree.
4357 // / @param aSubtree
4458 // / @param aInsertionParent The parent for the inserted subtree.
@@ -58,6 +72,21 @@ struct NodeTree
5872};
5973
6074
75+ template <class T_pose >
76+ NodeTree<T_pose> makeOneRootTree (T_pose aRootPose = {})
77+ {
78+ return NodeTree<T_pose>{
79+ .mHierarchy {
80+ Node{
81+ .mLastSibling = 0 ,
82+ },
83+ },
84+ .mLocalPose {aRootPose},
85+ .mGlobalPose {aRootPose},
86+ .mFirstRoot = 0 ,
87+ };
88+ }
89+
6190
6291template <class T_pose >
6392std::size_t NodeTree<T_pose>::size() const
@@ -68,6 +97,48 @@ std::size_t NodeTree<T_pose>::size() const
6897}
6998
7099
100+ template <class T_pose >
101+ bool NodeTree<T_pose>::hasChild(Node::Index aNode) const
102+ {
103+ return mHierarchy [aNode].mFirstChild != Node::gInvalidIndex ;
104+ }
105+
106+
107+ template <class T_pose >
108+ bool NodeTree<T_pose>::hasParent(Node::Index aNode) const
109+ {
110+ return mHierarchy [aNode].mParent != Node::gInvalidIndex ;
111+ }
112+
113+
114+ template <class T_pose >
115+ bool NodeTree<T_pose>::hasName(Node::Index aNode) const
116+ {
117+ // TODO: implement node names
118+ return false ;
119+ }
120+
121+
122+ template <class T_pose >
123+ [[nodiscard]] const std::string & NodeTree<T_pose>::getSafeName(Node::Index aNode, std::string & aClientStorage) const
124+ {
125+ if (hasName (aNode))
126+ {
127+ #if !defined(NDEBUG)
128+ // Give a consistent message if the client wrongfully use the storage as result.
129+ aClientStorage = " <nulled>" ;
130+ #endif
131+ // Should return the reference to the internally stored string, without copy to client storage
132+ /* return aTree.mNodeNames[aNode]*/
133+ throw std::logic_error{" not implemented" };
134+ }
135+ else
136+ {
137+ aClientStorage = " <node_" + std::to_string (aNode) + " >" ;
138+ return aClientStorage;
139+ }
140+ }
141+
71142template <class T_pose >
72143Node::Index NodeTree<T_pose>::addNode(Node::Index aParent, T_pose aLocalPose)
73144{
@@ -135,31 +206,75 @@ Node::Index NodeTree<T_pose>::addNode(Node::Index aParent, T_pose aLocalPose)
135206 return thisIndex;
136207}
137208
138- // TODO: ideally would not be exposed to clients, or moved to a generic header
139- namespace utils {
140209
141- template <class T_element >
142- std::vector<T_element> & append (std::vector<T_element> & aReceiver,
143- const std::vector<T_element> aAppended)
210+ template <class T_pose >
211+ void NodeTree<T_pose>::setLocalPose(Node::Index aNode, T_pose aLocalPose)
212+ {
213+ mLocalPose [aNode] = std::move (aLocalPose);
214+ recurseGlobalPose (aNode,
215+ hasParent (aNode) ? mGlobalPose [mHierarchy [aNode].mParent ]
216+ : T_pose{});
217+ }
218+
219+
220+ template <class T_pose >
221+ void NodeTree<T_pose>::recurseGlobalPose(Node::Index aNode,
222+ const T_pose & aParentGlobalPose)
223+ {
224+ mGlobalPose [aNode] = composeLeftToRight (mLocalPose [aNode], aParentGlobalPose);
225+
226+ for (Node::Index childIdx = mHierarchy [aNode].mFirstChild ;
227+ childIdx != Node::gInvalidIndex ;
228+ childIdx = mHierarchy [childIdx].mNextSibling )
144229 {
145- aReceiver.reserve (aReceiver.size () + aAppended.size ());
146- aReceiver.insert (aReceiver.end (), aAppended.begin (), aAppended.end ());
147- return aReceiver;
230+ recurseGlobalPose (childIdx, mGlobalPose [aNode]);
148231 }
232+ }
149233
150234
151- inline void shiftNode (Node & aNode, Node::Index aOffset, unsigned int aLevelOffset)
152- {
235+ // TODO: ideally would not be exposed to clients, or moved to a generic header
236+ namespace utils {
237+
238+ template <class T_element >
239+ std::vector<T_element> & append (std::vector<T_element> & aReceiver,
240+ const std::vector<T_element> aAppended)
241+ {
242+ aReceiver.reserve (aReceiver.size () + aAppended.size ());
243+ aReceiver.insert (aReceiver.end (), aAppended.begin (), aAppended.end ());
244+ return aReceiver;
245+ }
246+
247+
248+ inline void shiftNode (Node & aNode, Node::Index aOffset, unsigned int aLevelOffset)
249+ {
153250#define SHIFT (member ) if (aNode.##member != Node::gInvalidIndex ) aNode.##member += aOffset
154251
155- SHIFT (mParent );
156- SHIFT (mFirstChild );
157- SHIFT (mNextSibling );
158- SHIFT (mLastSibling );
159- aNode.mLevel += aLevelOffset;
252+ SHIFT (mParent );
253+ SHIFT (mFirstChild );
254+ SHIFT (mNextSibling );
255+ SHIFT (mLastSibling );
256+ aNode.mLevel += aLevelOffset;
160257
161258#undef SHIFT
259+ }
260+
261+ template <class T_pose >
262+ void appendSiblings (Node::Index & aFirstSibling,
263+ Node::Index aAppendedSibling,
264+ NodeTree<T_pose> & aTree)
265+ {
266+ if (aFirstSibling == Node::gInvalidIndex )
267+ {
268+ aFirstSibling = aAppendedSibling;
269+ }
270+ else
271+ {
272+ auto & lastSibling = aTree.mHierarchy [aFirstSibling].mLastSibling ;
273+ assert (aTree.mHierarchy [lastSibling].mNextSibling == Node::gInvalidIndex );
274+ aTree.mHierarchy [lastSibling].mNextSibling = aAppendedSibling;
275+ lastSibling = aTree.mHierarchy [aAppendedSibling].mLastSibling ;
162276 }
277+ }
163278
164279
165280}; // namespace utils
@@ -193,22 +308,24 @@ Node::Index NodeTree<T_pose>::insert(const NodeTree & aSubtree,
193308 {
194309 utils::shiftNode (mHierarchy [idx], initialSize, insertionLevel);
195310 }
311+ Node::Index shiftedSubtreeRoot = aSubtree.mFirstRoot + initialSize;
196312
197313 utils::append (mLocalPose , aSubtree.mLocalPose );
198314
199315 if (aInsertionParent != Node::gInvalidIndex )
200316 {
201- // TODO #scenegraph: Recalculate global poses
202- assert (false );
317+ // Resize global pose container to receive values during the recursive descend
318+ mGlobalPose .resize (initialSize + aSubtree.size ());
319+ recurseGlobalPose (shiftedSubtreeRoot, mGlobalPose [aInsertionParent]);
320+
321+ utils::appendSiblings (mHierarchy [aInsertionParent].mFirstChild ,
322+ shiftedSubtreeRoot,
323+ *this );
203324 }
204325 else // Insert the subtree as a root node
205326 {
206327 utils::append (mGlobalPose , aSubtree.mGlobalPose );
207-
208- auto & lastRoot = mHierarchy [mFirstRoot ].mLastSibling ;
209- assert (mHierarchy [lastRoot].mNextSibling == Node::gInvalidIndex );
210- mHierarchy [lastRoot].mNextSibling = aSubtree.mFirstRoot + initialSize;
211- lastRoot = aSubtree.mHierarchy [aSubtree.mFirstRoot ].mLastSibling + initialSize;
328+ utils::appendSiblings (mFirstRoot , shiftedSubtreeRoot, *this );
212329 }
213330
214331 return initialSize;
0 commit comments