OcTreeNodeJointAngles* OcTreeJointAngles::updateNode(const OcTreeKey& k)
{
if (root == NULL){
root = new OcTreeNodeJointAngles();
tree_size++;
}
OcTreeNodeJointAngles* currNode(root);
// follow or construct nodes down to last level...
for (int i = (tree_depth - 1); i >= 0; i--)
{
unsigned int pos = computeChildIdx(k, i);
// requested node does not exist
if (!currNode->childExists(pos))
{
currNode->createChild(pos);
tree_size++;
}
// descend tree
currNode = static_cast<OcTreeNodeJointAngles*> (currNode->getChild(pos));
assert(currNode);
}
return currNode;
}
// Note: do not inline this method, will decrease speed (KMW)
CountingOcTreeNode* CountingOcTree::updateNode(const OcTreeKey& k) {
CountingOcTreeNode* curNode (root);
curNode->increaseCount();
// follow or construct nodes down to last level...
for (int i=(tree_depth-1); i>=0; i--) {
unsigned int pos = computeChildIdx(k, i);
// requested node does not exist
if (!nodeChildExists(curNode, pos)) {
createNodeChild(curNode, pos);
}
// descent tree
curNode = getNodeChild(curNode, pos);
curNode->increaseCount(); // modify traversed nodes
}
return curNode;
}
