2503 lines
64 KiB
C
2503 lines
64 KiB
C
// SPDX-FileCopyrightText: 2025 Erin Catto
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "dynamic_tree.h"
|
|
|
|
#include "aabb.h"
|
|
#include "algorithm.h"
|
|
#include "core.h"
|
|
#include "platform.h"
|
|
#include "simd.h"
|
|
|
|
#include "box3d/collision.h"
|
|
#include "box3d/constants.h"
|
|
#include "box3d/math_functions.h"
|
|
|
|
#include <float.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
_Static_assert( B3_ROOT_NODE == 0, "bad root" );
|
|
_Static_assert( sizeof( b3TreeNode ) == 32, "expected size" );
|
|
_Static_assert( sizeof( b3TreeProxy ) == 24, "expected size" );
|
|
|
|
// The free sibling pair list is chained through the parent index of their first node. A pair comes from the
|
|
// free list when there is one and from the bump pointer otherwise. A rebuild empties the free list, so
|
|
// allocations can just bump.
|
|
static int b3AllocateSiblingPair( b3DynamicTree* tree )
|
|
{
|
|
// Allocate from free list.
|
|
if ( tree->pairFreeList != B3_NULL_INDEX )
|
|
{
|
|
int pair = tree->pairFreeList;
|
|
tree->pairFreeList = tree->parents[pair];
|
|
return pair;
|
|
}
|
|
|
|
// Extend the bump allocator if needed.
|
|
if ( tree->nodeEnd + 2 > tree->nodeCapacity )
|
|
{
|
|
int oldCapacity = tree->nodeCapacity;
|
|
int newCapacity = oldCapacity + ( oldCapacity >> 1 );
|
|
newCapacity += newCapacity & 1;
|
|
tree->nodes = B3_GROW_ZERO( tree->nodes, oldCapacity, newCapacity );
|
|
tree->parents = B3_GROW_ZERO( tree->parents, oldCapacity, newCapacity );
|
|
tree->nodeCapacity = newCapacity;
|
|
|
|
// The spare has to match, the rebuild allocates it again.
|
|
b3Free( tree->swapNodes, oldCapacity * sizeof( b3TreeNode ) );
|
|
tree->swapNodes = NULL;
|
|
}
|
|
|
|
int pair = tree->nodeEnd;
|
|
tree->nodeEnd += 2;
|
|
return pair;
|
|
}
|
|
|
|
static void b3FreePair( b3DynamicTree* tree, int pair )
|
|
{
|
|
B3_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );
|
|
tree->nodes[pair] = b3MakeEmptyNode();
|
|
tree->nodes[pair + 1] = b3MakeEmptyNode();
|
|
tree->parents[pair] = tree->pairFreeList;
|
|
tree->parents[pair + 1] = B3_NULL_INDEX;
|
|
tree->pairFreeList = pair;
|
|
}
|
|
|
|
b3DynamicTree b3DynamicTree_Create( int proxyCapacity )
|
|
{
|
|
int capacity = b3MaxInt( proxyCapacity, 16 );
|
|
|
|
// Intentionally _not_ initialized with brace initialization, which can leave
|
|
// uninitialized gaps.
|
|
b3DynamicTree tree;
|
|
|
|
// memset needed for deterministic serialization
|
|
memset( &tree, 0, sizeof( b3DynamicTree ) );
|
|
|
|
tree.version = B3_DYNAMIC_TREE_VERSION;
|
|
|
|
// A tree of n proxies has 2n - 1 nodes plus the empty node beside the root
|
|
tree.nodeCapacity = 2 * capacity;
|
|
tree.nodes = (b3TreeNode*)b3AllocZero( tree.nodeCapacity * sizeof( b3TreeNode ) );
|
|
tree.parents = (int32_t*)b3AllocZero( tree.nodeCapacity * sizeof( int32_t ) );
|
|
tree.pairFreeList = B3_NULL_INDEX;
|
|
|
|
// The root and the empty node always exist
|
|
tree.nodes[B3_ROOT_NODE] = b3MakeEmptyNode();
|
|
tree.nodes[B3_ROOT_NODE + 1] = b3MakeEmptyNode();
|
|
tree.parents[B3_ROOT_NODE] = B3_NULL_INDEX;
|
|
tree.parents[B3_ROOT_NODE + 1] = B3_NULL_INDEX;
|
|
|
|
// This is the bump index.
|
|
tree.nodeEnd = 2;
|
|
tree.dfsOrdered = true;
|
|
|
|
tree.proxyCapacity = capacity;
|
|
tree.proxyCount = 0;
|
|
tree.proxies = (b3TreeProxy*)b3AllocZero( tree.proxyCapacity * sizeof( b3TreeProxy ) );
|
|
|
|
// Build a linked list for the free list.
|
|
for ( int i = 0; i < tree.proxyCapacity - 1; ++i )
|
|
{
|
|
tree.proxies[i].node = B3_NULL_INDEX;
|
|
tree.proxies[i].next = i + 1;
|
|
}
|
|
|
|
tree.proxies[tree.proxyCapacity - 1].node = B3_NULL_INDEX;
|
|
tree.proxies[tree.proxyCapacity - 1].next = B3_NULL_INDEX;
|
|
tree.proxyFreeList = 0;
|
|
|
|
tree.swapNodes = NULL;
|
|
tree.leafIndices = NULL;
|
|
tree.leafNodes = NULL;
|
|
tree.leafBoxes = NULL;
|
|
tree.leafCenters = NULL;
|
|
tree.binIndices = NULL;
|
|
tree.rebuildCapacity = 0;
|
|
|
|
return tree;
|
|
}
|
|
|
|
void b3DynamicTree_Destroy( b3DynamicTree* tree )
|
|
{
|
|
b3Free( tree->nodes, tree->nodeCapacity * sizeof( b3TreeNode ) );
|
|
b3Free( tree->parents, tree->nodeCapacity * sizeof( int32_t ) );
|
|
b3Free( tree->proxies, tree->proxyCapacity * sizeof( b3TreeProxy ) );
|
|
b3Free( tree->swapNodes, tree->nodeCapacity * sizeof( b3TreeNode ) );
|
|
b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int32_t ) );
|
|
b3Free( tree->leafNodes, tree->rebuildCapacity * sizeof( b3TreeNode ) );
|
|
b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) );
|
|
b3Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b3Vec3 ) );
|
|
b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) );
|
|
|
|
memset( tree, 0, sizeof( b3DynamicTree ) );
|
|
}
|
|
|
|
// Allocate a proxy from the pool. Grow the pool if necessary.
|
|
static int b3AllocateProxy( b3DynamicTree* tree )
|
|
{
|
|
// Expand the pool as needed.
|
|
if ( tree->proxyFreeList == B3_NULL_INDEX )
|
|
{
|
|
B3_ASSERT( tree->proxyCount == tree->proxyCapacity );
|
|
|
|
// The free list is empty. Rebuild a bigger pool.
|
|
// A restored pool can have capacity 1, so guarantee strict growth.
|
|
int oldCapacity = tree->proxyCapacity;
|
|
tree->proxyCapacity += b3MaxInt( oldCapacity >> 1, 1 );
|
|
tree->proxies = B3_GROW_ZERO( tree->proxies, oldCapacity, tree->proxyCapacity );
|
|
|
|
// Build a linked list for the free list.
|
|
for ( int i = oldCapacity; i < tree->proxyCapacity - 1; ++i )
|
|
{
|
|
tree->proxies[i].node = B3_NULL_INDEX;
|
|
tree->proxies[i].next = i + 1;
|
|
}
|
|
|
|
tree->proxies[tree->proxyCapacity - 1].node = B3_NULL_INDEX;
|
|
tree->proxies[tree->proxyCapacity - 1].next = B3_NULL_INDEX;
|
|
tree->proxyFreeList = oldCapacity;
|
|
}
|
|
|
|
// Peel a proxy off the free list.
|
|
int proxyIndex = tree->proxyFreeList;
|
|
tree->proxyFreeList = tree->proxies[proxyIndex].next;
|
|
memset( tree->proxies + proxyIndex, 0, sizeof( b3TreeProxy ) );
|
|
tree->proxies[proxyIndex].node = B3_NULL_INDEX;
|
|
tree->proxies[proxyIndex].next = B3_NULL_INDEX;
|
|
++tree->proxyCount;
|
|
return proxyIndex;
|
|
}
|
|
|
|
// Return a proxy to the pool.
|
|
static void b3FreeProxy( b3DynamicTree* tree, int proxyId )
|
|
{
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
B3_ASSERT( 0 < tree->proxyCount );
|
|
tree->proxies[proxyId].node = B3_NULL_INDEX;
|
|
tree->proxies[proxyId].next = tree->proxyFreeList;
|
|
tree->proxyFreeList = proxyId;
|
|
--tree->proxyCount;
|
|
}
|
|
|
|
static inline int b3GetNodeHeight( const b3TreeNode* node )
|
|
{
|
|
return b3IsLeaf( node ) ? 0 : node->height;
|
|
}
|
|
|
|
// The internal node above a children pair
|
|
static inline b3TreeNode b3MakeInternalNode( const b3TreeNode* nodes, int pair )
|
|
{
|
|
const b3TreeNode* c1 = nodes + pair;
|
|
const b3TreeNode* c2 = nodes + pair + 1;
|
|
|
|
b3TreeNode node = { 0 };
|
|
b3StoreAABBV( &node.aabb, b3UnionPairV( nodes + pair ), true );
|
|
node.flagIndex = (uint32_t)pair | ( ( c1->flagIndex | c2->flagIndex ) & B3_MOVED_NODE );
|
|
node.height = 1 + b3MaxInt( b3GetNodeHeight( c1 ), b3GetNodeHeight( c2 ) );
|
|
return node;
|
|
}
|
|
|
|
static inline b3TreeNode b3MakeLeafNode( b3AABB aabb, int proxyId, uint64_t userData, bool moved )
|
|
{
|
|
b3TreeNode node = { 0 };
|
|
node.aabb = aabb;
|
|
node.flagIndex = (uint32_t)proxyId | B3_LEAF_NODE | ( moved ? B3_MOVED_NODE : 0 );
|
|
node.shapeIndex = (int32_t)(uint32_t)userData;
|
|
return node;
|
|
}
|
|
|
|
// A node landed at a new index, so tell what hangs below it
|
|
static inline void b3LinkChildren( b3DynamicTree* tree, int nodeIndex )
|
|
{
|
|
const b3TreeNode* node = tree->nodes + nodeIndex;
|
|
if ( b3IsLeaf( node ) )
|
|
{
|
|
tree->proxies[b3GetProxyId( node )].node = nodeIndex;
|
|
}
|
|
else
|
|
{
|
|
int pair = b3GetLeftChild( node );
|
|
tree->parents[pair] = nodeIndex;
|
|
tree->parents[pair + 1] = nodeIndex;
|
|
}
|
|
}
|
|
|
|
// The sweep refit visits indices from high to low, so it needs every child above its parent
|
|
static inline bool b3IsNodeOrdered( const b3TreeNode* nodes, int nodeIndex )
|
|
{
|
|
const b3TreeNode* node = nodes + nodeIndex;
|
|
return b3IsLeaf( node ) || nodeIndex < b3GetLeftChild( node );
|
|
}
|
|
|
|
// Greedy algorithm for sibling selection using the SAH
|
|
// We have three nodes A-(B,C) and want to add a leaf D, there are three choices.
|
|
// 1: make a new parent for A and D : E-(A-(B,C), D)
|
|
// 2: associate D with B
|
|
// a: B is a leaf : A-(E-(B,D), C)
|
|
// b: B is an internal node: A-(B{D},C)
|
|
// 3: associate D with C
|
|
// a: C is a leaf : A-(B, E-(C,D))
|
|
// b: C is an internal node: A-(B, C{D})
|
|
// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy.
|
|
|
|
// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula.
|
|
// cost of sibling H = area(union(H, D)) + increased area of ancestors
|
|
|
|
// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases:
|
|
// case1: D becomes a sibling of B
|
|
// case2: D becomes a descendant of B along with a new internal node of area(D).
|
|
static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD )
|
|
{
|
|
b3Vec3 centerD = b3AABB_Center( boxD );
|
|
float areaD = b3Perimeter( boxD );
|
|
|
|
const b3TreeNode* nodes = tree->nodes;
|
|
int nodeIndex = B3_ROOT_NODE;
|
|
|
|
if ( b3IsLeaf( nodes + nodeIndex ) )
|
|
{
|
|
return nodeIndex;
|
|
}
|
|
|
|
b3AABB rootBox = nodes[nodeIndex].aabb;
|
|
|
|
// Area of current node
|
|
float areaBase = b3Perimeter( rootBox );
|
|
|
|
// Area of inflated node
|
|
float directCost = b3Perimeter( b3AABB_Union( rootBox, boxD ) );
|
|
float inheritedCost = 0.0f;
|
|
|
|
int bestSibling = nodeIndex;
|
|
float bestCost = directCost;
|
|
|
|
// Descend the tree, following a single greedy path.
|
|
for ( ;; )
|
|
{
|
|
int child1 = b3GetLeftChild( nodes + nodeIndex );
|
|
int child2 = child1 + 1;
|
|
|
|
// Cost of creating a new parent for this node and the new leaf
|
|
float cost = directCost + inheritedCost;
|
|
|
|
// Sometimes there are multiple identical costs within tolerance.
|
|
// This breaks the ties using the centroid distance.
|
|
if ( cost < bestCost )
|
|
{
|
|
bestSibling = nodeIndex;
|
|
bestCost = cost;
|
|
}
|
|
|
|
// Inheritance cost seen by children
|
|
inheritedCost += directCost - areaBase;
|
|
|
|
bool leaf1 = b3IsLeaf( nodes + child1 );
|
|
bool leaf2 = b3IsLeaf( nodes + child2 );
|
|
|
|
// Cost of descending into child 1
|
|
float lowerCost1 = FLT_MAX;
|
|
b3AABB box1 = nodes[child1].aabb;
|
|
float directCost1 = b3Perimeter( b3AABB_Union( box1, boxD ) );
|
|
float area1 = 0.0f;
|
|
if ( leaf1 )
|
|
{
|
|
// Child 1 is a leaf
|
|
// Cost of creating new node and increasing area of node P
|
|
float cost1 = directCost1 + inheritedCost;
|
|
if ( cost1 < bestCost )
|
|
{
|
|
bestSibling = child1;
|
|
bestCost = cost1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Child 1 is an internal node
|
|
area1 = b3Perimeter( box1 );
|
|
|
|
// Lower bound cost of inserting under child 1. The minimum accounts for two possibilities:
|
|
// 1. Child1 could be the sibling with cost1 = inheritedCost + directCost1
|
|
// 2. A descendant of child1 could be the sibling with the lower bound cost of
|
|
// cost1 = inheritedCost + (directCost1 - area1) + areaD
|
|
// This minimum here leads to the minimum of these two costs.
|
|
lowerCost1 = inheritedCost + directCost1 + b3MinFloat( areaD - area1, 0.0f );
|
|
}
|
|
|
|
// Cost of descending into child 2
|
|
float lowerCost2 = FLT_MAX;
|
|
b3AABB box2 = nodes[child2].aabb;
|
|
float directCost2 = b3Perimeter( b3AABB_Union( box2, boxD ) );
|
|
float area2 = 0.0f;
|
|
if ( leaf2 )
|
|
{
|
|
float cost2 = directCost2 + inheritedCost;
|
|
if ( cost2 < bestCost )
|
|
{
|
|
bestSibling = child2;
|
|
bestCost = cost2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
area2 = b3Perimeter( box2 );
|
|
lowerCost2 = inheritedCost + directCost2 + b3MinFloat( areaD - area2, 0.0f );
|
|
}
|
|
|
|
if ( leaf1 && leaf2 )
|
|
{
|
|
break;
|
|
}
|
|
|
|
// Can the cost possibly be decreased?
|
|
if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 )
|
|
{
|
|
break;
|
|
}
|
|
|
|
if ( lowerCost1 == lowerCost2 && leaf1 == false )
|
|
{
|
|
B3_ASSERT( lowerCost1 < FLT_MAX );
|
|
B3_ASSERT( lowerCost2 < FLT_MAX );
|
|
|
|
// No clear choice based on lower bound surface area. This can happen when both
|
|
// children fully contain D. Fall back to node distance.
|
|
b3Vec3 d1 = b3Sub( b3AABB_Center( box1 ), centerD );
|
|
b3Vec3 d2 = b3Sub( b3AABB_Center( box2 ), centerD );
|
|
lowerCost1 = b3LengthSquared( d1 );
|
|
lowerCost2 = b3LengthSquared( d2 );
|
|
}
|
|
|
|
// Descend
|
|
if ( lowerCost1 < lowerCost2 && leaf1 == false )
|
|
{
|
|
nodeIndex = child1;
|
|
areaBase = area1;
|
|
directCost = directCost1;
|
|
}
|
|
else
|
|
{
|
|
nodeIndex = child2;
|
|
areaBase = area2;
|
|
directCost = directCost2;
|
|
}
|
|
}
|
|
|
|
return bestSibling;
|
|
}
|
|
|
|
// Swap a child of A with a grandchild under the other child.
|
|
// Example swapping B with G:
|
|
// (A (B (D E) C (F G)) -> (A (G) C (F B (D E))
|
|
static void b3SwapNodes( b3DynamicTree* tree, int iDown, int iUp )
|
|
{
|
|
b3TreeNode* nodes = tree->nodes;
|
|
B3_SWAP( nodes[iDown], nodes[iUp] );
|
|
b3LinkChildren( tree, iDown );
|
|
b3LinkChildren( tree, iUp );
|
|
|
|
if ( b3IsNodeOrdered( nodes, iDown ) == false || b3IsNodeOrdered( nodes, iUp ) == false )
|
|
{
|
|
tree->dfsOrdered = false;
|
|
}
|
|
|
|
// The sibling of the node that went down holds a different subtree now
|
|
int iC = iDown ^ 1;
|
|
nodes[iC] = b3MakeInternalNode( nodes, b3GetLeftChild( nodes + iC ) );
|
|
}
|
|
|
|
// Perform a left or right rotation if node A is imbalanced.
|
|
// Tree: (A (B (D E) C (F G))
|
|
static void b3RotateNodes( b3DynamicTree* tree, int iA )
|
|
{
|
|
b3TreeNode* nodes = tree->nodes;
|
|
const b3TreeNode* A = nodes + iA;
|
|
B3_ASSERT( b3IsLeaf( A ) == false );
|
|
|
|
int iB = b3GetLeftChild( A );
|
|
int iC = iB + 1;
|
|
const b3TreeNode* B = nodes + iB;
|
|
const b3TreeNode* C = nodes + iC;
|
|
|
|
bool leafB = b3IsLeaf( B );
|
|
bool leafC = b3IsLeaf( C );
|
|
if ( leafB && leafC )
|
|
{
|
|
return;
|
|
}
|
|
|
|
int bestDown = B3_NULL_INDEX;
|
|
int bestUp = B3_NULL_INDEX;
|
|
float bestDelta = 0.0f;
|
|
|
|
if ( leafC == false )
|
|
{
|
|
// Swap B with F or G
|
|
int iF = b3GetLeftChild( C );
|
|
int iG = iF + 1;
|
|
float areaC = b3Perimeter( C->aabb );
|
|
|
|
// B <-> F then C (B G)
|
|
float deltaBF = b3Perimeter( b3UnionV( B->aabb, nodes[iG].aabb ) ) - areaC;
|
|
if ( deltaBF < bestDelta )
|
|
{
|
|
bestDown = iB;
|
|
bestUp = iF;
|
|
bestDelta = deltaBF;
|
|
}
|
|
|
|
// B <-> G then C (F B)
|
|
float deltaBG = b3Perimeter( b3UnionV( B->aabb, nodes[iF].aabb ) ) - areaC;
|
|
if ( deltaBG < bestDelta )
|
|
{
|
|
bestDown = iB;
|
|
bestUp = iG;
|
|
bestDelta = deltaBG;
|
|
}
|
|
}
|
|
|
|
if ( leafB == false )
|
|
{
|
|
// Swap C with D or E
|
|
int iD = b3GetLeftChild( B );
|
|
int iE = iD + 1;
|
|
float areaB = b3Perimeter( B->aabb );
|
|
|
|
// C <-> D then B (C E)
|
|
float deltaCD = b3Perimeter( b3UnionV( C->aabb, nodes[iE].aabb ) ) - areaB;
|
|
if ( deltaCD < bestDelta )
|
|
{
|
|
bestDown = iC;
|
|
bestUp = iD;
|
|
bestDelta = deltaCD;
|
|
}
|
|
|
|
// C <-> E then B (D C)
|
|
float deltaCE = b3Perimeter( b3UnionV( C->aabb, nodes[iD].aabb ) ) - areaB;
|
|
if ( deltaCE < bestDelta )
|
|
{
|
|
bestDown = iC;
|
|
bestUp = iE;
|
|
bestDelta = deltaCE;
|
|
}
|
|
}
|
|
|
|
if ( bestDown != B3_NULL_INDEX )
|
|
{
|
|
b3SwapNodes( tree, bestDown, bestUp );
|
|
}
|
|
}
|
|
|
|
static void b3InsertLeaf( b3DynamicTree* tree, b3AABB aabb, int proxyId, bool moved, bool shouldRotate )
|
|
{
|
|
b3TreeProxy* proxy = tree->proxies + proxyId;
|
|
b3TreeNode leaf = b3MakeLeafNode( aabb, proxyId, proxy->userData, moved );
|
|
|
|
if ( b3IsEmptyNode( tree->nodes + B3_ROOT_NODE ) )
|
|
{
|
|
tree->nodes[B3_ROOT_NODE] = leaf;
|
|
proxy->node = B3_ROOT_NODE;
|
|
return;
|
|
}
|
|
|
|
int sibling = b3FindBestSibling( tree, aabb );
|
|
|
|
// The sibling's position becomes the new parent. The sibling moves down into a new pair
|
|
// beside the leaf, which puts it below its own children when it is internal.
|
|
int pair = b3AllocateSiblingPair( tree );
|
|
b3TreeNode* nodes = tree->nodes;
|
|
int32_t* parents = tree->parents;
|
|
|
|
nodes[pair] = nodes[sibling];
|
|
nodes[pair + 1] = leaf;
|
|
parents[pair] = sibling;
|
|
parents[pair + 1] = sibling;
|
|
b3LinkChildren( tree, pair );
|
|
proxy->node = pair + 1;
|
|
|
|
nodes[sibling] = b3MakeInternalNode( nodes, pair );
|
|
|
|
if ( b3IsNodeOrdered( nodes, sibling ) == false || b3IsNodeOrdered( nodes, pair ) == false )
|
|
{
|
|
tree->dfsOrdered = false;
|
|
}
|
|
|
|
// Walk back up the tree refitting ancestors, the root included.
|
|
int index = sibling;
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
if ( shouldRotate )
|
|
{
|
|
b3RotateNodes( tree, index );
|
|
}
|
|
|
|
nodes[index] = b3MakeInternalNode( nodes, b3GetLeftChild( nodes + index ) );
|
|
index = parents[index];
|
|
}
|
|
}
|
|
|
|
static void b3RemoveLeaf( b3DynamicTree* tree, int proxyId )
|
|
{
|
|
b3TreeNode* nodes = tree->nodes;
|
|
int32_t* parents = tree->parents;
|
|
|
|
int leaf = tree->proxies[proxyId].node;
|
|
B3_ASSERT( 0 <= leaf && leaf < tree->nodeEnd );
|
|
B3_ASSERT( b3IsLeaf( nodes + leaf ) && b3GetProxyId( nodes + leaf ) == proxyId );
|
|
|
|
if ( leaf == B3_ROOT_NODE )
|
|
{
|
|
nodes[B3_ROOT_NODE] = b3MakeEmptyNode();
|
|
return;
|
|
}
|
|
|
|
// The sibling takes the parent's position and the pair is freed. This keeps the order,
|
|
// the sibling only moves down.
|
|
int parent = parents[leaf];
|
|
nodes[parent] = nodes[leaf ^ 1];
|
|
b3LinkChildren( tree, parent );
|
|
b3FreePair( tree, leaf & ~1 );
|
|
|
|
// Update ancestors.
|
|
int index = parents[parent];
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
nodes[index] = b3MakeInternalNode( nodes, b3GetLeftChild( nodes + index ) );
|
|
index = parents[index];
|
|
}
|
|
}
|
|
|
|
// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow
|
|
// the node pool.
|
|
int b3CreateTreeProxyInternal( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData, bool markMoved )
|
|
{
|
|
B3_VALIDATE( b3IsValidAABB( aabb ) );
|
|
|
|
int proxyId = b3AllocateProxy( tree );
|
|
|
|
b3TreeProxy* proxy = tree->proxies + proxyId;
|
|
proxy->categoryBits = categoryBits;
|
|
proxy->userData = userData;
|
|
|
|
bool shouldRotate = true;
|
|
b3InsertLeaf( tree, aabb, proxyId, markMoved, shouldRotate );
|
|
|
|
return proxyId;
|
|
}
|
|
|
|
int b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData )
|
|
{
|
|
return b3CreateTreeProxyInternal( tree, aabb, categoryBits, userData, false );
|
|
}
|
|
|
|
void b3DynamicTree_DestroyProxy( b3DynamicTree* tree, int proxyId )
|
|
{
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
|
|
b3RemoveLeaf( tree, proxyId );
|
|
b3FreeProxy( tree, proxyId );
|
|
}
|
|
|
|
int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree )
|
|
{
|
|
return tree->proxyCount;
|
|
}
|
|
|
|
void b3DynamicTree_MoveProxyInternal( b3DynamicTree* tree, int proxyId, b3AABB aabb, bool markMoved )
|
|
{
|
|
B3_VALIDATE( b3IsValidAABB( aabb ) );
|
|
B3_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B3_HUGE );
|
|
B3_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B3_HUGE );
|
|
B3_VALIDATE( aabb.upperBound.z - aabb.lowerBound.z < B3_HUGE );
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
|
|
b3RemoveLeaf( tree, proxyId );
|
|
|
|
bool shouldRotate = false;
|
|
b3InsertLeaf( tree, aabb, proxyId, markMoved, shouldRotate );
|
|
}
|
|
|
|
void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb )
|
|
{
|
|
b3DynamicTree_MoveProxyInternal( tree, proxyId, aabb, false );
|
|
}
|
|
|
|
void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb )
|
|
{
|
|
B3_VALIDATE( b3IsValidAABB( aabb ) );
|
|
B3_VALIDATE( aabb.upperBound.x - aabb.lowerBound.x < B3_HUGE );
|
|
B3_VALIDATE( aabb.upperBound.y - aabb.lowerBound.y < B3_HUGE );
|
|
B3_VALIDATE( aabb.upperBound.z - aabb.lowerBound.z < B3_HUGE );
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
|
|
b3TreeNode* nodes = tree->nodes;
|
|
const int32_t* parents = tree->parents;
|
|
|
|
int index = tree->proxies[proxyId].node;
|
|
b3TreeNode* node = nodes + index;
|
|
B3_ASSERT( b3IsLeaf( node ) );
|
|
|
|
// Caller must ensure this
|
|
B3_VALIDATE( b3AABB_Contains( node->aabb, aabb ) == false );
|
|
|
|
node->aabb = aabb;
|
|
node->flagIndex |= B3_MOVED_NODE;
|
|
|
|
index = parents[index];
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
node = nodes + index;
|
|
bool changed = b3EnlargeAABB( &node->aabb, aabb );
|
|
|
|
// This is marked to ensure the root is marked in this loop or the one below.
|
|
node->flagIndex |= B3_MOVED_NODE;
|
|
|
|
index = parents[index];
|
|
|
|
if ( changed == false )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Mark all the way up to the root.
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
node = nodes + index;
|
|
if ( node->flagIndex & B3_MOVED_NODE )
|
|
{
|
|
// Early out because this ancestor was previously ascended and marked as moved.
|
|
break;
|
|
}
|
|
|
|
node->flagIndex |= B3_MOVED_NODE;
|
|
index = parents[index];
|
|
}
|
|
}
|
|
|
|
void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits )
|
|
{
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
tree->proxies[proxyId].categoryBits = categoryBits;
|
|
}
|
|
|
|
uint64_t b3DynamicTree_GetCategoryBits( b3DynamicTree* tree, int proxyId )
|
|
{
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
return tree->proxies[proxyId].categoryBits;
|
|
}
|
|
|
|
int b3DynamicTree_GetHeight( const b3DynamicTree* tree )
|
|
{
|
|
return b3GetNodeHeight( tree->nodes + B3_ROOT_NODE );
|
|
}
|
|
|
|
// The area ratio is the thing that SAH seeks to minimize. SAH
|
|
// cannot do anything about leaf boxes or the root box. It seeks
|
|
// to minimize the area of all non-root internal nodes. Divide this
|
|
// by the root area to make the metric non-dimensional.
|
|
// So this becomes a meaningful measure of tree quality.
|
|
float b3DynamicTree_GetAreaRatio( const b3DynamicTree* tree )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
const b3TreeNode* nodes = tree->nodes;
|
|
float rootArea = b3Perimeter( nodes[B3_ROOT_NODE].aabb );
|
|
if ( rootArea <= 0.0f )
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
// Free nodes and the empty node are leaf tagged.
|
|
float internalArea = 0.0f;
|
|
int nodeEnd = tree->nodeEnd;
|
|
for ( int i = 2; i < nodeEnd; ++i )
|
|
{
|
|
if ( b3IsLeaf( nodes + i ) == false )
|
|
{
|
|
internalArea += b3Perimeter( nodes[i].aabb );
|
|
}
|
|
}
|
|
|
|
return internalArea / rootArea;
|
|
}
|
|
|
|
b3AABB b3DynamicTree_GetRootBounds( const b3DynamicTree* tree )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return B3_LITERAL( b3AABB ){ b3Vec3_zero, b3Vec3_zero };
|
|
}
|
|
|
|
return tree->nodes[B3_ROOT_NODE].aabb;
|
|
}
|
|
|
|
#if B3_ENABLE_VALIDATION
|
|
|
|
static int b3ValidateSubtree( const b3DynamicTree* tree, int nodeIndex, int* leafCount )
|
|
{
|
|
B3_ASSERT( 0 <= nodeIndex && nodeIndex < tree->nodeEnd );
|
|
const b3TreeNode* node = tree->nodes + nodeIndex;
|
|
B3_ASSERT( b3IsEmptyNode( node ) == false );
|
|
B3_ASSERT( b3IsValidAABB( node->aabb ) );
|
|
|
|
if ( b3IsLeaf( node ) )
|
|
{
|
|
int proxyId = b3GetProxyId( node );
|
|
B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
B3_ASSERT( tree->proxies[proxyId].node == nodeIndex );
|
|
B3_ASSERT( (int32_t)(uint32_t)tree->proxies[proxyId].userData == node->shapeIndex );
|
|
*leafCount += 1;
|
|
return 0;
|
|
}
|
|
|
|
int pair = b3GetLeftChild( node );
|
|
B3_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );
|
|
B3_ASSERT( tree->parents[pair] == nodeIndex );
|
|
B3_ASSERT( tree->parents[pair + 1] == nodeIndex );
|
|
B3_ASSERT( tree->dfsOrdered == false || nodeIndex < pair );
|
|
|
|
const b3TreeNode* c1 = tree->nodes + pair;
|
|
const b3TreeNode* c2 = tree->nodes + pair + 1;
|
|
B3_ASSERT( b3AABB_Contains( node->aabb, c1->aabb ) );
|
|
B3_ASSERT( b3AABB_Contains( node->aabb, c2->aabb ) );
|
|
B3_ASSERT( b3IsNodeMoved( node ) == ( b3IsNodeMoved( c1 ) || b3IsNodeMoved( c2 ) ) );
|
|
|
|
// A bad tree can stack overflow, but that is validation on its own.
|
|
int height1 = b3ValidateSubtree( tree, pair, leafCount );
|
|
int height2 = b3ValidateSubtree( tree, pair + 1, leafCount );
|
|
int height = 1 + b3MaxInt( height1, height2 );
|
|
B3_ASSERT( node->height == height );
|
|
return height;
|
|
}
|
|
|
|
#endif
|
|
|
|
void b3DynamicTree_Validate( const b3DynamicTree* tree )
|
|
{
|
|
#if B3_ENABLE_VALIDATION
|
|
B3_ASSERT( 2 <= tree->nodeEnd && tree->nodeEnd <= tree->nodeCapacity );
|
|
B3_ASSERT( ( tree->nodeEnd & 1 ) == 0 );
|
|
B3_ASSERT( tree->parents[B3_ROOT_NODE] == B3_NULL_INDEX );
|
|
B3_ASSERT( b3IsEmptyNode( tree->nodes + B3_ROOT_NODE + 1 ) );
|
|
|
|
int freePairCount = 0;
|
|
int pair = tree->pairFreeList;
|
|
while ( pair != B3_NULL_INDEX )
|
|
{
|
|
B3_ASSERT( ( pair & 1 ) == 0 && 2 <= pair && pair < tree->nodeEnd );
|
|
B3_ASSERT( b3IsEmptyNode( tree->nodes + pair ) );
|
|
B3_ASSERT( b3IsEmptyNode( tree->nodes + pair + 1 ) );
|
|
pair = tree->parents[pair];
|
|
++freePairCount;
|
|
B3_ASSERT( 2 * freePairCount < tree->nodeEnd );
|
|
}
|
|
|
|
// Validate proxy free list.
|
|
int freeProxyCount = 0;
|
|
int freeIndex = tree->proxyFreeList;
|
|
while ( freeIndex != B3_NULL_INDEX )
|
|
{
|
|
B3_ASSERT( 0 <= freeIndex && freeIndex < tree->proxyCapacity );
|
|
B3_ASSERT( tree->proxies[freeIndex].node == B3_NULL_INDEX );
|
|
freeIndex = tree->proxies[freeIndex].next;
|
|
++freeProxyCount;
|
|
}
|
|
B3_ASSERT( tree->proxyCount + freeProxyCount == tree->proxyCapacity );
|
|
|
|
B3_ASSERT( tree->nodeEnd == 2 * b3MaxInt( tree->proxyCount, 1 ) + 2 * freePairCount );
|
|
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
B3_ASSERT( b3IsEmptyNode( tree->nodes + B3_ROOT_NODE ) );
|
|
return;
|
|
}
|
|
|
|
int leafCount = 0;
|
|
b3ValidateSubtree( tree, B3_ROOT_NODE, &leafCount );
|
|
B3_ASSERT( leafCount == tree->proxyCount );
|
|
|
|
#else
|
|
B3_UNUSED( tree );
|
|
#endif
|
|
}
|
|
|
|
void b3DynamicTree_ValidateNoMoved( const b3DynamicTree* tree )
|
|
{
|
|
#if B3_ENABLE_VALIDATION == 1
|
|
const b3TreeNode* nodes = tree->nodes;
|
|
int nodeEnd = tree->nodeEnd;
|
|
for ( int i = 0; i < nodeEnd; ++i )
|
|
{
|
|
B3_ASSERT( b3IsNodeMoved( nodes + i ) == false );
|
|
}
|
|
#else
|
|
B3_UNUSED( tree );
|
|
#endif
|
|
}
|
|
|
|
int b3DynamicTree_GetByteCount( const b3DynamicTree* tree )
|
|
{
|
|
size_t size = sizeof( b3DynamicTree );
|
|
size += tree->nodeCapacity * sizeof( b3TreeNode );
|
|
size += tree->nodeCapacity * sizeof( int32_t );
|
|
size += tree->proxyCapacity * sizeof( b3TreeProxy );
|
|
size += tree->swapNodes == NULL ? 0 : tree->nodeCapacity * sizeof( b3TreeNode );
|
|
|
|
// leafIndices
|
|
size += tree->rebuildCapacity * sizeof( int32_t );
|
|
// leafNodes
|
|
size += tree->rebuildCapacity * sizeof( b3TreeNode );
|
|
// leafBoxes
|
|
size += tree->rebuildCapacity * sizeof( b3AABB );
|
|
// leafCenters
|
|
size += tree->rebuildCapacity * sizeof( b3Vec3 );
|
|
// binIndices
|
|
size += tree->rebuildCapacity * sizeof( int32_t );
|
|
|
|
return (int)size;
|
|
}
|
|
|
|
B3_FORCE_INLINE bool b3TestCategory( uint64_t categoryBits, uint64_t maskBits, bool requireAllBits )
|
|
{
|
|
return requireAllBits ? ( categoryBits & maskBits ) == maskBits : ( categoryBits & maskBits ) != 0;
|
|
}
|
|
|
|
B3_FORCE_INLINE b3TreeStats b3QueryTreeView( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryCallbackFcn* callback, void* context, bool hasProxies )
|
|
{
|
|
b3TreeStats result = { 0 };
|
|
|
|
if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) )
|
|
{
|
|
return result;
|
|
}
|
|
|
|
const b3TreeNode* nodes = view->nodes;
|
|
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
stack[stackCount++] = b3GetRootPair( nodes );
|
|
|
|
b3AABBV boxv = b3LoadAABBV( &aabb );
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
result.nodeVisits += 1;
|
|
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
const b3TreeNode* node = nodes + pair + i;
|
|
if ( b3OverlapNode( boxv, node ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ( b3IsLeaf( node ) )
|
|
{
|
|
int proxyId = b3GetProxyId( node );
|
|
uint64_t userData = (uint64_t)(uint32_t)node->shapeIndex;
|
|
if ( hasProxies )
|
|
{
|
|
const b3TreeProxy* proxy = view->proxies + proxyId;
|
|
if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
userData = proxy->userData;
|
|
}
|
|
|
|
bool proceed = callback( proxyId, userData, context );
|
|
result.leafVisits += 1;
|
|
|
|
if ( proceed == false )
|
|
{
|
|
return result;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( stackCount < B3_TREE_STACK_SIZE - 1 )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( node );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
B3_FORCE_INLINE float b3DistanceToNodeSqr( b3Vec3 point, const b3TreeNode* node )
|
|
{
|
|
b3Vec3 r = b3Sub( point, b3Clamp( point, node->aabb.lowerBound, node->aabb.upperBound ) );
|
|
return b3Dot( r, r );
|
|
}
|
|
|
|
struct b3QueryClosestItem
|
|
{
|
|
int nodeIndex;
|
|
float distanceToNodeSqr;
|
|
};
|
|
|
|
B3_FORCE_INLINE b3TreeStats b3QueryClosestTreeView( const b3TreeView* view, b3Vec3 point, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr,
|
|
bool hasProxies )
|
|
{
|
|
b3TreeStats result = { 0 };
|
|
|
|
if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) )
|
|
{
|
|
return result;
|
|
}
|
|
|
|
const b3TreeNode* nodes = view->nodes;
|
|
|
|
float minSqr = *minDistanceSqr;
|
|
struct b3QueryClosestItem stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
|
|
int rootPair = b3GetRootPair( nodes );
|
|
struct b3QueryClosestItem seed1 = {
|
|
.nodeIndex = rootPair,
|
|
.distanceToNodeSqr = b3DistanceToNodeSqr( point, nodes + rootPair ),
|
|
};
|
|
|
|
if ( rootPair == B3_ROOT_NODE )
|
|
{
|
|
stack[stackCount++] = seed1;
|
|
}
|
|
else
|
|
{
|
|
struct b3QueryClosestItem seed2 = {
|
|
.nodeIndex = rootPair + 1,
|
|
.distanceToNodeSqr = b3DistanceToNodeSqr( point, nodes + rootPair + 1 ),
|
|
};
|
|
|
|
// Ensure we iterate the closest child first as we pop off the stack
|
|
if ( seed2.distanceToNodeSqr < seed1.distanceToNodeSqr )
|
|
{
|
|
stack[stackCount++] = seed1;
|
|
stack[stackCount++] = seed2;
|
|
}
|
|
else
|
|
{
|
|
stack[stackCount++] = seed2;
|
|
stack[stackCount++] = seed1;
|
|
}
|
|
}
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
struct b3QueryClosestItem item = stack[--stackCount];
|
|
const b3TreeNode* node = nodes + item.nodeIndex;
|
|
result.nodeVisits += 1;
|
|
|
|
if ( item.distanceToNodeSqr >= minSqr )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ( b3IsLeaf( node ) )
|
|
{
|
|
int proxyId = b3GetProxyId( node );
|
|
uint64_t userData = (uint64_t)(uint32_t)node->shapeIndex;
|
|
if ( hasProxies )
|
|
{
|
|
const b3TreeProxy* proxy = view->proxies + proxyId;
|
|
if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
userData = proxy->userData;
|
|
}
|
|
|
|
// callback to user code with minimum distance squared so far and proxy id
|
|
float dd = callback( minSqr, proxyId, userData, context );
|
|
|
|
if ( dd < minSqr )
|
|
{
|
|
minSqr = dd;
|
|
}
|
|
|
|
result.leafVisits += 1;
|
|
continue;
|
|
}
|
|
|
|
if ( stackCount + 2 > B3_TREE_STACK_SIZE )
|
|
{
|
|
B3_ASSERT( stackCount + 2 <= B3_TREE_STACK_SIZE );
|
|
continue;
|
|
}
|
|
|
|
int pair = b3GetLeftChild( node );
|
|
|
|
// Store the distance to node in the stack instead of recomputing after pop
|
|
struct b3QueryClosestItem item1 = {
|
|
.nodeIndex = pair,
|
|
.distanceToNodeSqr = b3DistanceToNodeSqr( point, nodes + pair ),
|
|
};
|
|
|
|
struct b3QueryClosestItem item2 = {
|
|
.nodeIndex = pair + 1,
|
|
.distanceToNodeSqr = b3DistanceToNodeSqr( point, nodes + pair + 1 ),
|
|
};
|
|
|
|
// Ensure we iterate the closest child first as we pop off the stack
|
|
if ( item2.distanceToNodeSqr < item1.distanceToNodeSqr )
|
|
{
|
|
stack[stackCount++] = item1;
|
|
stack[stackCount++] = item2;
|
|
}
|
|
else
|
|
{
|
|
stack[stackCount++] = item2;
|
|
stack[stackCount++] = item1;
|
|
}
|
|
}
|
|
|
|
*minDistanceSqr = minSqr;
|
|
|
|
return result;
|
|
}
|
|
|
|
// A lot of optimization work went into this. The children of a popped pair are both tested
|
|
// before either is followed, then the survivors are ordered so the closer one narrows first.
|
|
B3_FORCE_INLINE b3TreeStats b3RayCastTreeView( const b3TreeView* view, const b3RayCastInput* input, uint64_t maskBits,
|
|
bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context,
|
|
bool hasProxies )
|
|
{
|
|
b3TreeStats result = { 0 };
|
|
|
|
if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) )
|
|
{
|
|
return result;
|
|
}
|
|
|
|
b3Vec3 p1 = input->origin;
|
|
b3Vec3 d = input->translation;
|
|
|
|
b3V32 pv1 = b3LoadV( &p1.x );
|
|
b3V32 dv = b3LoadV( &d.x );
|
|
|
|
float maxFraction = input->maxFraction;
|
|
|
|
b3Vec3 p2 = b3MulAdd( p1, maxFraction, d );
|
|
|
|
// Build a bounding box for the segment.
|
|
b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) };
|
|
b3AABBV boxv = b3LoadAABBV( &segmentAABB );
|
|
|
|
const b3TreeNode* nodes = view->nodes;
|
|
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
stack[stackCount++] = b3GetRootPair( nodes );
|
|
|
|
b3RayCastInput subInput = *input;
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
result.nodeVisits += 1;
|
|
|
|
const b3TreeNode* hit[2];
|
|
bool isLeaf[2];
|
|
int hitCount = 0;
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
const b3TreeNode* node = nodes + pair + i;
|
|
|
|
if ( b3OverlapNode( boxv, node ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
b3AABB nodeAABB = node->aabb;
|
|
b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x );
|
|
b3V32 upper = b3LoadV( &nodeAABB.upperBound.x );
|
|
if ( b3TestBoundsRayOverlap( lower, upper, pv1, dv ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
isLeaf[hitCount] = b3IsLeaf( node );
|
|
hit[hitCount] = node;
|
|
hitCount += 1;
|
|
}
|
|
|
|
if ( hitCount == 2 && isLeaf[0] == false && isLeaf[1] == false )
|
|
{
|
|
b3Vec3 center1 = b3AABB_Center( hit[0]->aabb );
|
|
b3Vec3 center2 = b3AABB_Center( hit[1]->aabb );
|
|
float d1 = b3DistanceSquared( center1, p1 );
|
|
float d2 = b3DistanceSquared( center2, p1 );
|
|
|
|
// Want to push the closest one last. Both have the same isLeaf, so they don't swap.
|
|
if ( d1 < d2 )
|
|
{
|
|
B3_SWAP( hit[0], hit[1] );
|
|
}
|
|
}
|
|
|
|
for ( int i = 0; i < hitCount; ++i )
|
|
{
|
|
if ( isLeaf[i] )
|
|
{
|
|
int proxyId = b3GetProxyId( hit[i] );
|
|
uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex;
|
|
if ( hasProxies )
|
|
{
|
|
const b3TreeProxy* proxy = view->proxies + proxyId;
|
|
if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
userData = proxy->userData;
|
|
}
|
|
|
|
subInput.maxFraction = maxFraction;
|
|
|
|
float value = callback( &subInput, proxyId, userData, context );
|
|
result.leafVisits += 1;
|
|
|
|
// The user may return -1 to indicate this shape should be skipped
|
|
|
|
if ( value == 0.0f )
|
|
{
|
|
// The client has terminated the ray cast.
|
|
return result;
|
|
}
|
|
|
|
if ( 0.0f < value && value <= maxFraction )
|
|
{
|
|
// Update segment bounding box.
|
|
maxFraction = value;
|
|
p2 = b3MulAdd( p1, maxFraction, d );
|
|
segmentAABB.lowerBound = b3Min( p1, p2 );
|
|
segmentAABB.upperBound = b3Max( p1, p2 );
|
|
boxv = b3LoadAABBV( &segmentAABB );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( stackCount < B3_TREE_STACK_SIZE - 1 )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( hit[i] );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
// Follows structure of ray cast with small tweaks to handle a swept box.
|
|
B3_FORCE_INLINE b3TreeStats b3BoxCastTreeView( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits,
|
|
bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context,
|
|
bool hasProxies )
|
|
{
|
|
b3TreeStats result = { 0 };
|
|
|
|
if ( b3IsEmptyNode( view->nodes + B3_ROOT_NODE ) )
|
|
{
|
|
return result;
|
|
}
|
|
|
|
// The caller folds the shape radius and the world origin into the box
|
|
b3AABB originAABB = input->box;
|
|
|
|
b3Vec3 p1 = b3AABB_Center( originAABB );
|
|
b3Vec3 extension = b3AABB_Extents( originAABB );
|
|
|
|
b3Vec3 d = input->translation;
|
|
|
|
b3V32 pv1 = b3LoadV( &p1.x );
|
|
b3V32 dv = b3LoadV( &d.x );
|
|
b3V32 ev = b3LoadV( &extension.x );
|
|
|
|
float maxFraction = input->maxFraction;
|
|
|
|
// Build total box for the cast
|
|
b3Vec3 t = b3MulSV( maxFraction, input->translation );
|
|
b3AABB totalAABB = {
|
|
b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ),
|
|
b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ),
|
|
};
|
|
b3AABBV boxv = b3LoadAABBV( &totalAABB );
|
|
|
|
const b3TreeNode* nodes = view->nodes;
|
|
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
stack[stackCount++] = b3GetRootPair( nodes );
|
|
|
|
b3BoxCastInput subInput = *input;
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
result.nodeVisits += 1;
|
|
|
|
const b3TreeNode* hit[2];
|
|
bool isLeaf[2];
|
|
int hitCount = 0;
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
const b3TreeNode* node = nodes + pair + i;
|
|
|
|
if ( b3OverlapNode( boxv, node ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// radius extension is added to the node in this case
|
|
b3AABB nodeAABB = node->aabb;
|
|
b3V32 lower = b3SubV( b3LoadV( &nodeAABB.lowerBound.x ), ev );
|
|
b3V32 upper = b3AddV( b3LoadV( &nodeAABB.upperBound.x ), ev );
|
|
if ( b3TestBoundsRayOverlap( lower, upper, pv1, dv ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
isLeaf[hitCount] = b3IsLeaf( node );
|
|
hit[hitCount] = node;
|
|
hitCount += 1;
|
|
}
|
|
|
|
if ( hitCount == 2 && isLeaf[0] == false && isLeaf[1] == false )
|
|
{
|
|
b3Vec3 center1 = b3AABB_Center( hit[0]->aabb );
|
|
b3Vec3 center2 = b3AABB_Center( hit[1]->aabb );
|
|
float d1 = b3DistanceSquared( center1, p1 );
|
|
float d2 = b3DistanceSquared( center2, p1 );
|
|
|
|
// Want to push the closest one last. Both have the same isLeaf, so they don't swap.
|
|
if ( d1 < d2 )
|
|
{
|
|
B3_SWAP( hit[0], hit[1] );
|
|
}
|
|
}
|
|
|
|
for ( int i = 0; i < hitCount; ++i )
|
|
{
|
|
if ( isLeaf[i] )
|
|
{
|
|
int proxyId = b3GetProxyId( hit[i] );
|
|
uint64_t userData = (uint64_t)(uint32_t)hit[i]->shapeIndex;
|
|
if ( hasProxies )
|
|
{
|
|
const b3TreeProxy* proxy = view->proxies + proxyId;
|
|
if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
userData = proxy->userData;
|
|
}
|
|
|
|
subInput.maxFraction = maxFraction;
|
|
|
|
float value = callback( &subInput, proxyId, userData, context );
|
|
result.leafVisits += 1;
|
|
|
|
if ( value == 0.0f )
|
|
{
|
|
// The client has terminated the cast.
|
|
return result;
|
|
}
|
|
|
|
if ( 0.0f < value && value < maxFraction )
|
|
{
|
|
maxFraction = value;
|
|
t = b3MulSV( maxFraction, input->translation );
|
|
totalAABB.lowerBound = b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) );
|
|
totalAABB.upperBound = b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) );
|
|
boxv = b3LoadAABBV( &totalAABB );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ( stackCount < B3_TREE_STACK_SIZE - 1 )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( hit[i] );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryCallbackFcn* callback, void* context )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return B3_LITERAL( b3TreeStats ){ 0 };
|
|
}
|
|
|
|
b3TreeView view = b3MakeTreeView( tree );
|
|
return b3QueryTreeView( &view, aabb, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return B3_LITERAL( b3TreeStats ){ 0 };
|
|
}
|
|
|
|
b3TreeView view = b3MakeTreeView( tree );
|
|
return b3QueryClosestTreeView( &view, point, maskBits, requireAllBits, callback, context, minDistanceSqr, true );
|
|
}
|
|
|
|
b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeRayCastCallbackFcn* callback, void* context )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return B3_LITERAL( b3TreeStats ){ 0 };
|
|
}
|
|
|
|
b3TreeView view = b3MakeTreeView( tree );
|
|
return b3RayCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeBoxCastCallbackFcn* callback, void* context )
|
|
{
|
|
if ( tree->proxyCount == 0 )
|
|
{
|
|
return B3_LITERAL( b3TreeStats ){ 0 };
|
|
}
|
|
|
|
b3TreeView view = b3MakeTreeView( tree );
|
|
return b3BoxCastTreeView( &view, input, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
b3TreeStats b3TreeView_Query( const b3TreeView* view, b3AABB aabb, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryCallbackFcn* callback, void* context )
|
|
{
|
|
if ( view->proxies == NULL )
|
|
{
|
|
return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, false );
|
|
}
|
|
|
|
return b3QueryTreeView( view, aabb, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
b3TreeStats b3TreeView_QueryClosest( const b3TreeView* view, b3Vec3 point, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeQueryClosestCallbackFcn* callback, void* context, float* minDistanceSqr )
|
|
{
|
|
if ( view->proxies == NULL )
|
|
{
|
|
return b3QueryClosestTreeView( view, point, maskBits, requireAllBits, callback, context, minDistanceSqr, false );
|
|
}
|
|
|
|
return b3QueryClosestTreeView( view, point, maskBits, requireAllBits, callback, context, minDistanceSqr, true );
|
|
}
|
|
|
|
b3TreeStats b3TreeView_RayCast( const b3TreeView* view, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeRayCastCallbackFcn* callback, void* context )
|
|
{
|
|
if ( view->proxies == NULL )
|
|
{
|
|
return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, false );
|
|
}
|
|
|
|
return b3RayCastTreeView( view, input, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
b3TreeStats b3TreeView_BoxCast( const b3TreeView* view, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits,
|
|
b3TreeBoxCastCallbackFcn* callback, void* context )
|
|
{
|
|
if ( view->proxies == NULL )
|
|
{
|
|
return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, false );
|
|
}
|
|
|
|
return b3BoxCastTreeView( view, input, maskBits, requireAllBits, callback, context, true );
|
|
}
|
|
|
|
// Median split == 0, Surface area heuristic == 1
|
|
#define B3_TREE_HEURISTIC 0
|
|
|
|
#if B3_TREE_HEURISTIC == 0
|
|
|
|
// Median split heuristic
|
|
static int b3PartitionMid( int* indices, b3Vec3* centers, int count )
|
|
{
|
|
// Handle trivial case
|
|
if ( count <= 2 )
|
|
{
|
|
return count / 2;
|
|
}
|
|
|
|
b3Vec3 lowerBound = centers[0];
|
|
b3Vec3 upperBound = centers[0];
|
|
|
|
for ( int i = 1; i < count; ++i )
|
|
{
|
|
lowerBound = b3Min( lowerBound, centers[i] );
|
|
upperBound = b3Max( upperBound, centers[i] );
|
|
}
|
|
|
|
b3Vec3 d = b3Sub( upperBound, lowerBound );
|
|
b3Vec3 c = b3MulSV( 0.5f, b3Add( lowerBound, upperBound ) );
|
|
|
|
// Partition longest axis using the Hoare partition scheme
|
|
// https://en.wikipedia.org/wiki/Quicksort
|
|
// https://nicholasvadivelu.com/2021/01/11/array-partition/
|
|
int i1 = 0, i2 = count;
|
|
if ( d.x >= d.y && d.x >= d.z )
|
|
{
|
|
float pivot = c.x;
|
|
|
|
while ( i1 < i2 )
|
|
{
|
|
while ( i1 < i2 && centers[i1].x < pivot )
|
|
{
|
|
i1 += 1;
|
|
};
|
|
|
|
while ( i1 < i2 && centers[i2 - 1].x >= pivot )
|
|
{
|
|
i2 -= 1;
|
|
};
|
|
|
|
if ( i1 < i2 )
|
|
{
|
|
// Swap indices
|
|
{
|
|
int temp = indices[i1];
|
|
indices[i1] = indices[i2 - 1];
|
|
indices[i2 - 1] = temp;
|
|
}
|
|
|
|
// Swap centers
|
|
{
|
|
b3Vec3 temp = centers[i1];
|
|
centers[i1] = centers[i2 - 1];
|
|
centers[i2 - 1] = temp;
|
|
}
|
|
|
|
i1 += 1;
|
|
i2 -= 1;
|
|
}
|
|
}
|
|
}
|
|
else if ( d.y >= d.z )
|
|
{
|
|
float pivot = c.y;
|
|
|
|
while ( i1 < i2 )
|
|
{
|
|
while ( i1 < i2 && centers[i1].y < pivot )
|
|
{
|
|
i1 += 1;
|
|
};
|
|
|
|
while ( i1 < i2 && centers[i2 - 1].y >= pivot )
|
|
{
|
|
i2 -= 1;
|
|
};
|
|
|
|
if ( i1 < i2 )
|
|
{
|
|
// Swap indices
|
|
{
|
|
int temp = indices[i1];
|
|
indices[i1] = indices[i2 - 1];
|
|
indices[i2 - 1] = temp;
|
|
}
|
|
|
|
// Swap centers
|
|
{
|
|
b3Vec3 temp = centers[i1];
|
|
centers[i1] = centers[i2 - 1];
|
|
centers[i2 - 1] = temp;
|
|
}
|
|
|
|
i1 += 1;
|
|
i2 -= 1;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float pivot = c.z;
|
|
|
|
while ( i1 < i2 )
|
|
{
|
|
while ( i1 < i2 && centers[i1].z < pivot )
|
|
{
|
|
i1 += 1;
|
|
};
|
|
|
|
while ( i1 < i2 && centers[i2 - 1].z >= pivot )
|
|
{
|
|
i2 -= 1;
|
|
};
|
|
|
|
if ( i1 < i2 )
|
|
{
|
|
// Swap indices
|
|
{
|
|
int temp = indices[i1];
|
|
indices[i1] = indices[i2 - 1];
|
|
indices[i2 - 1] = temp;
|
|
}
|
|
|
|
// Swap centers
|
|
{
|
|
b3Vec3 temp = centers[i1];
|
|
centers[i1] = centers[i2 - 1];
|
|
centers[i2 - 1] = temp;
|
|
}
|
|
|
|
i1 += 1;
|
|
i2 -= 1;
|
|
}
|
|
}
|
|
}
|
|
B3_ASSERT( i1 == i2 );
|
|
|
|
if ( i1 > 0 && i1 < count )
|
|
{
|
|
return i1;
|
|
}
|
|
|
|
return count / 2;
|
|
}
|
|
|
|
#else
|
|
|
|
#define B3_BIN_COUNT 8
|
|
|
|
typedef struct b3TreeBin
|
|
{
|
|
b3AABB aabb;
|
|
int count;
|
|
} b3TreeBin;
|
|
|
|
typedef struct b3TreePlane
|
|
{
|
|
b3AABB leftAABB;
|
|
b3AABB rightAABB;
|
|
int leftCount;
|
|
int rightCount;
|
|
} b3TreePlane;
|
|
|
|
// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald
|
|
// Returns the left child count
|
|
static int b3PartitionSAH( int* indices, int* binIndices, b3AABB* boxes, int count )
|
|
{
|
|
B3_ASSERT( count > 0 );
|
|
|
|
b3TreeBin bins[B3_BIN_COUNT];
|
|
b3TreePlane planes[B3_BIN_COUNT - 1];
|
|
|
|
b3Vec3 center = b3AABB_Center( boxes[0] );
|
|
b3AABB centroidAABB;
|
|
centroidAABB.lowerBound = center;
|
|
centroidAABB.upperBound = center;
|
|
|
|
for ( int i = 1; i < count; ++i )
|
|
{
|
|
center = b3AABB_Center( boxes[i] );
|
|
centroidAABB.lowerBound = b3Min( centroidAABB.lowerBound, center );
|
|
centroidAABB.upperBound = b3Max( centroidAABB.upperBound, center );
|
|
}
|
|
|
|
b3Vec3 d = b3Sub( centroidAABB.upperBound, centroidAABB.lowerBound );
|
|
|
|
// Find longest axis
|
|
int axisIndex;
|
|
float invD;
|
|
if ( d.x > d.y )
|
|
{
|
|
axisIndex = 0;
|
|
invD = d.x;
|
|
}
|
|
else
|
|
{
|
|
axisIndex = 1;
|
|
invD = d.y;
|
|
}
|
|
|
|
invD = invD > 0.0f ? 1.0f / invD : 0.0f;
|
|
|
|
// Initialize bin bounds and count
|
|
for ( int i = 0; i < B3_BIN_COUNT; ++i )
|
|
{
|
|
bins[i].aabb.lowerBound = (b3Vec3){ FLT_MAX, FLT_MAX };
|
|
bins[i].aabb.upperBound = (b3Vec3){ -FLT_MAX, -FLT_MAX };
|
|
bins[i].count = 0;
|
|
}
|
|
|
|
// Assign boxes to bins and compute bin boxes
|
|
// TODO_ERIN optimize
|
|
float binCount = B3_BIN_COUNT;
|
|
float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y };
|
|
float minC = lowerBoundArray[axisIndex];
|
|
for ( int i = 0; i < count; ++i )
|
|
{
|
|
b3Vec3 c = b3AABB_Center( boxes[i] );
|
|
float cArray[2] = { c.x, c.y };
|
|
int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD );
|
|
binIndex = b3ClampInt( binIndex, 0, B3_BIN_COUNT - 1 );
|
|
binIndices[i] = binIndex;
|
|
bins[binIndex].count += 1;
|
|
bins[binIndex].aabb = b3AABB_Union( bins[binIndex].aabb, boxes[i] );
|
|
}
|
|
|
|
int planeCount = B3_BIN_COUNT - 1;
|
|
|
|
// Prepare all the left planes, candidates for left child
|
|
planes[0].leftCount = bins[0].count;
|
|
planes[0].leftAABB = bins[0].aabb;
|
|
for ( int i = 1; i < planeCount; ++i )
|
|
{
|
|
planes[i].leftCount = planes[i - 1].leftCount + bins[i].count;
|
|
planes[i].leftAABB = b3AABB_Union( planes[i - 1].leftAABB, bins[i].aabb );
|
|
}
|
|
|
|
// Prepare all the right planes, candidates for right child
|
|
planes[planeCount - 1].rightCount = bins[planeCount].count;
|
|
planes[planeCount - 1].rightAABB = bins[planeCount].aabb;
|
|
for ( int i = planeCount - 2; i >= 0; --i )
|
|
{
|
|
planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count;
|
|
planes[i].rightAABB = b3AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb );
|
|
}
|
|
|
|
// Find best split to minimize SAH
|
|
float minCost = FLT_MAX;
|
|
int bestPlane = 0;
|
|
for ( int i = 0; i < planeCount; ++i )
|
|
{
|
|
float leftArea = b3Perimeter( planes[i].leftAABB );
|
|
float rightArea = b3Perimeter( planes[i].rightAABB );
|
|
int leftCount = planes[i].leftCount;
|
|
int rightCount = planes[i].rightCount;
|
|
|
|
float cost = leftCount * leftArea + rightCount * rightArea;
|
|
if ( cost < minCost )
|
|
{
|
|
bestPlane = i;
|
|
minCost = cost;
|
|
}
|
|
}
|
|
|
|
// Partition node indices and boxes using the Hoare partition scheme
|
|
// https://en.wikipedia.org/wiki/Quicksort
|
|
// https://nicholasvadivelu.com/2021/01/11/array-partition/
|
|
int i1 = 0, i2 = count;
|
|
while ( i1 < i2 )
|
|
{
|
|
while ( i1 < i2 && binIndices[i1] < bestPlane )
|
|
{
|
|
i1 += 1;
|
|
};
|
|
|
|
while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane )
|
|
{
|
|
i2 -= 1;
|
|
};
|
|
|
|
if ( i1 < i2 )
|
|
{
|
|
// Swap indices
|
|
{
|
|
int temp = indices[i1];
|
|
indices[i1] = indices[i2 - 1];
|
|
indices[i2 - 1] = temp;
|
|
}
|
|
|
|
// Swap boxes
|
|
{
|
|
b3AABB temp = boxes[i1];
|
|
boxes[i1] = boxes[i2 - 1];
|
|
boxes[i2 - 1] = temp;
|
|
}
|
|
|
|
i1 += 1;
|
|
i2 -= 1;
|
|
}
|
|
}
|
|
B3_ASSERT( i1 == i2 );
|
|
|
|
if ( i1 > 0 && i1 < count )
|
|
{
|
|
return i1;
|
|
}
|
|
else
|
|
{
|
|
return count / 2;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
// Temporary data used to track the rebuild of a tree node.
|
|
typedef struct b3RebuildItem
|
|
{
|
|
// Where this node is written and the pair its children go in.
|
|
int nodeIndex;
|
|
int pair;
|
|
int childCount;
|
|
|
|
// Leaf indices
|
|
int startIndex;
|
|
int splitIndex;
|
|
int endIndex;
|
|
} b3RebuildItem;
|
|
|
|
typedef struct b3CopyItem
|
|
{
|
|
int oldPair;
|
|
int newIndex;
|
|
} b3CopyItem;
|
|
|
|
// Bump allocate a pair of sibling nodes. Returns index to the first one.
|
|
static inline int b3BumpPair( b3DynamicTree* tree, int parent )
|
|
{
|
|
int pair = tree->nodeEnd;
|
|
B3_ASSERT( pair + 2 <= tree->nodeCapacity );
|
|
tree->nodeEnd += 2;
|
|
tree->parents[pair] = parent;
|
|
tree->parents[pair + 1] = parent;
|
|
return pair;
|
|
}
|
|
|
|
static inline void b3SetLeftChild( b3TreeNode* node, int pair )
|
|
{
|
|
node->flagIndex = ( node->flagIndex & ~B3_NODE_INDEX_MASK ) | (uint32_t)pair;
|
|
}
|
|
|
|
// Copy a retained subtree from the old array into the rebuilt DFS array using
|
|
// a bump allocator.
|
|
static void b3CopySubtree( b3DynamicTree* tree, b3TreeNode node, int newIndex )
|
|
{
|
|
const b3TreeNode* oldNodes = tree->nodes;
|
|
b3TreeNode* newNodes = tree->swapNodes;
|
|
b3TreeProxy* proxies = tree->proxies;
|
|
|
|
if ( b3IsLeaf( &node ) )
|
|
{
|
|
newNodes[newIndex] = node;
|
|
proxies[b3GetProxyId( &node )].node = newIndex;
|
|
return;
|
|
}
|
|
|
|
b3CopyItem stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
|
|
int oldPair = b3GetLeftChild( &node );
|
|
int newPair = b3BumpPair( tree, newIndex );
|
|
b3SetLeftChild( &node, newPair );
|
|
|
|
// Copy the subtree root. This can be a left or right sibling (even or odd).
|
|
newNodes[newIndex] = node;
|
|
|
|
for ( ;; )
|
|
{
|
|
b3TreeNode* pair = newNodes + newPair;
|
|
|
|
// Copy the siblings.
|
|
pair[0] = oldNodes[oldPair];
|
|
pair[1] = oldNodes[oldPair + 1];
|
|
|
|
if ( b3IsLeaf( pair + 1 ) )
|
|
{
|
|
// Hook up proxy on right sibling.
|
|
int proxyId = b3GetProxyId( pair + 1 );
|
|
proxies[proxyId].node = newPair + 1;
|
|
}
|
|
else
|
|
{
|
|
// Push the left child of the right sibling.
|
|
if ( stackCount < B3_TREE_STACK_SIZE )
|
|
{
|
|
int leftChild = b3GetLeftChild( pair + 1 );
|
|
stack[stackCount++] = B3_LITERAL( b3CopyItem ){ leftChild, newPair + 1 };
|
|
}
|
|
else
|
|
{
|
|
// todo fail gracefully if the stack runs out in release
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE );
|
|
}
|
|
}
|
|
|
|
if ( b3IsLeaf( pair ) == false )
|
|
{
|
|
// Descend into left subtree.
|
|
int leftIndex = newPair;
|
|
oldPair = b3GetLeftChild( pair );
|
|
newPair = b3BumpPair( tree, leftIndex );
|
|
b3SetLeftChild( newNodes + leftIndex, newPair );
|
|
continue;
|
|
}
|
|
|
|
// Hook up proxy on left sibling.
|
|
proxies[b3GetProxyId( pair )].node = newPair;
|
|
|
|
if ( stackCount == 0 )
|
|
{
|
|
break;
|
|
}
|
|
|
|
// Descend a right subtree that was previously pushed.
|
|
b3CopyItem item = stack[--stackCount];
|
|
oldPair = item.oldPair;
|
|
newPair = b3BumpPair( tree, item.newIndex );
|
|
b3SetLeftChild( newNodes + item.newIndex, newPair );
|
|
}
|
|
}
|
|
|
|
static void b3PlaceLeaf( b3DynamicTree* tree, b3TreeNode leaf, int newIndex )
|
|
{
|
|
if ( b3IsLeaf( &leaf ) )
|
|
{
|
|
tree->swapNodes[newIndex] = leaf;
|
|
tree->proxies[b3GetProxyId( &leaf )].node = newIndex;
|
|
}
|
|
else
|
|
{
|
|
b3CopySubtree( tree, leaf, newIndex );
|
|
}
|
|
}
|
|
|
|
static void b3BuildTree( b3DynamicTree* tree, int leafCount )
|
|
{
|
|
b3TreeNode* nodes = tree->swapNodes;
|
|
const b3TreeNode* leaves = tree->leafNodes;
|
|
int* leafIndices = tree->leafIndices;
|
|
|
|
#if B3_TREE_HEURISTIC == 0
|
|
b3Vec3* leafCenters = tree->leafCenters;
|
|
#else
|
|
b3AABB* leafBoxes = tree->leafBoxes;
|
|
int* binIndices = tree->binIndices;
|
|
#endif
|
|
|
|
// Bump allocation into the swap nodes.
|
|
tree->nodeEnd = 2;
|
|
nodes[B3_ROOT_NODE + 1] = b3MakeEmptyNode();
|
|
tree->parents[B3_ROOT_NODE] = B3_NULL_INDEX;
|
|
tree->parents[B3_ROOT_NODE + 1] = B3_NULL_INDEX;
|
|
|
|
if ( leafCount == 1 )
|
|
{
|
|
b3PlaceLeaf( tree, leaves[leafIndices[0]], B3_ROOT_NODE );
|
|
return;
|
|
}
|
|
|
|
b3RebuildItem stack[B3_TREE_STACK_SIZE];
|
|
int top = 0;
|
|
|
|
stack[0].nodeIndex = B3_ROOT_NODE;
|
|
stack[0].pair = b3BumpPair( tree, B3_ROOT_NODE );
|
|
stack[0].childCount = -1;
|
|
stack[0].startIndex = 0;
|
|
stack[0].endIndex = leafCount;
|
|
#if B3_TREE_HEURISTIC == 0
|
|
stack[0].splitIndex = b3PartitionMid( leafIndices, leafCenters, leafCount );
|
|
#else
|
|
stack[0].splitIndex = b3PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount );
|
|
#endif
|
|
|
|
while ( true )
|
|
{
|
|
b3RebuildItem* item = stack + top;
|
|
item->childCount += 1;
|
|
|
|
if ( item->childCount == 2 )
|
|
{
|
|
// Both children written, so the parent node can be finalized.
|
|
nodes[item->nodeIndex] = b3MakeInternalNode( nodes, item->pair );
|
|
if ( top == 0 )
|
|
{
|
|
break;
|
|
}
|
|
|
|
top -= 1;
|
|
continue;
|
|
}
|
|
|
|
int slot = item->childCount;
|
|
int startIndex = slot == 0 ? item->startIndex : item->splitIndex;
|
|
int endIndex = slot == 0 ? item->splitIndex : item->endIndex;
|
|
int count = endIndex - startIndex;
|
|
B3_ASSERT( count > 0 );
|
|
|
|
int nodeIndex = item->pair + slot;
|
|
if ( count == 1 )
|
|
{
|
|
b3PlaceLeaf( tree, leaves[leafIndices[startIndex]], nodeIndex );
|
|
continue;
|
|
}
|
|
|
|
// todo fail gracefully if the stack runs out in release
|
|
B3_ASSERT( top < B3_TREE_STACK_SIZE - 1 );
|
|
|
|
top += 1;
|
|
b3RebuildItem* newItem = stack + top;
|
|
newItem->nodeIndex = nodeIndex;
|
|
newItem->pair = b3BumpPair( tree, nodeIndex );
|
|
newItem->childCount = -1;
|
|
newItem->startIndex = startIndex;
|
|
newItem->endIndex = endIndex;
|
|
newItem->splitIndex = startIndex;
|
|
#if B3_TREE_HEURISTIC == 0
|
|
newItem->splitIndex += b3PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count );
|
|
#else
|
|
newItem->splitIndex += b3PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count );
|
|
#endif
|
|
}
|
|
}
|
|
|
|
// Rebuild the stale parts of the tree. The entire tree is put into DFS order. This makes
|
|
// refitting much faster. This is done async with threading, so the cost is hidden.
|
|
// Not safe to access tree during this operation.
|
|
int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild )
|
|
{
|
|
int proxyCount = tree->proxyCount;
|
|
if ( proxyCount == 0 )
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
// An unordered tree is rebuilt even when nothing moved, the sweep refit needs the order
|
|
const b3TreeNode* nodes = tree->nodes;
|
|
const b3TreeNode* root = nodes + B3_ROOT_NODE;
|
|
if ( fullBuild == false && b3IsNodeMoved( root ) == false && tree->dfsOrdered )
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
if ( tree->swapNodes == NULL )
|
|
{
|
|
tree->swapNodes = b3AllocZero( tree->nodeCapacity * sizeof( b3TreeNode ) );
|
|
}
|
|
|
|
if ( proxyCount > tree->rebuildCapacity )
|
|
{
|
|
int oldCapacity = tree->rebuildCapacity;
|
|
int newCapacity = proxyCount + proxyCount / 2;
|
|
|
|
b3Free( tree->leafIndices, oldCapacity * sizeof( int32_t ) );
|
|
tree->leafIndices = (int32_t*)b3Alloc( newCapacity * sizeof( int32_t ) );
|
|
|
|
b3Free( tree->leafNodes, oldCapacity * sizeof( b3TreeNode ) );
|
|
tree->leafNodes = (b3TreeNode*)b3Alloc( newCapacity * sizeof( b3TreeNode ) );
|
|
|
|
#if B3_TREE_HEURISTIC == 0
|
|
b3Free( tree->leafCenters, oldCapacity * sizeof( b3Vec3 ) );
|
|
tree->leafCenters = (b3Vec3*)b3Alloc( newCapacity * sizeof( b3Vec3 ) );
|
|
#else
|
|
b3Free( tree->leafBoxes, oldCapacity * sizeof( b3AABB ) );
|
|
tree->leafBoxes = (b3AABB*)b3Alloc( newCapacity * sizeof( b3AABB ) );
|
|
b3Free( tree->binIndices, oldCapacity * sizeof( int32_t ) );
|
|
tree->binIndices = (int32_t*)b3Alloc( newCapacity * sizeof( int32_t ) );
|
|
#endif
|
|
tree->rebuildCapacity = newCapacity;
|
|
}
|
|
|
|
int* leafIndices = tree->leafIndices;
|
|
b3TreeNode* leaves = tree->leafNodes;
|
|
#if B3_TREE_HEURISTIC == 0
|
|
b3Vec3* leafCenters = tree->leafCenters;
|
|
#else
|
|
b3AABB* leafBoxes = tree->leafBoxes;
|
|
#endif
|
|
|
|
// Gather build leaves. A marked internal node is descended and abandoned in the old array.
|
|
// Everything else is a build leaf, kept subtrees included. A leaf root, or an unmarked root
|
|
// on an unordered array, is the one build leaf.
|
|
int leafCount = 0;
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
if ( b3IsLeaf( root ) == false && ( fullBuild || b3IsNodeMoved( root ) ) )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( root );
|
|
}
|
|
else
|
|
{
|
|
b3TreeNode node = *root;
|
|
if ( fullBuild == false )
|
|
{
|
|
node.flagIndex &= ~B3_MOVED_NODE;
|
|
}
|
|
leafIndices[0] = 0;
|
|
leaves[0] = node;
|
|
#if B3_TREE_HEURISTIC == 0
|
|
leafCenters[0] = b3AABB_Center( node.aabb );
|
|
#else
|
|
leafBoxes[0] = node.aabb;
|
|
#endif
|
|
leafCount = 1;
|
|
}
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
b3TreeNode node = nodes[pair + i];
|
|
if ( b3IsLeaf( &node ) == false && ( fullBuild || b3IsNodeMoved( &node ) ) )
|
|
{
|
|
// todo fail gracefully if the stack runs out in release
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE );
|
|
stack[stackCount++] = b3GetLeftChild( &node );
|
|
continue;
|
|
}
|
|
|
|
if ( fullBuild == false )
|
|
{
|
|
node.flagIndex &= ~B3_MOVED_NODE;
|
|
}
|
|
leafIndices[leafCount] = leafCount;
|
|
leaves[leafCount] = node;
|
|
#if B3_TREE_HEURISTIC == 0
|
|
leafCenters[leafCount] = b3AABB_Center( node.aabb );
|
|
#else
|
|
leafBoxes[leafCount] = node.aabb;
|
|
#endif
|
|
leafCount += 1;
|
|
}
|
|
}
|
|
|
|
B3_ASSERT( 0 < leafCount && leafCount <= proxyCount );
|
|
|
|
b3BuildTree( tree, leafCount );
|
|
|
|
// The spare is now the tree and the old array is the spare. The build is dense, so the
|
|
// stale tail of the old array sits above the end and is never read.
|
|
B3_SWAP( tree->nodes, tree->swapNodes );
|
|
tree->pairFreeList = B3_NULL_INDEX;
|
|
tree->dfsOrdered = true;
|
|
|
|
b3DynamicTree_Validate( tree );
|
|
if ( fullBuild == false )
|
|
{
|
|
b3DynamicTree_ValidateNoMoved( tree );
|
|
}
|
|
|
|
return leafCount;
|
|
}
|
|
|
|
// Set the moved flag on the ancestors of the proxy. Serial use case.
|
|
void b3DynamicTree_MarkProxyMovedSerial( b3DynamicTree* tree, int proxyId )
|
|
{
|
|
B3_VALIDATE( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
|
|
b3TreeNode* nodes = tree->nodes;
|
|
const int32_t* parents = tree->parents;
|
|
|
|
int index = tree->proxies[proxyId].node;
|
|
B3_VALIDATE( 0 <= index && index < tree->nodeEnd );
|
|
B3_VALIDATE( b3IsLeaf( nodes + index ) );
|
|
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
nodes[index].flagIndex |= B3_MOVED_NODE;
|
|
index = parents[index];
|
|
}
|
|
}
|
|
|
|
// Update a proxy AABB and flag the ancestors as moved. Thread-safe using atomics.
|
|
void b3DynamicTree_MarkProxyMoved( b3DynamicTree* tree, int proxyId, b3AABB aabb )
|
|
{
|
|
B3_VALIDATE( 0 <= proxyId && proxyId < tree->proxyCapacity );
|
|
|
|
b3TreeNode* nodes = tree->nodes;
|
|
const int32_t* parents = tree->parents;
|
|
|
|
int index = tree->proxies[proxyId].node;
|
|
B3_VALIDATE( 0 <= index && index < tree->nodeEnd );
|
|
|
|
b3TreeNode* node = nodes + index;
|
|
B3_VALIDATE( b3IsLeaf( node ) );
|
|
B3_VALIDATE( b3AABB_Contains( node->aabb, aabb ) == false );
|
|
|
|
// This is not raced since it is the leaf.
|
|
node->aabb = aabb;
|
|
node->flagIndex |= B3_MOVED_NODE;
|
|
|
|
index = parents[index];
|
|
while ( index != B3_NULL_INDEX )
|
|
{
|
|
node = nodes + index;
|
|
|
|
// Read first to avoid the FetchOr if possible.
|
|
if ( b3AtomicLoadU32Raw( &node->flagIndex ) & B3_MOVED_NODE )
|
|
{
|
|
break;
|
|
}
|
|
|
|
uint32_t previousFlags = b3AtomicFetchOrU32( &node->flagIndex, B3_MOVED_NODE );
|
|
if ( previousFlags & B3_MOVED_NODE )
|
|
{
|
|
// Ancestor already visited.
|
|
break;
|
|
}
|
|
|
|
index = parents[index];
|
|
}
|
|
}
|
|
|
|
// Clear the moved flags from the entire tree.
|
|
void b3DynamicTree_ClearMoved( b3DynamicTree* tree )
|
|
{
|
|
b3TreeNode* nodes = tree->nodes;
|
|
|
|
b3TreeNode* root = nodes + B3_ROOT_NODE;
|
|
if ( b3IsNodeMoved( root ) == false )
|
|
{
|
|
return;
|
|
}
|
|
|
|
root->flagIndex &= ~B3_MOVED_NODE;
|
|
if ( b3IsLeaf( root ) )
|
|
{
|
|
return;
|
|
}
|
|
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
stack[stackCount++] = b3GetLeftChild( root );
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
b3TreeNode* node = nodes + pair + i;
|
|
if ( node->flagIndex & B3_MOVED_NODE )
|
|
{
|
|
node->flagIndex &= ~B3_MOVED_NODE;
|
|
if ( b3IsLeaf( node ) == false )
|
|
{
|
|
if ( stackCount < B3_TREE_STACK_SIZE )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( node );
|
|
}
|
|
else
|
|
{
|
|
// Bad stuff will happen if the moved flags don't get cleared.
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int b3DynamicTree_GatherMovedProxies( const b3DynamicTree* tree, int* proxyIds )
|
|
{
|
|
const b3TreeNode* nodes = tree->nodes;
|
|
const b3TreeNode* root = nodes + B3_ROOT_NODE;
|
|
if ( b3IsNodeMoved( root ) == false )
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
if ( b3IsLeaf( root ) )
|
|
{
|
|
proxyIds[0] = b3GetProxyId( root );
|
|
return 1;
|
|
}
|
|
|
|
int count = 0;
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
stack[stackCount++] = b3GetLeftChild( root );
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int pair = stack[--stackCount];
|
|
for ( int i = 0; i < 2; ++i )
|
|
{
|
|
const b3TreeNode* node = nodes + pair + i;
|
|
if ( b3IsNodeMoved( node ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ( b3IsLeaf( node ) )
|
|
{
|
|
proxyIds[count++] = b3GetProxyId( node );
|
|
}
|
|
else
|
|
{
|
|
if ( stackCount < B3_TREE_STACK_SIZE )
|
|
{
|
|
stack[stackCount++] = b3GetLeftChild( node );
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( stackCount < B3_TREE_STACK_SIZE );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
// Slow refit for unit tests.
|
|
static void b3RefitSubtree( b3TreeNode* nodes, int rootIndex )
|
|
{
|
|
b3TreeNode* root = nodes + rootIndex;
|
|
if ( b3IsLeaf( root ) || b3IsNodeMoved( root ) == false )
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Negative entries indicate deferred unions. A parent is pushed before its children so the
|
|
// LIFO order guarantees both children are finished when the union runs.
|
|
int stack[B3_TREE_STACK_SIZE];
|
|
int stackCount = 0;
|
|
// ~0 == -1
|
|
stack[stackCount++] = ~rootIndex;
|
|
stack[stackCount++] = b3GetLeftChild( root );
|
|
stack[stackCount++] = b3GetLeftChild( root ) + 1;
|
|
|
|
while ( stackCount > 0 )
|
|
{
|
|
int item = stack[--stackCount];
|
|
if ( item < 0 )
|
|
{
|
|
b3TreeNode* node = nodes + ~item;
|
|
int pair = b3GetLeftChild( node );
|
|
node->aabb = b3UnionV( nodes[pair].aabb, nodes[pair + 1].aabb );
|
|
continue;
|
|
}
|
|
|
|
b3TreeNode* node = nodes + item;
|
|
if ( b3IsLeaf( node ) || b3IsNodeMoved( node ) == false )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if ( stackCount + 3 <= B3_TREE_STACK_SIZE )
|
|
{
|
|
int pair = b3GetLeftChild( node );
|
|
stack[stackCount++] = ~item;
|
|
stack[stackCount++] = pair;
|
|
stack[stackCount++] = pair + 1;
|
|
}
|
|
else
|
|
{
|
|
B3_ASSERT( stackCount + 3 <= B3_TREE_STACK_SIZE );
|
|
}
|
|
}
|
|
}
|
|
|
|
void b3DynamicTree_Refit( b3DynamicTree* tree )
|
|
{
|
|
if ( b3HasTreeMoved( tree ) == false )
|
|
{
|
|
return;
|
|
}
|
|
|
|
b3TreeNode* nodes = tree->nodes;
|
|
|
|
if ( tree->dfsOrdered == false )
|
|
{
|
|
// This only happens in unit tests.
|
|
b3RefitSubtree( nodes, B3_ROOT_NODE );
|
|
return;
|
|
}
|
|
|
|
for ( int pair = tree->nodeEnd - 2; pair >= 0; pair -= 2 )
|
|
{
|
|
b3TreeNode* node = nodes + pair;
|
|
uint32_t flags1 = node[0].flagIndex;
|
|
uint32_t flags2 = node[1].flagIndex;
|
|
|
|
// Did either move?
|
|
if ( ( ( flags1 | flags2 ) & B3_MOVED_NODE ) == 0 )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Is the node internal and moved?
|
|
bool refit1 = ( flags1 & ( B3_LEAF_NODE | B3_MOVED_NODE ) ) == B3_MOVED_NODE;
|
|
bool refit2 = ( flags2 & ( B3_LEAF_NODE | B3_MOVED_NODE ) ) == B3_MOVED_NODE;
|
|
|
|
// If the node needs a refit then get the child index. Otherwise use a valid dummy index.
|
|
// The store function won't actually modify the node at the dummy index. This
|
|
// avoids an unpredictable branch. SIMD FTW.
|
|
int children1 = refit1 ? (int)( flags1 & B3_NODE_INDEX_MASK ) : pair;
|
|
int children2 = refit2 ? (int)( flags2 & B3_NODE_INDEX_MASK ) : pair;
|
|
|
|
// Conditionally store the union.
|
|
b3StoreAABBV( &node[0].aabb, b3UnionPairV( nodes + children1 ), refit1 );
|
|
b3StoreAABBV( &node[1].aabb, b3UnionPairV( nodes + children2 ), refit2 );
|
|
}
|
|
}
|
|
|
|
static FILE* b3OpenTreeFile( const char* fileName, const char* mode )
|
|
{
|
|
FILE* file = NULL;
|
|
|
|
#if defined( _MSC_VER )
|
|
errno_t e = fopen_s( &file, fileName, mode );
|
|
if ( e != 0 )
|
|
{
|
|
return NULL;
|
|
}
|
|
#else
|
|
file = fopen( fileName, mode );
|
|
if ( file == NULL )
|
|
{
|
|
return NULL;
|
|
}
|
|
#endif
|
|
|
|
return file;
|
|
}
|
|
|
|
void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName )
|
|
{
|
|
if ( tree->nodes == NULL || tree->parents == NULL || tree->proxies == NULL )
|
|
{
|
|
return;
|
|
}
|
|
|
|
FILE* file = b3OpenTreeFile( fileName, "wb" );
|
|
if ( file == NULL )
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Copy to allow setting some fields to zero
|
|
b3DynamicTree temp = *tree;
|
|
|
|
// Zero pointers and temp data
|
|
temp.nodes = NULL;
|
|
temp.parents = NULL;
|
|
temp.proxies = NULL;
|
|
temp.swapNodes = NULL;
|
|
temp.leafIndices = NULL;
|
|
temp.leafNodes = NULL;
|
|
temp.leafBoxes = NULL;
|
|
temp.leafCenters = NULL;
|
|
temp.binIndices = NULL;
|
|
temp.rebuildCapacity = 0;
|
|
|
|
temp.nodeCapacity = tree->nodeEnd;
|
|
|
|
fwrite( &temp, sizeof( b3DynamicTree ), 1, file );
|
|
|
|
if ( tree->nodeEnd > 0 )
|
|
{
|
|
fwrite( tree->nodes, sizeof( b3TreeNode ), tree->nodeEnd, file );
|
|
fwrite( tree->parents, sizeof( int32_t ), tree->nodeEnd, file );
|
|
}
|
|
|
|
if ( tree->proxyCapacity > 0 )
|
|
{
|
|
fwrite( tree->proxies, sizeof( b3TreeProxy ), tree->proxyCapacity, file );
|
|
}
|
|
|
|
fclose( file );
|
|
}
|
|
|
|
b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale )
|
|
{
|
|
b3DynamicTree tree = { 0 };
|
|
|
|
FILE* file = b3OpenTreeFile( fileName, "rb" );
|
|
if ( file == NULL )
|
|
{
|
|
return tree;
|
|
}
|
|
|
|
int readCount = (int)fread( &tree, sizeof( b3DynamicTree ), 1, file );
|
|
if ( readCount != 1 )
|
|
{
|
|
fclose( file );
|
|
memset( &tree, 0, sizeof( b3DynamicTree ) );
|
|
return tree;
|
|
}
|
|
|
|
if ( tree.version != B3_DYNAMIC_TREE_VERSION )
|
|
{
|
|
fclose( file );
|
|
memset( &tree, 0, sizeof( b3DynamicTree ) );
|
|
return tree;
|
|
}
|
|
|
|
tree.nodes = NULL;
|
|
tree.parents = NULL;
|
|
tree.proxies = NULL;
|
|
tree.swapNodes = NULL;
|
|
tree.leafIndices = NULL;
|
|
tree.leafNodes = NULL;
|
|
tree.leafBoxes = NULL;
|
|
tree.leafCenters = NULL;
|
|
tree.binIndices = NULL;
|
|
tree.rebuildCapacity = 0;
|
|
|
|
bool ok = tree.nodeCapacity >= 2 && tree.proxyCapacity >= 1 && ( tree.nodeEnd & 1 ) == 0 && tree.nodeEnd >= 2 &&
|
|
tree.nodeEnd <= tree.nodeCapacity;
|
|
|
|
if ( ok )
|
|
{
|
|
tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) );
|
|
tree.parents = (int32_t*)b3Alloc( tree.nodeCapacity * sizeof( int32_t ) );
|
|
tree.proxies = (b3TreeProxy*)b3Alloc( tree.proxyCapacity * sizeof( b3TreeProxy ) );
|
|
|
|
ok = (int)fread( tree.nodes, sizeof( b3TreeNode ), tree.nodeCapacity, file ) == tree.nodeCapacity;
|
|
ok = ok && (int)fread( tree.parents, sizeof( int32_t ), tree.nodeCapacity, file ) == tree.nodeCapacity;
|
|
ok = ok && (int)fread( tree.proxies, sizeof( b3TreeProxy ), tree.proxyCapacity, file ) == tree.proxyCapacity;
|
|
}
|
|
|
|
if ( ok == false )
|
|
{
|
|
b3Free( tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) );
|
|
b3Free( tree.parents, tree.nodeCapacity * sizeof( int32_t ) );
|
|
b3Free( tree.proxies, tree.proxyCapacity * sizeof( b3TreeProxy ) );
|
|
fclose( file );
|
|
memset( &tree, 0, sizeof( b3DynamicTree ) );
|
|
return tree;
|
|
}
|
|
|
|
// An empty node has infinite bounds, so the scale has to skip it.
|
|
for ( int i = 0; i < tree.nodeEnd; ++i )
|
|
{
|
|
b3TreeNode* node = tree.nodes + i;
|
|
if ( b3IsEmptyNode( node ) )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
node->aabb.lowerBound = b3MulSV( scale, node->aabb.lowerBound );
|
|
node->aabb.upperBound = b3MulSV( scale, node->aabb.upperBound );
|
|
}
|
|
|
|
fclose( file );
|
|
|
|
return tree;
|
|
}
|