38 inline DynamicTree::TreeNode
43 return DynamicTree::TreeNode{
44 DynamicTree::BranchData{c1, c2},
GetEnclosingAABB(aabb1, aabb2), 1 + std::max(h1, h2), parent
48 std::pair<DynamicTree::Size, DynamicTree::Size>
51 const AABB& toBox) noexcept
53 if (nodeA.GetHeight() < nodeB.GetHeight())
55 return std::make_pair(indexA, indexB);
57 if (nodeB.GetHeight() < nodeA.GetHeight())
59 return std::make_pair(indexB, indexA);
65 return std::make_pair(indexA, indexB);
69 return std::make_pair(indexB, indexA);
71 return std::make_pair(indexA, indexB);
86 const auto oldNodeI = nodes[i];
87 const auto o = oldNodeI.GetOther();
88 const auto c1 = oldNodeI.AsBranch().child1;
89 const auto c2 = oldNodeI.AsBranch().child2;
90 auto c1Node = nodes[c1];
91 auto c2Node = nodes[c2];
92 const auto c1Height = c1Node.GetHeight();
93 const auto c2Height = c2Node.GetHeight();
95 if (c2Height > (c1Height + 1))
98 const auto c2c1 = c2Node.AsBranch().child1;
99 const auto c2c2 = c2Node.AsBranch().child2;
100 const auto c2c1Node = nodes[c2c1];
101 const auto c2c2Node = nodes[c2c2];
119 const auto ms = MakeMoveStay(c2c1Node, c2c1, c2c2Node, c2c2, c1Node.GetAABB());
120 const auto newNodeI = MakeNode(c1, c1Node.GetAABB(), c1Height,
121 ms.first, nodes[ms.first].GetAABB(), nodes[ms.first].GetHeight(),
123 c2Node = MakeNode(i, newNodeI.GetAABB(), newNodeI.GetHeight(),
124 ms.second, nodes[ms.second].GetAABB(), nodes[ms.second].GetHeight(),
126 nodes[ms.first].SetOther(i);
131 const auto oNode = nodes[o];
132 const auto oNodeBD = oNode.AsBranch();
133 assert(oNodeBD.child1 == i || oNodeBD.child2 == i);
134 nodes[o] = (oNodeBD.child1 == i)?
135 MakeNode(c2, c2Node.GetAABB(), c2Node.GetHeight(),
136 oNodeBD.child2, nodes[oNodeBD.child2].GetAABB(), nodes[oNodeBD.child2].GetHeight(),
138 MakeNode(oNodeBD.child1, nodes[oNodeBD.child1].GetAABB(), nodes[oNodeBD.child1].GetHeight(),
139 c2, c2Node.GetAABB(), c2Node.GetHeight(),
145 if (c1Height > (c2Height + 1))
148 const auto c1c1 = c1Node.AsBranch().child1;
149 const auto c1c2 = c1Node.AsBranch().child2;
150 const auto c1c1Node = nodes[c1c1];
151 const auto c1c2Node = nodes[c1c2];
169 const auto ms = MakeMoveStay(c1c1Node, c1c1, c1c2Node, c1c2, c2Node.GetAABB());
170 const auto newNodeI = MakeNode(ms.first, nodes[ms.first].GetAABB(), nodes[ms.first].GetHeight(),
171 c2, c2Node.GetAABB(), c2Height,
173 c1Node = MakeNode(ms.second, nodes[ms.second].GetAABB(), nodes[ms.second].GetHeight(),
174 i, newNodeI.GetAABB(), newNodeI.GetHeight(),
176 nodes[ms.first].SetOther(i);
181 const auto oNode = nodes[o];
182 const auto oNodeBD = oNode.AsBranch();
183 assert(oNodeBD.child1 == i || oNodeBD.child2 == i);
184 nodes[o] = (oNodeBD.child1 == i)?
185 MakeNode(c1, c1Node.GetAABB(), c1Node.GetHeight(),
186 oNodeBD.child2, nodes[oNodeBD.child2].GetAABB(), nodes[oNodeBD.child2].GetHeight(),
188 MakeNode(oNodeBD.child1, nodes[oNodeBD.child1].GetAABB(), nodes[oNodeBD.child1].GetHeight(),
189 c1, c1Node.GetAABB(), c1Node.GetHeight(),
195 nodes[i] = MakeNode(c1, c1Node.GetAABB(), c1Node.GetHeight(), c2, c2Node.GetAABB(), c2Height, o);
209 assert(nodes[nodes[index].AsBranch().child1].GetOther() == index);
210 assert(nodes[nodes[index].AsBranch().child2].GetOther() == index);
213 index = RebalanceAt(nodes, index);
233 const auto costFunc = [leafAABB](
const DynamicTree::TreeNode& childNode,
Length inheritCost) {
237 return isLeaf? leafCost: (leafCost -
GetPerimeter(childAabb));
242 const auto& node = nodes[index];
243 const auto branch = node.AsBranch();
244 const auto child1 = branch.child1;
245 const auto child2 = branch.child2;
246 assert(nodes[child1].GetOther() == index);
247 assert(nodes[child2].GetOther() == index);
248 const auto aabb = node.GetAABB();
252 assert(combinedPerimeter >= perimeter);
257 const auto cost = combinedPerimeter * 2;
260 const auto inheritanceCost = (combinedPerimeter - perimeter) * 2;
262 const auto cost1 = costFunc(nodes[child1], inheritanceCost);
263 const auto cost2 = costFunc(nodes[child2], inheritanceCost);
265 if ((cost < cost1) && (cost < cost2))
272 index = (cost1 < cost2)? child1: child2;
277 std::pair<DynamicTree::Size, DynamicTree::Size>
280 const auto parent = nodes[index].GetOther();
281 const auto grandParent = nodes[parent].GetOther();
282 const auto parentBD = nodes[parent].AsBranch();
283 const auto sibling = (parentBD.child1 == index)? parentBD.child2: parentBD.child1;
286 nodes[sibling].SetOther(grandParent);
289 const auto newBD =
ReplaceChild(nodes[grandParent].AsBranch(), parent, sibling);
290 const auto newAabb =
GetEnclosingAABB(nodes[newBD.child1].GetAABB(), nodes[newBD.child2].GetAABB());
291 const auto newHeight = 1 + std::max(nodes[newBD.child1].GetHeight(), nodes[newBD.child2].GetHeight());
292 nodes[grandParent].Assign(newBD, newAabb, newHeight);
294 return std::make_pair(UpdateUpwardFrom(nodes, grandParent), parent);
296 return std::make_pair(sibling, parent);
305 const auto sibling = FindLowestCostNode(nodes, aabb, rootIndex);
306 const auto oldParent = nodes[sibling].GetOther();
309 nodes[newParent] = DynamicTree::TreeNode{DynamicTree::BranchData{sibling, index},
311 nodes[sibling].SetOther(newParent);
312 nodes[index].SetOther(newParent);
315 const auto newBD =
ReplaceChild(nodes[oldParent].AsBranch(), sibling, newParent);
316 const auto newAabb =
GetEnclosingAABB(nodes[newBD.child1].GetAABB(), nodes[newBD.child2].GetAABB());
317 const auto newHeight = 1 + std::max(nodes[newBD.child1].GetHeight(), nodes[newBD.child2].GetHeight());
318 nodes[oldParent].Assign(newBD, newAabb, newHeight);
319 assert(nodes[nodes[oldParent].AsBranch().child1].GetOther() == oldParent);
320 assert(nodes[nodes[oldParent].AsBranch().child2].GetOther() == oldParent);
321 return UpdateUpwardFrom(nodes, oldParent);
331 const auto parent = nodes[index].GetOther();
332 const auto grandParent = nodes[parent].GetOther();
333 const auto parentBD = nodes[parent].AsBranch();
334 assert(parentBD.child1 == index || parentBD.child2 == index);
335 const auto sibling = (parentBD.child1 == index)? parentBD.child2: parentBD.child1;
337 nodes[sibling].SetOther(grandParent);
338 nodes[index].SetAABB(aabb);
342 assert(nodes[grandParent].AsBranch().child1 == parent || nodes[grandParent].AsBranch().child2 == parent);
343 const auto newBD =
ReplaceChild(nodes[grandParent].AsBranch(), parent, sibling);
344 const auto newAabb =
GetEnclosingAABB(nodes[newBD.child1].GetAABB(), nodes[newBD.child2].GetAABB());
345 const auto newHeight = 1 + std::max(nodes[newBD.child1].GetHeight(), nodes[newBD.child2].GetHeight());
346 nodes[grandParent].Assign(newBD, newAabb, newHeight);
348 assert(nodes[nodes[grandParent].AsBranch().child1].GetOther() == grandParent);
349 assert(nodes[nodes[grandParent].AsBranch().child2].GetOther() == grandParent);
350 rootIndex = UpdateUpwardFrom(nodes, grandParent);
357 const auto cheapest = FindLowestCostNode(nodes, aabb, rootIndex);
358 const auto cheapestParent = nodes[cheapest].GetOther();
361 nodes[parent] = DynamicTree::TreeNode{
362 DynamicTree::BranchData{cheapest, index},
364 1 + nodes[cheapest].GetHeight(), cheapestParent
368 const auto newBD =
ReplaceChild(nodes[cheapestParent].AsBranch(), cheapest, parent);
369 const auto newAabb =
GetEnclosingAABB(nodes[newBD.child1].GetAABB(), nodes[newBD.child2].GetAABB());
370 const auto newHeight = 1 + std::max(nodes[newBD.child1].GetHeight(), nodes[newBD.child2].GetHeight());
371 nodes[cheapestParent].Assign(newBD, newAabb, newHeight);
373 nodes[cheapest].SetOther(parent);
374 return UpdateUpwardFrom(nodes, parent);
382 m_nodes{nodeCapacity? Alloc<TreeNode>(nodeCapacity):
nullptr},
383 m_freeIndex{nodeCapacity? 0: GetInvalidSize()},
384 m_nodeCapacity{nodeCapacity}
389 const auto endCapacity = nodeCapacity - Size{1};
390 for (
auto i = decltype(nodeCapacity){0}; i < endCapacity; ++i)
392 new (&m_nodes[i]) TreeNode{i + 1};
394 new (&m_nodes[endCapacity]) TreeNode{};
399 m_nodes{Alloc<TreeNode>(other.m_nodeCapacity)},
400 m_rootIndex{other.m_rootIndex},
401 m_freeIndex{other.m_freeIndex},
402 m_nodeCount{other.m_nodeCount},
403 m_nodeCapacity{other.m_nodeCapacity},
404 m_leafCount{other.m_leafCount}
406 std::copy(&other.m_nodes[0], &other.m_nodes[other.m_nodeCapacity], &m_nodes[0]);
429 void DynamicTree::SetNodeCapacity(Size value) noexcept
431 assert(value > m_nodeCapacity);
434 m_nodeCapacity = value;
435 m_nodes = Realloc<TreeNode>(m_nodes, m_nodeCapacity);
439 const auto endCapacity = m_nodeCapacity - 1;
440 for (
auto i = m_nodeCount; i < endCapacity; ++i)
442 new (m_nodes + i) TreeNode{i + 1};
444 new (m_nodes + endCapacity) TreeNode{};
445 m_freeIndex = m_nodeCount;
450 const auto index = AllocateNode();
451 m_nodes[index] = TreeNode{data, aabb};
456 Height height, Size parent) noexcept
459 const auto index = AllocateNode();
460 m_nodes[index] = TreeNode{data, aabb, height, parent};
469 assert(m_nodeCount == m_nodeCapacity);
476 const auto index = m_freeIndex;
477 m_freeIndex = m_nodes[index].
GetOther();
482 void DynamicTree::FreeNode(Size index) noexcept
484 assert(index != GetInvalidSize());
485 assert(index < GetNodeCapacity());
486 assert(index != GetFreeIndex());
487 assert(m_nodeCount > 0);
489 assert(m_nodes[index].GetOther() == GetInvalidSize());
491 m_nodes[index] = TreeNode{m_freeIndex};
498 const auto it = std::find_if(m_nodes, m_nodes + m_nodeCapacity, [&](
TreeNode& node) {
505 const auto bd = node.AsBranch();
506 if (bd.child1 == index || bd.child2 == index)
513 return (it != m_nodes + m_nodeCapacity)?
static_cast<Size
>(it - m_nodes): GetInvalidSize();
519 const auto index = AllocateNode(data, aabb);
520 if (m_rootIndex != GetInvalidSize())
522 const auto newParent = AllocateNode();
523 m_rootIndex = InsertParent(m_nodes, newParent, aabb, index, m_rootIndex);
533 void DynamicTree::DestroyLeaf(
Size index) noexcept
535 assert(index != GetInvalidSize());
536 assert(index < m_nodeCapacity);
538 assert(m_leafCount > 0);
542 if (m_rootIndex != index)
544 const auto result = RemoveParent(m_nodes, index);
548 const auto found = FindReference(parent);
549 assert(found == GetInvalidSize());
555 assert(m_nodes[index].GetOther() == GetInvalidSize());
556 m_rootIndex = GetInvalidSize();
560 const auto found = FindReference(index);
561 assert(found == GetInvalidSize());
566 void DynamicTree::UpdateLeaf(
Size index,
const AABB& aabb)
568 assert(index != GetInvalidSize());
569 assert(index < m_nodeCapacity);
572 if (m_rootIndex != index)
574 m_rootIndex = UpdateNonRoot(m_nodes, index, aabb);
578 assert(m_nodes[index].GetOther() == GetInvalidSize());
579 m_nodes[index].SetAABB(aabb);
583 void DynamicTree::RebuildBottomUp()
585 const auto nodes = Alloc<Size>(m_nodeCount);
586 auto count =
Size{0};
589 for (
auto i = decltype(m_nodeCapacity){0}; i < m_nodeCapacity; ++i)
591 const auto height = m_nodes[i].GetHeight();
594 m_nodes[i].SetOther(GetInvalidSize());
600 m_nodes[i].SetOther(GetInvalidSize());
607 auto minCost = std::numeric_limits<Length>::infinity();
608 auto iMin = GetInvalidSize();
609 auto jMin = GetInvalidSize();
610 for (
auto i = decltype(count){0}; i < count; ++i)
612 const auto& aabbi = m_nodes[nodes[i]].GetAABB();
614 for (
auto j = i + 1; j < count; ++j)
616 const auto& aabbj = m_nodes[nodes[j]].GetAABB();
628 assert((iMin < m_nodeCount) && (jMin < m_nodeCount));
630 const auto index1 = nodes[iMin];
631 const auto index2 = nodes[jMin];
636 const auto height = 1 + std::max(m_nodes[index1].
GetHeight(), m_nodes[index2].
GetHeight());
639 const auto parent = AllocateNode(
BranchData{index1, index2}, aabb, height);
640 m_nodes[index1].SetOther(parent);
641 m_nodes[index2].SetOther(parent);
643 nodes[jMin] = nodes[count-1];
644 nodes[iMin] = parent;
648 m_rootIndex = nodes[0];
652 void DynamicTree::ShiftOrigin(
Length2 newOrigin)
655 for (
auto i = decltype(m_nodeCapacity){0}; i < m_nodeCapacity; ++i)
669 swap(lhs.m_nodes, rhs.m_nodes);
670 swap(lhs.m_rootIndex, rhs.m_rootIndex);
671 swap(lhs.m_freeIndex, rhs.m_freeIndex);
672 swap(lhs.m_nodeCount, rhs.m_nodeCount);
673 swap(lhs.m_nodeCapacity, rhs.m_nodeCapacity);
674 swap(lhs.m_leafCount, rhs.m_leafCount);
682 while (!
empty(stack))
684 const auto index = stack.
top();
686 if (index != DynamicTree::GetInvalidSize())
690 const auto height = tree.
GetHeight(index);
694 stack.
push(branchData.child1);
695 stack.
push(branchData.child2);
700 const auto sc = callback(index);
701 if (sc == DynamicTreeOpcode::End)
715 return callback(leafData.fixture, leafData.childIndex)?
716 DynamicTreeOpcode::Continue: DynamicTreeOpcode::End;
723 const auto nodeCapacity = tree.GetNodeCapacity();
724 for (
auto i = decltype(nodeCapacity){0}; i < nodeCapacity; ++i)
736 const auto root = tree.GetRootIndex();
737 if (root != DynamicTree::GetInvalidSize())
739 const auto rootPerimeter =
GetPerimeter(tree.GetAABB(root));
741 return total / rootPerimeter;
748 assert(index < tree.GetNodeCapacity());
751 const auto bd = tree.GetBranchData(index);
754 return 1 + std::max(height1, height2);
762 const auto nodeCapacity = tree.GetNodeCapacity();
763 for (
auto i = decltype(nodeCapacity){0}; i < nodeCapacity; ++i)
767 const auto bd = tree.GetBranchData(i);
768 const auto height1 = tree.GetHeight(bd.child1);
769 const auto height2 = tree.GetHeight(bd.child2);
770 const auto imbalance = (height2 >= height1)? height2 - height1: height1 - height2;
771 maxImbalance = std::max(maxImbalance, imbalance);
779 if (index == DynamicTree::GetInvalidSize())
785 assert((index != tree.GetRootIndex()) || (tree.GetOther(index) == DynamicTree::GetInvalidSize()));
787 const auto nodeCapacity = tree.GetNodeCapacity();
788 if (index >= nodeCapacity)
793 const auto height = tree.GetHeight(index);
802 const auto bd = tree.GetBranchData(index);
803 const auto child1 = bd.child1;
804 const auto child2 = bd.child2;
805 assert(tree.GetOther(child1) == index);
806 assert(tree.GetOther(child2) == index);
816 if (index == DynamicTree::GetInvalidSize())
821 const auto nodeCapacity = tree.GetNodeCapacity();
822 if (index >= nodeCapacity)
827 const auto height = tree.GetHeight(index);
833 const auto bd = tree.GetBranchData(index);
834 const auto child1 = bd.child1;
835 const auto child2 = bd.child2;
838 assert(tree.GetOther(child1) == index);
839 assert(tree.GetOther(child2) == index);
840 assert(height == (1 + std::max(tree.GetHeight(child1), tree.GetHeight(child2))));
841 assert(tree.GetAABB(index) ==
GetEnclosingAABB(tree.GetAABB(child1), tree.GetAABB(child2)));