From 72168220647901833599f1a54b6825ddbec05982 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 12 May 2026 15:24:20 -0700 Subject: [PATCH] Bug fixes and minor features (#2) - API to query compounds directly. - Fix some uninitialized stuff (thanks MSAN). - Fix for store impulses on overflow. - AABB margin reduced for small shapes - body contact recycling option - angular threshold for contact recycling - reduce AABB margin on smaller shapes - fix for hull vs tri when axis aligned - fully locked rotation uses zero inverse inertia --- .github/workflows/build.yml | 24 ++++++- include/box3d/collision.h | 1 + include/box3d/constants.h | 16 ++++- include/box3d/types.h | 46 ++++++++++-- samples/sample.cpp | 2 - samples/sample_character.cpp | 1 + samples/sample_issues.cpp | 74 ++++++++++++++++++-- samples/sample_joint.cpp | 1 - samples/sample_mesh.cpp | 6 +- samples/sample_robustness.cpp | 110 +++++++++++++++++++++++++++++ samples/sample_stacking.cpp | 54 ++++++++++++++ shared/CMakeLists.txt | 2 + shared/overflow_color.c | 82 ++++++++++++++++++++++ shared/overflow_color.h | 26 +++++++ src/bitset.h | 9 --- src/block_allocator.h | 9 --- src/body.c | 128 ++++++++++++++++++++-------------- src/body.h | 23 +++--- src/broad_phase.h | 9 --- src/compound.h | 32 --------- src/constraint_graph.h | 9 --- src/contact.c | 6 ++ src/contact.h | 12 +--- src/contact_solver.c | 106 ++++++++++++++++++++-------- src/contact_solver.h | 9 --- src/core.h | 10 --- src/dynamic_tree.c | 2 +- src/height_field.c | 6 +- src/id_pool.h | 15 +--- src/island.h | 9 --- src/joint.h | 9 --- src/manifold.h | 11 +-- src/mesh.c | 37 ++++++++++ src/mesh_contact.c | 2 +- src/parallel_for.h | 9 --- src/physics_world.c | 8 ++- src/physics_world.h | 9 --- src/scheduler.h | 9 --- src/sensor.h | 10 --- src/shape.c | 58 ++++++++++++++- src/shape.h | 16 ++--- src/simd.h | 8 --- src/solver.c | 16 ++--- src/solver.h | 13 +--- src/solver_set.h | 9 --- src/table.h | 9 --- src/triangle_manifold.c | 9 ++- src/types.c | 2 +- test/test_determinism.c | 4 +- test/test_world.c | 40 +++++++++++ 50 files changed, 763 insertions(+), 364 deletions(-) create mode 100644 shared/overflow_color.c create mode 100644 shared/overflow_color.h diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 05a89a6..4544fc6 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -36,7 +36,7 @@ jobs: timeout-minutes: 4 steps: - uses: actions/checkout@v4 - + - name: Configure CMake run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-fsanitize=thread -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=thread" -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON @@ -47,6 +47,26 @@ jobs: working-directory: ${{github.workspace}}/build run: ./bin/test + build-ubuntu-clang-msan: + name: ubuntu-clang-msan + runs-on: ubuntu-latest + if: github.event_name == 'push' || github.event.pull_request.draft == false + timeout-minutes: 10 + steps: + - uses: actions/checkout@v4 + + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_CXX_COMPILER=clang++-18 -DCMAKE_C_COMPILER=clang-18 -DCMAKE_C_FLAGS="-fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_CXX_FLAGS="-fsanitize=memory -fsanitize-memory-track-origins=1 -fno-omit-frame-pointer" -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=memory" -DBOX3D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + env: + MSAN_OPTIONS: abort_on_error=1:halt_on_error=1 + run: ./bin/test + build-macos: name: macos runs-on: macos-latest @@ -71,7 +91,7 @@ jobs: if: github.event_name == 'push' || github.event.pull_request.draft == false timeout-minutes: 4 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Configure CMake run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX3D_SAMPLES=OFF -DBOX3D_VALIDATE=ON -DCMAKE_COMPILE_WARNING_AS_ERROR=ON diff --git a/include/box3d/collision.h b/include/box3d/collision.h index 6d09dcc..961f262 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -783,6 +783,7 @@ B3_API b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, fl float columnFrequency ); B3_API b3MeshData* b3CreateTorusMesh( int radialResolution, int tubularResolution, float radius, float thickness ); B3_API b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ); +B3_API b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ); B3_API b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ); B3_API void b3DestroyMesh( b3MeshData* mesh ); B3_API int b3GetHeight( const b3MeshData* mesh ); diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 847fff4..d96aa55 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -64,16 +64,26 @@ B3_API float b3GetLengthUnitsPerMeter( void ); /// The default contact recycling distance. #define B3_CONTACT_RECYCLE_DISTANCE ( 10.0f * B3_LINEAR_SLOP ) +/// The default contact recycling world angle threshold. For performance this value +/// is cos(angle/2)^2. This value corresponds to 10 degrees. +#define B3_CONTACT_RECYCLE_ANGULAR_DISTANCE ( 0.99240388f ) + // This is used to fatten AABBs in the dynamic tree. This allows proxies // to move by a small amount without triggering a tree adjustment. This is in meters. // @warning modifying this can have a significant impact on performance -#define B3_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() ) +#define B3_MAX_AABB_MARGIN ( 0.05f * b3GetLengthUnitsPerMeter() ) + +// Per-shape AABB margin is a fraction of the shape extent (capped by B3_MAX_AABB_MARGIN). +// Small shapes get small margins; large shapes are clamped to the cap. +#define B3_AABB_MARGIN_FRACTION 0.125f // The time that a body must be still before it will go to sleep. In seconds. #define B3_TIME_TO_SLEEP 0.5f -// Body and shape name lengths -#define B3_NAME_LENGTH 64 +/// Maximum length of the body name. Must be at least 1. Includes null termination. +#ifndef B3_NAME_LENGTH +#define B3_NAME_LENGTH 18 +#endif // These generous limits allow for easy hashing. See b3ShapePairKey. #define B3_SHAPE_POWER 22 diff --git a/include/box3d/types.h b/include/box3d/types.h index 3521c65..49d629f 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -243,7 +243,7 @@ typedef struct b3BodyDef /// Sleep speed threshold, default is 0.05 meters per second float sleepThreshold; - /// Optional body name for debugging. Up to 31 characters (excluding null termination) + /// Optional body name for debugging. Up to B3_NAME_LENGTH characters (including null termination) const char* name; /// Use this to store application specific body data. @@ -258,10 +258,20 @@ typedef struct b3BodyDef /// Is this body initially awake or sleeping? bool isAwake; - /// Treat this body as high speed object that performs continuous collision detection + /// Treat this body as a high speed object that performs continuous collision detection /// against dynamic and kinematic bodies, but not other bullet bodies. /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic - /// continuous collision. + /// continuous collision. They do not guarantee accurate collision if both bodies are fast moving because + /// the bullet does a continuous check after all non-bullet bodies have moved. You could get unlucky and have + /// the bullet body end a time step very close to a non-bullet body and the non-bullet body then moves over + /// the bullet body. In continuous collision, initial overlap is ignored to avoid freezing bodies in place. + /// I do not recommend using them for game projectiles if precise collision timing is needed. Instead consider + /// using a ray or shape cast. You can use a marching ray or shape cast for projectile that moves over time. + /// If you want a fast moving projectile to collide with a fast moving target, you need to consider the relative + /// movement in your ray or shape cast. This is out of the scope of Box3D. + /// So what are good use cases for bullets? Pinball games or games with dynamic containers that hold other objects. + /// It should be a use case where it doesn't break the game if there is a collision missed, but having them + /// captured improves the quality of the game. bool isBullet; /// Used to disable a body. A disabled body does not move or collide. @@ -271,10 +281,9 @@ typedef struct b3BodyDef /// for circular objects, like wheels. bool allowFastRotation; - /// Enable continuous collision. This is true by default and should only be disabled for application reasons, such - /// as wheels on a vehicle. - /// todo not hooked up yet - bool enableContinuousCollision; + /// Enable contact recycling. True by default. Leaving this enabled improves performance + /// but may lead to ghost collision that should be avoided on characters. + bool enableContactRecycling; /// Used internally to detect a valid definition. DO NOT SET. int internalValue; @@ -1266,6 +1275,29 @@ typedef struct b3BodyPlaneResult b3PlaneResult result; } b3BodyPlaneResult; +typedef struct b3ChildShape +{ + union + { + b3Capsule capsule; + const b3Hull* hull; + b3Mesh mesh; + b3Sphere sphere; + }; + + b3Transform transform; + + // Index 0 is used for convex shapes. + // todo limit to 64K? + int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; + b3ShapeType type; +} b3ChildShape; + +typedef bool b3CompoundQueryFcn( const b3Compound* compound, int childIndex, void* context ); + +B3_API b3ChildShape b3GetCompoundChild( const b3Compound* compound, int childIndex ); +B3_API void b3QueryCompound( const b3Compound* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ); + /// These colors are used for debug draw and mostly match the named SVG colors. /// See https://www.rapidtables.com/web/color/index.html /// https://johndecember.com/html/spec/colorsvg.html diff --git a/samples/sample.cpp b/samples/sample.cpp index 8abec41..d1fbea8 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -395,8 +395,6 @@ void Sample::Render() m_context->debugDraw.drawingBounds = bounds; b3World_Draw( m_worldId, &m_context->debugDraw, B3_DEFAULT_MASK_BITS ); - - DrawTextLine( "camera radius = %g", m_camera->m_radius ); } void Sample::ResetProfile() diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index d05b07f..8182a24 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -436,3 +436,4 @@ public: }; static int sampleMover = SampleManager::Register( "Character", "Mover", BasicMover::Create ); + diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 3324205..f88a42a 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -20,19 +20,19 @@ public: if ( context->restart == false ) { m_camera->SetView( 45.0f, 30.0f, 15.0f, { 0.0f, 2.0f, 0.0f } ); - //m_camera->SetView( 45.0f, 30.0f, 300.0f, { 3910.62109f, 9862.50293f, 875.395081f } ); + // m_camera->SetView( 45.0f, 30.0f, 300.0f, { 3910.62109f, 9862.50293f, 875.395081f } ); } b3SetLengthUnitsPerMeter( 1.0f ); const char* dumpPrefix = "data/dumps/single_box/"; - #include "dumps/single_box/box3d_dump.inl" +#include "dumps/single_box/box3d_dump.inl" } ~DumpLoader() override { - for (b3MeshData* md : m_meshes) + for ( b3MeshData* md : m_meshes ) { b3DestroyMesh( md ); } @@ -189,7 +189,6 @@ public: m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); } - m_hull = nullptr; #if 0 @@ -256,7 +255,7 @@ public: } else { - for (int i = 0; i < m_count; ++i) + for ( int i = 0; i < m_count; ++i ) { DrawPoint( m_scene, m_points[i], 5.0f, b3_colorWhite ); } @@ -399,3 +398,68 @@ public: }; static int sampleConvexJitter = SampleManager::Register( "Issues", "Convex Jitter", ConvexJitter::Create ); + +class SBoxMover : public Sample +{ +public: + explicit SBoxMover( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 12.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + + //m_boxMesh = b3CreateBoxMesh( { 0.0f, 1.0f, 0.0f }, { 1.0f, 1.0f, 1.0f }, true ); + m_boxMesh = b3CreatePlatformMesh( { 0.0f, 1.5f, 0.0f }, 1.0f, 2.0f, 5.0f ); + // m_scale = { -0.6f, 1.0f, 2.0f }; + b3Vec3 scale = b3Vec3_one; + b3CreateMeshShape( groundId, &shapeDef, m_boxMesh, scale ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 3.5f, 0.0f }; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + bodyDef.enableContactRecycling = false; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( 0.25f, 1.0f, 0.25f ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + + ~SBoxMover() override + { + b3DestroyMesh( m_boxMesh ); + } + + void Render() override + { + Sample::Render(); + b3Transform transform = { { 0.0f, 1.1f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 3.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new SBoxMover( context ); + } + + b3MeshData* m_boxMesh; +}; + +static int sampleBoxMesh = SampleManager::Register( "Issues", "s&box mover", SBoxMover::Create ); diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp index e0b87c6..df32cd8 100644 --- a/samples/sample_joint.cpp +++ b/samples/sample_joint.cpp @@ -2452,7 +2452,6 @@ public: bodyDef.type = b3_dynamicBody; bodyDef.allowFastRotation = true; - bodyDef.enableContinuousCollision = false; bodyDef.rotation = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisY, b3Vec3_axisZ ); // b3Hull* hull = b3CreateCylinder( 0.25f, 0.4f, 0.0f, 16 ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 6568a24..a7168a7 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -75,6 +75,9 @@ public: bodyDef.type = b3_dynamicBody; //bodyDef.position = { 0.5f, 2.0f, -0.5f }; bodyDef.position = { 0.1f, 1.0f, -0.1f }; + //bodyDef.enableContactRecycling = m_shapeType != ShapeType::cylinder; + bodyDef.angularDamping = m_shapeType == ShapeType::cylinder ? 0.1f : 0.0f; + // bodyDef.linearVelocity = { 2.0f, 0.0f, 0.0f }; // bodyDef.position = { 0.208701894, 0.0791450217, -0.298710823 }; // bodyDef.rotation = { { 0.0808477178, -0.00305216131, 0.000387475739 }, 0.996721804 }; @@ -112,8 +115,7 @@ public: case ShapeType::cylinder: { - shapeDef.baseMaterial.rollingResistance = 0.01f; - //shapeDef.baseMaterial.friction = 0.1f; + shapeDef.baseMaterial.rollingResistance = 0.02f; b3CreateHullShape( m_bodyId, &shapeDef, m_cylinderHull ); } break; diff --git a/samples/sample_robustness.cpp b/samples/sample_robustness.cpp index f32befb..0fa8887 100644 --- a/samples/sample_robustness.cpp +++ b/samples/sample_robustness.cpp @@ -2,10 +2,12 @@ // SPDX-License-Identifier: MIT #include "camera.h" +#include "overflow_color.h" #include "sample.h" #include "scene.h" #include "box3d/box3d.h" +#include "box3d/constants.h" #include #include @@ -73,6 +75,72 @@ public: static int sampleHighMassRatio1 = SampleManager::Register( "Robustness", "HighMassRatio1", HighMassRatio1::Create ); +// A pyramid of 5cm boxes. Stacking tiny objects is challenging for physics engines due to rotational effects. +// This is also challenging for Box3D because of the AABB margin and linear slop are close to the shape size. This +// leads to many collision pairs and some shape overlap. +class TinyPyramid : public Sample +{ +public: + explicit TinyPyramid( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 20.0f, 2.5f, { 0.0f, 0.75f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeTransformedBoxHull( 40.0f, 1.0f, 40.0f, { { 0.0f, -1.0f, 0.0f }, b3Quat_identity } ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + { + m_extent = 0.025f; + int baseCount = 30; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull box = b3MakeBoxHull( m_extent, m_extent, m_extent ); + + for ( int i = 0; i < baseCount; ++i ) + { + float y = ( 2.0f * i + 1.0f ) * m_extent; + + for ( int j = i; j < baseCount; ++j ) + { + float x = ( i + 1.0f ) * m_extent + 2.0f * ( j - i ) * m_extent - baseCount * m_extent; + bodyDef.position = { x, y, 0.0f }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + } + } + } + } + + void Step() override + { + DrawTextLine( "%.1fcm boxes", 200.0f * m_extent ); + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new TinyPyramid( context ); + } + + float m_extent; +}; + +static int sampleTinyPyramid = SampleManager::Register( "Robustness", "Tiny Pyramid", TinyPyramid::Create ); + class OverlapRecovery : public Sample { public: @@ -402,3 +470,45 @@ public: }; static int sampleCart = SampleManager::Register( "Robustness", "Cart", Cart::Create ); + +// Drives the b3*_Overflow solver path. A heavy hub touches more dynamic +// neighbors than B3_DYNAMIC_COLOR_COUNT (= 20), so several contacts land in +// the overflow color. The HUD reports the per-step overflow contact count +// because the scene is visually unremarkable when working correctly — the +// point is that it stays unremarkable. +class OverflowColorPile : public Sample +{ +public: + explicit OverflowColorPile( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 5.0f, 3.0f, 6.0f, b3Vec3_zero ); + EnableGrid( m_scene, true ); + } + + m_data = CreateOverflowColorPile( m_worldId ); + } + + void Step() override + { + Sample::Step(); + + b3Counters counters = b3World_GetCounters( m_worldId ); + int overflowContacts = counters.colorCounts[B3_GRAPH_COLOR_COUNT - 1]; + + DrawTextLine( "neighbors = %d", m_data.neighborCount ); + DrawTextLine( "overflow contacts = %d", overflowContacts ); + DrawTextLine( "total contacts = %d", counters.contactCount ); + } + + static Sample* Create( SampleContext* context ) + { + return new OverflowColorPile( context ); + } + + OverflowColorPileData m_data; +}; + +static int sampleOverflowColorPile = SampleManager::Register( "Robustness", "Overflow Color Pile", OverflowColorPile::Create ); diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp index c58de83..0c52dbc 100644 --- a/samples/sample_stacking.cpp +++ b/samples/sample_stacking.cpp @@ -898,6 +898,60 @@ public: static int sampleArch = SampleManager::Register( "Stacking", "Arch", Arch::Create ); +class DoubleDomino : public Sample +{ +public: + explicit DoubleDomino( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 20.0f, { 0.0f, 4.0f, 0.0f } ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull groundBox = b3MakeBoxHull( 50.0f, 1.0f, 50.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &groundBox.base ); + } + + b3BoxHull box = b3MakeBoxHull( 0.125f, 0.5f, 0.25f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.density = 4.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + + int count = 15; + float x = -0.5f * count; + for ( int i = 0; i < count; ++i ) + { + bodyDef.position = { x, 0.5f, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + if ( i == 0 ) + { + b3Body_ApplyLinearImpulse( bodyId, { 0.2f, 0.0f, 0.0f }, { x, 1.0f, 0.0f }, true ); + } + + x += 1.01f; + } + } + + static Sample* Create( SampleContext* context ) + { + return new DoubleDomino( context ); + } +}; + +static int sampleDoubleDomino = SampleManager::Register( "Stacking", "Double Domino", DoubleDomino::Create ); + class Pyramid2D : public Sample { public: diff --git a/shared/CMakeLists.txt b/shared/CMakeLists.txt index aba87f1..f2cc0f2 100644 --- a/shared/CMakeLists.txt +++ b/shared/CMakeLists.txt @@ -7,6 +7,8 @@ set(BOX3D_SHARED_FILES determinism.h human.c human.h + overflow_color.c + overflow_color.h stability.c stability.h utils.c diff --git a/shared/overflow_color.c b/shared/overflow_color.c new file mode 100644 index 0000000..8e118d0 --- /dev/null +++ b/shared/overflow_color.c @@ -0,0 +1,82 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "overflow_color.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include + +#define OVERFLOW_PILE_RING_COUNT 5 +#define OVERFLOW_PILE_PER_RING 5 + +OverflowColorPileData CreateOverflowColorPile( b3WorldId worldId ) +{ + OverflowColorPileData data = { 0 }; + data.neighborCount = OVERFLOW_PILE_RING_COUNT * OVERFLOW_PILE_PER_RING; + + // Static ground (top surface at y = 0) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + // Tall, heavy hub. Tall so neighbors can ring around it in multiple + // vertical layers; heavy so it stays roughly still under uneven inward + // pressure from the ring. + float hubHalfX = 0.5f; + float hubHalfY = 2.5f; + float hubHalfZ = 0.5f; + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Vec3){ 0.0f, hubHalfY, 0.0f }; + data.hubId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( hubHalfX, hubHalfY, hubHalfZ ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 50.0f; + b3CreateHullShape( data.hubId, &shapeDef, &box.base ); + } + + // Neighbors: vertical rings around the hub, each box slightly overlapping + // the hub so a contact exists on the very first step. + float neighborHalf = 0.2f; + float ringRadius = hubHalfX + neighborHalf - 0.03f; + + b3BoxHull neighborBox = b3MakeBoxHull( neighborHalf, neighborHalf, neighborHalf ); + b3ShapeDef neighborShape = b3DefaultShapeDef(); + + float ringSpacing = 0.5f; + float baseY = neighborHalf + 0.05f; + + for ( int ring = 0; ring < OVERFLOW_PILE_RING_COUNT; ++ring ) + { + float y = baseY + ringSpacing * ring; + + // Offset alternate rings by half a slot so neighbors don't sit + // directly above each other. + float thetaOffset = ( ring & 1 ) ? ( B3_PI / OVERFLOW_PILE_PER_RING ) : 0.0f; + + for ( int slot = 0; slot < OVERFLOW_PILE_PER_RING; ++slot ) + { + float theta = thetaOffset + ( 2.0f * B3_PI * slot ) / OVERFLOW_PILE_PER_RING; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Vec3){ ringRadius * cosf( theta ), y, ringRadius * sinf( theta ) }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3CreateHullShape( bodyId, &neighborShape, &neighborBox.base ); + } + } + + return data; +} diff --git a/shared/overflow_color.h b/shared/overflow_color.h new file mode 100644 index 0000000..fc7dd57 --- /dev/null +++ b/shared/overflow_color.h @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "box3d/id.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +// One heavy dynamic hub surrounded by enough dynamic neighbors that the hub's +// degree in the dyn-dyn contact graph exceeds B3_DYNAMIC_COLOR_COUNT (= 20). +// The excess contacts land in the overflow color (B3_GRAPH_COLOR_COUNT - 1), +// exercising the b3*_Overflow solver path that no other scene reaches. +typedef struct OverflowColorPileData +{ + b3BodyId hubId; + int neighborCount; +} OverflowColorPileData; + +OverflowColorPileData CreateOverflowColorPile( b3WorldId worldId ); + +#ifdef __cplusplus +} +#endif diff --git a/src/bitset.h b/src/bitset.h index 8eb9bef..ff771d9 100644 --- a/src/bitset.h +++ b/src/bitset.h @@ -17,11 +17,6 @@ typedef struct b3BitSet } b3BitSet; -#ifdef __cplusplus -extern "C" -{ -#endif - b3BitSet b3CreateBitSet( uint32_t bitCapacity ); void b3DestroyBitSet( b3BitSet* bitSet ); void b3SetBitCountAndClear( b3BitSet* bitSet, uint32_t bitCount ); @@ -29,10 +24,6 @@ void b3InPlaceUnion( b3BitSet* setA, const b3BitSet* setB ); void b3GrowBitSet( b3BitSet* bitSet, uint32_t blockCount ); int b3CountSetBits( b3BitSet* bitSet ); -#ifdef __cplusplus -} -#endif - static inline void b3SetBit( b3BitSet* bitSet, uint32_t bitIndex ) { uint32_t blockIndex = bitIndex / 64; diff --git a/src/block_allocator.h b/src/block_allocator.h index 77ee538..6748074 100644 --- a/src/block_allocator.h +++ b/src/block_allocator.h @@ -26,11 +26,6 @@ typedef struct b3BlockAllocator int allocationCount; } b3BlockAllocator; -#ifdef __cplusplus -extern "C" -{ -#endif - // Element must be large enough to hold a pointer b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount ); void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); @@ -38,7 +33,3 @@ void b3DestroyBlockAllocator( b3BlockAllocator* allocator ); // Elements are guaranteed contiguous. void* b3AllocateElement( b3BlockAllocator* allocator ); void b3FreeElement( b3BlockAllocator* allocator, void* element ); - -#ifdef __cplusplus -} -#endif diff --git a/src/body.c b/src/body.c index d3e4e37..b71d4bd 100644 --- a/src/body.c +++ b/src/body.c @@ -226,6 +226,8 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) bodySim->flags |= def->isBullet ? b3_isBullet : 0; bodySim->flags |= def->allowFastRotation ? b3_allowFastRotation : 0; bodySim->flags |= def->type == b3_dynamicBody ? b3_dynamicFlag : 0; + bodySim->flags |= def->enableSleep ? b3_enableSleep : 0; + bodySim->flags |= def->enableContactRecycling ? b3_bodyEnableContactRecycling : 0; if ( setId == b3_awakeSet ) { @@ -292,7 +294,6 @@ b3BodyId b3CreateBody( b3WorldId worldId, const b3BodyDef* def ) body->inertia = b3Mat3_zero; body->type = def->type; body->flags = bodySim->flags; - body->enableSleep = def->enableSleep; // dynamic and kinematic bodies that are enabled need a island if ( setId >= b3_awakeSet ) @@ -872,6 +873,14 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body ) shapeId = s->nextShapeId; } + + // Apply fixed rotation + if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation ) + { + body->inertia = b3Mat3_zero; + bodySim->invInertiaLocal = b3Mat3_zero; + bodySim->invInertiaWorld = b3Mat3_zero; + } } void b3DumpBody( b3World* world, b3Body* body ) @@ -906,7 +915,7 @@ void b3DumpBody( b3World* world, b3Body* body ) b3Dump( " bd.angularVelocity = {%.9g, %.9g, %.9g};\n", w.x, w.y, w.z ); b3Dump( " bd.linearDamping = %.9g;\n", sim->linearDamping ); b3Dump( " bd.angularDamping = %.9g;\n", sim->angularDamping ); - b3Dump( " bd.enableSleep = bool(%d);\n", body->enableSleep ); + b3Dump( " bd.enableSleep = bool(%d);\n", body->flags & b3_enableSleep ); b3Dump( " bd.isAwake = bool(%d);\n", body->setIndex == b3_awakeSet ); b3Dump( " bd.gravityScale = %.9g;\n", sim->gravityScale ); b3Dump( " b3BodyId bodyId = b3CreateBody(m_worldId, &bd);\n" ); @@ -1005,7 +1014,6 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation ) b3BroadPhase* broadPhase = &world->broadPhase; b3Transform transform = bodySim->transform; - const float margin = B3_AABB_MARGIN; const float speculativeDistance = B3_SPECULATIVE_DISTANCE; int shapeId = body->headShapeId; @@ -1023,6 +1031,7 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Vec3 position, b3Quat rotation ) if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) { + float margin = shape->aabbMargin; b3AABB fatAABB; fatAABB.lowerBound.x = aabb.lowerBound.x - margin; fatAABB.lowerBound.y = aabb.lowerBound.y - margin; @@ -1894,7 +1903,7 @@ bool b3Body_IsSleepEnabled( b3BodyId bodyId ) { b3World* world = b3GetWorld( bodyId.world0 ); b3Body* body = b3GetBodyFullId( world, bodyId ); - return body->enableSleep; + return ( body->flags & b3_enableSleep ) == b3_enableSleep; } void b3Body_SetSleepThreshold( b3BodyId bodyId, float sleepThreshold ) @@ -1922,7 +1931,14 @@ void b3Body_EnableSleep( b3BodyId bodyId, bool enableSleep ) world->locked = true; b3Body* body = b3GetBodyFullId( world, bodyId ); - body->enableSleep = enableSleep; + + bool flag = ( body->flags & b3_enableSleep ) == b3_enableSleep; + if ( enableSleep == flag ) + { + return; + } + + body->flags = enableSleep ? body->flags | b3_enableSleep : body->flags & ~b3_enableSleep; if ( enableSleep == false ) { @@ -2107,60 +2123,70 @@ void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ) return; } - uint32_t newFlags = 0; - newFlags |= locks.linearX ? b3_lockLinearX : 0; - newFlags |= locks.linearY ? b3_lockLinearY : 0; - newFlags |= locks.linearZ ? b3_lockLinearZ : 0; - newFlags |= locks.angularX ? b3_lockAngularX : 0; - newFlags |= locks.angularY ? b3_lockAngularY : 0; - newFlags |= locks.angularZ ? b3_lockAngularZ : 0; + uint32_t newLocks = 0; + newLocks |= locks.linearX ? b3_lockLinearX : 0; + newLocks |= locks.linearY ? b3_lockLinearY : 0; + newLocks |= locks.linearZ ? b3_lockLinearZ : 0; + newLocks |= locks.angularX ? b3_lockAngularX : 0; + newLocks |= locks.angularY ? b3_lockAngularY : 0; + newLocks |= locks.angularZ ? b3_lockAngularZ : 0; b3Body* body = b3GetBodyFullId( world, bodyId ); - if ( ( body->flags & b3_allLocks ) != newFlags ) + if ( ( body->flags & b3_allLocks ) == newLocks ) { - body->flags &= ~b3_allLocks; - body->flags |= newFlags; + return; + } - b3BodySim* bodySim = b3GetBodySim( world, body ); - bodySim->flags &= ~b3_allLocks; - bodySim->flags |= newFlags; + bool fixedRotation1 = ( body->flags & b3_fixedRotation ) == b3_fixedRotation; + bool fixedRotation2 = ( newLocks & b3_fixedRotation ) == b3_fixedRotation; - b3BodyState* state = b3GetBodyState( world, body ); + body->flags &= ~b3_allLocks; + body->flags |= newLocks; - if ( state != NULL ) + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->flags &= ~b3_allLocks; + bodySim->flags |= newLocks; + + b3BodyState* state = b3GetBodyState( world, body ); + + if ( state != NULL ) + { + state->flags = body->flags; + + if ( locks.linearX ) { - state->flags = body->flags; - - if ( locks.linearX ) - { - state->linearVelocity.x = 0.0f; - } - - if ( locks.linearY ) - { - state->linearVelocity.y = 0.0f; - } - - if ( locks.linearZ ) - { - state->linearVelocity.z = 0.0f; - } - - if ( locks.angularX ) - { - state->angularVelocity.x = 0.0f; - } - - if ( locks.angularY ) - { - state->angularVelocity.y = 0.0f; - } - - if ( locks.angularZ ) - { - state->angularVelocity.z = 0.0f; - } + state->linearVelocity.x = 0.0f; } + + if ( locks.linearY ) + { + state->linearVelocity.y = 0.0f; + } + + if ( locks.linearZ ) + { + state->linearVelocity.z = 0.0f; + } + + if ( locks.angularX ) + { + state->angularVelocity.x = 0.0f; + } + + if ( locks.angularY ) + { + state->angularVelocity.y = 0.0f; + } + + if ( locks.angularZ ) + { + state->angularVelocity.z = 0.0f; + } + } + + if ( fixedRotation1 != fixedRotation2 ) + { + b3UpdateBodyMassData( world, body ); } } diff --git a/src/body.h b/src/body.h index e094895..cdd6d0a 100644 --- a/src/body.h +++ b/src/body.h @@ -53,15 +53,20 @@ enum b3BodyFlags // Used for b3BodyState flags. b3_dynamicFlag = 0x00001000, + b3_enableSleep = 0x00002000, + + b3_bodyEnableContactRecycling = 0x00004000, + // All lock flags b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, + + // If all these flags are set then the body has fixed rotation + b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, }; // Body organizational details that are not used in the solver. typedef struct b3Body { - char name[B3_NAME_LENGTH]; - void* userData; // index of solver set stored in b3World @@ -115,8 +120,7 @@ typedef struct b3Body // Used to check for invalid b3BodyId uint16_t generation; - // todo move into flags - bool enableSleep; + char name[B3_NAME_LENGTH]; } b3Body; // Body State @@ -210,11 +214,6 @@ typedef struct b3BodySim uint32_t flags; } b3BodySim; -#ifdef __cplusplus -extern "C" -{ -#endif - // Get a validated body from a world using an id. b3Body* b3GetBodyFullId( b3World* world, b3BodyId bodyId ); @@ -237,11 +236,7 @@ bool b3WakeBodyWithLock( b3World* world, b3Body* body ); void b3UpdateBodyMassData( b3World* world, b3Body* body ); void b3DumpBody( b3World* world, b3Body* body ); -#ifdef __cplusplus -} -#endif - -B3_INLINE b3Sweep b3MakeSweep( const b3BodySim* bodySim ) +static inline b3Sweep b3MakeSweep( const b3BodySim* bodySim ) { b3Sweep s; s.c1 = bodySim->center0; diff --git a/src/broad_phase.h b/src/broad_phase.h index 660d91f..dcd14ad 100644 --- a/src/broad_phase.h +++ b/src/broad_phase.h @@ -48,11 +48,6 @@ typedef struct b3BroadPhase b3HashSet pairSet; } b3BroadPhase; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3CreateBroadPhase( b3BroadPhase* bp, const b3Capacity* capacity ); void b3DestroyBroadPhase( b3BroadPhase* bp ); @@ -71,10 +66,6 @@ bool b3BroadPhase_TestOverlap( const b3BroadPhase* bp, int proxyKeyA, int proxyK void b3ValidateBroadphase( const b3BroadPhase* bp ); void b3ValidateNoEnlarged( const b3BroadPhase* bp ); -#ifdef __cplusplus -} -#endif - // 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 ) diff --git a/src/compound.h b/src/compound.h index 237556c..510402d 100644 --- a/src/compound.h +++ b/src/compound.h @@ -5,34 +5,6 @@ #include "box3d/types.h" -typedef struct b3ChildShape -{ - union - { - b3Capsule capsule; - const b3Hull* hull; - b3Mesh mesh; - b3Sphere sphere; - }; - - b3Transform transform; - - // Index 0 is used for convex shapes. - // todo limit to 64K? - int materialIndices[B3_MAX_COMPOUND_MESH_MATERIALS]; - b3ShapeType type; -} b3ChildShape; - -typedef bool b3CompoundQueryFcn( const b3Compound* compound, int childIndex, void* context ); - -#ifdef __cplusplus -extern "C" -{ -#endif - -b3ChildShape b3GetCompoundChild( const b3Compound* compound, int childIndex ); -void b3QueryCompound( const b3Compound* compound, b3AABB aabb, b3CompoundQueryFcn* fcn, void* context ); - b3TOIOutput b3CompoundTimeOfImpact( const b3Compound* compound, b3Transform transform, const b3ShapeProxy* proxy, const b3Sweep* sweep, float maxFraction ); @@ -40,7 +12,3 @@ b3TOIOutput b3CompoundTimeOfImpact( const b3Compound* compound, b3Transform tran b3Sweep b3MakeCompoundChildSweep( b3Transform compoundTransform, b3Transform childTransform ); int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3Compound* shape, const b3Capsule* mover ); - -#ifdef __cplusplus -} -#endif diff --git a/src/constraint_graph.h b/src/constraint_graph.h index 1776cc0..0cefba1 100644 --- a/src/constraint_graph.h +++ b/src/constraint_graph.h @@ -66,11 +66,6 @@ typedef struct b3ConstraintGraph b3GraphColor colors[B3_GRAPH_COLOR_COUNT]; } b3ConstraintGraph; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ); void b3DestroyGraph( b3ConstraintGraph* graph ); @@ -80,7 +75,3 @@ void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int col b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ); void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ); void b3RemoveJointFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); - -#ifdef __cplusplus -} -#endif diff --git a/src/contact.c b/src/contact.c index 50b52dc..ad82671 100644 --- a/src/contact.c +++ b/src/contact.c @@ -211,6 +211,12 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil contact->shapeIdB = shapeIdB; contact->childIndex = childIndex; + // Both bodies must enable recycling + if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 ) + { + contact->flags |= b3_contactRecycleFlag; + } + if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) { contact->flags |= b3_simMeshContact; diff --git a/src/contact.h b/src/contact.h index 1b8cbed..1125ab7 100644 --- a/src/contact.h +++ b/src/contact.h @@ -42,6 +42,8 @@ enum b3ContactFlags // This is contact is between a dynamic and static body b3_contactStaticFlag = 0x00000008, + b3_contactRecycleFlag = 0x00000010, + // Set when the shapes are touching b3_simTouchingFlag = 0x00010000, @@ -63,6 +65,7 @@ enum b3ContactFlags // This is a mesh contact b3_simMeshContact = 0x00400000, + b3_relativeTransformValid = 0x00800000, }; @@ -164,11 +167,6 @@ typedef struct b3ContactSpec b3DeclareArray( b3ContactSpec ); -#ifdef __cplusplus -extern "C" -{ -#endif - void b3InitializeContactRegisters( void ); void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ); @@ -179,7 +177,3 @@ bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Sha bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, b3Transform xfA, const b3Shape* shapeB, b3Transform xfB, bool isFast, b3Arena arena ); - -#ifdef __cplusplus -} -#endif diff --git a/src/contact_solver.c b/src/contact_solver.c index 218dbb8..501f858 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -43,6 +43,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3ManifoldConstraint* manifoldBase = context->manifoldConstraints; b3ContactConstraint* base = context->contactConstraints; + // Overflow constraints are stored separately if ( block.blockType == b3_overflowBlock ) { b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; @@ -77,6 +78,7 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); int contactId = specs[localIndex].contactId; b3Contact* contact = b3Array_Get( world->contacts, contactId ); + B3_ASSERT( contact->contactId == contactId ); int indexA = contact->bodySimIndexA; int indexB = contact->bodySimIndexB; @@ -677,53 +679,96 @@ void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) { b3World* world = context->world; + + // Mirror b3PrepareContacts_Mesh: the per-color flat arrays and the overflow color + // each have their own (base, spans, manifoldBase). + b3ContactPrepareSpan* spans = context->contactPrepareSpans; b3ContactConstraint* base = context->contactConstraints; + + if ( block.blockType == b3_overflowBlock ) + { + b3GraphColor* overflow = world->constraintGraph.colors + B3_OVERFLOW_INDEX; + spans = context->overflowSpans; + base = overflow->contactConstraints; + } + b3TaskContext* taskContext = world->taskContexts.data + workerIndex; b3BitSet* hitEventBitSet = &taskContext->hitEventBitSet; bool hasHitEvents = taskContext->hasHitEvents; float negHitThreshold = -world->hitEventThreshold; - for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + int index = block.startIndex; + int endIndex = block.startIndex + block.count; + + // Find color for start index. Linear search but fast. + int colorIndex = 0; + while ( spans[colorIndex + 1].start <= index ) { - b3ContactConstraint* contactConstraint = base + i; + colorIndex += 1; + } - // Having this contact pointer simplifies impulse storage - b3Contact* contact = contactConstraint->contact; + // Loop over block + while ( index < endIndex ) + { + int colorStart = spans[colorIndex].start; + int colorEndIndex = b3MinInt( spans[colorIndex + 1].start, endIndex ); - int manifoldCount = contactConstraint->manifoldCount; - B3_ASSERT( manifoldCount == contact->manifoldCount ); - - bool checkHitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; - bool flagged = false; - - for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) + // Loop over color + for ( ; index < colorEndIndex; ++index ) { - b3Manifold* manifold = contact->manifolds + manifoldIndex; - b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; - manifold->twistImpulse = constraint->twistImpulse; - manifold->frictionImpulse = b3Blend2( constraint->frictionImpulse.x, constraint->tangent1, - constraint->frictionImpulse.y, constraint->tangent2 ); - manifold->rollingImpulse = constraint->rollingImpulse; + b3ContactConstraint* contactConstraint = base + index; - int count = constraint->pointCount; - B3_ASSERT( count == manifold->pointCount ); - for ( int pointIndex = 0; pointIndex < count; ++pointIndex ) + int localIndex = index - colorStart; + B3_UNUSED( localIndex ); + B3_ASSERT( 0 <= localIndex && localIndex < spans[colorIndex].count ); + + // Having this contact pointer simplifies impulse storage + b3Contact* contact = contactConstraint->contact; + B3_ASSERT( contact != NULL ); + + // Catches the wrong-(base, spans) pairing: the contact pointer stashed by + // b3PrepareContacts_Mesh at this flat slot must reference the same contact + // the span at this slot describes. + B3_VALIDATE( contact->contactId == spans[colorIndex].contacts[localIndex].contactId ); + + int manifoldCount = contactConstraint->manifoldCount; + B3_ASSERT( manifoldCount == contact->manifoldCount ); + + bool checkHitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool flagged = false; + + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) { - b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; - b3ManifoldPoint* mp = manifold->points + pointIndex; - mp->normalImpulse = cp->normalImpulse; - mp->totalNormalImpulse = cp->totalNormalImpulse; - mp->normalVelocity = cp->relativeVelocity; + b3Manifold* manifold = contact->manifolds + manifoldIndex; + b3ManifoldConstraint* constraint = contactConstraint->constraints + manifoldIndex; + manifold->twistImpulse = constraint->twistImpulse; + manifold->frictionImpulse = b3Blend2( constraint->frictionImpulse.x, constraint->tangent1, + constraint->frictionImpulse.y, constraint->tangent2 ); + manifold->rollingImpulse = constraint->rollingImpulse; - if ( checkHitEvents && flagged == false && - mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + int count = constraint->pointCount; + B3_ASSERT( count == manifold->pointCount ); + for ( int pointIndex = 0; pointIndex < count; ++pointIndex ) { - b3SetBit( hitEventBitSet, contact->contactId ); - hasHitEvents = true; - flagged = true; + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->normalImpulse = cp->normalImpulse; + mp->totalNormalImpulse = cp->totalNormalImpulse; + mp->normalVelocity = cp->relativeVelocity; + + if ( checkHitEvents && flagged == false && + mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) + { + b3SetBit( hitEventBitSet, contact->contactId ); + hasHitEvents = true; + flagged = true; + } } } } + + // Advance to next color + colorIndex += 1; } taskContext->hasHitEvents = hasHitEvents; @@ -2423,6 +2468,5 @@ void b3StoreImpulses_Overflow( b3StepContext* context ) .colorIndex = B3_OVERFLOW_INDEX, }; - // Overflow runs serially on the orchestrator (worker 0) b3StoreImpulses_Mesh( block, context, 0 ); } diff --git a/src/contact_solver.h b/src/contact_solver.h index 67c767f..fab17ee 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -51,11 +51,6 @@ typedef struct b3ContactConstraint int manifoldCount; } b3ContactConstraint; -#ifdef __cplusplus -extern "C" -{ -#endif - int b3GetWideContactConstraintByteCount( void ); // Overflow contacts don't fit into the constraint graph coloring @@ -76,7 +71,3 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ); void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); - -#ifdef __cplusplus -} -#endif diff --git a/src/core.h b/src/core.h index a4d8599..3d407f6 100644 --- a/src/core.h +++ b/src/core.h @@ -124,12 +124,6 @@ typedef struct b3AtomicU32 #define B3_ALLOC( T, N ) (T*)b3Alloc( N * sizeof( T ) ); #define B3_FREE( M, T, N ) b3Free( M, N * sizeof( T ) ); - -#ifdef __cplusplus -extern "C" -{ -#endif - void* b3Alloc( size_t size ); void* b3AllocZeroed( size_t size ); void b3Free( void* mem, size_t size ); @@ -160,7 +154,3 @@ void b3OpenDump( const char* fileName ); void b3Dump( const char* string, ... ); void b3CloseDump( void ); int b3FetchAddMeshDumpIndex( void ); - -#ifdef __cplusplus -} -#endif diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 3f34b67..eb963ae 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -2038,7 +2038,7 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild ) // 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_aabbMargin + // this should be weighed against B3_MAX_AABB_MARGIN while ( true ) { if ( b3IsLeaf( node ) == true || ( ( node->flags & b3_enlargedNode ) == 0 && fullBuild == false ) ) diff --git a/src/height_field.c b/src/height_field.c index 93d2462..cb79d46 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -100,7 +100,9 @@ b3HeightField* b3CreateHeightField( const b3HeightFieldDef* data ) { - b3HeightField* hf = (b3HeightField*)b3Alloc( sizeof( b3HeightField ) ); + // Zero-init so the trailing pad after `clockwise` is defined — the construction-time + // hash below sweeps raw struct bytes and would otherwise pick up uninitialized padding. + b3HeightField* hf = (b3HeightField*)b3AllocZeroed( sizeof( b3HeightField ) ); int columnCount = data->countX; int rowCount = data->countZ; @@ -564,7 +566,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightField* heightField, const b3S b3CastOutput result = { 0 }; b3Vec3 shapeExtents = b3AABB_Extents( shapeBounds ); - b3Vec3 margin = { B3_AABB_MARGIN, B3_AABB_MARGIN, B3_AABB_MARGIN }; + b3Vec3 margin = { B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN, B3_MAX_AABB_MARGIN }; b3AABB combinedBounds = { b3Sub( b3Sub( heightField->aabb.lowerBound, shapeExtents ), margin ), b3Add( b3Add( heightField->aabb.upperBound, shapeExtents ), margin ) }; diff --git a/src/id_pool.h b/src/id_pool.h index 7659a38..999c7f6 100644 --- a/src/id_pool.h +++ b/src/id_pool.h @@ -11,11 +11,6 @@ typedef struct b3IdPool int nextIndex; } b3IdPool; -#ifdef __cplusplus -extern "C" -{ -#endif - b3IdPool b3CreateIdPool( void ); void b3DestroyIdPool( b3IdPool* pool ); @@ -23,21 +18,17 @@ int b3AllocId( b3IdPool* pool ); void b3FreeId( b3IdPool* pool, int id ); void b3ValidateFreeId( const b3IdPool* pool, int id ); -#ifdef __cplusplus -} -#endif - -B3_INLINE int b3GetIdCount( const b3IdPool* pool ) +static inline int b3GetIdCount( const b3IdPool* pool ) { return pool->nextIndex - pool->freeArray.count; } -B3_INLINE int b3GetIdCapacity( const b3IdPool* pool ) +static inline int b3GetIdCapacity( const b3IdPool* pool ) { return pool->nextIndex; } -B3_INLINE int b3GetIdBytes( const b3IdPool* pool ) +static inline int b3GetIdBytes( const b3IdPool* pool ) { return b3Array_ByteCount( pool->freeArray ); } diff --git a/src/island.h b/src/island.h index d37bb09..3b2d064 100644 --- a/src/island.h +++ b/src/island.h @@ -80,11 +80,6 @@ typedef struct b3IslandSim int islandId; } b3IslandSim; -#ifdef __cplusplus -extern "C" -{ -#endif - b3Island* b3CreateIsland( b3World* world, int setIndex ); void b3DestroyIsland( b3World* world, int islandId ); @@ -104,7 +99,3 @@ void b3SplitIsland( b3World* world, int baseId ); void b3SplitIslandTask( void* context ); void b3ValidateIsland( b3World* world, int islandId ); - -#ifdef __cplusplus -} -#endif diff --git a/src/joint.h b/src/joint.h index 49af3f4..cfbc21e 100644 --- a/src/joint.h +++ b/src/joint.h @@ -345,11 +345,6 @@ typedef struct b3JointSim }; } b3JointSim; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ); b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ); @@ -418,7 +413,3 @@ void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform trans void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3Transform transformA, b3Transform transformB, float scale ); - -#ifdef __cplusplus -} -#endif diff --git a/src/manifold.h b/src/manifold.h index 1ff93ed..0cb84b4 100644 --- a/src/manifold.h +++ b/src/manifold.h @@ -35,11 +35,6 @@ typedef enum b3FeatureOwner b3_featureShapeB = 1 } b3FeatureOwner; -#ifdef __cplusplus -extern "C" -{ -#endif - float b3EdgeEdgeSeparation( b3Vec3 p1, b3Vec3 e1, b3Vec3 c1, b3Vec3 p2, b3Vec3 e2, b3Vec3 c2 ); int b3FindIncidentFace( const b3Hull* hull, b3Vec3 refNormal, int vertexIndex ); b3FeaturePair b3MakeFeaturePair( b3FeatureOwner owner1, int index1, b3FeatureOwner owner2, int index2 ); @@ -52,14 +47,10 @@ int b3ClipPolygon( b3ClipVertex* out, b3ClipVertex* polygon, int count, b3Plane bool b3ValidatePolygon( b3ClipVertex* polygon, int count ); #endif -#ifdef __cplusplus -} -#endif - // For single point contact, such as sphere-sphere, sphere-capsule, sphere-triangle static const b3FeaturePair b3FeaturePair_single = { 0 }; -B3_INLINE uint32_t b3MakeFeatureId( b3FeaturePair pair ) +static inline uint32_t b3MakeFeatureId( b3FeaturePair pair ) { return ( (uint32_t)pair.owner1 << 24 ) | ( (uint32_t)pair.index1 << 16 ) | ( (uint32_t)pair.owner2 << 8 ) | (uint32_t)pair.index2; diff --git a/src/mesh.c b/src/mesh.c index e9507a1..1384398 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -928,6 +928,8 @@ static int b3BuildRecursive( b3ArrayC( b3MeshNode ) * nodes, int count, b3Primit node->data.asNode.childOffset = rightIndex - index; node->lowerBound = aabb.lowerBound; node->upperBound = aabb.upperBound; + // triangleOffset is leaf-only, but lives outside the union — zero it so mesh->hash is deterministic + node->triangleOffset = 0; return index; } @@ -1480,6 +1482,41 @@ b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) return b3CreateMesh( &def, NULL, 0 ); } +b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ) +{ + float hb = 0.5f * bottomWidth; + float ht = 0.5f * topWidth; + float hy = 0.5f * height; + b3Vec3 vertices[] = { + { ht, hy, ht }, { -ht, hy, ht }, { -hb, -hy, hb }, { hb, -hy, hb }, + { ht, hy, -ht }, { -ht, hy, -ht }, { -hb, -hy, -hb }, { hb, -hy, -hb }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 0, 1, 3, 1, 2, 3, // front + 0, 4, 1, 1, 4, 5, // top + 0, 3, 7, 4, 0, 7, // right + 4, 7, 5, 6, 5, 7, // back + 1, 5, 2, 6, 2, 5, // left + 3, 2, 7, 6, 7, 2, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = true; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + // todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ) { diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 122cf2f..543e402 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -96,7 +96,7 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transfo } // Enlarge to the query bounds to absorb small movement - float radius = B3_AABB_MARGIN + B3_SPECULATIVE_DISTANCE; + float radius = B3_MAX_AABB_MARGIN + B3_SPECULATIVE_DISTANCE; b3Vec3 extension = { radius, radius, radius }; meshContact->queryBounds.lowerBound = b3Sub( bounds->lowerBound, extension ); meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); diff --git a/src/parallel_for.h b/src/parallel_for.h index 237621a..a668fd0 100644 --- a/src/parallel_for.h +++ b/src/parallel_for.h @@ -13,11 +13,6 @@ typedef struct b3World b3World; // an index into per-worker state (e.g. world->taskContexts.data + workerIndex). typedef void b3ParallelForCallback( int startIndex, int endIndex, int workerIndex, void* context ); -#ifdef __cplusplus -extern "C" -{ -#endif - // Divide [0, itemCount) into blocks and process them with cooperative claiming: // up to world->workerCount tasks are enqueued, and each task loops, atomically // claiming the next unclaimed block until the range is drained. Blocks the @@ -26,7 +21,3 @@ extern "C" // stays bounded. void b3ParallelFor( b3World* world, b3ParallelForCallback* callback, int itemCount, int minRange, void* context, const char* name ); - -#ifdef __cplusplus -} -#endif diff --git a/src/physics_world.c b/src/physics_world.c index af13b0c..aac4757 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -581,8 +581,12 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. // However, this allows larger relative motion and has fewer tuning parameters (just one). if ( ( isFast == false || isMeshContact == false ) && recycleDistance > 0.0f && - contact->flags & b3_relativeTransformValid ) + (contact->flags & b3_relativeTransformValid) && (contact->flags & b3_contactRecycleFlag) ) { + float angleA = b3DotQuat( transformA.q, contact->cachedTransformA.q ); + float angleB = b3DotQuat( transformB.q, contact->cachedTransformB.q ); + float angularDistance = b3MinFloat( angleA * angleA, angleB * angleB ); + b3Transform xf = b3InvMulTransforms( transformA, transformB ); b3Transform xfc = b3InvMulTransforms( contact->cachedTransformA, contact->cachedTransformB ); b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent; @@ -594,7 +598,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* // 2*|qr.v| == 2*|sin(theta/2)| ~= theta for small angles. float distSquared = b3DistanceSquared( xf.p, xfc.p ); - if ( distSquared < recycleTolerance * recycleTolerance ) + if ( angularDistance > B3_CONTACT_RECYCLE_ANGULAR_DISTANCE && distSquared < recycleTolerance * recycleTolerance ) { float distance = sqrtf( distSquared ); float slack = recycleTolerance - distance; diff --git a/src/physics_world.h b/src/physics_world.h index c964d2f..c9a929a 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -273,11 +273,6 @@ typedef struct b3World bool inUse; } b3World; -#ifdef __cplusplus -extern "C" -{ -#endif - b3World* b3GetUnlockedWorldFromId( b3WorldId id ); b3World* b3GetWorldFromId( b3WorldId id ); @@ -288,10 +283,6 @@ void b3ValidateConnectivity( b3World* world ); void b3ValidateSolverSets( b3World* world ); void b3ValidateContacts( b3World* world ); -#ifdef __cplusplus -} -#endif - static inline b3Manifold* b3AllocateManifolds( b3World* world, int count ) { if ( count == 0 ) diff --git a/src/scheduler.h b/src/scheduler.h index 7a08e6e..f9aeff7 100644 --- a/src/scheduler.h +++ b/src/scheduler.h @@ -6,11 +6,6 @@ typedef void b3TaskCallback( void* taskContext ); typedef struct b3Scheduler b3Scheduler; -#ifdef __cplusplus -extern "C" -{ -#endif - b3Scheduler* b3CreateScheduler( int workerCount ); void b3DestroyScheduler( b3Scheduler* scheduler ); void b3ResetScheduler( b3Scheduler* scheduler ); @@ -18,7 +13,3 @@ void b3ResetScheduler( b3Scheduler* scheduler ); // See b3EnqueueTaskCallback and b3FinishTaskCallback void* b3SchedulerEnqueueTask( b3TaskCallback* task, void* taskContext, void* userContext, const char* name ); void b3SchedulerFinishTask( void* userTask, void* userContext ); - -#ifdef __cplusplus -} -#endif diff --git a/src/sensor.h b/src/sensor.h index 038d076..bb936fe 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -37,15 +37,5 @@ typedef struct b3SensorTaskContext b3BitSet eventBits; } b3SensorTaskContext; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3OverlapSensors( b3World* world ); - void b3DestroySensor( b3World* world, b3Shape* sensorShape ); - -#ifdef __cplusplus -} -#endif diff --git a/src/shape.c b/src/shape.c index ba9b923..018287a 100644 --- a/src/shape.c +++ b/src/shape.c @@ -26,11 +26,62 @@ static b3Shape* b3GetShape( b3World* world, b3ShapeId shapeId ) return shape; } +static float b3ComputeShapeMargin( b3Shape* shape ) +{ + float margin = 0.0f; + + switch ( shape->type ) + { + case b3_sphereShape: + { + margin = shape->sphere.radius; + } + break; + + case b3_capsuleShape: + { + margin = 0.5f * b3Distance( shape->capsule.center2, shape->capsule.center1 ) + shape->capsule.radius; + } + break; + + case b3_hullShape: + { + const b3Hull* hull = shape->hull; + const b3Vec3* points = b3GetHullPoints( hull ); + float maxExtentSqr = 0.0f; + int count = hull->vertexCount; + for ( int i = 0; i < count; ++i ) + { + float distSqr = b3DistanceSquared( points[i], hull->center ); + maxExtentSqr = b3MaxFloat( maxExtentSqr, distSqr ); + } + margin = sqrtf( maxExtentSqr ); + } + break; + + case b3_meshShape: + case b3_heightShape: + case b3_compoundShape: + { + // Static-only shapes: broadphase uses speculative distance for static + // proxies, so the per-shape margin is never consumed in practice. + // Return the cap so any incidental use is generous. + return B3_MAX_AABB_MARGIN; + } + + default: + B3_VALIDATE( false ); + return B3_MAX_AABB_MARGIN; + } + + return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin ); +} + static void b3UpdateShapeAABBs( b3Shape* shape, b3Transform transform, b3BodyType proxyType ) { // Compute a bounding box with a speculative margin const float speculativeDistance = B3_SPECULATIVE_DISTANCE; - const float aabbMargin = B3_AABB_MARGIN; + const float aabbMargin = shape->aabbMargin; b3AABB aabb = b3ComputeShapeAABB( shape, transform ); aabb.lowerBound.x -= speculativeDistance; @@ -139,6 +190,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3Transform shape->enablePreSolveEvents = def->enablePreSolveEvents; shape->proxyKey = B3_NULL_INDEX; shape->localCentroid = b3GetShapeCentroid( shape ); + shape->aabbMargin = b3ComputeShapeMargin( shape ); shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; shape->generation += 1; @@ -1425,6 +1477,7 @@ void b3Shape_SetSphere( b3ShapeId shapeId, const b3Sphere* sphere ) shape->sphere = *sphere; shape->type = b3_sphereShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); // need to wake bodies so they can react to the shape change bool wakeBodies = true; @@ -1450,6 +1503,7 @@ void b3Shape_SetCapsule( b3ShapeId shapeId, const b3Capsule* capsule ) shape->capsule = *capsule; shape->type = b3_capsuleShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); // need to wake bodies so they can react to the shape change bool wakeBodies = true; @@ -1475,6 +1529,7 @@ void b3Shape_SetHull( b3ShapeId shapeId, const b3Hull* hull ) shape->hull = b3CloneHull( hull ); shape->type = b3_hullShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); // need to wake bodies so they can react to the shape change bool wakeBodies = true; @@ -1504,6 +1559,7 @@ void b3Shape_SetMesh( b3ShapeId shapeId, const b3MeshData* meshData, b3Vec3 scal shape->mesh.data = meshData; shape->mesh.scale = b3SafeScale( scale ); shape->type = b3_meshShape; + shape->aabbMargin = b3ComputeShapeMargin( shape ); // need to wake bodies so they can react to the shape change bool wakeBodies = true; diff --git a/src/shape.h b/src/shape.h index 57b6301..28cfd28 100644 --- a/src/shape.h +++ b/src/shape.h @@ -23,6 +23,7 @@ typedef struct b3Shape b3ShapeType type; float density; float explosionScale; + float aabbMargin; b3AABB aabb; b3AABB fatAABB; @@ -56,11 +57,6 @@ typedef struct b3Shape } b3Shape; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3Transform transform, bool forcePairCreation ); void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ); @@ -111,7 +107,7 @@ float b3ComputeHullProjectedArea( const b3Hull* hull, b3Vec3 direction ); b3Triangle b3GetHeightFieldTriangle( const b3HeightField* heightField, int triangleIndex ); int b3GetHeightFieldMaterial( const b3HeightField* heightField, int triangleIndex ); -B3_INLINE int b3GetHeightFieldTriangleCount( const b3HeightField* heightField ) +static inline int b3GetHeightFieldTriangleCount( const b3HeightField* heightField ) { int cellCount = ( heightField->rowCount - 1 ) * ( heightField->columnCount - 1 ); return 2 * cellCount; @@ -122,11 +118,7 @@ b3Triangle b3GetMeshTriangle( const b3Mesh* mesh, int triangleIndex ); bool b3IsValidMesh( const b3MeshData* meshData ); void b3DumpShape( b3World* world, int shapeIndex ); -#ifdef __cplusplus -} -#endif - -B3_INLINE bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) +static inline bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) { if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 ) { @@ -136,7 +128,7 @@ B3_INLINE bool b3ShouldShapesCollide( b3Filter filterA, b3Filter filterB ) return ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0; } -B3_INLINE bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter ) +static inline bool b3ShouldQueryCollide( const b3Filter* shapeFilter, const b3QueryFilter* queryFilter ) { return ( shapeFilter->categoryBits & queryFilter->maskBits ) != 0 && ( shapeFilter->maskBits & queryFilter->categoryBits ) != 0; diff --git a/src/simd.h b/src/simd.h index ec9399c..ee9d1d4 100644 --- a/src/simd.h +++ b/src/simd.h @@ -362,13 +362,5 @@ static inline bool b3TestBoundsRayOverlap( b3V32 nodeMin, b3V32 nodeMax, b3V32 r return b3AllLessEq3V( edgeSeparation, b3_zeroV ); } -#ifdef __cplusplus -extern "C" { -#endif - bool b3TestBoundsTriangleOverlap( b3V32 nodeCenter, b3V32 nodeExtent, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); float b3IntersectRayTriangle( b3V32 rayStart, b3V32 rayDelta, b3V32 vertex1, b3V32 vertex2, b3V32 vertex3 ); - -#ifdef __cplusplus -} -#endif diff --git a/src/solver.c b/src/solver.c index 2edddf3..85c2848 100644 --- a/src/solver.c +++ b/src/solver.c @@ -530,10 +530,6 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* const float speculativeScalar = B3_SPECULATIVE_DISTANCE; const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar }; - // todo implement per shape margin as in Box2D - const float marginScalar = B3_AABB_MARGIN; - const b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; - if ( context.fraction < 1.0f ) { // Handle time of impact event @@ -564,6 +560,8 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* 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->enlargedAABB = true; @@ -591,6 +589,8 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false ) { + float marginScalar = shape->aabbMargin; + b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; shape->fatAABB = (b3AABB){ .lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ), .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), @@ -652,10 +652,6 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, const float speculativeScalar = B3_SPECULATIVE_DISTANCE; const b3Vec3 speculativeMargin = { speculativeScalar, speculativeScalar, speculativeScalar }; - // todo per shape margin - const float marginScalar = B3_AABB_MARGIN; - const b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; - for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) { b3BodyState* state = states + simIndex; @@ -715,7 +711,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, sim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); state->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); - if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) + if ( enableSleep == false || (body->flags & b3_enableSleep) == 0 || sleepVelocity > body->sleepThreshold ) { // Body is not sleepy body->sleepTime = 0.0f; @@ -805,6 +801,8 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, 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->enlargedAABB = true; diff --git a/src/solver.h b/src/solver.h index db4c324..c0e2c74 100644 --- a/src/solver.h +++ b/src/solver.h @@ -259,18 +259,9 @@ typedef struct b3StepContext char padding3[64]; } b3StepContext; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3Solve( b3World* world, b3StepContext* stepContext ); -#ifdef __cplusplus -} -#endif - -B3_INLINE b3Softness b3MakeSoft( float hertz, float zeta, float h ) +static inline b3Softness b3MakeSoft( float hertz, float zeta, float h ) { if ( hertz == 0.0f ) { @@ -307,7 +298,7 @@ B3_INLINE b3Softness b3MakeSoft( float hertz, float zeta, float h ) // In all cases: // massScale + impulseScale == 1 - return B3_LITERAL( b3Softness ){ + return ( b3Softness ){ .biasRate = omega / a1, .massScale = a2 * a3, .impulseScale = a3, diff --git a/src/solver_set.h b/src/solver_set.h index b916fcc..33c58ca 100644 --- a/src/solver_set.h +++ b/src/solver_set.h @@ -54,11 +54,6 @@ typedef struct b3SolverSet int setIndex; } b3SolverSet; -#ifdef __cplusplus -extern "C" -{ -#endif - void b3DestroySolverSet( b3World* world, int setIndex ); void b3WakeSolverSet( b3World* world, int setIndex ); @@ -70,7 +65,3 @@ void b3MergeSolverSets( b3World* world, int setIndex1, int setIndex2 ); void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ); void b3TransferJoint( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Joint* joint ); - -#ifdef __cplusplus -} -#endif diff --git a/src/table.h b/src/table.h index 2e54772..ec8f397 100644 --- a/src/table.h +++ b/src/table.h @@ -37,11 +37,6 @@ static inline uint64_t b3ShapePairKey( int s1, int s2, int c ) ( (uint64_t)( B3_CHILD_MASK & c ) ); } -#ifdef __cplusplus -extern "C" -{ -#endif - b3HashSet b3CreateSet( int32_t capacity ); void b3DestroySet( b3HashSet* set ); @@ -56,7 +51,3 @@ bool b3RemoveKey( b3HashSet* set, uint64_t key ); bool b3ContainsKey( const b3HashSet* set, uint64_t key ); int b3GetHashSetBytes( b3HashSet* set ); - -#ifdef __cplusplus -} -#endif diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index 38726c9..ec39d9e 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -1202,12 +1202,11 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 // Does an edge axis exist? if ( edgeQuery.indexA != B3_NULL_INDEX ) { - // Idea is to ignore the jenga problem when separated and just use the best axis - // bool separated = clippedFaceSeparation > 0.0f && edgeQuery.separation > 0.0f; - // float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); + // When axes are aligned the edge separation can be garbage. + // If a face axis has positive separation there may be no points. + float maxFaceSeparation = b3MaxFloat( faceQueryA.separation, faceQueryB.separation ); - // Do edge contact if face contact yield no points or only one point with significantly deeper overlap. - if ( manifold->pointCount == 0 || + if ( ( manifold->pointCount == 0 && edgeQuery.separation > maxFaceSeparation ) || ( manifold->pointCount == 1 && edgeQuery.separation > clippedFaceSeparation + linearSlop ) ) { B3_ASSERT( 0 <= edgeQuery.indexA && edgeQuery.indexA < 3 ); diff --git a/src/types.c b/src/types.c index 2a89696..bccae84 100644 --- a/src/types.c +++ b/src/types.c @@ -39,7 +39,7 @@ b3BodyDef b3DefaultBodyDef( void ) def.enableSleep = true; def.isAwake = true; def.isEnabled = true; - def.enableContinuousCollision = true; + def.enableContactRecycling = true; def.internalValue = B3_SECRET_COOKIE; return def; } diff --git a/test/test_determinism.c b/test/test_determinism.c index 1d97f8f..547082e 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -14,8 +14,8 @@ #define TracyCFrameMark #endif -#define EXPECTED_SLEEP_STEP 306 -#define EXPECTED_HASH 0xCBF958A1 +#define EXPECTED_SLEEP_STEP 300 +#define EXPECTED_HASH 0x2A58F9D0 static int SingleMultithreadingTest( int workerCount ) { diff --git a/test/test_world.c b/test/test_world.c index b82ef3d..9a8ea2f 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -2,6 +2,7 @@ // SPDX-License-Identifier: MIT #include "benchmarks.h" +#include "overflow_color.h" #include "stability.h" #include "test_macros.h" @@ -701,6 +702,44 @@ static int TestMeshDrop( void ) return 0; } +// Verifies the b3*_Overflow solver path. The scene puts >B3_DYNAMIC_COLOR_COUNT +// dyn-dyn contacts on a single hub body so several land in the overflow color. +// The new Prepare/Store contactId-pairing asserts in contact_solver.c fire here +// if Store reads constraints from the wrong (base, spans) pair, so the test +// catches the bug even though world state ends up plausible (memory layout is +// stable within a build, so determinism alone is not a witness). +static int TestOverflowColorPile( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = 1; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + OverflowColorPileData data = CreateOverflowColorPile( worldId ); + (void)data; + + float timeStep = 1.0f / 60.0f; + int subStepCount = 4; + + // One step would be enough to trip the asserts, but several steps also + // exercise the warm-start path (Store -> manifold impulse -> Prepare). + int stepCount = 10; + for ( int i = 0; i < stepCount; ++i ) + { + b3World_Step( worldId, timeStep, subStepCount ); + } + + // Confirm the scene actually populated the overflow color. Without this, + // a future change to graph coloring could silently turn the test into a + // no-op. + b3Counters counters = b3World_GetCounters( worldId ); + int overflowContacts = counters.colorCounts[B3_GRAPH_COLOR_COUNT - 1]; + + b3DestroyWorld( worldId ); + + ENSURE( overflowContacts > 0 ); + return 0; +} + int WorldTest( void ) { RUN_SUBTEST( HelloWorld ); @@ -714,6 +753,7 @@ int WorldTest( void ) RUN_SUBTEST( TestHitEvents ); RUN_SUBTEST( TestCompoundHitEvents ); RUN_SUBTEST( TestMeshDrop ); + RUN_SUBTEST( TestOverflowColorPile ); // RUN_SUBTEST( TestSetWorkerCount ); return 0;