From f555ee42084e0b43cbffa863f40bff8117c08896 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Wed, 16 Sep 2026 22:28:39 -0700 Subject: [PATCH] Optimize Broad-Phase (#159) Broad-phase optimization ported from Box2D. --- benchmark/amd7950x_sse2/convex_pile.csv | 16 +- benchmark/amd7950x_sse2/joint_grid.csv | 16 +- benchmark/amd7950x_sse2/junkyard.csv | 16 +- benchmark/amd7950x_sse2/large_pyramid.csv | 16 +- benchmark/amd7950x_sse2/large_world.csv | 16 +- benchmark/amd7950x_sse2/many_pyramids.csv | 16 +- benchmark/amd7950x_sse2/rain.csv | 16 +- benchmark/amd7950x_sse2/sleep.csv | 9 + benchmark/amd7950x_sse2/spinner.csv | 9 + benchmark/amd7950x_sse2/trees100.csv | 16 +- benchmark/amd7950x_sse2/trees25.csv | 16 +- benchmark/amd7950x_sse2/trees50.csv | 16 +- benchmark/amd7950x_sse2/washer.csv | 16 +- benchmark/main.c | 3 +- include/box3d/collision.h | 16 +- include/box3d/types.h | 116 +- samples/CMakeLists.txt | 6 +- samples/sample_benchmark.cpp | 77 +- samples/sample_tree.cpp | 82 +- shared/benchmarks.c | 313 +++ shared/benchmarks.h | 9 +- src/CMakeLists.txt | 1 + src/body.c | 6 + src/broad_phase.c | 896 +++++--- src/broad_phase.h | 45 +- src/compound.c | 60 +- src/container.h | 1 + src/core.c | 9 +- src/core.h | 6 +- src/dynamic_tree.c | 2384 +++++++++++---------- src/dynamic_tree.h | 89 + src/joint.c | 2 +- src/parallel_for.c | 2 + src/physics_world.c | 18 +- src/physics_world.h | 11 +- src/platform.h | 28 + src/recording.h | 4 +- src/shape.c | 8 + src/shape.h | 2 +- src/simd.h | 184 ++ src/solver.c | 166 +- src/table.c | 28 +- src/table.h | 21 + src/world_snapshot.c | 98 +- test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_compound.c | 4 +- test/test_container.c | 2 +- test/test_determinism.c | 20 +- test/test_dynamic_tree.c | 713 ++++++ test/test_world.c | 69 +- 51 files changed, 3834 insertions(+), 1862 deletions(-) create mode 100644 benchmark/amd7950x_sse2/sleep.csv create mode 100644 benchmark/amd7950x_sse2/spinner.csv create mode 100644 src/dynamic_tree.h create mode 100644 test/test_dynamic_tree.c diff --git a/benchmark/amd7950x_sse2/convex_pile.csv b/benchmark/amd7950x_sse2/convex_pile.csv index bc51d9a..4bf71ef 100644 --- a/benchmark/amd7950x_sse2/convex_pile.csv +++ b/benchmark/amd7950x_sse2/convex_pile.csv @@ -1,9 +1,9 @@ threads,ms -1,17336.6 -2,8857.22 -3,5945.65 -4,4508.71 -5,3674.55 -6,3105.97 -7,2696.86 -8,2410.11 +1,14441.9 +2,7238.43 +3,4870.13 +4,3697.34 +5,2998.67 +6,2533.13 +7,2209.12 +8,1983.89 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv index f5645ca..c09a287 100644 --- a/benchmark/amd7950x_sse2/joint_grid.csv +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,1586.48 -2,787.633 -3,531.491 -4,401.5 -5,325.625 -6,275.095 -7,238.344 -8,214.504 +1,1323.84 +2,691.063 +3,463.698 +4,353.723 +5,284.09 +6,240.864 +7,209.11 +8,185.953 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv index 1d7791b..50e839b 100644 --- a/benchmark/amd7950x_sse2/junkyard.csv +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -1,9 +1,9 @@ threads,ms -1,20044.1 -2,10386.4 -3,7087.11 -4,5437.88 -5,4451.58 -6,3792.59 -7,3293.41 -8,2970.84 +1,16467.5 +2,8687.63 +3,5966.06 +4,4569.55 +5,3727.89 +6,3175.51 +7,2801.72 +8,2512.32 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv index 0c7d8e6..e9b1cc8 100644 --- a/benchmark/amd7950x_sse2/large_pyramid.csv +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,2071.23 -2,1143.38 -3,778.58 -4,597.287 -5,487.523 -6,419.766 -7,366.281 -8,333.042 +1,1993.6 +2,1049.01 +3,712.917 +4,544.335 +5,443.056 +6,384.22 +7,331.626 +8,297.982 diff --git a/benchmark/amd7950x_sse2/large_world.csv b/benchmark/amd7950x_sse2/large_world.csv index 47edee3..4e0a4c5 100644 --- a/benchmark/amd7950x_sse2/large_world.csv +++ b/benchmark/amd7950x_sse2/large_world.csv @@ -1,9 +1,9 @@ threads,ms -1,10.938 -2,12.76 -3,13.5526 -4,13.9525 -5,15.3005 -6,15.6679 -7,15.4235 -8,14.7126 +1,7.6329 +2,9.7115 +3,10.7648 +4,11.2398 +5,12.029 +6,12.1204 +7,12.2289 +8,12.5125 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv index ac3b0d1..aa3aee5 100644 --- a/benchmark/amd7950x_sse2/many_pyramids.csv +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,2331.14 -2,1259.61 -3,827.118 -4,616.691 -5,495.451 -6,411.55 -7,355.527 -8,316.039 +1,2075.59 +2,1084.54 +3,722.057 +4,549.644 +5,437.772 +6,372.242 +7,319.13 +8,283.131 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv index 8b0058d..d41aa27 100644 --- a/benchmark/amd7950x_sse2/rain.csv +++ b/benchmark/amd7950x_sse2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,2434.57 -2,1325.14 -3,937.234 -4,737.677 -5,615.764 -6,536.387 -7,479.755 -8,444.147 +1,2102.47 +2,1147.85 +3,803.547 +4,630.96 +5,524.168 +6,452.1 +7,404.499 +8,370.325 diff --git a/benchmark/amd7950x_sse2/sleep.csv b/benchmark/amd7950x_sse2/sleep.csv new file mode 100644 index 0000000..ba99522 --- /dev/null +++ b/benchmark/amd7950x_sse2/sleep.csv @@ -0,0 +1,9 @@ +threads,ms +1,1144.18 +2,613.407 +3,423.032 +4,329.512 +5,272.901 +6,239.65 +7,216.5 +8,200.217 diff --git a/benchmark/amd7950x_sse2/spinner.csv b/benchmark/amd7950x_sse2/spinner.csv new file mode 100644 index 0000000..5dad23c --- /dev/null +++ b/benchmark/amd7950x_sse2/spinner.csv @@ -0,0 +1,9 @@ +threads,ms +1,3173.87 +2,1841.25 +3,1325.07 +4,1061.04 +5,902.193 +6,803.715 +7,734.266 +8,689.781 diff --git a/benchmark/amd7950x_sse2/trees100.csv b/benchmark/amd7950x_sse2/trees100.csv index 50fa9b4..27248b4 100644 --- a/benchmark/amd7950x_sse2/trees100.csv +++ b/benchmark/amd7950x_sse2/trees100.csv @@ -1,9 +1,9 @@ threads,ms -1,225.438 -2,141.14 -3,115.727 -4,101.289 -5,93.6898 -6,90.61 -7,86.1046 -8,86.7291 +1,170.276 +2,112.037 +3,88.2708 +4,78.4451 +5,74.3577 +6,71.868 +7,70.6687 +8,69.1284 diff --git a/benchmark/amd7950x_sse2/trees25.csv b/benchmark/amd7950x_sse2/trees25.csv index 73c74eb..99dfa5b 100644 --- a/benchmark/amd7950x_sse2/trees25.csv +++ b/benchmark/amd7950x_sse2/trees25.csv @@ -1,9 +1,9 @@ threads,ms -1,727.276 -2,433.755 -3,315.505 -4,270.352 -5,246.82 -6,228.451 -7,216.217 -8,210.749 +1,673.356 +2,395.07 +3,291.336 +4,249.378 +5,225.709 +6,211.1 +7,201.452 +8,194.379 diff --git a/benchmark/amd7950x_sse2/trees50.csv b/benchmark/amd7950x_sse2/trees50.csv index 1bc17fd..68aadc7 100644 --- a/benchmark/amd7950x_sse2/trees50.csv +++ b/benchmark/amd7950x_sse2/trees50.csv @@ -1,9 +1,9 @@ threads,ms -1,334.365 -2,191.522 -3,144.41 -4,127.041 -5,119.247 -6,113.65 -7,108.433 -8,105.905 +1,285.24 +2,174.635 +3,132.161 +4,116.509 +5,107.948 +6,102.503 +7,98.9163 +8,96.5196 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv index fc18745..c03dccd 100644 --- a/benchmark/amd7950x_sse2/washer.csv +++ b/benchmark/amd7950x_sse2/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,27059.6 -2,14569.2 -3,10081.7 -4,7825.43 -5,6480.34 -6,5518.81 -7,4901.6 -8,4256.68 +1,21430.4 +2,11407.7 +3,7908.09 +4,6102.55 +5,5012.68 +6,4272.59 +7,3783.33 +8,3415.5 diff --git a/benchmark/main.c b/benchmark/main.c index 8e8cf1c..a630c59 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -157,12 +157,13 @@ int main( int argc, char** argv ) { "large_world", GetLargeWorldCapacity, CreateLargeWorld, NULL, StepLargeWorld, 500 }, { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, { "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 }, + { "sleep", GetSleepCapacity, CreateSleep, NULL, StepSleep, 300 }, + { "spinner", GetSpinnerCapacity, CreateSpinner, DestroySpinner, NULL, 800 }, { "trees100", NULL, CreateTrees100, DestroyTrees, NULL, 500 }, { "trees50", NULL, CreateTrees50, DestroyTrees, NULL, 500 }, { "trees25", NULL, CreateTrees25, DestroyTrees, NULL, 500 }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, //{ "smash", CreateSmash, NULL, 300 }, - //{ "spinner", CreateSpinner, StepSpinner, 1400 }, //{ "tumbler", CreateTumbler, NULL, 750 }, }; diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 266998a..47a5434 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -48,8 +48,8 @@ B3_API b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, /// point. /// @param tree the dynamic tree to query /// @param point the query point -/// @param maskBits nodes are skipped if the bit-wise AND with the node category bits is zero -/// @param requireAllBits nodes are skipped if the bit-wise AND with the node category bits does not equal the maskBits +/// @param maskBits proxies are skipped if the bit-wise AND with the proxy category bits is zero +/// @param requireAllBits proxies are skipped if the bit-wise AND with the proxy category bits does not equal the maskBits /// @param callback a user provided instance of b3TreeQueryClosestCallbackFcn /// @param context a user context object that is provided to the callback /// @param minDistanceSqr the initial and final minimum squared distance. Provide a small initial to restrict the search and @@ -68,8 +68,8 @@ B3_API b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 /// However, this filtering may be approximate, so the user should still apply filtering to results. /// @param tree the dynamic tree to ray cast /// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) -/// @param maskBits bit mask test: `bool accept = (maskBits & node->categoryBits) != 0;` -/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & node->categoryBits) == maskBits;` +/// @param maskBits bit mask test: `bool accept = (maskBits & proxy->categoryBits) != 0;` +/// @param requireAllBits modifies bit mask test: `bool accept = (maskBits & proxy->categoryBits) == maskBits;` /// @param callback a callback function that is called for each proxy that is hit by the ray /// @param context user context that is passed to the callback /// @return performance data @@ -103,8 +103,8 @@ B3_API int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ); /// Validate this tree. For testing. B3_API void b3DynamicTree_Validate( const b3DynamicTree* tree ); -/// Validate this tree has no enlarged AABBs. For testing. -B3_API void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ); +/// Validate this tree has no moved nodes. For testing. +B3_API void b3DynamicTree_ValidateNoMoved( const b3DynamicTree* tree ); /// Save this tree to a file for debugging B3_API void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ); @@ -115,13 +115,13 @@ B3_API b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ); /// Get proxy user data B3_INLINE uint64_t b3DynamicTree_GetUserData( const b3DynamicTree* tree, int proxyId ) { - return tree->nodes[proxyId].userData; + return tree->proxies[proxyId].userData; } /// Get the AABB of a proxy B3_INLINE b3AABB b3DynamicTree_GetAABB( const b3DynamicTree* tree, int proxyId ) { - return tree->nodes[proxyId].aabb; + return tree->nodes[tree->proxies[proxyId].node].aabb; } /**@}*/ // tree diff --git a/include/box3d/types.h b/include/box3d/types.h index 847521e..ed4a1d9 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -1683,58 +1683,46 @@ typedef struct b3TOIOutput * @{ */ -/// Flags for tree nodes. For internal usage. -typedef enum b3TreeNodeFlags -{ - b3_allocatedNode = 0x0001, - b3_enlargedNode = 0x0002, - b3_leafNode = 0x0004, -} b3TreeNodeFlags; - -/// Tree node child indices. For internal usage. -typedef struct b3TreeNodeChildren -{ - int child1; ///< child node index 1 - int child2; ///< child node index 2 -} b3TreeNodeChildren; - -/// A node in the dynamic tree. This is private data placed here for performance reasons. -/// todo test padding to 64 bytes to avoid straddling cache lines +/// A node in the dynamic tree. Siblings sit together at an even index so two nodes fit in +/// a 64 byte cache line. The root is at index zero and index one always empty. typedef struct b3TreeNode { /// The node bounding box b3AABB aabb; // 24 - /// Category bits for collision filtering - uint64_t categoryBits; // 8 + /// bit 31 : 1 for leaf node + /// bit 30 : 1 for moved flag + /// bits 0-29 : index of the sibling pair node or the proxy id for a leaf + uint32_t flagIndex; // 4 union { - /// Children (internal node) - b3TreeNodeChildren children; + /// The height of an internal node. A leaf has zero height. + int32_t height; - /// User data (leaf node) - uint64_t userData; - }; // 8 - - union - { - /// The node parent index (allocated node) - int parent; - - /// The node freelist next index (free node) - int next; + /// The shape index for a leaf. Truncated from proxy user data. + int32_t shapeIndex; }; // 4 - - /// Height of the node. Leaves have a height of 0. - uint16_t height; // 2 - - /// @see b3TreeNodeFlags - uint16_t flags; // 2 } b3TreeNode; +/// Separate storage for tree leaves. +typedef struct b3TreeProxy +{ + /// User data is an index instead of void* because it is used internally as a shape index. + uint64_t userData; + + /// Category bits for collision filtering. + uint64_t categoryBits; + + /// The leaf node. B3_NULL_INDEX for a free proxy. + int32_t node; + + /// Next free proxy. + int32_t next; +} b3TreeProxy; + /// Dynamic tree version for compatibility testing. -#define B3_DYNAMIC_TREE_VERSION 0x93EDAF889FD30B4Aull +#define B3_DYNAMIC_TREE_VERSION 0x1D6F4C2A73B80E91ull /// The dynamic tree structure. This should be considered private data. /// It is placed here for performance reasons. @@ -1744,26 +1732,42 @@ typedef struct b3DynamicTree /// if the tree is serialized. uint64_t version; - /// The tree nodes + /// Array of nodes. The root is at index zero and index 1 is empty. + /// Otherwise siblings are paired at even indices. Has holes for free node pairs. b3TreeNode* nodes; - /// The root index - int root; + /// Parent index per node. The free list is interweaved. + int32_t* parents; - /// The number of nodes - int nodeCount; + /// Proxy data split from node array as cold data. + b3TreeProxy* proxies; + + /// Every allocated node has a lower index than this. + int32_t nodeEnd; /// The allocated node space - int nodeCapacity; + int32_t nodeCapacity; + + /// Free pairs below nodeEnd + int32_t pairFreeList; /// Number of proxies created - int proxyCount; + int32_t proxyCount; - /// Node free list - int freeList; + /// The allocated proxy space + int32_t proxyCapacity; + + /// Proxy free list + int32_t proxyFreeList; + + /// Array of nodes for rebuild. + b3TreeNode* swapNodes; /// Leaf indices for rebuild - int* leafIndices; + int32_t* leafIndices; + + /// Leaves for the rebuild. May represent a proxy or a retained subtree. + b3TreeNode* leafNodes; /// Leaf bounding boxes for rebuild b3AABB* leafBoxes; @@ -1772,10 +1776,14 @@ typedef struct b3DynamicTree b3Vec3* leafCenters; /// Bins for sorting during rebuild - int* binIndices; + int32_t* binIndices; /// Allocated space for rebuilding - int rebuildCapacity; + int32_t rebuildCapacity; + + /// Rebuild orders the nodes so the children follow parents. Cache friendly for queries + /// and refitting. The order can be disrupted by proxy creation. + bool dfsOrdered; } b3DynamicTree; /// These are performance results returned by dynamic tree queries. @@ -2488,7 +2496,11 @@ typedef struct b3CompoundData /// Offset of the tree node array in bytes from the struct address. int nodeOffset; - /// Immutable dynamic tree. The tree node pointer must be fixed up using the node offset + /// Offset of the tree proxy array in bytes from the struct address. + int proxyOffset; + + /// Immutable dynamic tree. The node and proxy pointers must be fixed up using the offsets + /// above. A baked tree is never inserted into, so the parent array stays null. b3DynamicTree tree; /// Offset of the material array in bytes from the struct address. diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 319fb85..9c9ca12 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -194,10 +194,12 @@ endif() # Include root is the samples dir so "gfx/..." and "host/..." prefixes resolve. # The data dir is on the path too so samples can #include recorded dumps -# (sample_issues pulls dumps/.../box3d_dump.inl at compile time). +# (sample_issues pulls dumps/.../box3d_dump.inl at compile time). src is on the +# path so sample_tree.cpp can reach dynamic_tree.h for the node layout helpers. target_include_directories(samples PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}" - "${CMAKE_CURRENT_SOURCE_DIR}/../data") + "${CMAKE_CURRENT_SOURCE_DIR}/../data" + "${CMAKE_SOURCE_DIR}/src") # gfx: the renderer + Draw*/adapter. shared: human, utils (Random*), the # benchmark/determinism helpers the samples lean on. diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 50b008d..fd13ae7 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -1439,7 +1439,7 @@ public: void Step() override { - if ( m_context->pause == false || m_context->singleStep == 0 ) + if ( m_context->pause == false || m_context->singleStep > 0 ) { StepJunkyard( m_worldId, m_stepCount ); } @@ -1486,3 +1486,78 @@ public: }; static int sampleConvexPile = RegisterSample( "Benchmark", "Convex Pile", BenchmarkConvexPile::Create ); + +class BenchmarkSpinner : public Sample +{ +public: + explicit BenchmarkSpinner( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 12.0f, 45.0f, { 0.0f, 12.0f, 0.0f } ); + } + + b3Capacity capacity = {}; + GetSpinnerCapacity( &capacity ); + CreateWorld( &capacity ); + + CreateSpinner( m_worldId ); + } + + ~BenchmarkSpinner() override + { + DestroySpinner(); + } + + void Step() override + { + Sample::Step(); + + DrawTextLine( "spinner angle = %.2f", GetSpinnerAngle() ); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkSpinner( context ); + } +}; + +static int benchmarkSpinner = RegisterSample( "Benchmark", "Spinner", BenchmarkSpinner::Create ); + +class BenchmarkSleep : public Sample +{ +public: + explicit BenchmarkSleep( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( -10.0f, 25.0f, 100.0f, { 0.0f, 3.0f, 0.0f } ); + } + + b3Capacity capacity = {}; + GetSleepCapacity( &capacity ); + CreateWorld( &capacity ); + + CreateSleep( m_worldId ); + SetGroundShape( GetGroundShapeId() ); + } + + void Step() override + { + if ( m_context->pause == false || m_context->singleStep > 0 ) + { + StepSleep( m_worldId, m_stepCount ); + } + + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new BenchmarkSleep( context ); + } +}; + +static int benchmarkSleep = RegisterSample( "Benchmark", "Sleep", BenchmarkSleep::Create ); diff --git a/samples/sample_tree.cpp b/samples/sample_tree.cpp index 9a760c0..f8ad5d6 100644 --- a/samples/sample_tree.cpp +++ b/samples/sample_tree.cpp @@ -12,6 +12,8 @@ #include "box3d/box3d.h" +#include "dynamic_tree.h" + #include #include #include @@ -88,45 +90,55 @@ public: } // Breath-first search to compute node depth values start with 0 at the root. + // The tree is walked a sibling pair at a time, so depth is tracked alongside + // each queued pair rather than looked up through the parent index. int ComputeDepths() { m_depths.resize( m_tree.nodeCapacity, 0 ); - if ( m_tree.nodes == nullptr || m_tree.nodeCount == 0 || m_tree.root == B3_NULL_INDEX ) + if ( m_tree.nodes == nullptr || m_tree.proxyCount == 0 ) { return 0; } - int* queue = (int*)malloc( m_tree.nodeCount * sizeof( int ) ); + b3TreeNode* nodes = m_tree.nodes; + m_depths[B3_ROOT_NODE] = 0; + + if ( b3IsLeaf( nodes + B3_ROOT_NODE ) ) + { + return 0; + } + + struct QueueItem + { + int pair; + int depth; + }; + + QueueItem* queue = (QueueItem*)malloc( m_tree.nodeEnd * sizeof( QueueItem ) ); int front = 0; int back = 0; - b3TreeNode* nodes = m_tree.nodes; - queue[0] = m_tree.root; - back += 1; + queue[back++] = { b3GetLeftChild( nodes + B3_ROOT_NODE ), 1 }; + int depth = 0; while ( back > front ) { - int index = queue[front]; - front += 1; + QueueItem item = queue[front++]; + depth = b3MaxInt( depth, item.depth ); - b3TreeNode* node = nodes + index; - if ( node->parent == B3_NULL_INDEX ) + for ( int i = 0; i < 2; ++i ) { - m_depths[index] = 0; - } - else - { - m_depths[index] = m_depths[node->parent] + 1; - depth = b3MaxInt( depth, m_depths[index] ); - } + int index = item.pair + i; + b3TreeNode* node = nodes + index; + m_depths[index] = item.depth; - if ( ( node->flags & b3_leafNode ) == 0 ) - { - B3_ASSERT( back < m_tree.nodeCount - 1 ); - queue[back++] = node->children.child1; - queue[back++] = node->children.child2; + if ( b3IsLeaf( node ) == false ) + { + B3_ASSERT( back < m_tree.nodeEnd ); + queue[back++] = { b3GetLeftChild( node ), item.depth + 1 }; + } } } @@ -284,17 +296,15 @@ public: int maxArea = 0.0f; int maxAreaIndex = -1; - for ( int i = 0; i < m_tree.nodeCapacity; ++i ) + for ( int i = 0; i < m_tree.nodeEnd; ++i ) { b3TreeNode* node = m_tree.nodes + i; - if ( ( node->flags & b3_leafNode ) == 0 ) + if ( b3IsLeaf( node ) == false || b3IsEmptyNode( node ) ) { continue; } - assert( node->flags & b3_allocatedNode ); - Proxy proxy = {}; proxy.aabb = node->aabb; @@ -305,8 +315,9 @@ public: maxAreaIndex = (int)m_proxies.size(); } - proxy.proxyId = i; - m_proxies[node->userData] = proxy; + int proxyId = b3GetProxyId( node ); + proxy.proxyId = proxyId; + m_proxies[m_tree.proxies[proxyId].userData] = proxy; } printf( "max index = %d\n", maxAreaIndex ); @@ -363,7 +374,7 @@ public: { srand( 42 ); - b3AABB bounds = m_tree.nodes[m_tree.root].aabb; + b3AABB bounds = m_tree.nodes[B3_ROOT_NODE].aabb; b3Vec3 extents = b3AABB_Extents( bounds ); float radius = ( extents.x + extents.y + extents.z ) / 3.0f; @@ -509,13 +520,11 @@ public: b3_colorCoral, b3_colorCornflowerBlue, b3_colorCornsilk, b3_colorCrimson, b3_colorCyan, }; - int capacity = m_tree.nodeCapacity; - for ( int i = 0; i < capacity; ++i ) + for ( int i = 0; i < m_tree.nodeEnd; ++i ) { b3TreeNode* node = nodes + i; - if ( m_depths[i] != m_drawLevel || ( node->flags & b3_allocatedNode ) == 0 ) + if ( m_depths[i] != m_drawLevel || b3IsEmptyNode( node ) ) { - // skip internal nodes continue; } @@ -528,12 +537,13 @@ public: } else { - uint16_t requiredFlags = b3_allocatedNode | b3_leafNode; - for ( int i = 0; i < m_tree.nodeCapacity; ++i ) + // Live leaves only. A mark on a leaf says its ancestors still need a refit, the + // leaf box itself is already current, so moved leaves still draw. + for ( int i = 0; i < m_tree.nodeEnd; ++i ) { b3TreeNode* node = nodes + i; - if ( node->flags != requiredFlags ) + if ( b3IsEmptyNode( node ) || b3IsLeaf( node ) == false ) { continue; } @@ -544,7 +554,7 @@ public: continue; } - Proxy& proxy = m_proxies[node->userData]; + Proxy& proxy = m_proxies[m_tree.proxies[b3GetProxyId( node )].userData]; if ( proxy.queryTimeStamp == m_timeStamp || proxy.rayTimeStamp == m_timeStamp ) { DrawBounds( node->aabb, 0.0f, MakeColor( b3_colorLightGray ) ); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index 3623717..b2880e6 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -1000,3 +1000,316 @@ void CreateConvexPile( b3WorldId worldId ) b3DestroyHull( convex ); } + +#define SPINNER_SEGMENT_COUNT 90 + +typedef struct +{ + b3MeshData* cylinderMesh; + b3JointId spinnerId; +} SpinnerData; + +static SpinnerData g_spinnerData; + +// An inward facing cylinder capped on both ends, axis along z. Every triangle is wound so its +// normal points at the axis, which is what holds the contents in. The caps do the same job along +// the axis that the 2D loop had for free. +static b3MeshData* CreateInwardCylinderMesh( float radius, float halfDepth, float yCenter, int segmentCount ) +{ + int vertexCount = 2 * segmentCount + 2; + int triangleCount = 4 * segmentCount; + + b3Vec3* vertices = malloc( vertexCount * sizeof( b3Vec3 ) ); + int* indices = malloc( 3 * triangleCount * sizeof( int ) ); + + for ( int i = 0; i < segmentCount; ++i ) + { + float angle = 2.0f * B3_PI * (float)i / (float)segmentCount; + b3CosSin cs = b3ComputeCosSin( angle ); + float x = radius * cs.cosine; + float y = radius * cs.sine + yCenter; + vertices[2 * i + 0] = (b3Vec3){ x, y, -halfDepth }; + vertices[2 * i + 1] = (b3Vec3){ x, y, halfDepth }; + } + + int centerNegative = 2 * segmentCount; + int centerPositive = 2 * segmentCount + 1; + vertices[centerNegative] = (b3Vec3){ 0.0f, yCenter, -halfDepth }; + vertices[centerPositive] = (b3Vec3){ 0.0f, yCenter, halfDepth }; + + int index = 0; + for ( int i = 0; i < segmentCount; ++i ) + { + int j = ( i + 1 ) % segmentCount; + + // Wall, normal towards the axis + indices[index++] = 2 * i + 0; + indices[index++] = 2 * i + 1; + indices[index++] = 2 * j + 1; + + indices[index++] = 2 * i + 0; + indices[index++] = 2 * j + 1; + indices[index++] = 2 * j + 0; + + // Near cap, normal towards positive z + indices[index++] = centerNegative; + indices[index++] = 2 * i + 0; + indices[index++] = 2 * j + 0; + + // Far cap, normal towards negative z + indices[index++] = centerPositive; + indices[index++] = 2 * j + 1; + indices[index++] = 2 * i + 1; + } + + assert( index == 3 * triangleCount ); + + b3MeshDef meshDef = { 0 }; + meshDef.vertices = vertices; + meshDef.vertexCount = vertexCount; + meshDef.indices = indices; + meshDef.triangleCount = triangleCount; + meshDef.identifyEdges = true; + + b3MeshData* meshData = b3CreateMesh( &meshDef, NULL, 0 ); + + free( vertices ); + free( indices ); + + return meshData; +} + +void GetSpinnerCapacity( b3Capacity* capacity ) +{ + capacity->staticShapeCount = 4; + capacity->staticBodyCount = 4; + capacity->dynamicShapeCount = 2000; + capacity->dynamicBodyCount = 2000; + capacity->contactCount = 20000; +} + +// A paddle on a motor churning a pile of small shapes inside a closed cylinder. The cylinder axis +// is z so the cross section matches the 2D benchmark and the hinge, which turns about the local +// frame z axis, lines up with it. The paddle circle is internally tangent to the cylinder at the +// bottom, so the tip scrapes the floor. +void CreateSpinner( b3WorldId worldId ) +{ + g_spinnerData = (SpinnerData){ 0 }; + + float radius = 12.0f; + float halfDepth = 1.0f; + float yCenter = radius; + + b3BodyId groundId; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + groundId = b3CreateBody( worldId, &bodyDef ); + + g_spinnerData.cylinderMesh = CreateInwardCylinderMesh( radius, halfDepth, yCenter, SPINNER_SEGMENT_COUNT ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.1f; + b3CreateMeshShape( groundId, &shapeDef, g_spinnerData.cylinderMesh, b3Vec3_one ); + } + + { + float halfLength = 0.5f * radius; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, yCenter - halfLength, 0.0f }; + bodyDef.enableSleep = false; + + b3BodyId spinnerId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull paddle = b3MakeBoxHull( 0.3f, halfLength + 0.1f, halfDepth + 0.1f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + b3CreateHullShape( spinnerId, &shapeDef, &paddle.base ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.bodyIdA = groundId; + jointDef.base.bodyIdB = spinnerId; + jointDef.base.localFrameA.p = (b3Vec3){ 0.0f, yCenter - halfLength, 0.0f }; + jointDef.enableMotor = true; + jointDef.motorSpeed = 8.0f; + jointDef.maxMotorTorque = FLT_MAX; + + g_spinnerData.spinnerId = b3CreateRevoluteJoint( worldId, &jointDef ); + } + + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.2f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.1f; + shapeDef.baseMaterial.restitution = 0.1f; + shapeDef.density = 0.25f; + + int bodyCount = BENCHMARK_DEBUG ? 500 : 1500; + + // The grid starts low where the cylinder is narrow, so the span has to fit the chord there + float spacing = 0.55f; + float yStart = 2.0f; + float chordHalfWidth = sqrtf( radius * radius - ( yCenter - yStart ) * ( yCenter - yStart ) ); + float xLimit = chordHalfWidth - 2.0f * spacing; + float zLimit = halfDepth - 2.0f * sphere.radius; + + float x = -xLimit; + float y = yStart; + float z = -zLimit; + + for ( int i = 0; i < bodyCount; ++i ) + { + bodyDef.position = (b3Pos){ x, y, z }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + z += spacing; + + if ( z > zLimit ) + { + z = -zLimit; + x += spacing; + + if ( x > xLimit ) + { + x = -xLimit; + y += spacing; + } + } + } +} + +void DestroySpinner( void ) +{ + if ( g_spinnerData.cylinderMesh != NULL ) + { + b3DestroyMesh( g_spinnerData.cylinderMesh ); + g_spinnerData.cylinderMesh = NULL; + } +} + +// The motor is unbounded, so a healthy run holds the commanded speed and the angle advances +// steadily. A stalling solver shows up here before it shows up in the step time. +float GetSpinnerAngle( void ) +{ + if ( b3Joint_IsValid( g_spinnerData.spinnerId ) == false ) + { + return 0.0f; + } + + return b3RevoluteJoint_GetAngle( g_spinnerData.spinnerId ); +} + +#define SLEEP_PYRAMID_COUNT 10 + +typedef struct +{ + b3BodyId bodyIdA[SLEEP_PYRAMID_COUNT]; + b3BodyId bodyIdB[SLEEP_PYRAMID_COUNT]; +} SleepData; + +static SleepData g_sleepData; + +// Reports two bodies that are certain to share a touching contact, so the filter joint below +// always lands inside one island. +static void CreateSleepPyramid( b3WorldId worldId, int baseCount, float extent, float centerX, b3BodyId* bodyIdA, + b3BodyId* bodyIdB ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.sleepThreshold = 1.0f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( extent, extent, extent ); + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * extent; + + for ( int j = i; j < baseCount; ++j ) + { + float x = ( i + 1.0f ) * extent + 2.0f * ( j - i ) * extent + centerX; + bodyDef.position = (b3Pos){ x, y, 0.0f }; + + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + if ( i == 0 && j == 0 ) + { + *bodyIdA = bodyId; + } + else if ( i == 1 && j == 1 ) + { + // Rests on the base body above, so the pair always has a touching contact + *bodyIdB = bodyId; + } + } + } +} + +void GetSleepCapacity( b3Capacity* capacity ) +{ + capacity->staticShapeCount = 4; + capacity->staticBodyCount = 4; + capacity->dynamicShapeCount = 2200; + capacity->dynamicBodyCount = 2200; + capacity->contactCount = 12000; +} + +// Stress tests waking and sleeping. Each pyramid settles into its own island and the step below +// wakes one of them again, so the island splitter never gets to rest. +void CreateSleep( b3WorldId worldId ) +{ + g_sleepData = (SleepData){ 0 }; + + int baseCount = BENCHMARK_DEBUG ? 8 : 20; + float extent = 0.5f; + + float baseWidth = 2.0f * extent * baseCount; + float pitch = baseWidth + 8.0f * extent; + float span = pitch * ( SLEEP_PYRAMID_COUNT - 1 ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.5f * span + baseWidth, 1.0f, 4.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + g_groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + for ( int i = 0; i < SLEEP_PYRAMID_COUNT; ++i ) + { + float centerX = -0.5f * span + i * pitch - 0.5f * baseWidth; + CreateSleepPyramid( worldId, baseCount, extent, centerX, g_sleepData.bodyIdA + i, g_sleepData.bodyIdB + i ); + } +} + +void StepSleep( b3WorldId worldId, int stepCount ) +{ + (void)stepCount; + + for ( int i = 0; i < SLEEP_PYRAMID_COUNT; ++i ) + { + if ( b3Body_IsAwake( g_sleepData.bodyIdA[i] ) == false ) + { + // Creating and destroying a joint engages the island splitter + b3FilterJointDef jointDef = b3DefaultFilterJointDef(); + jointDef.base.bodyIdA = g_sleepData.bodyIdA[i]; + jointDef.base.bodyIdB = g_sleepData.bodyIdB[i]; + b3JointId jointId = b3CreateFilterJoint( worldId, &jointDef ); + + // This wakes the island + b3DestroyJoint( jointId, true ); + + // Only one per step + break; + } + } +} diff --git a/shared/benchmarks.h b/shared/benchmarks.h index eb6c055..d74f1dc 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -42,9 +42,14 @@ void GetWasherCapacity( b3Capacity* capacity ); void CreateWasher( b3WorldId worldId ); void CreateConvexPile( b3WorldId worldId ); void GetConvexPileCapacity( b3Capacity* capacity ); +void GetSpinnerCapacity( b3Capacity* capacity ); +void CreateSpinner( b3WorldId worldId ); +void DestroySpinner( void ); +float GetSpinnerAngle( void ); +void GetSleepCapacity( b3Capacity* capacity ); +void CreateSleep( b3WorldId worldId ); +void StepSleep( b3WorldId worldId, int stepCount ); -// void CreateSpinner( b3WorldId worldId ); -// float StepSpinner( b3WorldId worldId, int stepCount ); // void CreateSmash( b3WorldId worldId ); // void CreateTumbler( b3WorldId worldId ); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 941c6fb..401d236 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -31,6 +31,7 @@ set(BOX3D_SOURCE_FILES distance.c distance_joint.c dynamic_tree.c + dynamic_tree.h height_field.c hull.c hull.h diff --git a/src/body.c b/src/body.c index f07b210..935f9e6 100644 --- a/src/body.c +++ b/src/body.c @@ -375,6 +375,12 @@ void b3DestroyBody( b3BodyId bodyId ) { b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + if ( shape->type == b3_compoundShape ) + { + B3_ASSERT( world->compoundShapeCount > 0 ); + world->compoundShapeCount -= 1; + } + if ( shape->sensorIndex != B3_NULL_INDEX ) { b3DestroySensor( world, shape ); diff --git a/src/broad_phase.c b/src/broad_phase.c index fa4f7f3..c85e6f9 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -6,28 +6,25 @@ #include "aabb.h" #include "arena_allocator.h" #include "body.h" +#include "compound.h" #include "contact.h" #include "core.h" +#include "dynamic_tree.h" #include "parallel_for.h" #include "physics_world.h" #include "platform.h" +#include "qsort.h" #include "shape.h" +#include "simd.h" +#include #include void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) { _Static_assert( b3_bodyTypeCount == 3, "must be three body types" ); - bp->movedProxies[b3_staticBody] = b3CreateBitSet( b3MaxInt( 16, capacity->staticShapeCount ) ); - bp->movedProxies[b3_kinematicBody] = b3CreateBitSet( 16 ); - bp->movedProxies[b3_dynamicBody] = b3CreateBitSet( b3MaxInt( 16, capacity->dynamicShapeCount ) ); - b3Array_Reserve( bp->moveArray, capacity->dynamicShapeCount ); - bp->moveResults = NULL; - bp->movePairs = NULL; - bp->movePairCapacity = 0; - b3AtomicStoreInt( &bp->movePairIndex, 0 ); - bp->pairSet = b3CreateSet( 2 * capacity->contactCount ); + bp->pairSet = b3CreateSet( b3MaxInt( 32, 2 * capacity->contactCount ) ); int staticCapacity = b3MaxInt( 16, capacity->staticShapeCount ); bp->trees[b3_staticBody] = b3DynamicTree_Create( staticCapacity ); @@ -37,6 +34,8 @@ void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ) int dynamicCapacity = b3MaxInt( 16, capacity->dynamicShapeCount ); bp->trees[b3_dynamicBody] = b3DynamicTree_Create( dynamicCapacity ); + + bp->movedSiblings = NULL; } void b3DestroyBroadPhase( b3BroadPhase* bp ) @@ -46,63 +45,29 @@ void b3DestroyBroadPhase( b3BroadPhase* bp ) b3DynamicTree_Destroy( bp->trees + i ); } - for ( int i = 0; i < b3_bodyTypeCount; ++i ) - { - b3DestroyBitSet( &bp->movedProxies[i] ); - } - b3Array_Destroy( bp->moveArray ); b3DestroySet( &bp->pairSet ); - *bp = (b3BroadPhase){ 0 }; - memset( bp, 0, sizeof( b3BroadPhase ) ); } -static void b3UnBufferMove( b3BroadPhase* bp, int proxyKey ) -{ - b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); - int proxyId = B3_PROXY_ID( proxyKey ); - b3BitSet* set = &bp->movedProxies[proxyType]; - - if ( b3GetBit( set, proxyId ) ) - { - b3ClearBit( set, proxyId ); - - // Purge from move buffer. Linear search. - // todo if I can iterate the move set then I don't need the moveArray - int count = bp->moveArray.count; - for ( int i = 0; i < count; ++i ) - { - if ( bp->moveArray.data[i] == proxyKey ) - { - b3Array_RemoveSwap( bp->moveArray, i ); - break; - } - } - } -} - int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aabb, uint64_t categoryBits, int shapeIndex, bool forcePairCreation ) { B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount ); - int proxyId = b3DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex ); + + bool mark = ( proxyType != b3_staticBody || forcePairCreation ); + + int proxyId = b3CreateTreeProxyInternal( bp->trees + proxyType, aabb, categoryBits, shapeIndex, mark ); int proxyKey = B3_PROXY_KEY( proxyId, proxyType ); - if ( proxyType != b3_staticBody || forcePairCreation ) - { - b3BufferMove( bp, proxyKey ); - } return proxyKey; } void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ) { - b3UnBufferMove( bp, proxyKey ); - b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); int proxyId = B3_PROXY_ID( proxyKey ); - B3_ASSERT( 0 <= proxyType && proxyType <= b3_bodyTypeCount ); + B3_ASSERT( 0 <= proxyType && proxyType < b3_bodyTypeCount ); b3DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId ); } @@ -111,165 +76,94 @@ void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); int proxyId = B3_PROXY_ID( proxyKey ); - b3DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb ); - b3BufferMove( bp, proxyKey ); + bool mark = true; + b3DynamicTree_MoveProxyInternal( bp->trees + proxyType, proxyId, aabb, mark ); } -void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +// Gather the sibling pairs with a moved node. This is done serially, it is cache friendly. +static int b3GatherMovedSiblings( const b3DynamicTree* tree, int* pairIndices ) { - B3_ASSERT( proxyKey != B3_NULL_INDEX ); - int typeIndex = B3_PROXY_TYPE( proxyKey ); - int proxyId = B3_PROXY_ID( proxyKey ); + const b3TreeNode* nodes = tree->nodes; + int nodeEnd = tree->nodeEnd; - B3_ASSERT( typeIndex != b3_staticBody ); + int count = 0; - b3DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb ); - b3BufferMove( bp, proxyKey ); + // Skip the root. + for ( int pair = 2; pair < nodeEnd; pair += 2 ) + { + // Push when either sibling moved. + if ( ( nodes[pair].flagIndex | nodes[pair + 1].flagIndex ) & B3_MOVED_NODE ) + { + pairIndices[count++] = pair; + } + } + + return count; } -typedef struct b3MovePair -{ - int shapeIndexA; - int shapeIndexB; - int childIndex; - b3MovePair* next; - bool heap; -} b3MovePair; +#define B3_CANDIDATE_BATCH 32 -typedef struct b3MoveResult +typedef struct b3CandidatePair { - b3MovePair* pairList; -} b3MoveResult; + int shapeIdA; + int shapeIdB; +} b3CandidatePair; -typedef struct b3QueryPairContext +typedef struct b3PairContext { b3World* world; - b3MoveResult* moveResult; - b3AABB aabb; - b3BodyType queryTreeType; - int queryProxyKey; - int queryShapeIndex; + b3Array( uint64_t ) * pairKeys; + b3CandidatePair batch[B3_CANDIDATE_BATCH]; + int batchCount; - int compoundProxyId; - int compoundShapeIndex; -} b3QueryPairContext; + // Compound shapes are static only, so only the static cross pass can meet one. Elsewhere + // this stays false and the batch never reads a shape type. + bool checkCompounds; +} b3PairContext; -// This is called from b3DynamicTree::Query when we are gathering pairs. -static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) +typedef struct b3NodePair { - b3QueryPairContext* queryContext = (b3QueryPairContext*)context; - b3World* world = queryContext->world; - int shapeIndex; - int childIndex = 0; + b3TreeNode a; + b3TreeNode b; +} b3NodePair; - if ( queryContext->compoundShapeIndex == B3_NULL_INDEX ) - { - // Outer query: userData is a shape index. - shapeIndex = (int)userData; +typedef struct b3IndexPair +{ + int a, b; +} b3IndexPair; - // A proxy cannot form a pair with itself. - if ( shapeIndex == queryContext->queryShapeIndex ) - { - return true; - } +static inline void b3PrefetchHash( b3HashSet* set, uint32_t hash ) +{ + uint32_t capacity = set->capacity; + uint32_t index = hash & ( capacity - 1 ); + b3Prefetch( set->items + index ); +} - b3Shape* shape = b3Array_Get( world->shapes, shapeIndex ); - if ( shape->type == b3_compoundShape ) - { - // Query bounds are float world space, so the demoted transform is the matching float frame - b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, shape->bodyId ), b3Pos_zero ); - b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), queryContext->aabb ); - - // recurse - queryContext->compoundShapeIndex = shapeIndex; - queryContext->compoundProxyId = proxyId; - - b3DynamicTree_Query( &shape->compound->tree, localAABB, B3_DEFAULT_MASK_BITS, false, b3PairQueryCallback, context ); - queryContext->compoundShapeIndex = B3_NULL_INDEX; - queryContext->compoundProxyId = B3_NULL_INDEX; - return true; - } - } - else - { - // Inner query into a compound shape: userData is the compound child index, not a shape - // index, so do not compare it against queryShapeIndex. - shapeIndex = queryContext->compoundShapeIndex; - proxyId = queryContext->compoundProxyId; - childIndex = (int)userData; - } - - b3BroadPhase* broadPhase = &queryContext->world->broadPhase; - - int proxyKey = B3_PROXY_KEY( proxyId, queryContext->queryTreeType ); - int queryProxyKey = queryContext->queryProxyKey; - - // A proxy cannot form a pair with itself. - B3_ASSERT( proxyKey != queryContext->queryProxyKey ); - - b3BodyType treeType = queryContext->queryTreeType; - b3BodyType queryProxyType = B3_PROXY_TYPE( queryProxyKey ); - - // De-duplication - // It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates - // early and in the worker. Most of the time the movedProxies bit sets contain dynamic and kinematic - // proxies, but sometimes static proxies are in there too (b3ShapeDef::invokeContactCreation or a - // modified static shape), so we always have to check. - - // Is this proxy also moving? - if ( queryProxyType == b3_dynamicBody ) - { - if ( treeType == b3_dynamicBody && proxyKey < queryProxyKey ) - { - bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); - if ( moved ) - { - // Both proxies are moving. Avoid duplicate pairs. - return true; - } - } - } - else - { - B3_ASSERT( treeType == b3_dynamicBody ); - bool moved = b3GetBit( &broadPhase->movedProxies[treeType], proxyId ); - if ( moved ) - { - // Both proxies are moving. Avoid duplicate pairs. - return true; - } - } - - uint64_t pairKey = b3ShapePairKey( shapeIndex, queryContext->queryShapeIndex, childIndex ); - if ( b3ContainsKey( &broadPhase->pairSet, pairKey ) ) - { - // contact exists - return true; - } - - // Order shapes so that B3_SHAPE_PAIR_KEY works correctly - int shapeIdA = shapeIndex; - int shapeIdB = queryContext->queryShapeIndex; +// The shape pair gauntlet. Everything here is a property of the pair, not of a compound child, +// so a compound runs it once for the whole pair. +static bool b3ShouldCreatePair( b3World* world, int shapeIdA, int shapeIdB ) +{ b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + int bodyIdA = shapeA->bodyId; int bodyIdB = shapeB->bodyId; // Are the shapes on the same body? if ( bodyIdA == bodyIdB ) { - return true; + return false; } // Sensors are handled elsewhere if ( shapeA->sensorIndex != B3_NULL_INDEX || shapeB->sensorIndex != B3_NULL_INDEX ) { - return true; + return false; } if ( b3ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false ) { - return true; + return false; } // Does a joint override collision? @@ -277,111 +171,451 @@ static bool b3PairQueryCallback( int proxyId, uint64_t userData, void* context ) b3Body* bodyB = b3Array_Get( world->bodies, bodyIdB ); if ( b3ShouldBodiesCollide( world, bodyA, bodyB ) == false ) { - return true; + return false; } // Custom user filter if ( ( shapeA->flags & b3_enableCustomFiltering ) || ( shapeB->flags & b3_enableCustomFiltering ) ) { - b3CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + b3CustomFilterFcn* customFilterFcn = world->customFilterFcn; if ( customFilterFcn != NULL ) { b3ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation }; b3ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation }; - bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); - if ( shouldCollide == false ) + if ( customFilterFcn( idA, idB, world->customFilterContext ) == false ) { - return true; + return false; } } } - // todo per thread to eliminate atomic? - int pairIndex = b3AtomicFetchAddInt( &broadPhase->movePairIndex, 1 ); - - b3MovePair* pair; - if ( pairIndex < broadPhase->movePairCapacity ) - { - pair = broadPhase->movePairs + pairIndex; - pair->heap = false; - } - else - { - // todo experimenting with ignoring this pair if we ran out of space - return true; - // pair = (b3MovePair*)b3Alloc( sizeof( b3MovePair ) ); - // pair->heap = true; - } - - pair->shapeIndexA = shapeIdA; - pair->shapeIndexB = shapeIdB; - pair->childIndex = childIndex; - pair->next = queryContext->moveResult->pairList; - queryContext->moveResult->pairList = pair; - - // continue the query return true; } -static void b3FindPairsTask( int startIndex, int endIndex, int workerIndex, void* context ) +typedef struct b3CompoundPairContext { - b3TracyCZoneNC( pair_task, "Pair Task", b3_colorAquamarine, true ); + b3PairContext* pairContext; + b3HashSet* pairSet; + int compoundShapeId; + int otherShapeId; +} b3CompoundPairContext; - B3_UNUSED( workerIndex ); +static bool b3CompoundChildCallback( int proxyId, uint64_t userData, void* context ) +{ + B3_UNUSED( proxyId ); - b3World* world = (b3World*)context; + b3CompoundPairContext* compoundContext = context; + int childIndex = (int)userData; + + uint64_t key = b3ShapePairKey( compoundContext->compoundShapeId, compoundContext->otherShapeId, childIndex ); + if ( b3ContainsKey( compoundContext->pairSet, key ) == false ) + { + b3Array_Push( *compoundContext->pairContext->pairKeys, key ); + } + + return true; +} + +// One of the two shapes is a compound, so the pair becomes one pair per overlapping child. The +// child boxes live in the compound frame, so the other shape's box has to be demoted and pulled +// into that frame first. +static void b3EmitCompoundPairs( b3PairContext* context, int compoundShapeId, int otherShapeId ) +{ + b3World* world = context->world; + + b3Shape* compoundShape = b3Array_Get( world->shapes, compoundShapeId ); + b3Shape* otherShape = b3Array_Get( world->shapes, otherShapeId ); + + int otherProxyKey = otherShape->proxyKey; + B3_ASSERT( otherProxyKey != B3_NULL_INDEX ); + b3AABB otherAABB = + b3DynamicTree_GetAABB( world->broadPhase.trees + B3_PROXY_TYPE( otherProxyKey ), B3_PROXY_ID( otherProxyKey ) ); + + // Query bounds are float world space, so the demoted transform is the matching float frame + b3Transform compoundTransform = b3ToRelativeTransform( b3GetBodyTransform( world, compoundShape->bodyId ), b3Pos_zero ); + b3AABB localAABB = b3AABB_Transform( b3InvertTransform( compoundTransform ), otherAABB ); + + b3CompoundPairContext compoundContext = { + .pairContext = context, + .pairSet = &world->broadPhase.pairSet, + .compoundShapeId = compoundShapeId, + .otherShapeId = otherShapeId, + }; + + int startCount = context->pairKeys->count; + + b3DynamicTree_Query( &compoundShape->compound->tree, localAABB, B3_DEFAULT_MASK_BITS, false, b3CompoundChildCallback, + &compoundContext ); + + if ( context->pairKeys->count > startCount && b3ShouldCreatePair( world, compoundShapeId, otherShapeId ) == false ) + { + b3Array_Resize( *context->pairKeys, startCount ); + } +} + +static void b3FlushCandidatePairs( b3PairContext* context ) +{ + b3World* world = context->world; b3BroadPhase* bp = &world->broadPhase; - b3QueryPairContext queryContext = { 0 }; - queryContext.world = world; - queryContext.compoundShapeIndex = B3_NULL_INDEX; + int count1 = context->batchCount; + context->batchCount = 0; + + b3CandidatePair plain[B3_CANDIDATE_BATCH]; + int plainCount = 0; + + if ( context->checkCompounds ) + { + // A compound expands into one pair per child, so its key is not known yet and it cannot + // ride the batched cull below. + const b3Shape* shapes = world->shapes.data; + for ( int i = 0; i < count1; ++i ) + { + b3CandidatePair candidate = context->batch[i]; + bool compoundA = shapes[candidate.shapeIdA].type == b3_compoundShape; + bool compoundB = shapes[candidate.shapeIdB].type == b3_compoundShape; + + if ( compoundA ) + { + // Compound versus compound is not supported + B3_ASSERT( compoundB == false ); + b3EmitCompoundPairs( context, candidate.shapeIdA, candidate.shapeIdB ); + } + else if ( compoundB ) + { + b3EmitCompoundPairs( context, candidate.shapeIdB, candidate.shapeIdA ); + } + else + { + plain[plainCount] = candidate; + plainCount += 1; + } + } + } + else + { + memcpy( plain, context->batch, count1 * sizeof( b3CandidatePair ) ); + plainCount = count1; + } + + // Prefetch hash set entries. Less than 1% gain in Box2D but the 3D shape is fatter. + uint64_t keys[B3_CANDIDATE_BATCH]; + uint32_t hashes[B3_CANDIDATE_BATCH]; + for ( int i = 0; i < plainCount; ++i ) + { + keys[i] = b3ShapePairKey( plain[i].shapeIdA, plain[i].shapeIdB, 0 ); + hashes[i] = b3KeyHash( keys[i] ); + b3PrefetchHash( &bp->pairSet, hashes[i] ); + } + + // Cull existing pairs. + b3CandidatePair candidates[B3_CANDIDATE_BATCH]; + int count2 = 0; + for ( int i = 0; i < plainCount; ++i ) + { + if ( b3ContainsHashedKey( &bp->pairSet, keys[i], hashes[i] ) == false ) + { + candidates[count2] = plain[i]; + keys[count2] = keys[i]; + count2 += 1; + } + } + + // Prefetch shapes. + const b3Shape* shapes = world->shapes.data; + for ( int i = 0; i < count2; ++i ) + { + b3Prefetch( shapes + candidates[i].shapeIdA ); + b3Prefetch( shapes + candidates[i].shapeIdB ); + } + + // Filter candidates. + for ( int i = 0; i < count2; ++i ) + { + if ( b3ShouldCreatePair( world, candidates[i].shapeIdA, candidates[i].shapeIdB ) ) + { + // The pair passed the gauntlet. A new contact will be created. + b3Array_Push( *context->pairKeys, keys[i] ); + } + } +} + +B3_FORCE_INLINE void b3AddCandidatePair( int shapeIdA, int shapeIdB, b3PairContext* context ) +{ + // Follow shape index order. + b3CandidatePair* candidate = context->batch + context->batchCount; + candidate->shapeIdA = b3MinInt( shapeIdA, shapeIdB ); + candidate->shapeIdB = b3MaxInt( shapeIdA, shapeIdB ); + context->batchCount += 1; + if ( context->batchCount == B3_CANDIDATE_BATCH ) + { + b3FlushCandidatePairs( context ); + } +} + +// Did either move and if so do they overlap? +B3_FORCE_INLINE bool b3TestPair( const b3TreeNode* a, const b3TreeNode* b ) +{ + if ( ( ( a->flagIndex | b->flagIndex ) & B3_MOVED_NODE ) == 0 ) + { + return false; + } + + return b3OverlapV( &a->aabb, &b->aabb ); +} + +static void b3CollideProxyAndSubtree( const b3TreeNode* proxy, const b3TreeNode* nodes, int pair, b3PairContext* context ) +{ + uint32_t proxyMark = proxy->flagIndex & B3_MOVED_NODE; + b3AABBV boxv = b3LoadAABBV( &proxy->aabb ); + int shapeId = proxy->shapeIndex; + + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = pair; + + while ( stackCount > 0 ) + { + pair = stack[--stackCount]; + for ( int i = 0; i < 2; ++i ) + { + const b3TreeNode* node = nodes + pair + i; + if ( ( ( node->flagIndex | proxyMark ) & B3_MOVED_NODE ) == 0 ) + { + continue; + } + + if ( b3OverlapNode( boxv, node ) == false ) + { + continue; + } + + if ( b3IsLeaf( node ) ) + { + b3AddCandidatePair( shapeId, node->shapeIndex, context ); + } + else + { + if ( stackCount < B3_TREE_STACK_SIZE ) + { + stack[stackCount++] = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); + } + } + } + } +} + +// Helper for b3CollideCrossPairs to avoid code duplication. +B3_FORCE_INLINE void b3VisitPair( const b3TreeNode* arrayA, const b3TreeNode* arrayB, const b3TreeNode* nodeA, + const b3TreeNode* nodeB, b3IndexPair* stack, int* stackCount, b3PairContext* context ) +{ + if ( b3TestPair( nodeA, nodeB ) == false ) + { + return; + } + + bool leafA = b3IsLeaf( nodeA ); + bool leafB = b3IsLeaf( nodeB ); + if ( leafA && leafB ) + { + b3AddCandidatePair( nodeA->shapeIndex, nodeB->shapeIndex, context ); + } + else if ( leafA ) + { + b3CollideProxyAndSubtree( nodeA, arrayB, b3GetLeftChild( nodeB ), context ); + } + else if ( leafB ) + { + b3CollideProxyAndSubtree( nodeB, arrayA, b3GetLeftChild( nodeA ), context ); + } + else + { + if ( *stackCount < B3_TREE_STACK_SIZE ) + { + stack[*stackCount] = B3_LITERAL( b3IndexPair ){ .a = b3GetLeftChild( nodeA ), .b = b3GetLeftChild( nodeB ) }; + *stackCount += 1; + } + else + { + B3_ASSERT( *stackCount < B3_TREE_STACK_SIZE ); + } + } +} + +// This collides two sub-trees against each other. They can live in the same dynamic tree. +// This can only generate pairs cross sub-tree, but not within a sub-tree. This fact means +// this does not generate duplicate pairs. +// For example consider the full binary tree A (B (D E) C (F G)) +// Colliding children of A (B and C) can give pairs (D,F) (D,G) (E,F) and (E,G). +// Then colliding children of B can give the pair (D,E) and for C (F,G). +// So no duplicates even when used for self-collision. +// Whenever a proxy is moved, the flag is propagated up the hierarchy to the root. So +// self collision gathers all those moved internal nodes and collides their subtrees together. +// See Real-time collision detection section 6.3.2. This is faster than querying every moved +// proxy against the whole tree. Scaling is linear instead of linear * log. +// Many other physics engines do this (Bepu, Rapier, etc). So nothing new here. +static void b3CollideCrossPairs( const b3TreeNode* arrayA, const b3TreeNode* arrayB, const b3TreeNode* subtreeA, + const b3TreeNode* subtreeB, b3PairContext* context ) +{ + b3IndexPair stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + + // Seed the stack. + b3VisitPair( arrayA, arrayB, subtreeA, subtreeB, stack, &stackCount, context ); + + while ( stackCount > 0 ) + { + b3IndexPair pair = stack[--stackCount]; + for ( int i = 0; i < 2; ++i ) + { + for ( int j = 0; j < 2; ++j ) + { + b3VisitPair( arrayA, arrayB, arrayA + pair.a + i, arrayB + pair.b + j, stack, &stackCount, context ); + } + } + } +} + +// This takes moved internal nodes and collides their sub-trees against each other. +static void b3SelfPairsTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( self_pairs, "Self", b3_colorCoral, true ); + + b3World* world = context; + b3BroadPhase* bp = &world->broadPhase; + const b3DynamicTree* tree = bp->trees + b3_dynamicBody; + const b3TreeNode* nodes = tree->nodes; + const int* siblingIndices = bp->movedSiblings; + + b3PairContext pairContext = { + .world = world, + .pairKeys = &world->taskContexts.data[workerIndex].pairKeys, + }; for ( int i = startIndex; i < endIndex; ++i ) { - // Initialize move result for this moved proxy - queryContext.moveResult = bp->moveResults + i; - queryContext.moveResult->pairList = NULL; - - int proxyKey = bp->moveArray.data[i]; - b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); - - int proxyId = B3_PROXY_ID( proxyKey ); - queryContext.queryProxyKey = proxyKey; - - const b3DynamicTree* baseTree = bp->trees + proxyType; - - // We have to query the tree with the fat AABB so that - // we don't fail to create a contact that may touch later. - b3AABB fatAABB = b3DynamicTree_GetAABB( baseTree, proxyId ); - queryContext.queryShapeIndex = (int)b3DynamicTree_GetUserData( baseTree, proxyId ); - queryContext.aabb = fatAABB; - - // Compound shape collision invocation is not supported - B3_VALIDATE( world->shapes.data[queryContext.queryShapeIndex].type != b3_compoundShape ); - - // Query trees. Only dynamic proxies collide with kinematic and static proxies. - // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. - // consider using bits = groupIndex > 0 ? B3_DEFAULT_MASK_BITS : maskBits - bool requireAllBits = false; - if ( proxyType == b3_dynamicBody ) - { - queryContext.queryTreeType = b3_kinematicBody; - b3DynamicTree_Query( bp->trees + b3_kinematicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, - &queryContext ); - - queryContext.queryTreeType = b3_staticBody; - b3DynamicTree_Query( bp->trees + b3_staticBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, - &queryContext ); - } - - // All proxies collide with dynamic proxies - // Using B3_DEFAULT_MASK_BITS so that b3Filter::groupIndex works. - queryContext.queryTreeType = b3_dynamicBody; - b3DynamicTree_Query( bp->trees + b3_dynamicBody, fatAABB, B3_DEFAULT_MASK_BITS, requireAllBits, b3PairQueryCallback, - &queryContext ); + int nodeIndex = siblingIndices[i]; + b3CollideCrossPairs( nodes, nodes, nodes + nodeIndex, nodes + nodeIndex + 1, &pairContext ); } - b3TracyCZoneEnd( pair_task ); + b3FlushCandidatePairs( &pairContext ); + + b3TracyCZoneEnd( self_pairs ); +} + +#define B3_CROSS_SEED_COUNT 64 +_Static_assert( ( B3_CROSS_SEED_COUNT & ( B3_CROSS_SEED_COUNT - 1 ) ) == 0, "must be power of 2" ); + +// This does a serial cross-tree breadth first search until the queue is full. Then it returns +// the queue pairs as seeds for a parallel search. +static int b3GatherCrossSeeds( const b3DynamicTree* treeA, const b3DynamicTree* treeB, b3NodePair* seeds ) +{ + const b3TreeNode* nodesA = treeA->nodes; + const b3TreeNode* nodesB = treeB->nodes; + + // Breadth-first search from the two roots as one pair. An empty tree has a sentinel root that + // survives nothing. + b3NodePair queue[2 * B3_CROSS_SEED_COUNT]; + int mask = 2 * B3_CROSS_SEED_COUNT - 1; + int head = 0; + int tail = 0; + + const b3TreeNode* rootA = nodesA + B3_ROOT_NODE; + const b3TreeNode* rootB = nodesB + B3_ROOT_NODE; + if ( b3TestPair( rootA, rootB ) ) + { + queue[tail & mask] = B3_LITERAL( b3NodePair ){ .a = *rootA, .b = *rootB }; + tail += 1; + } + + int seedCount = 0; + while ( head < tail && seedCount + ( tail - head ) + 3 < B3_CROSS_SEED_COUNT ) + { + b3NodePair pair = queue[head & mask]; + head += 1; + + if ( b3IsLeaf( &pair.a ) || b3IsLeaf( &pair.b ) ) + { + seeds[seedCount++] = pair; + continue; + } + + const b3TreeNode* a = nodesA + b3GetLeftChild( &pair.a ); + const b3TreeNode* b = nodesB + b3GetLeftChild( &pair.b ); + + // Nodes have two children each, so four combinations. + for ( int i = 0; i < 2; ++i ) + { + for ( int j = 0; j < 2; ++j ) + { + if ( b3TestPair( a + i, b + j ) ) + { + queue[tail & mask] = B3_LITERAL( b3NodePair ){ a[i], b[j] }; + tail += 1; + } + } + } + } + + while ( head < tail ) + { + seeds[seedCount] = queue[head & mask]; + seedCount += 1; + head += 1; + } + + return seedCount; +} + +typedef struct b3CrossContext +{ + b3World* world; + const b3NodePair* seeds; + int staticSeedCount; + bool checkCompounds; +} b3CrossContext; + +static void b3CrossPairsTask( int startIndex, int endIndex, int workerIndex, void* context ) +{ + b3TracyCZoneNC( cross_pairs, "Cross", b3_colorCoral, true ); + + b3CrossContext* crossContext = context; + b3World* world = crossContext->world; + b3BroadPhase* bp = &world->broadPhase; + const b3TreeNode* staticNodes = bp->trees[b3_staticBody].nodes; + const b3TreeNode* kinematicNodes = bp->trees[b3_kinematicBody].nodes; + const b3TreeNode* dynamicNodes = bp->trees[b3_dynamicBody].nodes; + + b3PairContext pairContext = { + .world = world, + .pairKeys = &world->taskContexts.data[workerIndex].pairKeys, + }; + + // Static seeds come first, so the two passes are contiguous sub-ranges. The batch is drained + // between them because only the static pass can meet a compound. + int staticEnd = b3MinInt( endIndex, crossContext->staticSeedCount ); + + pairContext.checkCompounds = crossContext->checkCompounds; + for ( int i = startIndex; i < staticEnd; ++i ) + { + b3NodePair seed = crossContext->seeds[i]; + b3CollideCrossPairs( dynamicNodes, staticNodes, &seed.a, &seed.b, &pairContext ); + } + b3FlushCandidatePairs( &pairContext ); + + pairContext.checkCompounds = false; + for ( int i = b3MaxInt( startIndex, crossContext->staticSeedCount ); i < endIndex; ++i ) + { + b3NodePair seed = crossContext->seeds[i]; + b3CollideCrossPairs( dynamicNodes, kinematicNodes, &seed.a, &seed.b, &pairContext ); + } + b3FlushCandidatePairs( &pairContext ); + + b3TracyCZoneEnd( cross_pairs ); } static void b3UpdateTreesTask( void* context ) @@ -416,89 +650,135 @@ void b3UpdateBroadPhasePairs( b3World* world ) { b3BroadPhase* bp = &world->broadPhase; - int moveCount = bp->moveArray.count; + bool needUpdate = b3HasTreeMoved( bp->trees + b3_staticBody ); + needUpdate = needUpdate || b3NeedsRebuild( bp->trees + b3_kinematicBody ); + needUpdate = needUpdate || b3NeedsRebuild( bp->trees + b3_dynamicBody ); - if ( moveCount == 0 ) + if ( needUpdate == false ) { - // A destroyed shape may lead to no moves, but the tree could still be enlarged. - b3EnqueueTreeUpdate( world ); return; } - b3TracyCZoneNC( update_pairs, "Pairs", b3_colorMediumSlateBlue, true ); + b3TracyCZoneNC( update_pairs, "Find Pairs", b3_colorMediumSlateBlue, true ); b3Stack* alloc = &world->stack; - // todo these could be in the step context - bp->moveResults = (b3MoveResult*)b3StackAlloc( alloc, moveCount * sizeof( b3MoveResult ), "move results" ); - bp->movePairCapacity = 16 * moveCount; - bp->movePairs = (b3MovePair*)b3StackAlloc( alloc, bp->movePairCapacity * sizeof( b3MovePair ), "move pairs" ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b3Array_Clear( world->taskContexts.data[i].pairKeys ); + } - b3AtomicStoreInt( &bp->movePairIndex, 0 ); + // Generate pairs by querying the dynamic body tree against itself and against + // the kinematic and static trees. + { + // Get the sibling pairs of the dynamic body tree that have moved. + const b3DynamicTree* dynamicTree = bp->trees + b3_dynamicBody; + int pairCapacity = b3MaxInt( dynamicTree->nodeEnd / 2, 1 ); + bp->movedSiblings = b3StackAlloc( alloc, pairCapacity * sizeof( int ), "moved pairs" ); + int dynamicMoveCount = b3GatherMovedSiblings( dynamicTree, bp->movedSiblings ); -#ifndef NDEBUG - extern b3AtomicInt b3_probeCount; - b3AtomicStoreInt( &b3_probeCount, 0 ); -#endif + // Get seeds for colliding against the static and kinematic trees. + b3NodePair crossSeeds[2 * B3_CROSS_SEED_COUNT]; + int staticSeedCount = b3GatherCrossSeeds( dynamicTree, bp->trees + b3_staticBody, crossSeeds ); + B3_ASSERT( staticSeedCount <= B3_CROSS_SEED_COUNT ); + int kinematicSeedCount = b3GatherCrossSeeds( dynamicTree, bp->trees + b3_kinematicBody, crossSeeds + staticSeedCount ); + B3_ASSERT( kinematicSeedCount <= B3_CROSS_SEED_COUNT ); + int crossMoveCount = staticSeedCount + kinematicSeedCount; - int minRange = 64; - b3ParallelFor( world, b3FindPairsTask, moveCount, minRange, world, "pairs" ); + // Collide the dynamic body tree against the static and kinematic trees. + b3CrossContext crossContext = { + .world = world, + .seeds = crossSeeds, + .staticSeedCount = staticSeedCount, + .checkCompounds = world->compoundShapeCount > 0, + }; + b3ParallelFor( world, &b3CrossPairsTask, crossMoveCount, 1, &crossContext, "cross pairs" ); + + // Collide the dynamic body tree against itself. + b3ParallelFor( world, &b3SelfPairsTask, dynamicMoveCount, 64, world, "self pairs" ); + } + + b3DynamicTree_ClearMoved( bp->trees + b3_staticBody ); + + b3TracyCZoneEnd( update_pairs ); b3TracyCZoneNC( create_contacts, "Create Contacts", b3_colorCoral, true ); // Update stale trees. b3EnqueueTreeUpdate( world ); - // Single-threaded work - // - Clear move flags - // - Create contacts in deterministic order - for ( int i = 0; i < moveCount; ++i ) + // Pairs arrive in deterministic order but scrambled relative to body and shape order. + // Sorting them here improves solver performance and makes contact order independent of + // tree structure. + int pairCount = 0; + for ( int i = 0; i < world->workerCount; ++i ) { - b3MoveResult* result = bp->moveResults + i; - b3MovePair* pair = result->pairList; - while ( pair != NULL ) + pairCount += world->taskContexts.data[i].pairKeys.count; + } + + uint64_t* pairKeys = b3StackAlloc( alloc, b3MaxInt( pairCount, 1 ) * sizeof( uint64_t ), "pair keys" ); + int keyCount = 0; + for ( int i = 0; i < world->workerCount; ++i ) + { + const b3Array( uint64_t )* workerKeys = &world->taskContexts.data[i].pairKeys; + if ( workerKeys->count > 0 ) { - int shapeIdA = pair->shapeIndexA; - int shapeIdB = pair->shapeIndexB; - int childIndex = pair->childIndex; - - b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); - b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); - - b3CreateContact( world, shapeA, shapeB, childIndex ); - - if ( pair->heap ) - { - b3MovePair* temp = pair; - pair = pair->next; - b3Free( temp, sizeof( b3MovePair ) ); - } - else - { - pair = pair->next; - } + memcpy( pairKeys + keyCount, workerKeys->data, workerKeys->count * sizeof( uint64_t ) ); + keyCount += workerKeys->count; } } - // Reset move buffer: clear only the bits that were set this step. - // Invariant: bit set in movedProxies[type] iff proxyKey is present in moveArray. - for ( int i = 0; i < bp->moveArray.count; ++i ) - { - int proxyKey = bp->moveArray.data[i]; - b3ClearBit( &bp->movedProxies[B3_PROXY_TYPE( proxyKey )], B3_PROXY_ID( proxyKey ) ); - } - b3Array_Clear( bp->moveArray ); + B3_ASSERT( keyCount == pairCount ); - b3StackFree( alloc, bp->movePairs ); - bp->movePairs = NULL; - b3StackFree( alloc, bp->moveResults ); - bp->moveResults = NULL; + { +#define LESS( i, j ) ( pairKeys[(int)( i )] < pairKeys[(int)( j )] ) +#define SWAP( i, j ) \ + do \ + { \ + uint64_t tmp_ = pairKeys[(int)( i )]; \ + pairKeys[(int)( i )] = pairKeys[(int)( j )]; \ + pairKeys[(int)( j )] = tmp_; \ + } \ + while ( 0 ) + + QSORT( pairCount, LESS, SWAP ); + +#undef LESS +#undef SWAP + } + + for ( int i = 0; i < keyCount; ++i ) + { + uint64_t key = pairKeys[i]; + + // The traversal is cross subtree and the seeds are disjoint, so a duplicate key means a + // real bug. Trap it in a validation build, and skip rather than double add to the pair + // set, which a later destroy would then remove once. + bool duplicate = i > 0 && key == pairKeys[i - 1]; + B3_VALIDATE( duplicate == false ); + if ( duplicate ) + { + continue; + } + + int shapeIdA = (int)( ( key >> ( 64 - B3_SHAPE_POWER ) ) & B3_SHAPE_MASK ); + int shapeIdB = (int)( ( key >> ( 64 - 2 * B3_SHAPE_POWER ) ) & B3_SHAPE_MASK ); + int childIndex = (int)( key & B3_CHILD_MASK ); + + b3Shape* shapeA = b3Array_Get( world->shapes, shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, shapeIdB ); + + b3CreateContact( world, shapeA, shapeB, childIndex ); + } + + b3StackFree( alloc, pairKeys ); + + b3StackFree( alloc, bp->movedSiblings ); + bp->movedSiblings = NULL; b3ValidateSolverSets( world ); b3TracyCZoneEnd( create_contacts ); - - b3TracyCZoneEnd( update_pairs ); } bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ) @@ -529,13 +809,13 @@ void b3ValidateBroadPhase( const b3BroadPhase* bp ) // todo validate every shape AABB is contained in tree AABB } -void b3ValidateNoEnlarged( const b3BroadPhase* bp ) +void b3ValidateNoMoved( const b3BroadPhase* bp ) { #if B3_ENABLE_VALIDATION == 1 for ( int j = 0; j < b3_bodyTypeCount; ++j ) { const b3DynamicTree* tree = bp->trees + j; - b3DynamicTree_ValidateNoEnlarged( tree ); + b3DynamicTree_ValidateNoMoved( tree ); } #else B3_UNUSED( bp ); diff --git a/src/broad_phase.h b/src/broad_phase.h index dcd626b..4564f9f 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -3,16 +3,13 @@ #pragma once -#include "bitset.h" -#include "container.h" +#include "dynamic_tree.h" #include "table.h" #include "box3d/collision.h" #include "box3d/types.h" typedef struct b3Shape b3Shape; -typedef struct b3MovePair b3MovePair; -typedef struct b3MoveResult b3MoveResult; typedef struct b3Stack b3Stack; typedef struct b3World b3World; @@ -26,20 +23,11 @@ typedef struct b3World b3World; /// It is up to the client to consume the new pairs and to track subsequent overlap. typedef struct b3BroadPhase { + // One tree for each body type. b3DynamicTree trees[b3_bodyTypeCount]; - // Per body-type bit sets indexed by proxyId, marking proxies moved this step. - // Paired with moveArray which preserves deterministic insertion order for pair queries. - b3BitSet movedProxies[b3_bodyTypeCount]; - b3Array( int ) moveArray; - - // These are the results from the pair query and are used to create new contacts - // in deterministic order. - // todo these could be in the step context - b3MoveResult* moveResults; - b3MovePair* movePairs; - int movePairCapacity; - b3AtomicInt movePairIndex; + // The moved siblings gathered from the dynamic body tree. + int* movedSiblings; // Tracks shape pairs that have a b3Contact // todo pairSet can grow quite large on the first time step and remain large @@ -54,7 +42,6 @@ int b3BroadPhase_CreateProxy( b3BroadPhase* bp, b3BodyType proxyType, b3AABB aab void b3BroadPhase_DestroyProxy( b3BroadPhase* bp, int proxyKey ); void b3BroadPhase_MoveProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); -void b3BroadPhase_EnlargeProxy( b3BroadPhase* bp, int proxyKey, b3AABB aabb ); int b3BroadPhase_GetShapeIndex( b3BroadPhase* bp, int proxyKey ); @@ -62,18 +49,18 @@ void b3UpdateBroadPhasePairs( b3World* world ); bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyKeyB ); void b3ValidateBroadPhase( const b3BroadPhase* bp ); -void b3ValidateNoEnlarged( const b3BroadPhase* bp ); +void b3ValidateNoMoved( const b3BroadPhase* bp ); -// This is what triggers new contact pairs to be created -// Warning: this must be called in deterministic order -static inline void b3BufferMove( b3BroadPhase* bp, int queryProxy ) +static inline void b3BroadPhase_MarkProxyMovedSerial( b3BroadPhase* bp, int proxyKey ) { - b3BodyType proxyType = B3_PROXY_TYPE( queryProxy ); - int proxyId = B3_PROXY_ID( queryProxy ); - b3BitSet* set = &bp->movedProxies[proxyType]; - if ( b3GetBit( set, proxyId ) == false ) - { - b3SetBitGrow( set, proxyId ); - b3Array_Push( bp->moveArray, queryProxy ); - } + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + b3DynamicTree_MarkProxyMovedSerial( bp->trees + proxyType, proxyId ); +} + +static inline void b3BroadPhase_MarkProxyMoved( b3BroadPhase* bp, int proxyKey, b3AABB aabb ) +{ + b3BodyType proxyType = B3_PROXY_TYPE( proxyKey ); + int proxyId = B3_PROXY_ID( proxyKey ); + b3DynamicTree_MarkProxyMoved( bp->trees + proxyType, proxyId, aabb ); } diff --git a/src/compound.c b/src/compound.c index 886935c..38b5571 100644 --- a/src/compound.c +++ b/src/compound.c @@ -3,6 +3,7 @@ #include "compound.h" +#include "dynamic_tree.h" #include "hull.h" #include "math_internal.h" #include "shape.h" @@ -52,6 +53,16 @@ static inline b3TreeNode* b3GetCompoundNodes( b3CompoundData* compound ) return (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); } +static inline b3TreeProxy* b3GetCompoundProxies( b3CompoundData* compound ) +{ + if ( compound->proxyOffset == 0 ) + { + return NULL; + } + + return (b3TreeProxy*)( (intptr_t)compound + compound->proxyOffset ); +} + const b3SurfaceMaterial* b3GetCompoundMaterials( const b3CompoundData* compound ) { if ( compound->materialOffset == 0 ) @@ -274,13 +285,13 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) // Instances int capsuleCount = def->capsuleCount; - b3CompoundCapsule* capsuleInstances = b3AllocZeroed( capsuleCount * sizeof( b3CompoundCapsule ) ); + b3CompoundCapsule* capsuleInstances = b3AllocZero( capsuleCount * sizeof( b3CompoundCapsule ) ); int hullCount = def->hullCount; - b3HullInstance* hullInstances = b3AllocZeroed( hullCount * sizeof( b3HullInstance ) ); + b3HullInstance* hullInstances = b3AllocZero( hullCount * sizeof( b3HullInstance ) ); int meshCount = def->meshCount; - b3MeshInstance* meshInstances = b3AllocZeroed( meshCount * sizeof( b3MeshInstance ) ); + b3MeshInstance* meshInstances = b3AllocZero( meshCount * sizeof( b3MeshInstance ) ); int sphereCount = def->sphereCount; - b3CompoundSphere* sphereInstances = b3AllocZeroed( sphereCount * sizeof( b3CompoundSphere ) ); + b3CompoundSphere* sphereInstances = b3AllocZero( sphereCount * sizeof( b3CompoundSphere ) ); // Determine material capacity. int materialCapacity = convexCount; @@ -294,7 +305,7 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) b3MaterialMap materialMap; b3MaterialMap_init( &materialMap ); b3MaterialMap_reserve( &materialMap, materialCapacity ); - b3SurfaceMaterial* materials = b3AllocZeroed( materialCapacity * sizeof( b3SurfaceMaterial ) ); + b3SurfaceMaterial* materials = b3AllocZero( materialCapacity * sizeof( b3SurfaceMaterial ) ); int materialCount = 0; for ( int i = 0; i < def->capsuleCount; ++i ) @@ -322,7 +333,7 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) } // Hulls - b3SharedHull* sharedHulls = b3AllocZeroed( hullCount * sizeof( b3SharedHull ) ); + b3SharedHull* sharedHulls = b3AllocZero( hullCount * sizeof( b3SharedHull ) ); int sharedHullCount = 0; if ( hullCount > 0 ) @@ -378,7 +389,7 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) } // Meshes - b3SharedMesh* sharedMeshes = b3AllocZeroed( meshCount * sizeof( b3SharedMesh ) ); + b3SharedMesh* sharedMeshes = b3AllocZero( meshCount * sizeof( b3SharedMesh ) ); int sharedMeshCount = 0; if ( meshCount > 0 ) @@ -471,14 +482,16 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) } B3_ASSERT( materialCount <= materialCapacity ); - B3_ASSERT( tree.nodeCount > 0 ); + B3_ASSERT( tree.proxyCount > 0 ); b3DynamicTree_Rebuild( &tree, true ); - // Tree nodes + // Tree nodes and proxies. The rebuild is dense, so only the live nodes travel. size_t byteCount = b3AlignUp8( sizeof( b3CompoundData ) ); int nodeOffset = (int)byteCount; - byteCount += b3AlignUp8( tree.nodeCapacity * sizeof( b3TreeNode ) ); + byteCount += b3AlignUp8( tree.nodeEnd * sizeof( b3TreeNode ) ); + int proxyOffset = (int)byteCount; + byteCount += b3AlignUp8( tree.proxyCount * sizeof( b3TreeProxy ) ); int materialOffset = (int)byteCount; byteCount += b3AlignUp8( materialCount * sizeof( b3SurfaceMaterial ) ); int capsuleOffset = (int)byteCount; @@ -525,17 +538,25 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) compound->version = B3_COMPOUND_VERSION; compound->byteCount = (int)byteCount; compound->nodeOffset = nodeOffset; + compound->proxyOffset = proxyOffset; memcpy( &compound->tree, &tree, sizeof( b3DynamicTree ) ); // todo clean up this mess - compound->tree.freeList = 0; + compound->tree.nodeCapacity = tree.nodeEnd; + compound->tree.pairFreeList = B3_NULL_INDEX; + compound->tree.proxyCapacity = tree.proxyCount; + compound->tree.proxyFreeList = B3_NULL_INDEX; + compound->tree.swapNodes = NULL; compound->tree.leafIndices = NULL; + compound->tree.leafNodes = NULL; compound->tree.leafBoxes = NULL; compound->tree.leafCenters = NULL; compound->tree.binIndices = NULL; compound->tree.rebuildCapacity = 0; compound->tree.nodes = NULL; + compound->tree.parents = NULL; + compound->tree.proxies = NULL; compound->materialOffset = materialOffset; compound->materialCount = materialCount; compound->capsuleOffset = capsuleOffset; @@ -547,11 +568,15 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) compound->sphereOffset = sphereOffset; compound->sphereCount = sphereCount; - // Tree nodes + // Tree nodes and proxies b3TreeNode* nodes = b3GetCompoundNodes( compound ); - memcpy( nodes, tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); + memcpy( nodes, tree.nodes, tree.nodeEnd * sizeof( b3TreeNode ) ); compound->tree.nodes = nodes; + b3TreeProxy* proxies = b3GetCompoundProxies( compound ); + memcpy( proxies, tree.proxies, tree.proxyCount * sizeof( b3TreeProxy ) ); + compound->tree.proxies = proxies; + // Materials B3_ASSERT( materialCount > 0 ); b3SurfaceMaterial* destinationMaterials = (b3SurfaceMaterial*)( (intptr_t)compound + compound->materialOffset ); @@ -638,8 +663,9 @@ void b3DestroyCompound( b3CompoundData* compound ) uint8_t* b3ConvertCompoundToBytes( b3CompoundData* compound ) { - // scrub this pointer before serialization + // scrub these pointers before serialization compound->tree.nodes = NULL; + compound->tree.proxies = NULL; return (uint8_t*)compound; } @@ -661,13 +687,14 @@ b3CompoundData* b3ConvertBytesToCompound( uint8_t* bytes, int byteCount ) return NULL; } - if ( compound->nodeOffset <= 0 ) + if ( compound->nodeOffset <= 0 || compound->proxyOffset <= 0 ) { return NULL; } // this mutates the input bytes compound->tree.nodes = (b3TreeNode*)( (intptr_t)compound + compound->nodeOffset ); + compound->tree.proxies = (b3TreeProxy*)( (intptr_t)compound + compound->proxyOffset ); return compound; } @@ -676,8 +703,7 @@ b3AABB b3ComputeCompoundAABB( const b3CompoundData* shape, b3Transform transform B3_ASSERT( shape->nodeOffset > 0 ); const b3TreeNode* nodes = (const b3TreeNode*)( (intptr_t)shape + shape->nodeOffset ); - int root = shape->tree.root; - b3AABB aabb = nodes[root].aabb; + b3AABB aabb = nodes[B3_ROOT_NODE].aabb; return b3AABB_Transform( transform, aabb ); } diff --git a/src/container.h b/src/container.h index 2831cb6..471db1a 100644 --- a/src/container.h +++ b/src/container.h @@ -182,3 +182,4 @@ B3_INLINE int b3RemoveHelper( void* data, int* count, int index, int elementSize #define b3Array_ByteCount( a ) ( ( a ).capacity * (int)sizeof( *( a ).data ) ) b3DeclareArrayNative( int ); +b3DeclareArrayNative( uint64_t ); diff --git a/src/core.c b/src/core.c index 910a752..0235855 100644 --- a/src/core.c +++ b/src/core.c @@ -230,12 +230,19 @@ void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ) return newMem; } +void* b3GrowAllocZeroed( void* oldMem, int oldSize, int newSize ) +{ + void* newMem = b3GrowAlloc( oldMem, oldSize, newSize ); + memset( (char*)newMem + oldSize, 0, (size_t)( newSize - oldSize ) ); + return newMem; +} + int b3GetByteCount( void ) { return b3AtomicLoadInt( &b3_byteCount ); } -void* b3AllocZeroed( size_t size ) +void* b3AllocZero( size_t size ) { void* mem = b3Alloc( size ); memset( mem, 0, size ); diff --git a/src/core.h b/src/core.h index 8482638..46eafa8 100644 --- a/src/core.h +++ b/src/core.h @@ -133,9 +133,13 @@ typedef struct b3AtomicU32 #define B3_FREE( M, T, N ) b3Free( M, N * sizeof( T ) ); void* b3Alloc( size_t size ); -void* b3AllocZeroed( size_t size ); +void* b3AllocZero( size_t size ); void b3Free( void* mem, size_t size ); void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ); +void* b3GrowAllocZeroed( void* oldMem, int oldSize, int newSize ); + +#define B3_GROW( A, N, M ) b3GrowAlloc( ( A ), (int)( ( N ) * sizeof( *( A ) ) ), (int)( ( M ) * sizeof( *( A ) ) ) ) +#define B3_GROW_ZERO( A, N, M ) b3GrowAllocZeroed( ( A ), (int)( ( N ) * sizeof( *( A ) ) ), (int)( ( M ) * sizeof( *( A ) ) ) ) B3_PRINTF_FORMAT( 1, 2 ) void b3Log( const char* format, ... ); diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 98370dc..3ec9d36 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1,9 +1,12 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "dynamic_tree.h" + #include "aabb.h" +#include "algorithm.h" #include "core.h" -#include "joint.h" +#include "platform.h" #include "simd.h" #include "box3d/collision.h" @@ -14,69 +17,100 @@ #include #include -#define B3_TREE_STACK_SIZE 1024 +_Static_assert( B3_ROOT_NODE == 0, "bad root" ); +_Static_assert( sizeof( b3TreeNode ) == 32, "expected size" ); +_Static_assert( sizeof( b3TreeProxy ) == 24, "expected size" ); -static b3TreeNode b3_defaultTreeNode = { - .aabb = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f } }, - .categoryBits = B3_DEFAULT_CATEGORY_BITS, - .children = - { - .child1 = B3_NULL_INDEX, - .child2 = B3_NULL_INDEX, - }, - .parent = B3_NULL_INDEX, - .height = 0, - .flags = b3_allocatedNode, -}; - -static inline bool b3IsLeaf( const b3TreeNode* node ) +// 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 ) { - return node->flags & b3_leafNode; + // 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 inline bool b3IsAllocated( const b3TreeNode* node ) +static void b3FreePair( b3DynamicTree* tree, int pair ) { - return node->flags & b3_allocatedNode; -} - -static inline uint16_t b3MaxUInt16( uint16_t a, uint16_t b ) -{ - return a > b ? a : b; + 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; - tree.root = B3_NULL_INDEX; - // maximum node count for a full binary tree is 2 * leafCount - 1 - tree.nodeCapacity = 2 * capacity - 1; - tree.nodeCount = 0; + // 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; - tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); + // 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; - memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b3TreeNode ) ); + // 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. - // todo use a bump allocator until the capacity is consumed (see b3PoolAllocator) - for ( int i = 0; i < tree.nodeCapacity - 1; ++i ) + for ( int i = 0; i < tree.proxyCapacity - 1; ++i ) { - tree.nodes[i].next = i + 1; + tree.proxies[i].node = B3_NULL_INDEX; + tree.proxies[i].next = i + 1; } - tree.nodes[tree.nodeCapacity - 1].next = B3_NULL_INDEX; - tree.freeList = 0; - - tree.proxyCount = 0; + 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; @@ -88,61 +122,113 @@ b3DynamicTree b3DynamicTree_Create( int proxyCapacity ) void b3DynamicTree_Destroy( b3DynamicTree* tree ) { b3Free( tree->nodes, tree->nodeCapacity * sizeof( b3TreeNode ) ); - b3Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + 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( int ) ); + b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) ); memset( tree, 0, sizeof( b3DynamicTree ) ); } -// Allocate a node from the pool. Grow the pool if necessary. -static int b3AllocateNode( b3DynamicTree* tree ) +// Allocate a proxy from the pool. Grow the pool if necessary. +static int b3AllocateProxy( b3DynamicTree* tree ) { - // Expand the node pool as needed. - if ( tree->freeList == B3_NULL_INDEX ) + // Expand the pool as needed. + if ( tree->proxyFreeList == B3_NULL_INDEX ) { - B3_ASSERT( tree->nodeCount == tree->nodeCapacity ); + B3_ASSERT( tree->proxyCount == tree->proxyCapacity ); // The free list is empty. Rebuild a bigger pool. - b3TreeNode* oldNodes = tree->nodes; - int oldCapacity = tree->nodeCapacity; - tree->nodeCapacity += oldCapacity >> 1; - tree->nodes = (b3TreeNode*)b3Alloc( tree->nodeCapacity * sizeof( b3TreeNode ) ); - B3_ASSERT( oldNodes != NULL ); - memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b3TreeNode ) ); - memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b3TreeNode ) ); - b3Free( oldNodes, oldCapacity * sizeof( b3TreeNode ) ); + // 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. The parent pointer becomes the "next" pointer. - // todo avoid building freelist? - for ( int i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + // Build a linked list for the free list. + for ( int i = oldCapacity; i < tree->proxyCapacity - 1; ++i ) { - tree->nodes[i].next = i + 1; + tree->proxies[i].node = B3_NULL_INDEX; + tree->proxies[i].next = i + 1; } - tree->nodes[tree->nodeCapacity - 1].next = B3_NULL_INDEX; - tree->freeList = tree->nodeCount; + tree->proxies[tree->proxyCapacity - 1].node = B3_NULL_INDEX; + tree->proxies[tree->proxyCapacity - 1].next = B3_NULL_INDEX; + tree->proxyFreeList = oldCapacity; } - // Peel a node off the free list. - int nodeIndex = tree->freeList; - b3TreeNode* node = tree->nodes + nodeIndex; - tree->freeList = node->next; - *node = b3_defaultTreeNode; - ++tree->nodeCount; - return nodeIndex; + // 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 node to the pool. -static void b3FreeNode( b3DynamicTree* tree, int nodeId ) +// Return a proxy to the pool. +static void b3FreeProxy( b3DynamicTree* tree, int proxyId ) { - B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); - B3_ASSERT( 0 < tree->nodeCount ); - tree->nodes[nodeId].next = tree->freeList; - tree->nodes[nodeId].flags = 0; - tree->freeList = nodeId; - --tree->nodeCount; + 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 @@ -168,9 +254,14 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) float areaD = b3Perimeter( boxD ); const b3TreeNode* nodes = tree->nodes; - int rootIndex = tree->root; + int nodeIndex = B3_ROOT_NODE; - b3AABB rootBox = nodes[rootIndex].aabb; + if ( b3IsLeaf( nodes + nodeIndex ) ) + { + return nodeIndex; + } + + b3AABB rootBox = nodes[nodeIndex].aabb; // Area of current node float areaBase = b3Perimeter( rootBox ); @@ -179,15 +270,14 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) float directCost = b3Perimeter( b3AABB_Union( rootBox, boxD ) ); float inheritedCost = 0.0f; - int bestSibling = rootIndex; + int bestSibling = nodeIndex; float bestCost = directCost; - // Descend the tree from root, following a single greedy path. - int index = rootIndex; - while ( b3IsLeaf( nodes + index ) == false ) + // Descend the tree, following a single greedy path. + for ( ;; ) { - int child1 = nodes[index].children.child1; - int child2 = nodes[index].children.child2; + 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; @@ -196,7 +286,7 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) // This breaks the ties using the centroid distance. if ( cost < bestCost ) { - bestSibling = index; + bestSibling = nodeIndex; bestCost = cost; } @@ -216,8 +306,6 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) // Child 1 is a leaf // Cost of creating new node and increasing area of node P float cost1 = directCost1 + inheritedCost; - - // Need this here due to while condition above if ( cost1 < bestCost ) { bestSibling = child1; @@ -229,7 +317,11 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) // Child 1 is an internal node area1 = b3Perimeter( box1 ); - // Lower bound cost of inserting under child 1. + // 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 ); } @@ -240,11 +332,7 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) float area2 = 0.0f; if ( leaf2 ) { - // Child 2 is a leaf - // Cost of creating new node and increasing area of node P float cost2 = directCost2 + inheritedCost; - - // Need this here due to while condition above if ( cost2 < bestCost ) { bestSibling = child2; @@ -253,11 +341,7 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) } else { - // Child 2 is an internal node area2 = b3Perimeter( box2 ); - - // Lower bound cost of inserting under child 2. This is not the cost - // of child 2, it is the best we can hope for under child 2. lowerCost2 = inheritedCost + directCost2 + b3MinFloat( areaD - area2, 0.0f ); } @@ -288,504 +372,231 @@ static int b3FindBestSibling( const b3DynamicTree* tree, b3AABB boxD ) // Descend if ( lowerCost1 < lowerCost2 && leaf1 == false ) { - index = child1; + nodeIndex = child1; areaBase = area1; directCost = directCost1; } else { - index = child2; + nodeIndex = child2; areaBase = area2; directCost = directCost2; } - - B3_ASSERT( b3IsLeaf( nodes + index ) == false ); } return bestSibling; } -enum b3RotateType +// 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 ) { - b3_rotateNone, - b3_rotateBF, - b3_rotateBG, - b3_rotateCD, - b3_rotateCE -}; + 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. -// Returns the new root index. +// Tree: (A (B (D E) C (F G)) static void b3RotateNodes( b3DynamicTree* tree, int iA ) { - B3_ASSERT( iA != B3_NULL_INDEX ); - b3TreeNode* nodes = tree->nodes; + const b3TreeNode* A = nodes + iA; + B3_ASSERT( b3IsLeaf( A ) == false ); - b3TreeNode* A = nodes + iA; - if ( b3IsLeaf( A ) == true ) + 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 iB = A->children.child1; - int iC = A->children.child2; - B3_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); - B3_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + int bestDown = B3_NULL_INDEX; + int bestUp = B3_NULL_INDEX; + float bestDelta = 0.0f; - b3TreeNode* B = nodes + iB; - b3TreeNode* C = nodes + iC; - - bool isLeafB = b3IsLeaf( B ); - bool isLeafC = b3IsLeaf( C ); - - if ( isLeafB == true && isLeafC == false ) + if ( leafC == false ) { - int iF = C->children.child1; - int iG = C->children.child2; - b3TreeNode* F = nodes + iF; - b3TreeNode* G = nodes + iG; - B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); - B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); - - // Base cost - float costBase = b3Perimeter( C->aabb ); - - // Cost of swapping B and F - b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); - float costBF = b3Perimeter( aabbBG ); - - // Cost of swapping B and G - b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); - float costBG = b3Perimeter( aabbBF ); - - if ( costBase < costBF && costBase < costBG ) - { - // Rotation does not improve cost - return; - } - - if ( costBF < costBG ) - { - // Swap B and F - A->children.child1 = iF; - C->children.child1 = iB; - - B->parent = iC; - F->parent = iA; - - C->aabb = aabbBG; - - C->height = 1 + b3MaxUInt16( B->height, G->height ); - A->height = 1 + b3MaxUInt16( C->height, F->height ); - C->categoryBits = B->categoryBits | G->categoryBits; - A->categoryBits = C->categoryBits | F->categoryBits; - C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; - A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; - } - else - { - // Swap B and G - A->children.child1 = iG; - C->children.child2 = iB; - - B->parent = iC; - G->parent = iA; - - C->aabb = aabbBF; - - C->height = 1 + b3MaxUInt16( B->height, F->height ); - A->height = 1 + b3MaxUInt16( C->height, G->height ); - C->categoryBits = B->categoryBits | F->categoryBits; - A->categoryBits = C->categoryBits | G->categoryBits; - C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; - A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; - } - } - else if ( isLeafC == true && isLeafB == false ) - { - // C is a leaf and B is internal - - int iD = B->children.child1; - int iE = B->children.child2; - b3TreeNode* D = nodes + iD; - b3TreeNode* E = nodes + iE; - B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); - B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); - - // Base cost - float costBase = b3Perimeter( B->aabb ); - - // Cost of swapping C and D - b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); - float costCD = b3Perimeter( aabbCE ); - - // Cost of swapping C and E - b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); - float costCE = b3Perimeter( aabbCD ); - - if ( costBase < costCD && costBase < costCE ) - { - // Rotation does not improve cost - return; - } - - if ( costCD < costCE ) - { - // Swap C and D - A->children.child2 = iD; - B->children.child1 = iC; - - C->parent = iB; - D->parent = iA; - - B->aabb = aabbCE; - - B->height = 1 + b3MaxUInt16( C->height, E->height ); - A->height = 1 + b3MaxUInt16( B->height, D->height ); - B->categoryBits = C->categoryBits | E->categoryBits; - A->categoryBits = B->categoryBits | D->categoryBits; - B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; - A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; - } - else - { - // Swap C and E - A->children.child2 = iE; - B->children.child2 = iC; - - C->parent = iB; - E->parent = iA; - - B->aabb = aabbCD; - - B->height = 1 + b3MaxUInt16( C->height, D->height ); - A->height = 1 + b3MaxUInt16( B->height, E->height ); - B->categoryBits = C->categoryBits | D->categoryBits; - A->categoryBits = B->categoryBits | E->categoryBits; - B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; - A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; - } - } - else if ( isLeafB == false && isLeafC == false ) - { - // All grand children exist so there are many options for rotation - int iD = B->children.child1; - int iE = B->children.child2; - int iF = C->children.child1; - int iG = C->children.child2; - - B3_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); - B3_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); - B3_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); - B3_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); - - b3TreeNode* D = nodes + iD; - b3TreeNode* E = nodes + iE; - b3TreeNode* F = nodes + iF; - b3TreeNode* G = nodes + iG; - - // Base cost - float areaB = b3Perimeter( B->aabb ); + // Swap B with F or G + int iF = b3GetLeftChild( C ); + int iG = iF + 1; float areaC = b3Perimeter( C->aabb ); - float costBase = areaB + areaC; - enum b3RotateType bestRotation = b3_rotateNone; - float bestCost = costBase; - // Cost of swapping B and F - b3AABB aabbBG = b3AABB_Union( B->aabb, G->aabb ); - float costBF = areaB + b3Perimeter( aabbBG ); - if ( costBF < bestCost ) + // B <-> F then C (B G) + float deltaBF = b3Perimeter( b3UnionV( B->aabb, nodes[iG].aabb ) ) - areaC; + if ( deltaBF < bestDelta ) { - bestRotation = b3_rotateBF; - bestCost = costBF; + bestDown = iB; + bestUp = iF; + bestDelta = deltaBF; } - // Cost of swapping B and G - b3AABB aabbBF = b3AABB_Union( B->aabb, F->aabb ); - float costBG = areaB + b3Perimeter( aabbBF ); - if ( costBG < bestCost ) + // B <-> G then C (F B) + float deltaBG = b3Perimeter( b3UnionV( B->aabb, nodes[iF].aabb ) ) - areaC; + if ( deltaBG < bestDelta ) { - bestRotation = b3_rotateBG; - bestCost = costBG; + 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; } - // Cost of swapping C and D - b3AABB aabbCE = b3AABB_Union( C->aabb, E->aabb ); - float costCD = areaC + b3Perimeter( aabbCE ); - if ( costCD < bestCost ) + // C <-> E then B (D C) + float deltaCE = b3Perimeter( b3UnionV( C->aabb, nodes[iD].aabb ) ) - areaB; + if ( deltaCE < bestDelta ) { - bestRotation = b3_rotateCD; - bestCost = costCD; + bestDown = iC; + bestUp = iE; + bestDelta = deltaCE; } + } - // Cost of swapping C and E - b3AABB aabbCD = b3AABB_Union( C->aabb, D->aabb ); - float costCE = areaC + b3Perimeter( aabbCD ); - if ( costCE < bestCost ) - { - bestRotation = b3_rotateCE; - // bestCost = costCE; - } - - switch ( bestRotation ) - { - case b3_rotateNone: - break; - - case b3_rotateBF: - A->children.child1 = iF; - C->children.child1 = iB; - - B->parent = iC; - F->parent = iA; - - C->aabb = aabbBG; - - C->height = 1 + b3MaxUInt16( B->height, G->height ); - A->height = 1 + b3MaxUInt16( C->height, F->height ); - C->categoryBits = B->categoryBits | G->categoryBits; - A->categoryBits = C->categoryBits | F->categoryBits; - C->flags |= ( B->flags | G->flags ) & b3_enlargedNode; - A->flags |= ( C->flags | F->flags ) & b3_enlargedNode; - break; - - case b3_rotateBG: - A->children.child1 = iG; - C->children.child2 = iB; - - B->parent = iC; - G->parent = iA; - - C->aabb = aabbBF; - - C->height = 1 + b3MaxUInt16( B->height, F->height ); - A->height = 1 + b3MaxUInt16( C->height, G->height ); - C->categoryBits = B->categoryBits | F->categoryBits; - A->categoryBits = C->categoryBits | G->categoryBits; - C->flags |= ( B->flags | F->flags ) & b3_enlargedNode; - A->flags |= ( C->flags | G->flags ) & b3_enlargedNode; - break; - - case b3_rotateCD: - A->children.child2 = iD; - B->children.child1 = iC; - - C->parent = iB; - D->parent = iA; - - B->aabb = aabbCE; - - B->height = 1 + b3MaxUInt16( C->height, E->height ); - A->height = 1 + b3MaxUInt16( B->height, D->height ); - B->categoryBits = C->categoryBits | E->categoryBits; - A->categoryBits = B->categoryBits | D->categoryBits; - B->flags |= ( C->flags | E->flags ) & b3_enlargedNode; - A->flags |= ( B->flags | D->flags ) & b3_enlargedNode; - break; - - case b3_rotateCE: - A->children.child2 = iE; - B->children.child2 = iC; - - C->parent = iB; - E->parent = iA; - - B->aabb = aabbCD; - - B->height = 1 + b3MaxUInt16( C->height, D->height ); - A->height = 1 + b3MaxUInt16( B->height, E->height ); - B->categoryBits = C->categoryBits | D->categoryBits; - A->categoryBits = B->categoryBits | E->categoryBits; - B->flags |= ( C->flags | D->flags ) & b3_enlargedNode; - A->flags |= ( B->flags | E->flags ) & b3_enlargedNode; - break; - - default: - B3_ASSERT( false ); - break; - } + if ( bestDown != B3_NULL_INDEX ) + { + b3SwapNodes( tree, bestDown, bestUp ); } } -// It would be nicer if the root had zero height but maintaining this would drastically increase -// insertion cost because whole sub-trees would need the height to be updated. -static void b3InsertLeaf( b3DynamicTree* tree, int leaf, bool shouldRotate ) +static void b3InsertLeaf( b3DynamicTree* tree, b3AABB aabb, int proxyId, bool moved, bool shouldRotate ) { - if ( tree->root == B3_NULL_INDEX ) + b3TreeProxy* proxy = tree->proxies + proxyId; + b3TreeNode leaf = b3MakeLeafNode( aabb, proxyId, proxy->userData, moved ); + + if ( b3IsEmptyNode( tree->nodes + B3_ROOT_NODE ) ) { - tree->root = leaf; - tree->nodes[tree->root].parent = B3_NULL_INDEX; + tree->nodes[B3_ROOT_NODE] = leaf; + proxy->node = B3_ROOT_NODE; return; } - // Stage 1: find the best sibling for this node - b3AABB leafAABB = tree->nodes[leaf].aabb; - int sibling = b3FindBestSibling( tree, leafAABB ); + int sibling = b3FindBestSibling( tree, aabb ); - // Stage 2: create a new parent for the leaf and sibling - int oldParent = tree->nodes[sibling].parent; - int newParent = b3AllocateNode( tree ); - - // warning: node pointer can change after allocation + // 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; - nodes[newParent].parent = oldParent; - nodes[newParent].userData = UINT64_MAX; - nodes[newParent].aabb = b3AABB_Union( leafAABB, nodes[sibling].aabb ); - nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; - nodes[newParent].height = nodes[sibling].height + 1; + int32_t* parents = tree->parents; - if ( oldParent != B3_NULL_INDEX ) - { - // The sibling was not the root. - if ( nodes[oldParent].children.child1 == sibling ) - { - nodes[oldParent].children.child1 = newParent; - } - else - { - nodes[oldParent].children.child2 = newParent; - } + nodes[pair] = nodes[sibling]; + nodes[pair + 1] = leaf; + parents[pair] = sibling; + parents[pair + 1] = sibling; + b3LinkChildren( tree, pair ); + proxy->node = pair + 1; - nodes[newParent].children.child1 = sibling; - nodes[newParent].children.child2 = leaf; - nodes[sibling].parent = newParent; - nodes[leaf].parent = newParent; - } - else + nodes[sibling] = b3MakeInternalNode( nodes, pair ); + + if ( b3IsNodeOrdered( nodes, sibling ) == false || b3IsNodeOrdered( nodes, pair ) == false ) { - // The sibling was the root. - nodes[newParent].children.child1 = sibling; - nodes[newParent].children.child2 = leaf; - nodes[sibling].parent = newParent; - nodes[leaf].parent = newParent; - tree->root = newParent; + tree->dfsOrdered = false; } - // Stage 3: walk back up the tree fixing heights and AABBs - int index = nodes[leaf].parent; + // Walk back up the tree refitting ancestors, the root included. + int index = sibling; while ( index != B3_NULL_INDEX ) { - int child1 = nodes[index].children.child1; - int child2 = nodes[index].children.child2; - - B3_ASSERT( child1 != B3_NULL_INDEX ); - B3_ASSERT( child2 != B3_NULL_INDEX ); - - nodes[index].aabb = b3AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); - nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; - nodes[index].height = 1 + b3MaxUInt16( nodes[child1].height, nodes[child2].height ); - nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b3_enlargedNode; - if ( shouldRotate ) { b3RotateNodes( tree, index ); } - index = nodes[index].parent; + nodes[index] = b3MakeInternalNode( nodes, b3GetLeftChild( nodes + index ) ); + index = parents[index]; } } -static void b3RemoveLeaf( b3DynamicTree* tree, int leaf ) +static void b3RemoveLeaf( b3DynamicTree* tree, int proxyId ) { - if ( leaf == tree->root ) + 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 ) { - tree->root = B3_NULL_INDEX; + nodes[B3_ROOT_NODE] = b3MakeEmptyNode(); return; } - b3TreeNode* nodes = tree->nodes; + // 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 ); - int parent = nodes[leaf].parent; - int grandParent = nodes[parent].parent; - int sibling; - if ( nodes[parent].children.child1 == leaf ) + // Update ancestors. + int index = parents[parent]; + while ( index != B3_NULL_INDEX ) { - sibling = nodes[parent].children.child2; - } - else - { - sibling = nodes[parent].children.child1; - } - - if ( grandParent != B3_NULL_INDEX ) - { - // Destroy parent and connect sibling to grandParent. - if ( nodes[grandParent].children.child1 == parent ) - { - nodes[grandParent].children.child1 = sibling; - } - else - { - nodes[grandParent].children.child2 = sibling; - } - nodes[sibling].parent = grandParent; - b3FreeNode( tree, parent ); - - // Adjust ancestor bounds. - int index = grandParent; - while ( index != B3_NULL_INDEX ) - { - b3TreeNode* node = nodes + index; - b3TreeNode* child1 = nodes + node->children.child1; - b3TreeNode* child2 = nodes + node->children.child2; - - // Fast union using SSE - //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); - //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); - //__m128 lower = _mm_min_ps(aabb1, aabb2); - //__m128 upper = _mm_max_ps(aabb1, aabb2); - //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); - //_mm_store_ps(&node->aabb.lowerBound.x, aabb); - - node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); - node->categoryBits = child1->categoryBits | child2->categoryBits; - node->height = 1 + b3MaxUInt16( child1->height, child2->height ); - - index = node->parent; - } - } - else - { - tree->root = sibling; - tree->nodes[sibling].parent = B3_NULL_INDEX; - b3FreeNode( tree, parent ); + 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 b3DynamicTree_CreateProxy( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData ) +int b3CreateTreeProxyInternal( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData, bool markMoved ) { - B3_ASSERT( b3IsValidAABB( aabb ) ); + B3_VALIDATE( b3IsValidAABB( aabb ) ); - int proxyId = b3AllocateNode( tree ); - b3TreeNode* node = tree->nodes + proxyId; + int proxyId = b3AllocateProxy( tree ); - node->aabb = aabb; - node->userData = userData; - node->categoryBits = categoryBits; - node->height = 0; - node->flags = b3_allocatedNode | b3_leafNode; + b3TreeProxy* proxy = tree->proxies + proxyId; + proxy->categoryBits = categoryBits; + proxy->userData = userData; bool shouldRotate = true; - b3InsertLeaf( tree, proxyId, shouldRotate ); - - tree->proxyCount += 1; + 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->nodeCapacity ); - B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity ); b3RemoveLeaf( tree, proxyId ); - b3FreeNode( tree, proxyId ); - - B3_ASSERT( tree->proxyCount > 0 ); - tree->proxyCount -= 1; + b3FreeProxy( tree, proxyId ); } int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ) @@ -793,43 +604,56 @@ int b3DynamicTree_GetProxyCount( const b3DynamicTree* tree ) return tree->proxyCount; } -void b3DynamicTree_MoveProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) +void b3DynamicTree_MoveProxyInternal( b3DynamicTree* tree, int proxyId, b3AABB aabb, bool markMoved ) { - B3_ASSERT( b3IsValidAABB( aabb ) ); - B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); - B3_ASSERT( b3IsLeaf( tree->nodes + proxyId ) ); + 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 ); - tree->nodes[proxyId].aabb = aabb; - bool shouldRotate = false; - b3InsertLeaf( tree, proxyId, shouldRotate ); + 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 ) { - b3TreeNode* nodes = tree->nodes; B3_VALIDATE( b3IsValidAABB( aabb ) ); - B3_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); - B3_VALIDATE( b3IsLeaf( tree->nodes + proxyId ) ); + 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( nodes[proxyId].aabb, aabb ) == false ); + B3_VALIDATE( b3AABB_Contains( node->aabb, aabb ) == false ); - b3TreeNode* node = nodes + proxyId; node->aabb = aabb; + node->flagIndex |= B3_MOVED_NODE; - int parentIndex = node->parent; - while ( parentIndex != B3_NULL_INDEX ) + index = parents[index]; + while ( index != B3_NULL_INDEX ) { - node = nodes + parentIndex; + node = nodes + index; bool changed = b3EnlargeAABB( &node->aabb, aabb ); - // todo not sure why this node is marked as enlarged even if it didn't change - node->flags |= b3_enlargedNode; + // This is marked to ensure the root is marked in this loop or the one below. + node->flagIndex |= B3_MOVED_NODE; - parentIndex = node->parent; + index = parents[index]; if ( changed == false ) { @@ -837,246 +661,179 @@ void b3DynamicTree_EnlargeProxy( b3DynamicTree* tree, int proxyId, b3AABB aabb ) } } - while ( parentIndex != B3_NULL_INDEX ) + // Mark all the way up to the root. + while ( index != B3_NULL_INDEX ) { - node = nodes + parentIndex; - if ( node->flags & b3_enlargedNode ) + node = nodes + index; + if ( node->flagIndex & B3_MOVED_NODE ) { - // early out because this ancestor was previously ascended and marked as enlarged + // Early out because this ancestor was previously ascended and marked as moved. break; } - node->flags |= b3_enlargedNode; - parentIndex = node->parent; + node->flagIndex |= B3_MOVED_NODE; + index = parents[index]; } } void b3DynamicTree_SetCategoryBits( b3DynamicTree* tree, int proxyId, uint64_t categoryBits ) { - b3TreeNode* nodes = tree->nodes; - - B3_ASSERT( b3IsLeaf( nodes + proxyId ) ); - - nodes[proxyId].categoryBits = categoryBits; - - // Fix up category bits in ancestor internal nodes - int nodeIndex = nodes[proxyId].parent; - while ( nodeIndex != B3_NULL_INDEX ) - { - b3TreeNode* node = nodes + nodeIndex; - int child1 = node->children.child1; - B3_ASSERT( child1 != B3_NULL_INDEX ); - int child2 = node->children.child2; - B3_ASSERT( child2 != B3_NULL_INDEX ); - node->categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; - - nodeIndex = node->parent; - } + 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->nodeCapacity ); - return tree->nodes[proxyId].categoryBits; + B3_ASSERT( 0 <= proxyId && proxyId < tree->proxyCapacity ); + return tree->proxies[proxyId].categoryBits; } int b3DynamicTree_GetHeight( const b3DynamicTree* tree ) { - if ( tree->root == B3_NULL_INDEX ) - { - return 0; - } - - return tree->nodes[tree->root].height; + 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->root == B3_NULL_INDEX ) + if ( tree->proxyCount == 0 ) { return 0.0f; } - const b3TreeNode* root = tree->nodes + tree->root; - float rootArea = b3Perimeter( root->aabb ); - - float totalArea = 0.0f; - for ( int i = 0; i < tree->nodeCapacity; ++i ) + const b3TreeNode* nodes = tree->nodes; + float rootArea = b3Perimeter( nodes[B3_ROOT_NODE].aabb ); + if ( rootArea <= 0.0f ) { - const b3TreeNode* node = tree->nodes + i; - if ( b3IsAllocated( node ) == false || b3IsLeaf( node ) || i == tree->root ) - { - continue; - } - - totalArea += b3Perimeter( node->aabb ); + return 0.0f; } - return totalArea / rootArea; + // 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->root != B3_NULL_INDEX ) + if ( tree->proxyCount == 0 ) { - return tree->nodes[tree->root].aabb; + return B3_LITERAL( b3AABB ){ b3Vec3_zero, b3Vec3_zero }; } - b3AABB empty = { b3Vec3_zero, b3Vec3_zero }; - return empty; + return tree->nodes[B3_ROOT_NODE].aabb; } #if B3_ENABLE_VALIDATION -// Compute the height of a sub-tree. -static int b3ComputeHeightRecurse( const b3DynamicTree* tree, int nodeId ) + +static int b3ValidateSubtree( const b3DynamicTree* tree, int nodeIndex, int* leafCount ) { - B3_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); - b3TreeNode* node = tree->nodes + nodeId; + 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 height1 = b3ComputeHeightRecurse( tree, node->children.child1 ); - int height2 = b3ComputeHeightRecurse( tree, node->children.child2 ); - return 1 + b3MaxInt( height1, height2 ); -} + 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 ); -static int b3ComputeHeight( const b3DynamicTree* tree ) -{ - int height = b3ComputeHeightRecurse( tree, tree->root ); + 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; } -static void b3ValidateStructure( const b3DynamicTree* tree, int index ) -{ - if ( index == B3_NULL_INDEX ) - { - return; - } - - if ( index == tree->root ) - { - B3_ASSERT( tree->nodes[index].parent == B3_NULL_INDEX ); - } - - const b3TreeNode* node = tree->nodes + index; - - B3_ASSERT( node->flags == 0 || ( node->flags & b3_allocatedNode ) != 0 ); - - if ( b3IsLeaf( node ) ) - { - B3_ASSERT( node->height == 0 ); - return; - } - - int child1 = node->children.child1; - int child2 = node->children.child2; - - B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); - B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); - - B3_ASSERT( tree->nodes[child1].parent == index ); - B3_ASSERT( tree->nodes[child2].parent == index ); - - if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b3_enlargedNode ) - { - B3_ASSERT( node->flags & b3_enlargedNode ); - } - - b3ValidateStructure( tree, child1 ); - b3ValidateStructure( tree, child2 ); -} - -static void b3ValidateMetrics( const b3DynamicTree* tree, int index ) -{ - if ( index == B3_NULL_INDEX ) - { - return; - } - - const b3TreeNode* node = tree->nodes + index; - - B3_VALIDATE( b3IsValidAABB( node->aabb ) ); - - if ( b3IsLeaf( node ) ) - { - B3_ASSERT( node->height == 0 ); - return; - } - - int child1 = node->children.child1; - int child2 = node->children.child2; - - B3_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); - B3_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); - - int height1 = tree->nodes[child1].height; - int height2 = tree->nodes[child2].height; - int height = 1 + b3MaxInt( height1, height2 ); - B3_ASSERT( node->height == height ); - - // b3AABB aabb = b3AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); - - B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); - B3_ASSERT( b3AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); - - // B3_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); - // B3_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); - // B3_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); - // B3_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); - - uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; - B3_ASSERT( node->categoryBits == categoryBits ); - - b3ValidateMetrics( tree, child1 ); - b3ValidateMetrics( tree, child2 ); -} #endif void b3DynamicTree_Validate( const b3DynamicTree* tree ) { #if B3_ENABLE_VALIDATION - if ( tree->root == B3_NULL_INDEX ) + 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; } - b3ValidateStructure( tree, tree->root ); - b3ValidateMetrics( tree, tree->root ); + int leafCount = 0; + b3ValidateSubtree( tree, B3_ROOT_NODE, &leafCount ); + B3_ASSERT( leafCount == tree->proxyCount ); - int freeCount = 0; - int freeIndex = tree->freeList; - while ( freeIndex != B3_NULL_INDEX ) - { - B3_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); - freeIndex = tree->nodes[freeIndex].next; - ++freeCount; - } - - int height = b3DynamicTree_GetHeight( tree ); - int computedHeight = b3ComputeHeight( tree ); - B3_ASSERT( height == computedHeight ); - - B3_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); #else B3_UNUSED( tree ); #endif } -void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ) +void b3DynamicTree_ValidateNoMoved( const b3DynamicTree* tree ) { #if B3_ENABLE_VALIDATION == 1 - int capacity = tree->nodeCapacity; const b3TreeNode* nodes = tree->nodes; - for ( int i = 0; i < capacity; ++i ) + int nodeEnd = tree->nodeEnd; + for ( int i = 0; i < nodeEnd; ++i ) { - const b3TreeNode* node = nodes + i; - if ( node->flags & b3_allocatedNode ) - { - B3_ASSERT( ( node->flags & b3_enlargedNode ) == 0 ); - } + B3_ASSERT( b3IsNodeMoved( nodes + i ) == false ); } #else B3_UNUSED( tree ); @@ -1085,62 +842,86 @@ void b3DynamicTree_ValidateNoEnlarged( const b3DynamicTree* tree ) int b3DynamicTree_GetByteCount( const b3DynamicTree* tree ) { - size_t size = sizeof( b3DynamicTree ) + sizeof( b3TreeNode ) * tree->nodeCapacity + - tree->rebuildCapacity * ( sizeof( int ) + sizeof( b3AABB ) + sizeof( b3Vec3 ) + sizeof( int ) ); + 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; +} + b3TreeStats b3DynamicTree_Query( const b3DynamicTree* tree, b3AABB aabb, uint64_t maskBits, bool requireAllBits, b3TreeQueryCallbackFcn* callback, void* context ) { b3TreeStats result = { 0 }; - if ( tree->nodeCount == 0 ) + if ( tree->proxyCount == 0 ) { return result; } + const b3TreeNode* nodes = tree->nodes; + int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = tree->root; + stack[stackCount++] = b3GetRootPair( nodes ); + + b3AABBV boxv = b3LoadAABBV( &aabb ); while ( stackCount > 0 ) { - int nodeId = stack[--stackCount]; - if ( nodeId == B3_NULL_INDEX ) - { - // todo huh? - B3_ASSERT( false ); - continue; - } - - const b3TreeNode* node = tree->nodes + nodeId; + int pair = stack[--stackCount]; result.nodeVisits += 1; - // Assuming branch prediction deals with requireAllBits well - uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); - - if ( bitMatch && b3AABB_Overlaps( node->aabb, aabb ) ) + for ( int i = 0; i < 2; ++i ) { + const b3TreeNode* node = nodes + pair + i; + if ( b3OverlapNode( boxv, node ) == false ) + { + continue; + } + if ( b3IsLeaf( node ) ) { - // callback to user code with proxy id - bool proceed = callback( nodeId, node->userData, context ); - result.leafVisits += 1; - - if ( proceed == false ) + int proxyId = b3GetProxyId( node ); + const b3TreeProxy* proxy = tree->proxies + proxyId; + if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) ) { - return result; + bool proceed = callback( proxyId, proxy->userData, context ); + result.leafVisits += 1; + + if ( proceed == false ) + { + return result; + } } } else { - B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); if ( stackCount < B3_TREE_STACK_SIZE - 1 ) { - stack[stackCount++] = node->children.child1; - stack[stackCount++] = node->children.child2; + stack[stackCount++] = b3GetLeftChild( node ); + } + else + { + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); } } } @@ -1166,78 +947,108 @@ b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, { b3TreeStats result = { 0 }; - if ( tree->nodeCount == 0 ) + if ( tree->proxyCount == 0 ) { return result; } + const b3TreeNode* nodes = tree->nodes; + float minSqr = *minDistanceSqr; struct b3QueryClosestItem stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - float rootDistanceSqr = b3DistanceToNodeSqr( point, tree->nodes + tree->root ); - stack[stackCount++] = (struct b3QueryClosestItem){ - .nodeIndex = tree->root, - .distanceToNodeSqr = rootDistanceSqr, + 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 = tree->nodes + item.nodeIndex; + const b3TreeNode* node = nodes + item.nodeIndex; result.nodeVisits += 1; - uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); - - if ( bitMatch ) + if ( item.distanceToNodeSqr >= minSqr ) { - if ( item.distanceToNodeSqr < minSqr ) + continue; + } + + if ( b3IsLeaf( node ) ) + { + int proxyId = b3GetProxyId( node ); + const b3TreeProxy* proxy = tree->proxies + proxyId; + if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false ) { - if ( b3IsLeaf( node ) ) - { - // callback to user code with minimum distance squared so far and proxy id - float dd = callback( minSqr, item.nodeIndex, node->userData, context ); - - if ( dd < minSqr ) - { - minSqr = dd; - } - - result.leafVisits += 1; - } - else - { - B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); - if ( stackCount < B3_TREE_STACK_SIZE - 1 ) - { - int child1 = node->children.child1; - int child2 = node->children.child2; - - // Store the distance to node in the stack instead of recomputing after pop - struct b3QueryClosestItem item1 = { - .nodeIndex = child1, - .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child1 ), - }; - - struct b3QueryClosestItem item2 = { - .nodeIndex = child2, - .distanceToNodeSqr = b3DistanceToNodeSqr( point, tree->nodes + child2 ), - }; - - // 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; - } - } - } + continue; } + + // callback to user code with minimum distance squared so far and proxy id + float dd = callback( minSqr, proxyId, proxy->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; } } @@ -1246,12 +1057,14 @@ b3TreeStats b3DynamicTree_QueryClosest( const b3DynamicTree* tree, b3Vec3 point, 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. b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeRayCastCallbackFcn* callback, void* context ) { b3TreeStats result = { 0 }; - if ( tree->nodeCount == 0 ) + if ( tree->proxyCount == 0 ) { return result; } @@ -1268,87 +1081,104 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp // Build a bounding box for the segment. b3AABB segmentAABB = { b3Min( p1, p2 ), b3Max( p1, p2 ) }; + b3AABBV boxv = b3LoadAABBV( &segmentAABB ); + + const b3TreeNode* nodes = tree->nodes; int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = tree->root; - - const b3TreeNode* nodes = tree->nodes; + stack[stackCount++] = b3GetRootPair( nodes ); b3RayCastInput subInput = *input; while ( stackCount > 0 ) { - int nodeId = stack[--stackCount]; - if ( nodeId == B3_NULL_INDEX ) - { - // todo is this possible? - B3_ASSERT( false ); - continue; - } - - const b3TreeNode* node = nodes + nodeId; + int pair = stack[--stackCount]; result.nodeVisits += 1; - b3AABB nodeAABB = node->aabb; - - // todo look at disassembly - uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); - - if ( bitMatch == 0 || b3AABB_Overlaps( nodeAABB, segmentAABB ) == false ) + const b3TreeNode* hit[2]; + bool isLeaf[2]; + int hitCount = 0; + for ( int i = 0; i < 2; ++i ) { - continue; - } + const b3TreeNode* node = nodes + pair + i; - b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x ); - b3V32 upper = b3LoadV( &nodeAABB.upperBound.x ); - - bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); - if ( edgeOverlap == false ) - { - continue; - } - - if ( b3IsLeaf( node ) ) - { - subInput.maxFraction = maxFraction; - - float value = callback( &subInput, nodeId, node->userData, context ); - result.leafVisits += 1; - - // The user may return -1 to indicate this shape should be skipped - - if ( value == 0.0f ) + if ( b3OverlapNode( boxv, node ) == false ) { - // The client has terminated the ray cast. - return result; + continue; } - if ( 0.0f < value && value <= maxFraction ) + b3AABB nodeAABB = node->aabb; + b3V32 lower = b3LoadV( &nodeAABB.lowerBound.x ); + b3V32 upper = b3LoadV( &nodeAABB.upperBound.x ); + if ( b3TestBoundsRayOverlap( lower, upper, pv1, dv ) == false ) { - // Update segment bounding box. - maxFraction = value; - p2 = b3MulAdd( p1, maxFraction, d ); - segmentAABB.lowerBound = b3Min( p1, p2 ); - segmentAABB.upperBound = b3Max( p1, p2 ); + 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] ); } } - else + + for ( int i = 0; i < hitCount; ++i ) { - B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); - if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + if ( isLeaf[i] ) { - b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); - b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); - if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + int proxyId = b3GetProxyId( hit[i] ); + const b3TreeProxy* proxy = tree->proxies + proxyId; + + if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false ) { - stack[stackCount++] = node->children.child2; - stack[stackCount++] = node->children.child1; + continue; + } + + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, proxyId, proxy->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 { - stack[stackCount++] = node->children.child1; - stack[stackCount++] = node->children.child2; + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); } } } @@ -1357,14 +1187,15 @@ b3TreeStats b3DynamicTree_RayCast( const b3DynamicTree* tree, const b3RayCastInp return result; } +// Follows structure of ray cast with small tweaks to handle a swept box. b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInput* input, uint64_t maskBits, bool requireAllBits, b3TreeBoxCastCallbackFcn* callback, void* context ) { - b3TreeStats stats = { 0 }; + b3TreeStats result = { 0 }; - if ( tree->nodeCount == 0 ) + if ( tree->proxyCount == 0 ) { - return stats; + return result; } // The caller folds the shape radius and the world origin into the box @@ -1387,87 +1218,111 @@ b3TreeStats b3DynamicTree_BoxCast( const b3DynamicTree* tree, const b3BoxCastInp b3Min( originAABB.lowerBound, b3Add( originAABB.lowerBound, t ) ), b3Max( originAABB.upperBound, b3Add( originAABB.upperBound, t ) ), }; + b3AABBV boxv = b3LoadAABBV( &totalAABB ); - b3BoxCastInput subInput = *input; const b3TreeNode* nodes = tree->nodes; int stack[B3_TREE_STACK_SIZE]; int stackCount = 0; - stack[stackCount++] = tree->root; + stack[stackCount++] = b3GetRootPair( nodes ); + + b3BoxCastInput subInput = *input; while ( stackCount > 0 ) { - int nodeId = stack[--stackCount]; - if ( nodeId == B3_NULL_INDEX ) + int pair = stack[--stackCount]; + result.nodeVisits += 1; + + const b3TreeNode* hit[2]; + bool isLeaf[2]; + int hitCount = 0; + for ( int i = 0; i < 2; ++i ) { - B3_ASSERT( false ); - continue; - } + const b3TreeNode* node = nodes + pair + i; - const b3TreeNode* node = nodes + nodeId; - stats.nodeVisits += 1; - - uint64_t bitMatch = requireAllBits ? ( node->categoryBits & maskBits ) == maskBits : ( node->categoryBits & maskBits ); - - if ( bitMatch == 0 || b3AABB_Overlaps( node->aabb, totalAABB ) == false ) - { - continue; - } - - // radius extension is added to the node in this case - b3V32 lower = b3SubV( b3LoadV( &node->aabb.lowerBound.x ), ev ); - b3V32 upper = b3AddV( b3LoadV( &node->aabb.upperBound.x ), ev ); - bool edgeOverlap = b3TestBoundsRayOverlap( lower, upper, pv1, dv ); - if ( edgeOverlap == false ) - { - continue; - } - - if ( b3IsLeaf( node ) ) - { - subInput.maxFraction = maxFraction; - - float value = callback( &subInput, nodeId, node->userData, context ); - stats.leafVisits += 1; - - if ( value == 0.0f ) + if ( b3OverlapNode( boxv, node ) == false ) { - // The client has terminated the cast. - return stats; + continue; } - if ( 0.0f < value && value < maxFraction ) + // 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 ) { - 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 ) ); + 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] ); } } - else + + for ( int i = 0; i < hitCount; ++i ) { - B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); - if ( stackCount < B3_TREE_STACK_SIZE - 1 ) + if ( isLeaf[i] ) { - b3Vec3 c1 = b3AABB_Center( nodes[node->children.child1].aabb ); - b3Vec3 c2 = b3AABB_Center( nodes[node->children.child2].aabb ); - if ( b3DistanceSquared( c1, p1 ) < b3DistanceSquared( c2, p1 ) ) + int proxyId = b3GetProxyId( hit[i] ); + const b3TreeProxy* proxy = tree->proxies + proxyId; + + if ( b3TestCategory( proxy->categoryBits, maskBits, requireAllBits ) == false ) { - stack[stackCount++] = node->children.child2; - stack[stackCount++] = node->children.child1; + continue; + } + + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, proxyId, proxy->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 { - stack[stackCount++] = node->children.child1; - stack[stackCount++] = node->children.child2; + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE - 1 ); } } } } - return stats; + return result; } + // Median split == 0, Surface area heuristic == 1 #define B3_TREE_HEURISTIC 0 @@ -1788,30 +1643,141 @@ static int b3PartitionSAH( int* indices, int* binIndices, b3AABB* boxes, int cou #endif -// Temporary data used to track the rebuild of a tree node -struct b3RebuildItem +// 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; -// Returns root node index -static int b3BuildTree( b3DynamicTree* tree, int leafCount ) +typedef struct b3CopyItem { - b3TreeNode* nodes = tree->nodes; - int* leafIndices = tree->leafIndices; + int oldPair; + int newIndex; +} b3CopyItem; - if ( leafCount == 1 ) +// 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 ) ) { - nodes[leafIndices[0]].parent = B3_NULL_INDEX; - return leafIndices[0]; + 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 @@ -1819,11 +1785,23 @@ static int b3BuildTree( b3DynamicTree* tree, int leafCount ) int* binIndices = tree->binIndices; #endif - // todo large stack item - struct b3RebuildItem stack[B3_TREE_STACK_SIZE]; + // 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 = b3AllocateNode( tree ); + 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; @@ -1835,128 +1813,57 @@ static int b3BuildTree( b3DynamicTree* tree, int leafCount ) while ( true ) { - struct b3RebuildItem* item = stack + top; - + b3RebuildItem* item = stack + top; item->childCount += 1; if ( item->childCount == 2 ) { - // This internal node has both children established - + // Both children written, so the parent node can be finalized. + nodes[item->nodeIndex] = b3MakeInternalNode( nodes, item->pair ); if ( top == 0 ) { - // all done break; } - struct b3RebuildItem* parentItem = stack + ( top - 1 ); - b3TreeNode* parentNode = nodes + parentItem->nodeIndex; - - if ( parentItem->childCount == 0 ) - { - B3_ASSERT( parentNode->children.child1 == B3_NULL_INDEX ); - parentNode->children.child1 = item->nodeIndex; - } - else - { - B3_ASSERT( parentItem->childCount == 1 ); - B3_ASSERT( parentNode->children.child2 == B3_NULL_INDEX ); - parentNode->children.child2 = item->nodeIndex; - } - - b3TreeNode* node = nodes + item->nodeIndex; - - B3_ASSERT( node->parent == B3_NULL_INDEX ); - node->parent = parentItem->nodeIndex; - - B3_ASSERT( node->children.child1 != B3_NULL_INDEX ); - B3_ASSERT( node->children.child2 != B3_NULL_INDEX ); - b3TreeNode* child1 = nodes + node->children.child1; - b3TreeNode* child2 = nodes + node->children.child2; - - node->aabb = b3AABB_Union( child1->aabb, child2->aabb ); - node->height = 1 + b3MaxUInt16( child1->height, child2->height ); - node->categoryBits = child1->categoryBits | child2->categoryBits; - - // Pop stack top -= 1; + continue; } - else + + 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 ) { - int startIndex, endIndex; - if ( item->childCount == 0 ) - { - startIndex = item->startIndex; - endIndex = item->splitIndex; - } - else - { - B3_ASSERT( item->childCount == 1 ); - startIndex = item->splitIndex; - endIndex = item->endIndex; - } - - int count = endIndex - startIndex; - - if ( count == 1 ) - { - int childIndex = leafIndices[startIndex]; - b3TreeNode* node = nodes + item->nodeIndex; - - if ( item->childCount == 0 ) - { - B3_ASSERT( node->children.child1 == B3_NULL_INDEX ); - node->children.child1 = childIndex; - } - else - { - B3_ASSERT( item->childCount == 1 ); - B3_ASSERT( node->children.child2 == B3_NULL_INDEX ); - node->children.child2 = childIndex; - } - - b3TreeNode* childNode = nodes + childIndex; - B3_ASSERT( childNode->parent == B3_NULL_INDEX ); - childNode->parent = item->nodeIndex; - } - else - { - B3_ASSERT( count > 0 ); - B3_ASSERT( top < B3_TREE_STACK_SIZE ); - - top += 1; - struct b3RebuildItem* newItem = stack + top; - newItem->nodeIndex = b3AllocateNode( tree ); - newItem->childCount = -1; - newItem->startIndex = startIndex; - newItem->endIndex = endIndex; -#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 - newItem->splitIndex += startIndex; - } + 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 } - - b3TreeNode* rootNode = nodes + stack[0].nodeIndex; - B3_ASSERT( rootNode->parent == B3_NULL_INDEX ); - B3_ASSERT( rootNode->children.child1 != B3_NULL_INDEX ); - B3_ASSERT( rootNode->children.child2 != B3_NULL_INDEX ); - - b3TreeNode* child1 = nodes + rootNode->children.child1; - b3TreeNode* child2 = nodes + rootNode->children.child2; - - rootNode->aabb = b3AABB_Union( child1->aabb, child2->aabb ); - rootNode->height = 1 + b3MaxUInt16( child1->height, child2->height ); - rootNode->categoryBits = child1->categoryBits | child2->categoryBits; - - return stack[0].nodeIndex; } -// Not safe to access tree during this operation because it may grow +// 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; @@ -1965,112 +1872,366 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) return 0; } - // Ensure capacity for rebuild space + // 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, tree->rebuildCapacity * sizeof( int ) ); - tree->leafIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); + 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, tree->rebuildCapacity * sizeof( b3Vec3 ) ); + b3Free( tree->leafCenters, oldCapacity * sizeof( b3Vec3 ) ); tree->leafCenters = (b3Vec3*)b3Alloc( newCapacity * sizeof( b3Vec3 ) ); #else - b3Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b3AABB ) ); + b3Free( tree->leafBoxes, oldCapacity * sizeof( b3AABB ) ); tree->leafBoxes = (b3AABB*)b3Alloc( newCapacity * sizeof( b3AABB ) ); - b3Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); - tree->binIndices = (int*)b3Alloc( newCapacity * sizeof( int ) ); + b3Free( tree->binIndices, oldCapacity * sizeof( int32_t ) ); + tree->binIndices = (int32_t*)b3Alloc( newCapacity * sizeof( int32_t ) ); #endif tree->rebuildCapacity = newCapacity; } - int leafCount = 0; - int stack[B3_TREE_STACK_SIZE]; - int stackCount = 0; - - int nodeIndex = tree->root; - b3TreeNode* nodes = tree->nodes; - b3TreeNode* node = nodes + nodeIndex; - - // These are the nodes that get sorted to rebuild the tree. - // I'm using indices because the node pool may grow during the build. int* leafIndices = tree->leafIndices; - + b3TreeNode* leaves = tree->leafNodes; #if B3_TREE_HEURISTIC == 0 b3Vec3* leafCenters = tree->leafCenters; #else b3AABB* leafBoxes = tree->leafBoxes; #endif - // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are - // considered leaves in the tree rebuild. - // Free all internal nodes that have grown. - // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency - // this should be weighed against B3_MAX_AABB_MARGIN - while ( true ) + // 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 ) ) ) { - if ( b3IsLeaf( node ) == true || ( ( node->flags & b3_enlargedNode ) == 0 && fullBuild == false ) ) - { - leafIndices[leafCount] = nodeIndex; + stack[stackCount++] = b3GetLeftChild( root ); + } + else + { + b3TreeNode node = *root; + node.flagIndex &= ~B3_MOVED_NODE; + leafIndices[0] = 0; + leaves[0] = node; #if B3_TREE_HEURISTIC == 0 - leafCenters[leafCount] = b3AABB_Center( node->aabb ); + leafCenters[0] = b3AABB_Center( node.aabb ); #else - leafBoxes[leafCount] = node->aabb; + leafBoxes[0] = node.aabb; #endif - leafCount += 1; + leafCount = 1; + } - // Detach - node->parent = B3_NULL_INDEX; - } - else + while ( stackCount > 0 ) + { + int pair = stack[--stackCount]; + for ( int i = 0; i < 2; ++i ) { - int doomedNodeIndex = nodeIndex; - - // Handle children - nodeIndex = node->children.child1; - - B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); - if ( stackCount < B3_TREE_STACK_SIZE ) + b3TreeNode node = nodes[pair + i]; + if ( b3IsLeaf( &node ) == false && ( fullBuild || b3IsNodeMoved( &node ) ) ) { - stack[stackCount++] = node->children.child2; + // todo fail gracefully if the stack runs out in release + B3_ASSERT( stackCount < B3_TREE_STACK_SIZE ); + stack[stackCount++] = b3GetLeftChild( &node ); + continue; } - node = nodes + nodeIndex; - - // Remove doomed node - b3FreeNode( tree, doomedNodeIndex ); - - continue; + 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; } + } - if ( stackCount == 0 ) + 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 ); + 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; } - nodeIndex = stack[--stackCount]; - node = nodes + nodeIndex; + 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; } -#if B3_ENABLE_VALIDATION == 1 - int capacity = tree->nodeCapacity; - for ( int i = 0; i < capacity; ++i ) + root->flagIndex &= ~B3_MOVED_NODE; + if ( b3IsLeaf( root ) ) { - if ( nodes[i].flags & b3_allocatedNode ) + 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 ) { - B3_ASSERT( ( nodes[i].flags & b3_enlargedNode ) == 0 ); + 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 ); + } + } + } } } -#endif +} - B3_ASSERT( leafCount <= proxyCount ); +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; + } - tree->root = b3BuildTree( tree, leafCount ); + if ( b3IsLeaf( root ) ) + { + proxyIds[0] = b3GetProxyId( root ); + return 1; + } - b3DynamicTree_Validate( tree ); + int count = 0; + int stack[B3_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = b3GetLeftChild( root ); - return leafCount; + 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 ) @@ -2096,6 +2257,11 @@ static FILE* b3OpenTreeFile( const char* fileName, const char* mode ) 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 ) { @@ -2107,19 +2273,29 @@ void b3DynamicTree_Save( const b3DynamicTree* tree, const char* fileName ) // 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; - // Write tree struct + temp.nodeCapacity = tree->nodeEnd; + fwrite( &temp, sizeof( b3DynamicTree ), 1, file ); - // Write the node array, this includes the free list - if ( tree->nodeCapacity > 0 && tree->nodes != NULL ) + if ( tree->nodeEnd > 0 ) { - fwrite( tree->nodes, sizeof( b3TreeNode ), tree->nodeCapacity, file ); + 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 ); @@ -2139,6 +2315,7 @@ b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ) if ( readCount != 1 ) { fclose( file ); + memset( &tree, 0, sizeof( b3DynamicTree ) ); return tree; } @@ -2149,37 +2326,54 @@ b3DynamicTree b3DynamicTree_Load( const char* fileName, float scale ) return tree; } - if ( tree.nodeCapacity > 0 ) - { - tree.nodes = (b3TreeNode*)b3Alloc( tree.nodeCapacity * sizeof( b3TreeNode ) ); - readCount = (int)fread( tree.nodes, sizeof( b3TreeNode ), tree.nodeCapacity, file ); - if ( readCount != tree.nodeCapacity ) - { - b3Free( tree.nodes, tree.nodeCapacity * sizeof( b3TreeNode ) ); - fclose( file ); - memset( &tree, 0, sizeof( b3DynamicTree ) ); - return tree; - } - - for ( int i = 0; i < tree.nodeCapacity; ++i ) - { - b3TreeNode* node = tree.nodes + i; - node->aabb.lowerBound = b3MulSV( scale, node->aabb.lowerBound ); - node->aabb.upperBound = b3MulSV( scale, node->aabb.upperBound ); - } - } - else - { - tree.nodes = NULL; - } - - // Zero temp data fields + 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; diff --git a/src/dynamic_tree.h b/src/dynamic_tree.h new file mode 100644 index 0000000..aa12df9 --- /dev/null +++ b/src/dynamic_tree.h @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#define B3_TREE_STACK_SIZE 512 + +// Used to mark a node as being moved such that pairs need to be generated and the tree +// may need to be rebuilt. This mark comes from a few sources: +// - updating the transform and AABB in the solver from movement +// - a joint disabling collision +// - creating a proxy +// - setting the transform on a body + +#define B3_MOVED_NODE ( 1u << 30 ) +#define B3_LEAF_NODE ( 1u << 31 ) +#define B3_NODE_INDEX_MASK ( 0xFFFFFFFFu & ~( B3_MOVED_NODE | B3_LEAF_NODE ) ) + +// Used to indicate empty nodes. It also passes as a leaf so that internal node +// processing will naturally skip it. +#define B3_EMPTY_NODE ( B3_NODE_INDEX_MASK | B3_LEAF_NODE ) + +#define B3_ROOT_NODE 0 + +B3_FORCE_INLINE bool b3IsLeaf( const b3TreeNode* node ) +{ + return ( node->flagIndex & B3_LEAF_NODE ) == B3_LEAF_NODE; +} + +B3_FORCE_INLINE bool b3IsNodeMoved( const b3TreeNode* node ) +{ + return ( node->flagIndex & B3_MOVED_NODE ) == B3_MOVED_NODE; +} + +B3_FORCE_INLINE bool b3IsEmptyNode( const b3TreeNode* node ) +{ + return node->flagIndex == B3_EMPTY_NODE; +} + +B3_FORCE_INLINE int b3GetLeftChild( const b3TreeNode* node ) +{ + return (int)( node->flagIndex & B3_NODE_INDEX_MASK ); +} + +B3_FORCE_INLINE int b3GetProxyId( const b3TreeNode* node ) +{ + return (int)( node->flagIndex & B3_NODE_INDEX_MASK ); +} + +B3_FORCE_INLINE int b3GetRootPair( const b3TreeNode* nodes ) +{ + const b3TreeNode* root = nodes + B3_ROOT_NODE; + return b3IsLeaf( root ) ? B3_ROOT_NODE : b3GetLeftChild( root ); +} + +B3_FORCE_INLINE b3TreeNode b3MakeEmptyNode( void ) +{ + return B3_LITERAL( b3TreeNode ){ + .aabb = + { + .lowerBound = { INFINITY, INFINITY, INFINITY }, + .upperBound = { -INFINITY, -INFINITY, -INFINITY }, + }, + .flagIndex = B3_EMPTY_NODE, + .height = 0, + }; +} + +static inline bool b3HasTreeMoved( const b3DynamicTree* tree ) +{ + return b3IsNodeMoved( tree->nodes + B3_ROOT_NODE ); +} + +static inline bool b3NeedsRebuild( const b3DynamicTree* tree ) +{ + return b3IsNodeMoved( tree->nodes + B3_ROOT_NODE ) || tree->dfsOrdered == false; +} + +int b3CreateTreeProxyInternal( b3DynamicTree* tree, b3AABB aabb, uint64_t categoryBits, uint64_t userData, bool markMoved ); +void b3DynamicTree_MoveProxyInternal( b3DynamicTree* tree, int proxyId, b3AABB aabb, bool markMoved ); + +void b3DynamicTree_MarkProxyMovedSerial( b3DynamicTree* tree, int proxyId ); +void b3DynamicTree_MarkProxyMoved( b3DynamicTree* tree, int proxyId, b3AABB aabb ); +void b3DynamicTree_ClearMoved( b3DynamicTree* tree ); +int b3DynamicTree_GatherMovedProxies( const b3DynamicTree* tree, int* proxyIds ); +void b3DynamicTree_Refit( b3DynamicTree* tree ); diff --git a/src/joint.c b/src/joint.c index 0549dc0..068a29d 100644 --- a/src/joint.c +++ b/src/joint.c @@ -953,7 +953,7 @@ void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) if ( shape->proxyKey != B3_NULL_INDEX ) { - b3BufferMove( &world->broadPhase, shape->proxyKey ); + b3BroadPhase_MarkProxyMovedSerial( &world->broadPhase, shape->proxyKey ); } shapeId = shape->nextShapeId; diff --git a/src/parallel_for.c b/src/parallel_for.c index c16ac18..5a497a4 100644 --- a/src/parallel_for.c +++ b/src/parallel_for.c @@ -78,6 +78,8 @@ void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCou // block size grows once items exceed maxBlockCount * minRange // so the block count stays bounded and per-block sync overhead stays low. // Benchmarking shows 32 is optimal for the convex pile benchmark and others. + // This is a lot different than Box2D because the 3D narrow phase cost + // varies a lot. int blocksPerWorker = 32; int maxBlockCount = blocksPerWorker * workerCount; diff --git a/src/physics_world.c b/src/physics_world.c index 0f5a5b3..3a68a80 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -175,11 +175,11 @@ static void b3CreateWorkerContexts( b3World* world ) { world->taskContexts.data[i].arena = b3CreateArena( 128 * 1024 ); b3Array_Reserve( world->taskContexts.data[i].sensorHits, 8 ); + b3Array_Reserve( world->taskContexts.data[i].pairKeys, 64 ); world->taskContexts.data[i].contactStateBitSet = b3CreateBitSet( 1024 ); world->taskContexts.data[i].hitEventBitSet = b3CreateBitSet( 1024 ); world->taskContexts.data[i].hasHitEvents = false; world->taskContexts.data[i].jointStateBitSet = b3CreateBitSet( 1024 ); - world->taskContexts.data[i].enlargedSimBitSet = b3CreateBitSet( 256 ); world->taskContexts.data[i].awakeIslandBitSet = b3CreateBitSet( 256 ); world->taskContexts.data[i].splitIslandId = B3_NULL_INDEX; @@ -193,10 +193,10 @@ static void b3DestroyWorkerContexts( b3World* world ) { b3DestroyArena( &world->taskContexts.data[i].arena ); b3Array_Destroy( world->taskContexts.data[i].sensorHits ); + b3Array_Destroy( world->taskContexts.data[i].pairKeys ); b3DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); b3DestroyBitSet( &world->taskContexts.data[i].hitEventBitSet ); b3DestroyBitSet( &world->taskContexts.data[i].jointStateBitSet ); - b3DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); b3DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); b3DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits ); @@ -1148,7 +1148,7 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount ) world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); world->userTreeTask = NULL; world->activeTaskCount -= 1; - b3ValidateNoEnlarged( &world->broadPhase ); + b3ValidateNoMoved( &world->broadPhase ); } // Update sensors @@ -2419,22 +2419,14 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) int staticTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_staticBody ); int kinematicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_kinematicBody ); int dynamicTreeBytes = b3DynamicTree_GetByteCount( world->broadPhase.trees + b3_dynamicBody ); - int movedBytes = 0; - for ( int i = 0; i < b3_bodyTypeCount; ++i ) - { - movedBytes += b3GetBitSetBytes( &world->broadPhase.movedProxies[i] ); - } - int moveArrayBytes = b3Array_ByteCount( world->broadPhase.moveArray ); b3HashSet* pairSet = &world->broadPhase.pairSet; int pairSetBytes = b3GetHashSetBytes( pairSet ); - total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + movedBytes + moveArrayBytes + pairSetBytes; + total += (uint64_t)staticTreeBytes + kinematicTreeBytes + dynamicTreeBytes + pairSetBytes; b3Log( "broad-phase" ); b3Log( "static tree: %d", staticTreeBytes ); b3Log( "kinematic tree: %d", kinematicTreeBytes ); b3Log( "dynamic tree: %d", dynamicTreeBytes ); - b3Log( "movedProxies: %d", movedBytes ); - b3Log( "moveArray: %d", moveArrayBytes ); b3Log( "pairSet: %d (%d, %d)", pairSetBytes, pairSet->count, pairSet->capacity ); // Manifold block allocators, one per manifold point count @@ -2512,10 +2504,10 @@ void b3World_DumpMemoryStats( b3WorldId worldId ) { b3TaskContext* taskContext = world->taskContexts.data + i; taskContextBytes += b3Array_ByteCount( taskContext->sensorHits ); + taskContextBytes += b3Array_ByteCount( taskContext->pairKeys ); taskContextBytes += b3GetBitSetBytes( &taskContext->contactStateBitSet ); taskContextBytes += b3GetBitSetBytes( &taskContext->jointStateBitSet ); taskContextBytes += b3GetBitSetBytes( &taskContext->hitEventBitSet ); - taskContextBytes += b3GetBitSetBytes( &taskContext->enlargedSimBitSet ); taskContextBytes += b3GetBitSetBytes( &taskContext->awakeIslandBitSet ); } diff --git a/src/physics_world.h b/src/physics_world.h index 8bd3df8..47263a6 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -76,6 +76,9 @@ typedef struct b3TaskContext // Collect per thread sensor continuous hit events. b3Array( b3SensorHit ) sensorHits; + // Broad-phase pairs. + b3Array( uint64_t ) pairKeys; + // These bits align with the b3ConstraintGraph::contactBlocks and signal a change in contact status b3BitSet contactStateBitSet; @@ -88,10 +91,6 @@ typedef struct b3TaskContext // Fast-path flag: true when this worker set at least one bit in hitEventBitSet this step. bool hasHitEvents; - // Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array - // that is very large when there are many static shapes. - b3BitSet enlargedSimBitSet; - // Used to put islands to sleep b3BitSet awakeIslandBitSet; @@ -177,6 +176,10 @@ typedef struct b3World // These are sparse arrays that point into the pools above b3Array( b3Shape ) shapes; + // Compound shapes are static only, so only the broad-phase static pass can meet one. This + // lets a world without compounds skip the shape type read in the pair batch. + int compoundShapeCount; + // Reference counted store of shared hull data keyed by content. Shapes hold a // pointer to the hull stored in the db. Type erased to avoid leaking the verstable map // type into this header. diff --git a/src/platform.h b/src/platform.h index 399cea5..c6a6d40 100644 --- a/src/platform.h +++ b/src/platform.h @@ -12,6 +12,11 @@ #include #elif defined( _MSC_VER ) #include +// _MM_HINT_T0 lives here. Prefetch is a platform intrinsic, not part of the SIMD kernel +// selection, so it has to survive a BOX3D_DISABLE_SIMD build. +#if defined( _M_X64 ) || defined( _M_IX86 ) +#include +#endif #endif #if defined( _MSC_VER ) @@ -104,3 +109,26 @@ static inline uint32_t b3AtomicLoadU32( b3AtomicU32* a ) #error "Unsupported platform" #endif } + +// Relaxed load on a plain word. Lets a racing reader peek before paying for a read modify write. +static inline uint32_t b3AtomicLoadU32Raw( uint32_t* a ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) + return (uint32_t)__iso_volatile_load32( (volatile __int32*)a ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( a, __ATOMIC_RELAXED ); +#else +#error "Unsupported platform" +#endif +} + +static inline uint32_t b3AtomicFetchOrU32( uint32_t* a, uint32_t mask ) +{ +#if defined( _MSC_VER ) + return (uint32_t)_InterlockedOr( (long*)a, (long)mask ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_fetch_or( a, mask, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} diff --git a/src/recording.h b/src/recording.h index a198f70..b6b7af8 100644 --- a/src/recording.h +++ b/src/recording.h @@ -51,8 +51,8 @@ typedef struct b3World b3World; #define B3_REC_VERSION_MAJOR 6 // Minor tracks op-stream additions that keep the 48 byte header shape. -// Minor version 4 added b3Shape_SetMeshMaterial, b3Shape_SetHull, b3Shape_SetMesh -#define B3_REC_VERSION_MINOR 4 +// Minor version 6 moved the broad phase to the tree against tree pair traversal +#define B3_REC_VERSION_MINOR 6 // File header, fixed 48 bytes. Contains the registry locator so the player // can load geometry before replaying any ops. diff --git a/src/shape.c b/src/shape.c index 1f0c305..bd40c96 100644 --- a/src/shape.c +++ b/src/shape.c @@ -192,6 +192,8 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran if ( shape->type == b3_compoundShape ) { + world->compoundShapeCount += 1; + // Own a copy of the compound materials so every shape frees its array the same way. Compounds // are few, so the copy is cheap and avoids aliasing the geometry blob. int materialCount = shape->compound->materialCount; @@ -481,6 +483,12 @@ static void b3DestroyShapeInternal( b3World* world, b3Shape* shape, b3Body* body { int shapeId = shape->id; + if ( shape->type == b3_compoundShape ) + { + B3_ASSERT( world->compoundShapeCount > 0 ); + world->compoundShapeCount -= 1; + } + // Remove the shape from the body's doubly linked list. if ( shape->prevShapeId != B3_NULL_INDEX ) { diff --git a/src/shape.h b/src/shape.h index ed76dd4..7082bc7 100644 --- a/src/shape.h +++ b/src/shape.h @@ -19,7 +19,7 @@ typedef enum b3ShapeFlags b3_enableCustomFiltering = 0x04, b3_enableHitEvents = 0x08, b3_enablePreSolveEvents = 0x10, - b3_enlargedAABB = 0x20, + // 0x20 is free, it was the enlarged AABB flag before the broad-phase marked the tree b3_enableSpeculative = 0x40, } b3ShapeFlags; diff --git a/src/simd.h b/src/simd.h index 6e313a0..258c31e 100644 --- a/src/simd.h +++ b/src/simd.h @@ -5,6 +5,9 @@ #include "core.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + #include #if defined( B3_SIMD_NEON ) @@ -921,3 +924,184 @@ static inline int b3MinIndexW( b3FloatW a, int bitCount ) } #endif + +#if defined( B3_SIMD_NEON ) + +typedef struct b3AABBV +{ + float32x4_t lower; + float32x4_t upper; +} b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + const float* base = &aabb->lowerBound.x; + + // Offset to avoid reading off the end (avoid UB). + // [lz ux uy uz] + float32x4_t v1 = vld1q_f32( base + 2 ); + b3AABBV result; + // [lx ly lz -] + result.lower = vld1q_f32( base ); + // [ux uy uz -] + result.upper = vextq_f32( v1, v1, 1 ); + return result; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + static const uint32_t laneMask[4] = { 0, 0, 0, 0xFFFFFFFFu }; + uint32x4_t test = vandq_u32( vcleq_f32( a.lower, b.upper ), vcleq_f32( b.lower, a.upper ) ); + return vminvq_u32( vorrq_u32( test, vld1q_u32( laneMask ) ) ) != 0; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + b3AABBV result; + result.lower = vminq_f32( a.lower, b.lower ); + result.upper = vmaxq_f32( a.upper, b.upper ); + return result; +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); +} + +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + float32x4_t raw0 = vsetq_lane_f32( vgetq_lane_f32( value.upper, 0 ), value.lower, 3 ); + float32x4_t rotated = vextq_f32( value.upper, value.upper, 1 ); + float32x4_t raw1 = vcombine_f32( vget_high_f32( raw0 ), vget_low_f32( rotated ) ); + + float* base = &aabb->lowerBound.x; + uint32x4_t mask = vdupq_n_u32( condition ? 0xFFFFFFFFu : 0u ); + vst1q_f32( base, vbslq_f32( mask, raw0, vld1q_f32( base ) ) ); + vst1q_f32( base + 2, vbslq_f32( mask, raw1, vld1q_f32( base + 2 ) ) ); +} + +#elif defined( B3_SIMD_SSE2 ) + +typedef struct b3AABBV +{ + __m128 lower; + __m128 upper; +} b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + const float* base = &aabb->lowerBound.x; + // Same offset as Neon to avoid UB. + __m128 v1 = _mm_loadu_ps( base + 2 ); + b3AABBV result; + result.lower = _mm_loadu_ps( base ); + result.upper = _mm_shuffle_ps( v1, v1, _MM_SHUFFLE( 3, 3, 2, 1 ) ); + return result; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + __m128 test = _mm_and_ps( _mm_cmple_ps( a.lower, b.upper ), _mm_cmple_ps( b.lower, a.upper ) ); + return ( _mm_movemask_ps( test ) & 0x7 ) == 0x7; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, b3LoadAABBV( &node->aabb ) ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( b3LoadAABBV( a ), b3LoadAABBV( b ) ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + b3AABBV result; + result.lower = _mm_min_ps( a.lower, b.lower ); + result.upper = _mm_max_ps( a.upper, b.upper ); + return result; +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3UnionAABBV( b3LoadAABBV( &pair[0].aabb ), b3LoadAABBV( &pair[1].aabb ) ); +} + +// Conditionally store an AABB. Avoids a branch. +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + __m128 lane3 = _mm_castsi128_ps( _mm_set_epi32( -1, 0, 0, 0 ) ); + __m128 uxSplat = _mm_shuffle_ps( value.upper, value.upper, _MM_SHUFFLE( 0, 0, 0, 0 ) ); + __m128 raw0 = _mm_or_ps( _mm_andnot_ps( lane3, value.lower ), _mm_and_ps( lane3, uxSplat ) ); + __m128 raw1 = _mm_shuffle_ps( raw0, value.upper, _MM_SHUFFLE( 2, 1, 3, 2 ) ); + + float* base = &aabb->lowerBound.x; + __m128 mask = _mm_castsi128_ps( _mm_set1_epi32( condition ? -1 : 0 ) ); + __m128 old0 = _mm_loadu_ps( base ); + __m128 old1 = _mm_loadu_ps( base + 2 ); + _mm_storeu_ps( base, _mm_or_ps( _mm_and_ps( mask, raw0 ), _mm_andnot_ps( mask, old0 ) ) ); + _mm_storeu_ps( base + 2, _mm_or_ps( _mm_and_ps( mask, raw1 ), _mm_andnot_ps( mask, old1 ) ) ); +} + +#else + +typedef b3AABB b3AABBV; + +B3_FORCE_INLINE b3AABBV b3LoadAABBV( const b3AABB* aabb ) +{ + return *aabb; +} + +B3_FORCE_INLINE bool b3OverlapAABBV( b3AABBV a, b3AABBV b ) +{ + return a.lowerBound.x <= b.upperBound.x && a.lowerBound.y <= b.upperBound.y && a.lowerBound.z <= b.upperBound.z && + b.lowerBound.x <= a.upperBound.x && b.lowerBound.y <= a.upperBound.y && b.lowerBound.z <= a.upperBound.z; +} + +B3_FORCE_INLINE bool b3OverlapNode( b3AABBV av, const b3TreeNode* node ) +{ + return b3OverlapAABBV( av, node->aabb ); +} + +B3_FORCE_INLINE bool b3OverlapV( const b3AABB* a, const b3AABB* b ) +{ + return b3OverlapAABBV( *a, *b ); +} + +B3_FORCE_INLINE b3AABBV b3UnionAABBV( b3AABBV a, b3AABBV b ) +{ + return b3AABB_Union( a, b ); +} + +B3_FORCE_INLINE b3AABBV b3UnionPairV( const b3TreeNode* pair ) +{ + return b3AABB_Union( pair[0].aabb, pair[1].aabb ); +} + +B3_FORCE_INLINE void b3StoreAABBV( b3AABB* aabb, b3AABBV value, bool condition ) +{ + if ( condition ) + { + *aabb = value; + } +} + +#endif + +B3_FORCE_INLINE b3AABB b3UnionV( b3AABB a, b3AABB b ) +{ + b3AABB result; + b3StoreAABBV( &result, b3UnionAABBV( b3LoadAABBV( &a ), b3LoadAABBV( &b ) ), true ); + return result; +} diff --git a/src/solver.c b/src/solver.c index ad5d003..bfb5ccd 100644 --- a/src/solver.c +++ b/src/solver.c @@ -589,8 +589,14 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; - shape->flags |= b3_enlargedAABB; fastBodySim->flags |= b3_enlargeBounds; + + // Regular bodies mark the hierarchy as moved using atomic operations. + // Bullets are handled separately at a later stage. + if ( isBullet == false ) + { + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } } shapeId = shape->nextShapeId; @@ -621,8 +627,12 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), }; - shape->flags |= b3_enlargedAABB; fastBodySim->flags |= b3_enlargeBounds; + + if ( isBullet == false ) + { + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } } shapeId = shape->nextShapeId; @@ -680,7 +690,6 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, b3BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; b3TaskContext* taskContext = world->taskContexts.data + workerIndex; - b3BitSet* enlargedSimBitSet = &taskContext->enlargedSimBitSet; b3BitSet* awakeIslandBitSet = &taskContext->awakeIslandBitSet; const float speculativeScalar = B3_SPECULATIVE_DISTANCE; @@ -817,42 +826,31 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, } // Update shapes AABBs - b3WorldTransform transform = sim->transform; - bool isFast = ( sim->flags & b3_isFast ) != 0; - int shapeId = body->headShapeId; - while ( shapeId != B3_NULL_INDEX ) + // For fast non-bullet bodies the AABB has already been updated in b3SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage + if ( ( sim->flags & b3_isFast ) == 0 ) { - b3Shape* shape = b3Array_Get( world->shapes, shapeId ); - - if ( isFast ) + b3WorldTransform transform = sim->transform; + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) { - // For fast non-bullet bodies the AABB has already been updated in b3SolveContinuous - // For fast bullet bodies the AABB will be updated at a later stage + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); - // Add to enlarged shapes regardless of AABB changes. - // Bit-set to keep the move array sorted - b3SetBit( enlargedSimBitSet, simIndex ); - } - else - { b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); shape->aabb = aabb; - B3_ASSERT( ( shape->flags & b3_enlargedAABB ) == 0 ); - if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) { float marginScalar = shape->aabbMargin; b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; - shape->flags |= b3_enlargedAABB; - // Bit-set to keep the move array sorted - b3SetBit( enlargedSimBitSet, simIndex ); + // Mark the hierarchy as moved using atomic operations. + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); } - } - shapeId = shape->nextShapeId; + shapeId = shape->nextShapeId; + } } } @@ -1870,18 +1868,28 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) b3TracyCZoneNC( update_transforms, "Update Transforms", b3_colorMediumSeaGreen, true ); uint64_t transformTicks = b3GetTicks(); - // Prepare contact, enlarged body, and island bit sets used in body finalization. + // Prepare the island bit set used in body finalization. int awakeIslandCount = awakeSet->islandSims.count; for ( int i = 0; i < world->workerCount; ++i ) { b3TaskContext* taskContext = world->taskContexts.data + i; b3Array_Clear( taskContext->sensorHits ); - b3SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); b3SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); taskContext->splitIslandId = B3_NULL_INDEX; taskContext->splitSleepTime = 0.0f; } + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the + // broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b3ValidateNoMoved( &world->broadPhase ); + // Finalize bodies. Must happen after the constraint solver and after island splitting. b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 16, stepContext, "ccd" ); @@ -2073,90 +2081,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) b3TracyCZoneNC( refit_bvh, "Refit BVH", b3_colorFireBrick, true ); uint64_t refitTicks = b3GetTicks(); - // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the - // broad-phase. - if ( world->userTreeTask != NULL ) - { - world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); - world->userTreeTask = NULL; - world->activeTaskCount -= 1; - } - - b3ValidateNoEnlarged( &world->broadPhase ); - - // Gather bits for all sim bodies that have enlarged AABBs - b3BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; - for ( int i = 1; i < world->workerCount; ++i ) - { - b3InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); - } - - // Enlarge broad-phase proxies and build move array - // Apply shape AABB changes to broad-phase. This also create the move array which must be - // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. - // This has to happen before bullets are processed. - { - b3BroadPhase* broadPhase = &world->broadPhase; - uint32_t wordCount = enlargedBodyBitSet->blockCount; - uint64_t* bits = enlargedBodyBitSet->bits; - - // Fast array access is important here - b3Body* bodyArray = world->bodies.data; - b3BodySim* bodySimArray = awakeSet->bodySims.data; - b3Shape* shapeArray = world->shapes.data; - - for ( uint32_t k = 0; k < wordCount; ++k ) - { - uint64_t word = bits[k]; - while ( word != 0 ) - { - uint32_t ctz = b3CTZ64( word ); - uint32_t bodySimIndex = 64 * k + ctz; - - b3BodySim* bodySim = bodySimArray + bodySimIndex; - - b3Body* body = bodyArray + bodySim->bodyId; - - int shapeId = body->headShapeId; - if ( ( bodySim->flags & ( b3_isBullet | b3_isFast ) ) == ( b3_isBullet | b3_isFast ) ) - { - // Fast bullet bodies don't have their final AABB yet - while ( shapeId != B3_NULL_INDEX ) - { - b3Shape* shape = shapeArray + shapeId; - - // Shape is fast. It's aabb will be enlarged in continuous collision. - // Update the move array here for determinism because bullets are processed - // below in non-deterministic order. - b3BufferMove( broadPhase, shape->proxyKey ); - - shapeId = shape->nextShapeId; - } - } - else - { - while ( shapeId != B3_NULL_INDEX ) - { - b3Shape* shape = shapeArray + shapeId; - - // The AABB may not have been enlarged, despite the body being flagged as enlarged. - // For example, a body with multiple shapes may have not have all shapes enlarged. - // A fast body may have been flagged as enlarged despite having no shapes enlarged. - if ( shape->flags & b3_enlargedAABB ) - { - b3BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); - shape->flags &= ~b3_enlargedAABB; - } - - shapeId = shape->nextShapeId; - } - } - - // Clear the smallest set bit - word = word & ( word - 1 ); - } - } - } + b3DynamicTree_Refit( world->broadPhase.trees + b3_kinematicBody ); + b3DynamicTree_Refit( world->broadPhase.trees + b3_dynamicBody ); b3ValidateBroadPhase( &world->broadPhase ); @@ -2207,23 +2133,17 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) while ( shapeId != B3_NULL_INDEX ) { b3Shape* shape = shapeArray + shapeId; - if ( ( shape->flags & b3_enlargedAABB ) == 0 ) - { - shapeId = shape->nextShapeId; - continue; - } - - // clear flag - shape->flags &= ~b3_enlargedAABB; int proxyKey = shape->proxyKey; int proxyId = B3_PROXY_ID( proxyKey ); - B3_ASSERT( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); + B3_VALIDATE( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); - // all fast bullet shapes should already be in the move buffer - B3_ASSERT( b3GetBit( &broadPhase->movedProxies[b3_dynamicBody], proxyId ) ); + b3AABB treeAABB = b3DynamicTree_GetAABB( dynamicTree, proxyId ); - b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + if ( b3AABB_Contains( treeAABB, shape->fatAABB ) == false ) + { + b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + } shapeId = shape->nextShapeId; } diff --git a/src/table.c b/src/table.c index fb066b8..dc13c33 100644 --- a/src/table.c +++ b/src/table.c @@ -52,25 +52,6 @@ void b3ClearSet( b3HashSet* set ) memset( set->items, 0, set->capacity * sizeof( b3SetItem ) ); } -// I need a good hash because the keys are built from pairs of increasing integers. -// A simple hash like hash = (integer1 XOR integer2) has many collisions. -// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ -// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ -// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ -// todo try: -// https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ -static inline uint32_t b3KeyHash( uint64_t key ) -{ - uint64_t h = key; - h ^= h >> 33; - h *= 0xff51afd7ed558ccdL; - h ^= h >> 33; - h *= 0xc4ceb9fe1a85ec53L; - h ^= h >> 33; - - return (uint32_t)h; -} - static int32_t b3FindSlot( const b3HashSet* set, uint64_t key, uint32_t hash ) { uint32_t capacity = set->capacity; @@ -139,6 +120,15 @@ bool b3ContainsKey( const b3HashSet* set, uint64_t key ) return set->items[index].key == key; } +// The caller already has the hash, which lets a batch prefetch the slots before probing them. +bool b3ContainsHashedKey( const b3HashSet* set, uint64_t key, uint32_t hash ) +{ + B3_ASSERT( key != 0 ); + B3_ASSERT( hash == b3KeyHash( key ) ); + int32_t index = b3FindSlot( set, key, hash ); + return set->items[index].key == key; +} + int b3GetHashSetBytes( b3HashSet* set ) { return set->capacity * (int)sizeof( b3SetItem ); diff --git a/src/table.h b/src/table.h index ec8f397..9e12b99 100644 --- a/src/table.h +++ b/src/table.h @@ -3,11 +3,31 @@ #pragma once +#include "box3d/base.h" #include "box3d/constants.h" #include #include +// I need a good hash because the keys are built from pairs of increasing integers. +// A simple hash like hash = (integer1 XOR integer2) has many collisions. +// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ +// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ +// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ +// todo try: +// https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ +B3_FORCE_INLINE uint32_t b3KeyHash( uint64_t key ) +{ + uint64_t h = key; + h ^= h >> 33; + h *= 0xff51afd7ed558ccdL; + h ^= h >> 33; + h *= 0xc4ceb9fe1a85ec53L; + h ^= h >> 33; + + return (uint32_t)h; +} + typedef struct b3SetItem { uint64_t key; @@ -49,5 +69,6 @@ bool b3AddKey( b3HashSet* set, uint64_t key ); bool b3RemoveKey( b3HashSet* set, uint64_t key ); bool b3ContainsKey( const b3HashSet* set, uint64_t key ); +bool b3ContainsHashedKey( const b3HashSet* set, uint64_t key, uint32_t hash ); int b3GetHashSetBytes( b3HashSet* set ); diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 88e5be9..3bd6cf0 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -33,7 +33,7 @@ // Snapshot image magic 'BNS3' and version #define B3_SNAP_MAGIC 0x33534E42u -#define B3_SNAP_VERSION 3u // added b3Body::safetyFactor +#define B3_SNAP_VERSION 5u // broad-phase pair traversal, move buffer removed #define B3_SNAP_FLAG_VALIDATION 0x1u #define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u @@ -64,6 +64,7 @@ static uint32_t b3ComputeLayoutHash( void ) MIX( sizeof( b3GraphColor ) ) MIX( sizeof( b3DynamicTree ) ) MIX( sizeof( b3TreeNode ) ) + MIX( sizeof( b3TreeProxy ) ) MIX( sizeof( b3SetItem ) ) MIX( sizeof( b3IdPool ) ) MIX( sizeof( b3SurfaceMaterial ) ) @@ -277,18 +278,26 @@ static void b3DesHashSet( b3SnapReader* r, b3HashSet* hs ) } } -// DynamicTree: version, scalars, full nodeCapacity nodes (rebuild scratch excluded) +// DynamicTree: version, scalars, then nodes and parents to nodeEnd and proxies at capacity. +// The empty node and the free pairs are sentinels below the end and travel as they are. Only the +// stale tail above the end, the spare array and the rebuild scratch are skipped. static void b3SerTree( b3RecBuffer* buf, const b3DynamicTree* tree ) { b3SnapW_Bytes( buf, &tree->version, sizeof( uint64_t ) ); - b3SnapW_I32( buf, tree->root ); - b3SnapW_I32( buf, tree->nodeCount ); - b3SnapW_I32( buf, tree->nodeCapacity ); - b3SnapW_I32( buf, tree->freeList ); + b3SnapW_I32( buf, tree->nodeEnd ); + b3SnapW_I32( buf, tree->pairFreeList ); b3SnapW_I32( buf, tree->proxyCount ); - if ( tree->nodeCapacity > 0 ) + b3SnapW_I32( buf, tree->proxyCapacity ); + b3SnapW_I32( buf, tree->proxyFreeList ); + b3SnapW_I32( buf, tree->dfsOrdered ? 1 : 0 ); + if ( tree->nodeEnd > 0 ) { - b3SnapW_Bytes( buf, tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3SnapW_Bytes( buf, tree->nodes, tree->nodeEnd * (int)sizeof( b3TreeNode ) ); + b3SnapW_Bytes( buf, tree->parents, tree->nodeEnd * (int)sizeof( int32_t ) ); + } + if ( tree->proxyCapacity > 0 ) + { + b3SnapW_Bytes( buf, tree->proxies, tree->proxyCapacity * (int)sizeof( b3TreeProxy ) ); } } @@ -296,29 +305,50 @@ static void b3DesTree( b3SnapReader* r, b3DynamicTree* tree ) { uint64_t version; b3SnapR_Bytes( r, &version, sizeof( uint64_t ) ); - int root = b3SnapR_I32( r ); - int nodeCount = b3SnapR_I32( r ); - int nodeCapacity = b3SnapR_I32( r ); - int freeList = b3SnapR_I32( r ); + int nodeEnd = b3SnapR_I32( r ); + int pairFreeList = b3SnapR_I32( r ); int proxyCount = b3SnapR_I32( r ); + int proxyCapacity = b3SnapR_I32( r ); + int proxyFreeList = b3SnapR_I32( r ); + bool dfsOrdered = b3SnapR_I32( r ) != 0; - if ( r->ok && b3SnapCheckCount( r, nodeCapacity, (int)sizeof( b3TreeNode ), (int)sizeof( b3TreeNode ) ) == false ) + if ( r->ok && b3SnapCheckCount( r, nodeEnd, (int)sizeof( b3TreeNode ), (int)sizeof( b3TreeNode ) ) == false ) { r->ok = false; } - // Free existing allocation including any rebuild scratch + if ( r->ok && b3SnapCheckCount( r, proxyCapacity, (int)sizeof( b3TreeProxy ), (int)sizeof( b3TreeProxy ) ) == false ) + { + r->ok = false; + } + + if ( r->ok && ( nodeEnd < 2 || ( nodeEnd & 1 ) != 0 || proxyCapacity < 1 ) ) + { + r->ok = false; + } + + // Free existing allocation including the spare array and any rebuild scratch b3Free( tree->nodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); + b3Free( tree->parents, tree->nodeCapacity * (int)sizeof( int32_t ) ); + b3Free( tree->proxies, tree->proxyCapacity * (int)sizeof( b3TreeProxy ) ); + b3Free( tree->swapNodes, tree->nodeCapacity * (int)sizeof( b3TreeNode ) ); b3Free( tree->leafIndices, tree->rebuildCapacity * (int)sizeof( int ) ); + b3Free( tree->leafNodes, tree->rebuildCapacity * (int)sizeof( b3TreeNode ) ); b3Free( tree->leafBoxes, tree->rebuildCapacity * (int)sizeof( b3AABB ) ); b3Free( tree->leafCenters, tree->rebuildCapacity * (int)sizeof( b3Vec3 ) ); b3Free( tree->binIndices, tree->rebuildCapacity * (int)sizeof( int ) ); 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->nodeEnd = 0; tree->nodeCapacity = 0; + tree->proxyCapacity = 0; tree->rebuildCapacity = 0; if ( !r->ok ) @@ -327,17 +357,21 @@ static void b3DesTree( b3SnapReader* r, b3DynamicTree* tree ) } tree->version = version; - tree->root = root; - tree->nodeCount = nodeCount; - tree->nodeCapacity = nodeCapacity; - tree->freeList = freeList; + tree->nodeEnd = nodeEnd; + tree->nodeCapacity = nodeEnd; + tree->pairFreeList = pairFreeList; tree->proxyCount = proxyCount; + tree->proxyCapacity = proxyCapacity; + tree->proxyFreeList = proxyFreeList; + tree->dfsOrdered = dfsOrdered; - if ( nodeCapacity > 0 ) - { - tree->nodes = (b3TreeNode*)b3Alloc( nodeCapacity * (int)sizeof( b3TreeNode ) ); - b3SnapR_Bytes( r, tree->nodes, nodeCapacity * (int)sizeof( b3TreeNode ) ); - } + tree->nodes = (b3TreeNode*)b3Alloc( nodeEnd * (int)sizeof( b3TreeNode ) ); + tree->parents = (int32_t*)b3Alloc( nodeEnd * (int)sizeof( int32_t ) ); + b3SnapR_Bytes( r, tree->nodes, nodeEnd * (int)sizeof( b3TreeNode ) ); + b3SnapR_Bytes( r, tree->parents, nodeEnd * (int)sizeof( int32_t ) ); + + tree->proxies = (b3TreeProxy*)b3Alloc( proxyCapacity * (int)sizeof( b3TreeProxy ) ); + b3SnapR_Bytes( r, tree->proxies, proxyCapacity * (int)sizeof( b3TreeProxy ) ); } // Solver set: setIndex + 4 arrays (note: contactIndices is int array, not contactSims) @@ -446,6 +480,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world ) b3SnapW_Bytes( buf, &world->inv_h, sizeof( float ) ); b3SnapW_Bytes( buf, &world->inv_dt, sizeof( float ) ); b3SnapW_I32( buf, world->endEventArrayIndex ); + b3SnapW_I32( buf, world->compoundShapeCount ); b3SnapW_Bytes( buf, &world->maxCapacity, sizeof( b3Capacity ) ); uint8_t flags = 0; flags |= world->enableSleep ? 0x01u : 0u; @@ -470,6 +505,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world ) b3SnapR_Bytes( r, &world->inv_h, sizeof( float ) ); b3SnapR_Bytes( r, &world->inv_dt, sizeof( float ) ); world->endEventArrayIndex = b3SnapR_I32( r ); + world->compoundShapeCount = b3SnapR_I32( r ); b3SnapR_Bytes( r, &world->maxCapacity, sizeof( b3Capacity ) ); uint8_t flags = 0; b3SnapR_Bytes( r, &flags, 1 ); @@ -1078,11 +1114,6 @@ int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ) { b3SerTree( buf, &bp->trees[t] ); } - for ( int t = 0; t < b3_bodyTypeCount; ++t ) - { - b3SerBitSet( buf, &bp->movedProxies[t] ); - } - b3SerPodArray( buf, bp->moveArray ); b3SerHashSet( buf, &bp->pairSet ); // Constraint graph @@ -1307,17 +1338,8 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re { b3DesTree( r, &bp->trees[t] ); } - for ( int t = 0; t < b3_bodyTypeCount; ++t ) - { - b3DesBitSet( r, &bp->movedProxies[t] ); - } - - b3Array_Destroy( bp->moveArray ); - b3Array_Create( bp->moveArray ); - b3DesPodArray( r, bp->moveArray ); - b3DesHashSet( r, &bp->pairSet ); - // Transient moveResults/movePairs stay at shell's NULL/0 + // The gathered moved siblings are per step scratch and stay at the shell's NULL } // 11. Constraint graph diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index bf561bf..598b1e2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -11,6 +11,7 @@ set(BOX3D_TEST_FILES test_container.c test_determinism.c test_distance.c + test_dynamic_tree.c test_hash.c test_height_field.c test_hull.c diff --git a/test/main.c b/test/main.c index bd00ecb..cc98311 100644 --- a/test/main.c +++ b/test/main.c @@ -34,6 +34,7 @@ extern int CompoundTest( void ); extern int ContainerTest( void ); extern int DeterminismTest( void ); extern int DistanceTest( void ); +extern int DynamicTreeTest( void ); extern int HashTest( void ); extern int HeightFieldTest( void ); extern int HullTest( void ); @@ -109,6 +110,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( ContainerTest ); MAYBE_RUN_TEST( DeterminismTest ); MAYBE_RUN_TEST( DistanceTest ); + MAYBE_RUN_TEST( DynamicTreeTest ); MAYBE_RUN_TEST( HashTest ); MAYBE_RUN_TEST( HeightFieldTest ); MAYBE_RUN_TEST( HullTest ); diff --git a/test/test_compound.c b/test/test_compound.c index 40447bc..6ff10a3 100644 --- a/test/test_compound.c +++ b/test/test_compound.c @@ -76,8 +76,10 @@ static int CompoundCreateMixed( void ) ENSURE( compound->sharedHullCount == 1 ); ENSURE( compound->sharedMeshCount == 1 ); - ENSURE( compound->tree.nodeCount > 0 ); + ENSURE( compound->tree.nodeEnd >= 2 ); + ENSURE( compound->tree.proxyCount > 0 ); ENSURE( compound->tree.nodes != NULL ); + ENSURE( compound->tree.proxies != NULL ); b3DestroyCompound( compound ); b3DestroyMesh( meshData ); diff --git a/test/test_container.c b/test/test_container.c index e3df4d3..0f93e7e 100644 --- a/test/test_container.c +++ b/test/test_container.c @@ -8,7 +8,7 @@ #include #include -b3DeclareArrayNative( uint64_t ); +// uint64_t is already declared in container.h for the broad-phase pair keys b3DeclareArrayNative( int16_t ); b3DeclareArrayNative( uint8_t ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 02861e8..3ce4bcc 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -18,25 +18,25 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 272 -#define RAGDOLL_HASH 0xBF43ECD7 -#define WAVE_PILE_SLEEP_STEP 291 -#define WAVE_PILE_HASH 0x8B6E2CD2 +#define RAGDOLL_SLEEP_STEP 277 +#define RAGDOLL_HASH 0xF7B5B2D1 +#define WAVE_PILE_SLEEP_STEP 323 +#define WAVE_PILE_HASH 0x2200A547 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x1737F5BC #define QUERY_SPAWN_HIT_COUNT 59 -#define QUERY_SPAWN_QUERY_HASH 0x31F090DC +#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC #define MESH_DROP_SLEEP_STEP 251 #define MESH_DROP_HASH 0x465381C5 #else -#define RAGDOLL_SLEEP_STEP 286 -#define RAGDOLL_HASH 0x0211682A -#define WAVE_PILE_SLEEP_STEP 282 -#define WAVE_PILE_HASH 0xFC161D3D +#define RAGDOLL_SLEEP_STEP 272 +#define RAGDOLL_HASH 0xF4992B41 +#define WAVE_PILE_SLEEP_STEP 316 +#define WAVE_PILE_HASH 0xAA7D15A7 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xB9F993A5 #define QUERY_SPAWN_HIT_COUNT 59 -#define QUERY_SPAWN_QUERY_HASH 0xB9D3863D +#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D #define MESH_DROP_SLEEP_STEP 251 #define MESH_DROP_HASH 0xE58C7240 #endif diff --git a/test/test_dynamic_tree.c b/test/test_dynamic_tree.c new file mode 100644 index 0000000..fb9a42e --- /dev/null +++ b/test/test_dynamic_tree.c @@ -0,0 +1,713 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "dynamic_tree.h" +#include "test_macros.h" + +#include "box3d/collision.h" +#include "box3d/constants.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +#define PROXY_COUNT 300 + +static uint32_t s_seed = 12345; + +static uint32_t NextRandom( void ) +{ + s_seed = 1664525u * s_seed + 1013904223u; + return s_seed; +} + +static float RandomFloat( float lo, float hi ) +{ + float t = (float)( NextRandom() >> 8 ) / (float)( 1 << 24 ); + return lo + t * ( hi - lo ); +} + +static b3AABB RandomBox( float extent, float size ) +{ + b3Vec3 center = { RandomFloat( -extent, extent ), RandomFloat( -extent, extent ), RandomFloat( -extent, extent ) }; + b3Vec3 half = { RandomFloat( 0.1f, size ), RandomFloat( 0.1f, size ), RandomFloat( 0.1f, size ) }; + return (b3AABB){ b3Sub( center, half ), b3Add( center, half ) }; +} + +typedef struct HitSet +{ + int ids[PROXY_COUNT]; + int count; + bool bad; +} HitSet; + +// Every test seeds user data with the index of the box it created the proxy from, so the sets +// are keyed on user data. Proxy ids recycle, box indices do not. +// A callback cannot fail the test directly, so bad news rides back on the context. +static bool CollectCallback( int proxyId, uint64_t userData, void* context ) +{ + HitSet* set = context; + if ( set->count >= PROXY_COUNT || (int)userData < 0 || (int)userData >= PROXY_COUNT ) + { + set->bad = true; + return false; + } + + (void)proxyId; + set->ids[set->count] = (int)userData; + set->count += 1; + return true; +} + +static int CompareInt( const void* a, const void* b ) +{ + int x = *(const int*)a; + int y = *(const int*)b; + return x < y ? -1 : ( x > y ? 1 : 0 ); +} + +static int SameSet( HitSet* a, HitSet* b ) +{ + ENSURE( a->bad == false ); + ENSURE( b->bad == false ); + ENSURE( a->count == b->count ); + qsort( a->ids, (size_t)a->count, sizeof( int ), CompareInt ); + qsort( b->ids, (size_t)b->count, sizeof( int ), CompareInt ); + for ( int i = 0; i < a->count; ++i ) + { + ENSURE( a->ids[i] == b->ids[i] ); + } + + return 0; +} + +// The tree must agree with a linear scan over the stored boxes. This is the gate on the +// SIMD box helpers, which have three separate lane layouts. +static int TreeQueryBruteForce( void ) +{ + s_seed = 12345; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 6.0f, 1.5f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + ENSURE( proxies[i] == i ); + } + + b3DynamicTree_Validate( &tree ); + ENSURE( b3DynamicTree_GetProxyCount( &tree ) == PROXY_COUNT ); + + int totalHits = 0; + for ( int round = 0; round < 200; ++round ) + { + b3AABB queryBox = RandomBox( 7.0f, 3.0f ); + + HitSet treeHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, B3_DEFAULT_MASK_BITS, false, CollectCallback, &treeHits ); + + HitSet bruteHits = { 0 }; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + if ( b3AABB_Overlaps( boxes[i], queryBox ) ) + { + bruteHits.ids[bruteHits.count] = i; + bruteHits.count += 1; + } + } + + ENSURE( SameSet( &treeHits, &bruteHits ) == 0 ); + totalHits += treeHits.count; + } + + // Guard against the comparison above agreeing on nothing + ENSURE( totalHits > 1000 ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +// Moving and enlarging every proxy exercises remove, insert, the ordered flag, the sweep refit +// and the partial rebuild, then the query has to still agree with the scan. +static int TreeMoveRefitRebuild( void ) +{ + s_seed = 777; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 6.0f, 1.0f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + } + + b3DynamicTree_Rebuild( &tree, true ); + b3DynamicTree_Validate( &tree ); + ENSURE( tree.dfsOrdered ); + + int totalHits = 0; + for ( int round = 0; round < 8; ++round ) + { + // Half the proxies move, half grow in place + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + if ( ( i & 1 ) == 0 ) + { + boxes[i] = RandomBox( 6.0f, 1.0f ); + b3DynamicTree_MoveProxy( &tree, proxies[i], boxes[i] ); + } + else + { + b3AABB grown = b3AABB_Inflate( boxes[i], 0.35f ); + boxes[i] = grown; + b3DynamicTree_EnlargeProxy( &tree, proxies[i], grown ); + } + } + + b3DynamicTree_Refit( &tree ); + b3DynamicTree_Validate( &tree ); + + b3DynamicTree_Rebuild( &tree, false ); + b3DynamicTree_Validate( &tree ); + b3DynamicTree_ValidateNoMoved( &tree ); + ENSURE( tree.dfsOrdered ); + + // Every stored box must still be inside its leaf and inside the root + b3AABB rootBounds = b3DynamicTree_GetRootBounds( &tree ); + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + ENSURE( b3AABB_Contains( b3DynamicTree_GetAABB( &tree, proxies[i] ), boxes[i] ) ); + ENSURE( b3AABB_Contains( rootBounds, boxes[i] ) ); + ENSURE( (int)b3DynamicTree_GetUserData( &tree, proxies[i] ) == i ); + } + + b3AABB queryBox = RandomBox( 7.0f, 3.0f ); + + HitSet treeHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, B3_DEFAULT_MASK_BITS, false, CollectCallback, &treeHits ); + + HitSet bruteHits = { 0 }; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + if ( b3AABB_Overlaps( boxes[i], queryBox ) ) + { + bruteHits.ids[bruteHits.count] = i; + bruteHits.count += 1; + } + } + + ENSURE( SameSet( &treeHits, &bruteHits ) == 0 ); + totalHits += treeHits.count; + } + + // Guard against the comparison above agreeing on nothing + ENSURE( totalHits > 50 ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +// Destroying proxies in a scrambled order recycles sibling pairs through the free list and +// leaves the array unordered, which is the path the fallback refit has to handle. +static int TreeDestroyScrambled( void ) +{ + s_seed = 4242; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + bool alive[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 10.0f, 0.8f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + alive[i] = true; + } + + for ( int i = 0; i < PROXY_COUNT; i += 3 ) + { + b3DynamicTree_DestroyProxy( &tree, proxies[i] ); + alive[i] = false; + } + + b3DynamicTree_Validate( &tree ); + + // Re-create into the holes + for ( int i = 0; i < PROXY_COUNT; i += 3 ) + { + boxes[i] = RandomBox( 10.0f, 0.8f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + alive[i] = true; + } + + b3DynamicTree_Validate( &tree ); + ENSURE( b3DynamicTree_GetProxyCount( &tree ) == PROXY_COUNT ); + + // The user data has to have followed each proxy through the churn + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + ENSURE( alive[i] ); + ENSURE( (int)b3DynamicTree_GetUserData( &tree, proxies[i] ) == i ); + ENSURE( b3AABB_Contains( b3DynamicTree_GetAABB( &tree, proxies[i] ), boxes[i] ) ); + } + + b3AABB queryBox = { { -10.0f, -10.0f, -10.0f }, { 10.0f, 10.0f, 10.0f } }; + HitSet treeHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, B3_DEFAULT_MASK_BITS, false, CollectCallback, &treeHits ); + + HitSet bruteHits = { 0 }; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + if ( b3AABB_Overlaps( boxes[i], queryBox ) ) + { + bruteHits.ids[bruteHits.count] = i; + bruteHits.count += 1; + } + } + + ENSURE( SameSet( &treeHits, &bruteHits ) == 0 ); + ENSURE( treeHits.count == PROXY_COUNT ); + + // Empty the tree + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + b3DynamicTree_DestroyProxy( &tree, proxies[i] ); + } + + b3DynamicTree_Validate( &tree ); + ENSURE( b3DynamicTree_GetProxyCount( &tree ) == 0 ); + ENSURE( b3DynamicTree_GetHeight( &tree ) == 0 ); + + // Queries on an empty tree must be silent + HitSet emptyHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, B3_DEFAULT_MASK_BITS, false, CollectCallback, &emptyHits ); + ENSURE( emptyHits.count == 0 ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +typedef struct LeafRootHit +{ + int count; + int proxyId; + uint64_t userData; +} LeafRootHit; + +static bool LeafRootCallback( int proxyId, uint64_t userData, void* context ) +{ + LeafRootHit* hit = context; + hit->count += 1; + hit->proxyId = proxyId; + hit->userData = userData; + return true; +} + +// A one proxy tree is a leaf at the root with an empty node beside it, which every traversal +// has to survive. +static int TreeLeafRoot( void ) +{ + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB box = { { -1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, 1.0f } }; + int proxyId = b3DynamicTree_CreateProxy( &tree, box, B3_DEFAULT_CATEGORY_BITS, 7 ); + + b3DynamicTree_Validate( &tree ); + ENSURE( b3DynamicTree_GetProxyCount( &tree ) == 1 ); + ENSURE( b3DynamicTree_GetHeight( &tree ) == 0 ); + ENSURE( b3DynamicTree_GetAreaRatio( &tree ) == 0.0f ); + ENSURE( b3DynamicTree_GetUserData( &tree, proxyId ) == 7 ); + + LeafRootHit hit = { 0 }; + b3AABB hitBox = { { 0.5f, 0.5f, 0.5f }, { 2.0f, 2.0f, 2.0f } }; + b3DynamicTree_Query( &tree, hitBox, B3_DEFAULT_MASK_BITS, false, LeafRootCallback, &hit ); + ENSURE( hit.count == 1 ); + ENSURE( hit.proxyId == proxyId ); + ENSURE( hit.userData == 7 ); + + hit.count = 0; + b3AABB missBox = { { 4.0f, 4.0f, 4.0f }, { 5.0f, 5.0f, 5.0f } }; + b3DynamicTree_Query( &tree, missBox, B3_DEFAULT_MASK_BITS, false, LeafRootCallback, &hit ); + ENSURE( hit.count == 0 ); + + // The rebuild has one build leaf and the refit has nothing above it + b3DynamicTree_Rebuild( &tree, true ); + b3DynamicTree_Validate( &tree ); + b3DynamicTree_Refit( &tree ); + b3DynamicTree_Validate( &tree ); + ENSURE( b3DynamicTree_GetProxyCount( &tree ) == 1 ); + + b3AABB grown = b3AABB_Inflate( box, 1.0f ); + b3DynamicTree_EnlargeProxy( &tree, proxyId, grown ); + ENSURE( b3AABB_Contains( b3DynamicTree_GetAABB( &tree, proxyId ), grown ) ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +// Category bits live on the proxy now, so the filter has to bite at the leaf. +static int TreeCategoryFilter( void ) +{ + s_seed = 99; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + uint64_t bits[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 8.0f, 0.6f ); + bits[i] = 1ull << ( i % 3 ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], bits[i], (uint64_t)i ); + ENSURE( b3DynamicTree_GetCategoryBits( &tree, proxies[i] ) == bits[i] ); + } + + b3AABB queryBox = { { -8.0f, -8.0f, -8.0f }, { 8.0f, 8.0f, 8.0f } }; + + // A single bit mask makes requireAll equivalent to the any-bit test, so the last pass is + // the one that proves requireAll accepts rather than just rejects + static const uint64_t masks[3] = { 0x1ull, 0x3ull, 0x2ull }; + static const bool requireAlls[3] = { false, false, true }; + + for ( int pass = 0; pass < 3; ++pass ) + { + uint64_t mask = masks[pass]; + bool requireAll = requireAlls[pass]; + + HitSet treeHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, mask, requireAll, CollectCallback, &treeHits ); + + HitSet bruteHits = { 0 }; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + bool accept = requireAll ? ( bits[i] & mask ) == mask : ( bits[i] & mask ) != 0; + if ( accept && b3AABB_Overlaps( boxes[i], queryBox ) ) + { + bruteHits.ids[bruteHits.count] = i; + bruteHits.count += 1; + } + } + + ENSURE( SameSet( &treeHits, &bruteHits ) == 0 ); + ENSURE( treeHits.count > PROXY_COUNT / 4 ); + } + + // requireAll with two bits can never match a single bit proxy + HitSet noHits = { 0 }; + b3DynamicTree_Query( &tree, queryBox, 0x5ull, true, CollectCallback, &noHits ); + ENSURE( noHits.count == 0 ); + + // Changing the bits has to change the filter without touching the structure + b3DynamicTree_SetCategoryBits( &tree, proxies[0], 0x8ull ); + ENSURE( b3DynamicTree_GetCategoryBits( &tree, proxies[0] ) == 0x8ull ); + b3DynamicTree_Validate( &tree ); + + HitSet oneHit = { 0 }; + b3DynamicTree_Query( &tree, queryBox, 0x8ull, false, CollectCallback, &oneHit ); + ENSURE( oneHit.count == 1 ); + ENSURE( oneHit.ids[0] == 0 ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +// Returning -1 skips the proxy without shrinking the fraction, so the traversal keeps going +// and reports every proxy whose node box the segment crosses. +static float RayCastCollectCallback( const b3RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + (void)input; + (void)proxyId; + HitSet* set = context; + if ( set->count >= PROXY_COUNT || (int)userData < 0 || (int)userData >= PROXY_COUNT ) + { + set->bad = true; + return 0.0f; + } + + set->ids[set->count] = (int)userData; + set->count += 1; + return -1.0f; +} + +static float BoxCastCollectCallback( const b3BoxCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + (void)input; + (void)proxyId; + HitSet* set = context; + if ( set->count >= PROXY_COUNT || (int)userData < 0 || (int)userData >= PROXY_COUNT ) + { + set->bad = true; + return 0.0f; + } + + set->ids[set->count] = (int)userData; + set->count += 1; + return -1.0f; +} + +// The casts pop a pair, test both nodes and order the survivors. A ray that skips every hit +// must still reach every proxy its segment crosses. +static int TreeCastBruteForce( void ) +{ + s_seed = 31337; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 6.0f, 1.0f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + } + + b3DynamicTree_Rebuild( &tree, true ); + b3DynamicTree_Validate( &tree ); + + int totalCrossings = 0; + for ( int round = 0; round < 100; ++round ) + { + b3Vec3 origin = { RandomFloat( -8.0f, 8.0f ), RandomFloat( -8.0f, 8.0f ), RandomFloat( -8.0f, 8.0f ) }; + b3Vec3 translation = { RandomFloat( -16.0f, 16.0f ), RandomFloat( -16.0f, 16.0f ), RandomFloat( -16.0f, 16.0f ) }; + + b3RayCastInput rayInput = { .origin = origin, .translation = translation, .maxFraction = 1.0f }; + + HitSet treeHits = { 0 }; + b3DynamicTree_RayCast( &tree, &rayInput, B3_DEFAULT_MASK_BITS, false, RayCastCollectCallback, &treeHits ); + ENSURE( treeHits.bad == false ); + + // The tree may report a superset because it tests the node box, not the shape. Every + // proxy the segment actually crosses has to be in there. + b3Vec3 p2 = b3MulAdd( origin, 1.0f, translation ); + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + float minFraction = 0.0f; + float maxFraction = 1.0f; + if ( b3RayCastAABB( boxes[i], origin, p2, &minFraction, &maxFraction ) == false ) + { + continue; + } + + bool found = false; + for ( int j = 0; j < treeHits.count; ++j ) + { + if ( treeHits.ids[j] == i ) + { + found = true; + break; + } + } + + ENSURE( found ); + totalCrossings += 1; + } + + // Every reported proxy has to at least overlap the segment box + b3AABB segmentBox = { b3Min( origin, p2 ), b3Max( origin, p2 ) }; + for ( int j = 0; j < treeHits.count; ++j ) + { + ENSURE( b3AABB_Overlaps( boxes[treeHits.ids[j]], segmentBox ) ); + } + + // A zero extent box cast has to reach the same proxies as the ray + b3AABB castBox = { origin, origin }; + b3BoxCastInput boxInput = { .box = castBox, .translation = translation, .maxFraction = 1.0f }; + + HitSet boxHits = { 0 }; + b3DynamicTree_BoxCast( &tree, &boxInput, B3_DEFAULT_MASK_BITS, false, BoxCastCollectCallback, &boxHits ); + ENSURE( boxHits.bad == false ); + + for ( int j = 0; j < treeHits.count; ++j ) + { + bool found = false; + for ( int k = 0; k < boxHits.count; ++k ) + { + if ( boxHits.ids[k] == treeHits.ids[j] ) + { + found = true; + break; + } + } + + ENSURE( found ); + } + } + + // Guard against the comparisons above agreeing on nothing + ENSURE( totalCrossings > 100 ); + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +typedef struct ClosestResult +{ + const b3AABB* boxes; + b3Vec3 point; + bool bad; +} ClosestResult; + +static float DistanceToBoxSqr( b3Vec3 point, b3AABB box ) +{ + b3Vec3 r = b3Sub( point, b3Clamp( point, box.lowerBound, box.upperBound ) ); + return b3Dot( r, r ); +} + +static float ClosestCollectCallback( float distanceSqrMin, int proxyId, uint64_t userData, void* context ) +{ + ClosestResult* result = context; + if ( (int)userData != proxyId ) + { + result->bad = true; + return distanceSqrMin; + } + + float d = DistanceToBoxSqr( result->point, result->boxes[proxyId] ); + return d < distanceSqrMin ? d : distanceSqrMin; +} + +// The closest query orders by distance and prunes on the running minimum, so seeding it from +// the root pair has to be right or it prunes away a whole half of the tree. +static int TreeQueryClosest( void ) +{ + s_seed = 5150; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 6.0f, 0.9f ); + int proxyId = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + ENSURE( proxyId == i ); + } + + for ( int round = 0; round < 100; ++round ) + { + b3Vec3 point = { RandomFloat( -8.0f, 8.0f ), RandomFloat( -8.0f, 8.0f ), RandomFloat( -8.0f, 8.0f ) }; + + float bruteBest = FLT_MAX; + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + float d = DistanceToBoxSqr( point, boxes[i] ); + if ( d < bruteBest ) + { + bruteBest = d; + } + } + + // The callback reports the exact box distance, so the pruned search has to land on the + // same minimum as the scan. Same arithmetic both ways, so this is an exact compare. + ClosestResult result = { .boxes = boxes, .point = point }; + float minDistanceSqr = FLT_MAX; + b3DynamicTree_QueryClosest( &tree, point, B3_DEFAULT_MASK_BITS, false, ClosestCollectCallback, &result, + &minDistanceSqr ); + + ENSURE( result.bad == false ); + ENSURE( minDistanceSqr == bruteBest ); + } + + b3DynamicTree_Destroy( &tree ); + return 0; +} + +// A saved tree carries only its live node range. A baked compound tree has no parent array, so +// saving one has to bail out rather than write from a null pointer. +static int TreeSaveLoadRoundtrip( void ) +{ + s_seed = 4242; + + b3DynamicTree tree = b3DynamicTree_Create( 16 ); + + b3AABB boxes[PROXY_COUNT]; + int proxies[PROXY_COUNT]; + + for ( int i = 0; i < PROXY_COUNT; ++i ) + { + boxes[i] = RandomBox( 6.0f, 1.0f ); + proxies[i] = b3DynamicTree_CreateProxy( &tree, boxes[i], B3_DEFAULT_CATEGORY_BITS, (uint64_t)i ); + } + + // Destroy a few so the pair free list is populated and the node tail is stale + for ( int i = 0; i < PROXY_COUNT; i += 37 ) + { + b3DynamicTree_DestroyProxy( &tree, proxies[i] ); + } + + b3DynamicTree_Rebuild( &tree, true ); + b3DynamicTree_Validate( &tree ); + + const char* treePath = "test_dynamic_tree_roundtrip.dat"; + b3DynamicTree_Save( &tree, treePath ); + + b3DynamicTree loaded = b3DynamicTree_Load( treePath, 1.0f ); + remove( treePath ); + + ENSURE( loaded.nodes != NULL ); + ENSURE( loaded.nodeEnd == tree.nodeEnd ); + ENSURE( loaded.nodeCapacity == tree.nodeEnd ); + ENSURE( b3DynamicTree_GetProxyCount( &loaded ) == b3DynamicTree_GetProxyCount( &tree ) ); + ENSURE( memcmp( loaded.nodes, tree.nodes, (size_t)tree.nodeEnd * sizeof( b3TreeNode ) ) == 0 ); + ENSURE( memcmp( loaded.parents, tree.parents, (size_t)tree.nodeEnd * sizeof( int32_t ) ) == 0 ); + b3DynamicTree_Validate( &loaded ); + + b3DynamicTree_Destroy( &loaded ); + b3DynamicTree_Destroy( &tree ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + + b3CompoundHullDef hulls[2]; + hulls[0].hull = &box.base; + hulls[0].transform = (b3Transform){ { -1.0f, 0.0f, 0.0f }, b3Quat_identity }; + hulls[0].material = b3DefaultSurfaceMaterial(); + hulls[1].hull = &box.base; + hulls[1].transform = (b3Transform){ { 1.0f, 0.0f, 0.0f }, b3Quat_identity }; + hulls[1].material = b3DefaultSurfaceMaterial(); + + b3CompoundDef compoundDef = { 0 }; + compoundDef.hulls = hulls; + compoundDef.hullCount = 2; + b3CompoundData* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + ENSURE( compound->tree.parents == NULL ); + ENSURE( compound->tree.proxyCapacity == compound->tree.proxyCount ); + + const char* compoundPath = "test_dynamic_tree_compound.dat"; + remove( compoundPath ); + b3DynamicTree_Save( &compound->tree, compoundPath ); + + b3DynamicTree noTree = b3DynamicTree_Load( compoundPath, 1.0f ); + remove( compoundPath ); + ENSURE( noTree.nodes == NULL ); + + b3DestroyCompound( compound ); + return 0; +} + +int DynamicTreeTest( void ) +{ + RUN_SUBTEST( TreeLeafRoot ); + RUN_SUBTEST( TreeQueryBruteForce ); + RUN_SUBTEST( TreeCategoryFilter ); + RUN_SUBTEST( TreeMoveRefitRebuild ); + RUN_SUBTEST( TreeDestroyScrambled ); + RUN_SUBTEST( TreeCastBruteForce ); + RUN_SUBTEST( TreeQueryClosest ); + RUN_SUBTEST( TreeSaveLoadRoundtrip ); + + return 0; +} diff --git a/test/test_world.c b/test/test_world.c index f26a442..33b1bed 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #include "benchmarks.h" +#include "dynamic_tree.h" #include "overflow_color.h" #include "physics_world.h" #include "test_macros.h" @@ -971,13 +972,13 @@ static int EnableContactRecyclingTest( void ) return 0; } -static int CountEnlargedNodes( const b3DynamicTree* tree ) +static int CountMovedNodes( const b3DynamicTree* tree ) { int count = 0; - for ( int i = 0; i < tree->nodeCapacity; ++i ) + for ( int i = 0; i < tree->nodeEnd; ++i ) { const b3TreeNode* node = tree->nodes + i; - if ( ( node->flags & b3_allocatedNode ) != 0 && ( node->flags & b3_enlargedNode ) != 0 ) + if ( b3IsEmptyNode( node ) == false && b3IsNodeMoved( node ) ) { count += 1; } @@ -1020,22 +1021,73 @@ static int TestEnlargedProxyDestroyed( void ) float timeStep = 1.0f / 60.0f; b3World_Step( worldId, timeStep, 4 ); - ENSURE( CountEnlargedNodes( tree ) > 0 ); + ENSURE( CountMovedNodes( tree ) > 0 ); b3DestroyBody( moverId ); - // The mover was the only proxy in the move buffer - ENSURE( world->broadPhase.moveArray.count == 0 ); - ENSURE( CountEnlargedNodes( tree ) > 0 ); + // Removing the leaf re-derives the marks along its path, so the tree is already consistent + // here and nothing is left marked to drive a rebuild. What the destroy can still leave + // stale is the DFS order the sweep refit needs, so the step has to rebuild regardless. + b3DynamicTree_Validate( tree ); + ENSURE( CountMovedNodes( tree ) == 0 ); b3World_Step( worldId, timeStep, 4 ); - ENSURE( CountEnlargedNodes( tree ) == 0 ); + ENSURE( CountMovedNodes( tree ) == 0 ); + ENSURE( tree->dfsOrdered ); b3DestroyWorld( worldId ); return 0; } +// b3DestroyBody tears shapes down inline instead of going through the shape destroy path, so it +// needs its own compound census decrement. A leaked count pins the broad-phase on the slow +// per-candidate compound path for the life of the world and rides along into snapshots. +static int TestCompoundShapeCount( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3World* world = b3GetWorldFromId( worldId ); + ENSURE( world->compoundShapeCount == 0 ); + + b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); + + b3CompoundHullDef hull; + hull.hull = &box.base; + hull.transform = (b3Transform){ b3Vec3_zero, b3Quat_identity }; + hull.material = b3DefaultSurfaceMaterial(); + + b3CompoundDef compoundDef = { 0 }; + compoundDef.hulls = &hull; + compoundDef.hullCount = 1; + b3CompoundData* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + // Shape destroyed on its own + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + b3ShapeId shapeId = b3CreateBakedCompoundShape( bodyId, &shapeDef, compound ); + ENSURE( world->compoundShapeCount == 1 ); + b3DestroyShape( shapeId, true ); + ENSURE( world->compoundShapeCount == 0 ); + b3DestroyBody( bodyId ); + + // Shape carried away by its body + bodyId = b3CreateBody( worldId, &bodyDef ); + b3CreateBakedCompoundShape( bodyId, &shapeDef, compound ); + ENSURE( world->compoundShapeCount == 1 ); + b3DestroyBody( bodyId ); + ENSURE( world->compoundShapeCount == 0 ); + + b3DestroyWorld( worldId ); + b3DestroyCompound( compound ); + return 0; +} + // Identical hull data is shared through a reference counted world database. static int TestHullDatabase( void ) { @@ -1247,6 +1299,7 @@ int WorldTest( void ) RUN_SUBTEST( TestSetWorkerCount ); RUN_SUBTEST( TestHullDatabase ); RUN_SUBTEST( TestEnlargedProxyDestroyed ); + RUN_SUBTEST( TestCompoundShapeCount ); return 0; }