diff --git a/benchmark/amd7950x_sse2/compound_capsules.csv b/benchmark/amd7950x_sse2/compound_capsules.csv deleted file mode 100644 index e0179bc..0000000 --- a/benchmark/amd7950x_sse2/compound_capsules.csv +++ /dev/null @@ -1,9 +0,0 @@ -threads,ms -1,730.575 -2,424.323 -3,324.045 -4,276.202 -5,242.944 -6,226.599 -7,211.694 -8,205.856 diff --git a/benchmark/amd7950x_sse2/compound_hulls.csv b/benchmark/amd7950x_sse2/compound_hulls.csv deleted file mode 100644 index 2854909..0000000 --- a/benchmark/amd7950x_sse2/compound_hulls.csv +++ /dev/null @@ -1,9 +0,0 @@ -threads,ms -1,462.769 -2,292.812 -3,230.578 -4,197.733 -5,178.241 -6,166.567 -7,161.298 -8,157.091 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv index 898d654..01362c8 100644 --- a/benchmark/amd7950x_sse2/joint_grid.csv +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,1796.4 -2,954.375 -3,641.176 -4,481.819 -5,390.785 -6,328.059 -7,287.637 -8,258.124 +1,1405.95 +2,771.262 +3,525.161 +4,396.555 +5,319.261 +6,273.429 +7,236.346 +8,212.377 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv index 50a6450..096c056 100644 --- a/benchmark/amd7950x_sse2/junkyard.csv +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -1,9 +1,9 @@ threads,ms -1,24717.7 -2,13513.9 -3,9716.35 -4,7493.39 -5,6140.22 -6,5221.34 -7,4550.24 -8,4106.65 +1,24484.3 +2,13666.7 +3,9769.51 +4,7573.72 +5,6165.49 +6,5234.91 +7,4569.35 +8,4096.21 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv index 852f52b..19b651d 100644 --- a/benchmark/amd7950x_sse2/large_pyramid.csv +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,1682.1 -2,878.51 -3,613.252 -4,478.075 -5,387.426 -6,337.026 -7,294.737 -8,270.737 +1,1776.98 +2,887.705 +3,611.107 +4,477.959 +5,393.24 +6,338.688 +7,301.258 +8,272.38 diff --git a/benchmark/amd7950x_sse2/large_world.csv b/benchmark/amd7950x_sse2/large_world.csv new file mode 100644 index 0000000..cc2e0e8 --- /dev/null +++ b/benchmark/amd7950x_sse2/large_world.csv @@ -0,0 +1,9 @@ +threads,ms +1,9.6721 +2,12.2776 +3,13.8174 +4,15.0718 +5,14.2408 +6,14.8061 +7,15.4252 +8,15.7804 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv index cdbba91..a340236 100644 --- a/benchmark/amd7950x_sse2/many_pyramids.csv +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,2575.34 -2,1278.03 -3,878.515 -4,629.084 -5,507.933 -6,413.473 -7,362.456 -8,320.241 +1,2540.44 +2,1220.31 +3,813.901 +4,609.715 +5,497.305 +6,413.889 +7,361.679 +8,319.254 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv index ec9581b..8684c76 100644 --- a/benchmark/amd7950x_sse2/rain.csv +++ b/benchmark/amd7950x_sse2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,2627.76 -2,1427.39 -3,1012.48 -4,796.119 -5,664.231 -6,577.576 -7,518.164 -8,483.436 +1,2314.36 +2,1306.31 +3,932.724 +4,735.735 +5,610.991 +6,529.984 +7,478.004 +8,442.816 diff --git a/benchmark/amd7950x_sse2/trees100.csv b/benchmark/amd7950x_sse2/trees100.csv new file mode 100644 index 0000000..29fac99 --- /dev/null +++ b/benchmark/amd7950x_sse2/trees100.csv @@ -0,0 +1,9 @@ +threads,ms +1,252.889 +2,159.521 +3,130.152 +4,114.981 +5,110.742 +6,98.8688 +7,99.0887 +8,99.0417 diff --git a/benchmark/amd7950x_sse2/trees25.csv b/benchmark/amd7950x_sse2/trees25.csv new file mode 100644 index 0000000..83313ef --- /dev/null +++ b/benchmark/amd7950x_sse2/trees25.csv @@ -0,0 +1,9 @@ +threads,ms +1,816.219 +2,486.479 +3,371.869 +4,315.809 +5,282.72 +6,262.329 +7,252.624 +8,244.159 diff --git a/benchmark/amd7950x_sse2/trees50.csv b/benchmark/amd7950x_sse2/trees50.csv new file mode 100644 index 0000000..7b2874c --- /dev/null +++ b/benchmark/amd7950x_sse2/trees50.csv @@ -0,0 +1,9 @@ +threads,ms +1,334.239 +2,226.053 +3,182.466 +4,159.489 +5,149.676 +6,140.113 +7,132.326 +8,132.251 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv index e1caa96..997ec05 100644 --- a/benchmark/amd7950x_sse2/washer.csv +++ b/benchmark/amd7950x_sse2/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,31480.4 -2,17010.7 -3,11890.1 -4,9295.49 -5,7696.31 -6,6645.53 -7,5843.28 -8,5249.02 +1,30675.7 +2,16370.1 +3,11431 +4,8895.99 +5,7391.02 +6,6300.97 +7,5618.21 +8,5073.84 diff --git a/benchmark/amd7950x_sse2/welded_hulls.csv b/benchmark/amd7950x_sse2/welded_hulls.csv deleted file mode 100644 index 3b28fcd..0000000 --- a/benchmark/amd7950x_sse2/welded_hulls.csv +++ /dev/null @@ -1,9 +0,0 @@ -threads,ms -1,726.988 -2,430.503 -3,324.672 -4,268.082 -5,232.24 -6,213.849 -7,199.971 -8,193.328 diff --git a/benchmark/main.c b/benchmark/main.c index 81ee8c4..68658fc 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -86,7 +86,7 @@ int main( int argc, char** argv ) { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, { "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, - { "static_floor", GetStaticFloorCapacity, CreateStaticFloor, NULL, StepStaticFloor, 500 }, + { "large_world", GetLargeWorldCapacity, CreateLargeWorld, NULL, StepLargeWorld, 500 }, //{ "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 961f262..96596dd 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* b3CreateHollowBoxMesh( b3Vec3 center, b3Vec3 extent ); 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 ); @@ -1472,7 +1473,7 @@ B3_API void b3CollideCapsuleAndTriangle( b3LocalManifold* manifold, int capacity const b3Vec3* triangleB, b3SimplexCache* cache ); // b3MeshEdgeFlags -B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Vec3* triangleB, +B3_API void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, b3SATCache* cache ); B3_API void b3CollideSphereAndTriangle( b3LocalManifold* manifold, int capacity, const b3Sphere* sphereA, const b3Vec3* triangleB ); @@ -1534,10 +1535,7 @@ typedef struct b3Point2D { b3Vec2 p; float separation; - int16_t originalIndex; - bool persisted; + int originalIndex; } b3Point2D; -B3_API int b3Hull2D( b3Point2D* pts, int count, b3Point2D* hull ); -B3_API int b3SimplifyHull2D( b3Point2D* hull, int count, int target ); -B3_API int b3CullPoints( b3Point2D* points, int count, int target ); +B3_API int b3CullPoints( b3Point2D* points, int count ); diff --git a/include/box3d/types.h b/include/box3d/types.h index 49d629f..fad01d6 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -498,6 +498,11 @@ typedef struct b3Counters // Number of contacts recycled in the most recent step. int recycledContactCount; + + // Maximum number of time of impact iterations + int distanceIterations; + int pushBackIterations; + int rootIterations; } b3Counters; //! @endcond diff --git a/samples/sample.cpp b/samples/sample.cpp index d1fbea8..5f6e739 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -400,6 +400,10 @@ void Sample::Render() void Sample::ResetProfile() { m_stepCount = 0; + memset( m_profiles, 0, sizeof( m_profiles ) ); + m_currentProfileIndex = 0; + m_profileReadIndex = 0; + m_profileWriteIndex = 0; } void Sample::UpdateUI() @@ -717,6 +721,7 @@ void Sample::UpdateUI() ImGui::Text( "islands/tasks = %d/%d", s.islandCount, s.taskCount ); ImGui::Text( "tree height static/movable = %d/%d", s.staticTreeHeight, s.treeHeight ); ImGui::Text( "sat call/hit = %d/%d", s.satCallCount, s.satCacheHitCount ); + ImGui::Text( "toi d/p/r = %d/%d/%d", s.distanceIterations, s.pushBackIterations, s.rootIterations ); ImGui::Text( "stack allocator size = %d K", s.stackUsed / 1024 ); ImGui::Text( "arena capacity = %d K", s.arenaCapacity / 1024 ); ImGui::Text( "total allocation = %d K", s.byteCount / 1024 ); diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 6e19044..8a4f401 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -702,43 +702,6 @@ public: static int sampleHeightFieldBenchmark = SampleManager::Register( "Benchmark", "Height Field", BenchmarkHeightField::Create ); -class BenchmarkCompoundCapsules : public Sample -{ -public: - explicit BenchmarkCompoundCapsules( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - float distance = 220.0f; - float height = 20.0f; - m_camera->SetView( 25.0f, 10.0f, distance, { 0.0f, height, 0.0f } ); - } - - CreateCompoundCapsules( m_worldId ); - } - - ~BenchmarkCompoundCapsules() override - { - DestroyCompoundCapsules(); - } - - void Render() override - { - Sample::Render(); - b3Transform transform = { { 0.0f, 0.1f, 0.0f }, b3Quat_identity }; - DrawTransform( m_scene, transform, 4.0f ); - } - - static Sample* Create( SampleContext* context ) - { - return new BenchmarkCompoundCapsules( context ); - } -}; - -static int sampleCompoundCapsules = - SampleManager::Register( "Benchmark", "Compounds Capsules", BenchmarkCompoundCapsules::Create ); - class BenchmarkFallingTrees : public Sample { public: @@ -820,36 +783,7 @@ public: int m_gridSize; }; -static int sampleCompoundHulls = SampleManager::Register( "Benchmark", "Falling Trees", BenchmarkFallingTrees::Create ); - -class WeldedHulls : public Sample -{ -public: - explicit WeldedHulls( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - float distance = 220.0f; - float height = 20.0f; - m_camera->SetView( 25.0f, 10.0f, distance, { 0.0f, height, 0.0f } ); - } - - CreateWeldedHulls( m_worldId ); - } - - ~WeldedHulls() override - { - DestroyWeldedHulls(); - } - - static Sample* Create( SampleContext* context ) - { - return new WeldedHulls( context ); - } -}; - -static int sampleWeldedHulls = SampleManager::Register( "Benchmark", "Welded Hulls", WeldedHulls::Create ); +static int sampleFallingTrees = SampleManager::Register( "Benchmark", "Falling Trees", BenchmarkFallingTrees::Create ); struct ShapeUserData { @@ -1095,10 +1029,10 @@ public: static int benchmarkWasher = SampleManager::Register( "Benchmark", "Washer", BenchmarkWasher::Create ); -class BenchmarkStaticFloor : public Sample +class BenchmarkLargeWorld : public Sample { public: - explicit BenchmarkStaticFloor( SampleContext* context ) + explicit BenchmarkLargeWorld( SampleContext* context ) : Sample( context ) { if ( context->restart == false ) @@ -1108,25 +1042,25 @@ public: } b3Capacity capacity = {}; - GetStaticFloorCapacity( &capacity ); + GetLargeWorldCapacity( &capacity ); CreateWorld( &capacity ); - CreateStaticFloor( m_worldId ); + CreateLargeWorld( m_worldId ); } void Step() override { - StepStaticFloor( m_worldId, m_stepCount ); + StepLargeWorld( m_worldId, m_stepCount ); Sample::Step(); } static Sample* Create( SampleContext* context ) { - return new BenchmarkStaticFloor( context ); + return new BenchmarkLargeWorld( context ); } }; -static int sampleStaticFloor = SampleManager::Register( "Benchmark", "Static Floor", BenchmarkStaticFloor::Create ); +static int sampleLargeWorld = SampleManager::Register( "Benchmark", "Large World", BenchmarkLargeWorld::Create ); class BenchmarkHull : public Sample { diff --git a/samples/sample_bodies.cpp b/samples/sample_bodies.cpp index 40d7ada..d58095d 100644 --- a/samples/sample_bodies.cpp +++ b/samples/sample_bodies.cpp @@ -914,7 +914,7 @@ public: bodyDef.name = "angular xz"; bodyDef.position = { 2.0f, 2.0f, 0.0f }; bodyDef.motionLocks.angularX = true; - //bodyDef.motionLocks.angularY = true; + // bodyDef.motionLocks.angularY = true; bodyDef.motionLocks.angularZ = true; b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); b3CreateHullShape( bodyId, &shapeDef, &cube.base ); @@ -963,3 +963,61 @@ public: }; static int sampleLockMixing = SampleManager::Register( "Bodies", "Lock Mixing", LockMixing::Create ); + +// A fully rotation locked body uses a zero inverse inertia tensor +class FixedRotation : public Sample +{ +public: + explicit FixedRotation( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 10.0f, b3Vec3_zero ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 15.0f, 1.0f, 15.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + b3Capsule capsule = { { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.3f }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.5f, 0.0f }; + + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.3f, 0.5f, 0.0f }; + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.enableSleep = false; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + + capsule.radius = 0.2f; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new FixedRotation( context ); + } +}; + +static int sampleFixedRotation = SampleManager::Register( "Bodies", "Fixed Rotation", FixedRotation::Create ); diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index 8182a24..b47c868 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -437,3 +437,1101 @@ public: static int sampleMover = SampleManager::Register( "Character", "Mover", BasicMover::Create ); +struct ClosestShapeCastContext +{ + b3ShapeId ignoreShapes[16]; + int ignoreCount; + float closestFraction; + b3Vec3 closestNormal; + b3Vec3 closestPoint; + b3ShapeId closestShape; + bool hit; + bool startedSolid; +}; + +static float ClosestShapeCastCallback( b3ShapeId shapeId, b3Vec3 point, b3Vec3 normal, float fraction, uint64_t userMaterialId, + int triangleIndex, int childIndex, void* context ) +{ + auto* ctx = static_cast( context ); + + for ( int i = 0; i < ctx->ignoreCount; ++i ) + { + if ( B3_ID_EQUALS( shapeId, ctx->ignoreShapes[i] ) ) + { + return -1.0f; + } + } + + if ( fraction == 0.0f ) + { + ctx->startedSolid = true; + return -1.0f; + } + + if ( fraction < ctx->closestFraction ) + { + ctx->closestFraction = fraction; + ctx->closestNormal = normal; + ctx->closestPoint = point; + ctx->closestShape = shapeId; + ctx->hit = true; + } + + return ctx->closestFraction; +} + +// --- Trace result matching s&box's SceneTraceResult --- + +struct TraceResult +{ + b3Vec3 endPosition; + b3Vec3 normal; + b3Vec3 hitPoint; + float fraction; + bool hit; + bool startedSolid; +}; + +// --- RigidbodyCharacter --- + +struct RigidbodyCharacter +{ + // s&box unit conversion: 1 unit = 1 inch = 0.0254m (40 units per meter) + static constexpr float SRC = 0.0254f; + + // Tuning — s&box defaults (converted to meters) + static constexpr float m_walkSpeed = 230.0f * SRC; + static constexpr float m_runSpeed = 350.0f * SRC; + static constexpr float m_jumpSpeed = 300.0f * SRC; + static constexpr float m_maxSlopeAngle = 45.0f; + static constexpr float m_characterGravity = 15.0f; + static constexpr float m_characterMass = 500.0f; + static constexpr float m_jumpCooldownTime = 0.2f; + + // s&box PlayerController parameters + static constexpr float m_stepUpHeight = 18.0f * SRC; + static constexpr float m_stepDownHeight = 18.0f * SRC; + static constexpr float m_skin = 0.095f * SRC; + static constexpr float m_brakePower = 0.2f; + static constexpr float m_surfaceFriction = 0.6f; + static constexpr float m_airFriction = 0.1f; + + // Character dimensions (radius=16, height=72 in s&box units) + static constexpr float m_bodyRadius = 16.0f * SRC; + static constexpr float m_totalHeight = 72.0f * SRC; + static constexpr float m_feetHeight = m_totalHeight * 0.5f; + + // Physics handles + b3BodyId m_bodyId; + b3ShapeId m_bodyCapsuleId; + b3ShapeId m_feetBoxId; + + // All shapes on this body (for ignore list in traces) + b3ShapeId m_ownShapes[4]; + int m_ownShapeCount; + + // State + b3Vec3 m_groundNormal; + b3Vec3 m_groundVelocity; + float m_jumpCooldown; + bool m_onGround; + bool m_sprint; + + // Step-up state: position to restore after physics step + bool m_didStep; + b3Vec3 m_stepPosition; + + // Debug readouts + b3Vec3 m_lastWishVelocity; + b3Vec3 m_massCenterWorld; + + Sample* m_sample; + + void Initialize( Sample* sample, b3Vec3 position ) + { + m_sample = sample; + m_onGround = false; + m_sprint = false; + m_jumpCooldown = 0.0f; + m_groundNormal = b3Vec3_axisY; + m_groundVelocity = b3Vec3_zero; + m_lastWishVelocity = b3Vec3_zero; + m_massCenterWorld = position; + m_didStep = false; + m_stepPosition = b3Vec3_zero; + + // Create dynamic body with all rotation locked + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = position; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + bodyDef.enableSleep = false; + bodyDef.enableContactRecycling = false; + bodyDef.name = "character"; + + bodyDef.gravityScale = m_characterGravity / 10.0f; + + m_bodyId = b3CreateBody( sample->m_worldId, &bodyDef ); + + // --- Feet box (lower half): dynamic friction for braking/sliding --- + // s&box BodyBox: half-extent = BodyRadius * 0.5 (box width = BodyRadius) + { + float halfExtX = m_bodyRadius * 0.5f; + float halfExtY = m_feetHeight * 0.5f; + float halfExtZ = m_bodyRadius * 0.5f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.baseMaterial.customColor = b3_colorLimeGreen; + + float feetVolume = 8.0f * halfExtX * halfExtY * halfExtZ; + shapeDef.density = ( m_characterMass * 0.4f ) / feetVolume; + + b3Transform feetTransform = { { 0.0f, -m_totalHeight * 0.5f + halfExtY, 0.0f }, b3Quat_identity }; + b3BoxHull feetBox = b3MakeTransformedBoxHull( halfExtX, halfExtY, halfExtZ, feetTransform ); + m_feetBoxId = b3CreateHullShape( m_bodyId, &shapeDef, &feetBox.base ); + } + + // --- Body capsule (upper half): zero friction so we slide on walls --- + { + float capsuleRadius = m_bodyRadius * 0.707f; + float capsuleBottom = -m_totalHeight * 0.5f + m_feetHeight * 0.5f + capsuleRadius; + float capsuleTop = m_totalHeight * 0.5f - capsuleRadius; + + if ( capsuleTop > capsuleBottom ) + { + b3Capsule capsule = { + { 0.0f, capsuleBottom, 0.0f }, + { 0.0f, capsuleTop, 0.0f }, + capsuleRadius, + }; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.baseMaterial.customColor = b3_colorCornflowerBlue; + + float h = capsuleTop - capsuleBottom; + float r = capsuleRadius; + float capsuleVolume = B3_PI * r * r * ( h + 4.0f * r / 3.0f ); + shapeDef.density = ( m_characterMass * 0.6f ) / capsuleVolume; + + m_bodyCapsuleId = b3CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); + } + } + + // Cache own shapes for ignore list + m_ownShapeCount = b3Body_GetShapes( m_bodyId, m_ownShapes, 4 ); + + UpdateMassCenter( 0.0f ); + } + + // --- TraceBody: box shape cast matching s&box's TraceBody --- + // Casts a box from `from` to `to` with given radius and height scale. + TraceResult TraceBody( b3Vec3 from, b3Vec3 to, float radiusScale = 1.0f, float heightScale = 1.0f ) const + { + TraceResult result = {}; + result.endPosition = to; + result.normal = b3Vec3_axisY; + result.hitPoint = to; + result.fraction = 1.0f; + result.hit = false; + result.startedSolid = false; + + b3Vec3 translation = to - from; + float translationLen = b3Length( translation ); + if ( translationLen < 1e-6f ) + { + return result; + } + + // Build box half extents matching s&box's BodyBox + float halfW = m_bodyRadius * 0.5f * radiusScale; + float halfH = m_totalHeight * heightScale * 0.5f; + float halfD = m_bodyRadius * 0.5f * radiusScale; + + // 8 corners of the box centered at `from`, with box bottom at from.y + // s&box BodyBox: min=(−r*0.5*s, −r*0.5*s, 0), max=(r*0.5*s, r*0.5*s, height*hScale) + // In box3d Y is up, so min.y=0 (feet), max.y=height*hScale (top) + float boxMinY = 0.0f; + float boxMaxY = m_totalHeight * heightScale; + float boxCenterY = ( boxMinY + boxMaxY ) * 0.5f; + + b3Vec3 points[8]; + for ( int i = 0; i < 8; ++i ) + { + float sx = ( i & 1 ) ? halfW : -halfW; + float sy = ( i & 2 ) ? halfH : -halfH; + float sz = ( i & 4 ) ? halfD : -halfD; + points[i] = { from.x + sx, from.y + boxCenterY + sy, from.z + sz }; + } + + b3ShapeProxy proxy; + proxy.points = points; + proxy.count = 8; + proxy.radius = 0.0f; + + ClosestShapeCastContext ctx = {}; + for ( int i = 0; i < m_ownShapeCount; ++i ) + { + ctx.ignoreShapes[i] = m_ownShapes[i]; + } + ctx.ignoreCount = m_ownShapeCount; + ctx.closestFraction = 1.0f; + ctx.hit = false; + ctx.startedSolid = false; + ctx.closestShape = b3_nullShapeId; + + b3QueryFilter filter = b3DefaultQueryFilter(); + b3World_CastShape( m_sample->m_worldId, &proxy, translation, filter, ClosestShapeCastCallback, &ctx ); + + result.startedSolid = ctx.startedSolid; + if ( ctx.hit ) + { + result.hit = true; + result.fraction = ctx.closestFraction; + result.normal = ctx.closestNormal; + result.hitPoint = ctx.closestPoint; + result.endPosition = from + ctx.closestFraction * translation; + } + + return result; + } + + bool IsStandableSurface( b3Vec3 normal ) const + { + float maxSlopeCos = cosf( m_maxSlopeAngle * B3_PI / 180.0f ); + return b3Dot( normal, b3Vec3_axisY ) >= maxSlopeCos; + } + + // Get feet position from body center position + b3Vec3 GetFeetPosition() const + { + b3Vec3 pos = b3Body_GetPosition( m_bodyId ); + return { pos.x, pos.y - m_totalHeight * 0.5f, pos.z }; + } + + // Convert feet position back to body center for SetTransform + b3Vec3 FeetToCenter( b3Vec3 feetPos ) const + { + return { feetPos.x, feetPos.y + m_totalHeight * 0.5f, feetPos.z }; + } + + // --- CategorizeGround: s&box-style box cast with radius shrinking --- + void CategorizeGround() + { + Scene* scene = m_sample->m_scene; + b3Vec3 feet = GetFeetPosition(); + + // s&box: from = WorldPosition + Up*4, to = WorldPosition + Down*2 (Source units) + b3Vec3 from = { feet.x, feet.y + 4.0f * SRC, feet.z }; + b3Vec3 to = { feet.x, feet.y - 2.0f * SRC, feet.z }; + + float radiusScale = 1.0f; + TraceResult tr = TraceBody( from, to, radiusScale, 0.5f ); + + // Shrink radius if started solid or hit non-standable surface + while ( tr.startedSolid || ( tr.hit && !IsStandableSurface( tr.normal ) ) ) + { + radiusScale -= 0.1f; + if ( radiusScale < 0.7f ) + { + UpdateGround( false, b3Vec3_axisY ); + DrawLine( scene, from, to, b3_colorRed ); + return; + } + tr = TraceBody( from, to, radiusScale, 0.5f ); + } + + if ( !tr.startedSolid && tr.hit && IsStandableSurface( tr.normal ) && m_jumpCooldown <= 0.0f ) + { + UpdateGround( true, tr.normal ); + DrawLine( scene, from, tr.hitPoint, b3_colorGreen ); + DrawPoint( scene, tr.hitPoint, 5.0f, b3_colorGreen ); + } + else + { + UpdateGround( false, b3Vec3_axisY ); + DrawLine( scene, from, to, b3_colorGray ); + } + } + + void UpdateGround( bool onGround, b3Vec3 normal ) + { + m_onGround = onGround; + m_groundNormal = normal; + if ( !onGround ) + { + m_groundVelocity = b3Vec3_zero; + } + } + + // --- Reground / StickToGround: snap character to surface when on ground --- + void Reground( float stepSize ) + { + if ( !m_onGround ) + { + return; + } + + Scene* scene = m_sample->m_scene; + b3Vec3 pos = b3Body_GetPosition( m_bodyId ); + + b3Vec3 from = { pos.x, pos.y + 0.05f, pos.z }; + b3Vec3 to = { pos.x, pos.y - stepSize, pos.z }; + + float radiusScale = 1.0f; + TraceResult tr = TraceBody( from, to, radiusScale, 0.5f ); + + while ( tr.startedSolid ) + { + radiusScale -= 0.1f; + if ( radiusScale < 0.7f ) + { + return; + } + tr = TraceBody( from, to, radiusScale, 0.5f ); + } + + if ( tr.hit ) + { + b3Vec3 targetPos = { tr.endPosition.x, tr.endPosition.y + 0.01f, tr.endPosition.z }; + float deltaY = targetPos.y - pos.y; + + b3Quat rot = b3Body_GetRotation( m_bodyId ); + b3Body_SetTransform( m_bodyId, targetPos, rot ); + + // If we moved upward, kill vertical velocity to prevent bouncing + if ( deltaY > 0.01f ) + { + b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId ); + vel.y = 0.0f; + b3Body_SetLinearVelocity( m_bodyId, vel ); + } + + DrawLine( scene, from, tr.endPosition, b3_colorCyan ); + } + } + + // --- TryStep: 4-phase trace-based step-up algorithm --- + // Returns true if a step was taken and m_stepPosition was set. + bool TryStep( float maxStepHeight ) + { + Scene* scene = m_sample->m_scene; + b3Vec3 pos = b3Body_GetPosition( m_bodyId ); + b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId ); + + // Only step when on ground and moving + if ( !m_onGround ) + { + return false; + } + + // Horizontal velocity direction + b3Vec3 hVel = { vel.x, 0.0f, vel.z }; + float hSpeed = b3Length( hVel ); + if ( hSpeed < 0.01f ) + { + return false; + } + b3Vec3 moveDir = { hVel.x / hSpeed, 0.0f, hVel.z / hSpeed }; + + // Phase 1 — FORWARD: trace body forward in velocity direction + // Start slightly behind (offset by skin) + float forwardDist = hSpeed * ( 1.0f / 60.0f ) + m_bodyRadius; // one frame of movement + radius + b3Vec3 forwardFrom = pos - m_skin * moveDir; + b3Vec3 forwardTo = pos + forwardDist * moveDir; + + float radiusScale = 1.0f; + TraceResult trForward = TraceBody( forwardFrom, forwardTo, radiusScale ); + + while ( trForward.startedSolid ) + { + radiusScale -= 0.1f; + if ( radiusScale < 0.6f ) + { + DrawLine( scene, forwardFrom, forwardTo, b3_colorRed ); + return false; + } + trForward = TraceBody( forwardFrom, forwardTo, radiusScale ); + } + + if ( !trForward.hit ) + { + // No obstacle ahead, no step needed + return false; + } + + DrawLine( scene, forwardFrom, trForward.endPosition, b3_colorYellow ); + + // Remaining velocity direction after hit + b3Vec3 hitPos = trForward.endPosition; + + // Phase 2 — UP: trace straight up from hit position + b3Vec3 upFrom = hitPos; + b3Vec3 upTo = { hitPos.x, hitPos.y + maxStepHeight, hitPos.z }; + TraceResult trUp = TraceBody( upFrom, upTo, radiusScale ); + + if ( trUp.startedSolid ) + { + DrawLine( scene, upFrom, upTo, b3_colorRed ); + return false; + } + + b3Vec3 topPos = trUp.hit ? trUp.endPosition : upTo; + float upDistance = topPos.y - upFrom.y; + if ( upDistance < 0.005f ) + { + // Too tight to step up + DrawLine( scene, upFrom, topPos, b3_colorRed ); + return false; + } + + DrawLine( scene, upFrom, topPos, b3_colorYellow ); + + // Phase 3 — ACROSS: from top position, trace in move direction + float acrossDist = forwardDist * ( 1.0f - trForward.fraction ) + m_bodyRadius * 0.5f; + b3Vec3 acrossFrom = topPos; + b3Vec3 acrossTo = topPos + acrossDist * moveDir; + TraceResult trAcross = TraceBody( acrossFrom, acrossTo, radiusScale ); + + if ( trAcross.startedSolid ) + { + DrawLine( scene, acrossFrom, acrossTo, b3_colorRed ); + return false; + } + + b3Vec3 acrossPos = trAcross.hit ? trAcross.endPosition : acrossTo; + DrawLine( scene, acrossFrom, acrossPos, b3_colorYellow ); + + // Phase 4 — DOWN: from across position, trace straight down + b3Vec3 downFrom = acrossPos; + b3Vec3 downTo = { acrossPos.x, acrossPos.y - maxStepHeight, acrossPos.z }; + TraceResult trDown = TraceBody( downFrom, downTo, radiusScale ); + + if ( !trDown.hit ) + { + DrawLine( scene, downFrom, downTo, b3_colorRed ); + return false; + } + + if ( !IsStandableSurface( trDown.normal ) ) + { + DrawLine( scene, downFrom, trDown.endPosition, b3_colorRed ); + return false; + } + + // Check we actually stepped up (not just laterally) + float stepHeight = trDown.endPosition.y - pos.y; + if ( stepHeight < 0.01f ) + { + return false; + } + + DrawLine( scene, downFrom, trDown.endPosition, b3_colorYellow ); + DrawPoint( scene, trDown.endPosition, 8.0f, b3_colorYellow ); + + // Teleport body to step position + b3Vec3 stepPos = { trDown.endPosition.x, trDown.endPosition.y + 0.01f, trDown.endPosition.z }; + b3Quat rot = b3Body_GetRotation( m_bodyId ); + b3Body_SetTransform( m_bodyId, stepPos, rot ); + + // Kill vertical velocity, scale horizontal by 0.9 + b3Vec3 newVel = b3Body_GetLinearVelocity( m_bodyId ); + newVel.x *= 0.9f; + newVel.y = 0.0f; + newVel.z *= 0.9f; + b3Body_SetLinearVelocity( m_bodyId, newVel ); + + m_stepPosition = stepPos; + return true; + } + + void RestoreStep() + { + if ( !m_didStep ) + { + return; + } + + // After physics, restore to step position to prevent double-velocity + b3Quat rot = b3Body_GetRotation( m_bodyId ); + b3Body_SetTransform( m_bodyId, m_stepPosition, rot ); + m_didStep = false; + } + + // --- AddClamped: add vector but cap the addition's magnitude --- + static b3Vec3 AddClamped( b3Vec3 current, b3Vec3 add, float maxAddLength ) + { + float addLen = b3Length( add ); + if ( addLen > maxAddLength && addLen > 0.0f ) + { + add = ( maxAddLength / addLen ) * add; + } + return current + add; + } + + // --- UpdateMassCenter: s&box formula --- + void UpdateMassCenter( float wishSpeed ) + { + b3MassData massData = b3Body_GetMassData( m_bodyId ); + float halfHeight = m_totalHeight * 0.5f; + + if ( m_onGround ) + { + // s&box: massCenter = clamp(WishSpeed, 0, halfHeight) + float centerOffset = b3ClampFloat( wishSpeed, 0.0f, halfHeight ); + massData.center = { 0.0f, centerOffset - halfHeight, 0.0f }; + } + else + { + massData.center = { 0.0f, 0.0f, 0.0f }; + } + + b3Body_SetMassData( m_bodyId, massData ); + } + + // --- UpdateBody: set friction, gravity, damping per s&box --- + void UpdateBody( b3Vec3 wishVelocity ) + { + float wishLen = b3Length( wishVelocity ); + b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId ); + float velLen = b3Length( vel ); + + // Feet friction — s&box: wantsBrakes when wish < 5 Source/s or wish < vel * 0.9 + float feetFriction = 0.0f; + if ( m_onGround ) + { + bool wantsBrakes = wishLen < ( 5.0f * SRC ) || wishLen < velLen * 0.9f; + if ( wantsBrakes ) + { + // s&box: 1 + 100 * BrakePower * GroundFriction = 1 + 100 * 0.2 * 0.6 = 13.0 + feetFriction = 1.0f + 100.0f * m_brakePower * m_surfaceFriction; + } + } + b3Shape_SetFriction( m_feetBoxId, feetFriction ); + + // Mass center + UpdateMassCenter( wishLen ); + + // Gravity: s&box disables gravity when stationary on stable ground + // s&box: wantsGravity if !onGround || velocity > 1 Source/s || groundVel > 1 Source/s + bool wantsGravity = false; + if ( !m_onGround ) + wantsGravity = true; + if ( velLen > ( 1.0f * SRC ) ) + wantsGravity = true; + if ( b3Length( m_groundVelocity ) > ( 1.0f * SRC ) ) + wantsGravity = true; + b3Body_SetGravityScale( m_bodyId, wantsGravity ? ( m_characterGravity / 10.0f ) : 0.0f ); + + // Linear damping — s&box: brakes when wish < 1 unit/s && groundVel < 1 unit/s + bool wantsDamping = m_onGround && wishLen < ( 1.0f * SRC ) && b3Length( m_groundVelocity ) < ( 1.0f * SRC ); + b3Body_SetLinearDamping( m_bodyId, wantsDamping ? 10.0f * m_brakePower : m_airFriction ); + } + + // --- AddVelocity: s&box's MoveMode.Walk velocity model --- + void AddVelocity( b3Vec3 wishVelocity ) + { + // Walk mode strips vertical component + b3Vec3 wish = { wishVelocity.x, 0.0f, wishVelocity.z }; + float wishLen = b3Length( wish ); + if ( wishLen < 0.001f ) + { + return; + } + + float groundFrictionFactor = 0.25f + m_surfaceFriction * 10.0f; + b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId ); + float savedY = vel.y; + + b3Vec3 velocity = vel - m_groundVelocity; + float speed = b3Length( velocity ); + float maxSpeed = b3MaxFloat( wishLen, speed ); + + if ( m_onGround ) + { + float amount = 1.0f * groundFrictionFactor; + velocity = AddClamped( velocity, amount * wish, wishLen * amount ); + } + else + { + float amount = 0.05f; + velocity = AddClamped( velocity, amount * wish, wishLen ); + } + + // Cap at max speed + float newSpeed = b3Length( velocity ); + if ( newSpeed > maxSpeed && newSpeed > 0.0f ) + { + velocity = ( maxSpeed / newSpeed ) * velocity; + } + + velocity = velocity + m_groundVelocity; + if ( m_onGround ) + { + velocity.y = savedY; + } + + b3Body_SetLinearVelocity( m_bodyId, velocity ); + } + + // --- PreStep: UpdateBody + AddVelocity + TryStep --- + void PreStep( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle ) + { + if ( m_jumpCooldown > 0.0f ) + { + m_jumpCooldown -= timeStep; + } + + // Compute wish velocity from input + float maxSpeed = m_sprint ? m_runSpeed : m_walkSpeed; + b3Vec3 wishVelocity = maxSpeed * throttle.x * forward + maxSpeed * throttle.y * right; + float wishSpeed = b3Length( wishVelocity ); + if ( wishSpeed > maxSpeed ) + { + wishVelocity = ( maxSpeed / wishSpeed ) * wishVelocity; + } + m_lastWishVelocity = wishVelocity; + + // 1. UpdateBody — set friction, mass center, gravity, damping + UpdateBody( wishVelocity ); + + // 2. AddVelocity — apply wish velocity (s&box model) + AddVelocity( wishVelocity ); + + // 3. TryStep — 4-phase step-up + m_didStep = TryStep( m_stepUpHeight ); + + m_massCenterWorld = b3Body_GetWorldCenterOfMass( m_bodyId ); + } + + // --- PostStep: RestoreStep + Reground + CategorizeGround --- + void PostStep( float timeStep ) + { + // 1. RestoreStep — teleport back to step position if we stepped + RestoreStep(); + + // 2. Reground / StickToGround + Reground( m_stepDownHeight ); + + // 3. CategorizeGround + CategorizeGround(); + } + + void Jump() + { + if ( m_onGround && m_jumpCooldown <= 0.0f ) + { + b3Vec3 velocity = b3Body_GetLinearVelocity( m_bodyId ); + velocity.y = m_jumpSpeed; + b3Body_SetLinearVelocity( m_bodyId, velocity ); + m_onGround = false; + m_jumpCooldown = m_jumpCooldownTime; + } + } + + void Step( float timeStep, b3Vec3 forward, b3Vec3 right, b3Vec2 throttle ) + { + PreStep( timeStep, forward, right, throttle ); + } + + void LateStep( float timeStep ) + { + PostStep( timeStep ); + } + + void DrawDebug() const + { + Scene* scene = m_sample->m_scene; + b3Vec3 pos = b3Body_GetPosition( m_bodyId ); + b3Vec3 vel = b3Body_GetLinearVelocity( m_bodyId ); + + // Draw velocity vector (purple) + DrawLine( scene, pos, pos + vel, b3_colorPurple ); + + // Draw wish velocity (orange) + DrawLine( scene, pos, pos + m_lastWishVelocity, b3_colorOrange ); + + // Draw mass center (yellow dot) + DrawPoint( scene, m_massCenterWorld, 8.0f, b3_colorYellow ); + + // Draw ground indicator + if ( m_onGround ) + { + b3Vec3 bottom = { pos.x, pos.y - m_totalHeight * 0.5f, pos.z }; + DrawLine( scene, bottom, bottom + 0.3f * m_groundNormal, b3_colorGreen ); + } + } +}; + +class RigidBodyCharacter : public Sample +{ +public: + explicit RigidBodyCharacter( SampleContext* context ) + : Sample( context ) + { + b3Vec3 startPosition = { 7.5f, 2.0f, 9.0f }; + + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 5.0f, startPosition ); + } + + m_character.Initialize( this, startPosition ); + + // --- Main level mesh (test_map01) --- + { + m_levelMesh = CreateMeshData( "data/meshes/test_map01.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3SurfaceMaterial materials[3]; + materials[0] = { 0.6f, 0.0f, 0 }; + materials[1] = { 0.6f, 1.0f, 1 }; + materials[2] = { 0.1f, 0.0f, 2 }; + shapeDef.materials = materials; + shapeDef.materialCount = 3; + + b3CreateMeshShape( body, &shapeDef, m_levelMesh, b3Vec3_one ); + } + + // --- Stairs mesh --- + { + m_stairs = CreateMeshData( "data/meshes/stairs.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -10.0f, 0.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( body, &shapeDef, m_stairs, { 0.75f, 0.75f, -1.5f } ); + } + + // --- High-poly building mesh --- + { + m_building = CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -5.0f, 0.0f, -10.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( body, &shapeDef, m_building, b3Vec3_one ); + } + + // --- Voxel meshes (dense tri terrain) --- + { + m_voxel01 = CreateMeshData( "data/meshes/voxel_mesh_01.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 10.0f, 0.0f, -10.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( body, &shapeDef, m_voxel01, b3Vec3_one ); + } + { + m_voxel02 = CreateMeshData( "data/meshes/voxel_mesh_02.obj", 1.0f, false, false, true, true ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 10.0f, 0.0f, 10.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( body, &shapeDef, m_voxel02, b3Vec3_one ); + } + + // --- Height field terrain --- + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 20.0f, 0.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + m_heightField = b3CreateWave( 50.0f, 50.0f, b3Vec3_one, 0.02f, 0.04f, true ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3SurfaceMaterial materials[3]; + materials[0] = { 0.6f, 0.0f, 0 }; + materials[1] = { 0.6f, 1.0f, 1 }; + materials[2] = { 0.1f, 0.0f, 2 }; + shapeDef.materials = materials; + shapeDef.materialCount = 3; + + b3CreateHeightFieldShape( body, &shapeDef, m_heightField ); + } + + // --- Hull obstacles on top of mesh level --- + b3ShapeDef hullShapeDef = b3DefaultShapeDef(); + hullShapeDef.baseMaterial.friction = 0.6f; + + // Ramp (tilted box) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 6.0f, 1.0f, 4.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, -20.0f * B3_DEG_TO_RAD ); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorOliveDrab; + b3BoxHull box = b3MakeBoxHull( 3.0f, 0.15f, 1.5f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Steep ramp (too steep to stand on) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 6.0f, 2.0f, -4.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisZ, -50.0f * B3_DEG_TO_RAD ); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorIndianRed; + b3BoxHull box = b3MakeBoxHull( 2.5f, 0.15f, 1.5f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Elevated platforms with gaps + for ( int i = 0; i < 3; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { -4.0f + 3.5f * i, 1.2f, -5.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorSlateGray; + b3BoxHull box = b3MakeBoxHull( 1.2f, 0.15f, 1.2f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Step-height test (increasing lip heights) + for ( int i = 0; i < 5; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + float lipHeight = 0.05f + 0.08f * i; + bodyDef.position = { -8.0f, lipHeight, -1.0f + 2.0f * i }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorCornflowerBlue; + b3BoxHull box = b3MakeBoxHull( 1.0f, lipHeight, 0.6f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Wall + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 1.5f, 10.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorDarkSlateGray; + b3BoxHull box = b3MakeBoxHull( 4.0f, 1.5f, 0.2f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Dynamic boxes to push around + for ( int i = 0; i < 3; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 3.0f + 1.5f * i, 0.5f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorGold; + b3BoxHull box = b3MakeBoxHull( 0.4f, 0.4f, 0.4f ); + b3CreateHullShape( body, &hullShapeDef, &box.base ); + } + + // Dynamic sphere + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { -3.0f, 1.0f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + hullShapeDef.baseMaterial.customColor = b3_colorOrange; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( body, &hullShapeDef, &sphere ); + } + + m_camera->m_thirdPerson = true; + m_showDebug = true; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_DISABLED ); + } + + ~RigidBodyCharacter() override + { + m_camera->m_thirdPerson = false; + glfwSetInputMode( m_window, GLFW_CURSOR, GLFW_CURSOR_NORMAL ); + b3DestroyMesh( m_levelMesh ); + b3DestroyMesh( m_stairs ); + b3DestroyMesh( m_building ); + b3DestroyMesh( m_voxel01 ); + b3DestroyMesh( m_voxel02 ); + b3DestroyHeightField( m_heightField ); + } + + void Keyboard( int key, int action, int mods ) override + { + if ( key == GLFW_KEY_T && action == GLFW_PRESS ) + { + ToggleThirdPerson(); + } + + if ( key == GLFW_KEY_V && action == GLFW_PRESS ) + { + m_showDebug = !m_showDebug; + } + } + + void Step() override + { + float hertz = m_context->hertz; + float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f; + + // Read input + b3Vec2 throttle = { 0.0f, 0.0f }; + b3Vec3 forward = -m_camera->GetForward(); + b3Vec3 right = m_camera->GetRight(); + forward.y = 0.0f; + + // Normalize forward to horizontal plane + float forwardLen = b3Length( forward ); + if ( forwardLen > 0.001f ) + { + forward *= 1.0f / forwardLen; + } + + if ( m_camera->m_thirdPerson ) + { + if ( glfwGetKey( m_window, GLFW_KEY_W ) ) + { + throttle.x += 1.0f; + } + if ( glfwGetKey( m_window, GLFW_KEY_S ) ) + { + throttle.x -= 1.0f; + } + if ( glfwGetKey( m_window, GLFW_KEY_A ) ) + { + throttle.y -= 1.0f; + } + if ( glfwGetKey( m_window, GLFW_KEY_D ) ) + { + throttle.y += 1.0f; + } + + if ( glfwGetKey( m_window, GLFW_KEY_SPACE ) ) + { + m_character.Jump(); + } + + m_character.m_sprint = m_character.m_onGround && glfwGetKey( m_window, GLFW_KEY_LEFT_SHIFT ) != 0; + } + + // Pre-step: manipulate velocity before physics + m_character.Step( timeStep, forward, right, throttle ); + + // Run physics + Sample::Step(); + + // Post-step: re-categorize ground and apply corrections + m_character.LateStep( timeStep ); + + // Update camera pivot to follow character + b3Vec3 pos = b3Body_GetPosition( m_character.m_bodyId ); + if ( m_camera->m_thirdPerson ) + { + m_camera->m_pivot = pos; + m_camera->UpdateTransform(); + + // Sphere collision: cast a sphere from pivot toward camera position + // to prevent clipping through geometry + float cameraRadius = 0.15f; + b3Vec3 desiredPos = m_camera->m_position; + b3Vec3 translation = desiredPos - pos; + float desiredDist = b3Length( translation ); + + if ( desiredDist > 0.01f ) + { + // Single-point sphere proxy (sphere = 1 point + radius) + b3Vec3 sphereCenter = pos; + b3ShapeProxy proxy; + proxy.points = &sphereCenter; + proxy.count = 1; + proxy.radius = cameraRadius; + + b3QueryFilter filter = b3DefaultQueryFilter(); + b3RayResult rayResult = b3World_CastRayClosest( m_worldId, pos, translation, filter ); + + if ( rayResult.hit ) + { + // Pull camera in front of the hit point + float clampedDist = rayResult.fraction * desiredDist - cameraRadius; + if ( clampedDist < 0.1f ) + clampedDist = 0.1f; + + b3Vec3 dir = { translation.x / desiredDist, translation.y / desiredDist, translation.z / desiredDist }; + m_camera->m_position = pos + clampedDist * dir; + } + } + } + + // Debug visualization + if ( m_showDebug ) + { + m_character.DrawDebug(); + } + + // HUD text + b3Vec3 vel = b3Body_GetLinearVelocity( m_character.m_bodyId ); + float speed = sqrtf( vel.x * vel.x + vel.z * vel.z ); + DrawTextLine( "Rigid Body Character (s&box-style)" ); + DrawTextLine( "position: %.2f %.2f %.2f", pos.x, pos.y, pos.z ); + DrawTextLine( "velocity: %.2f %.2f %.2f (horizontal: %.2f)", vel.x, vel.y, vel.z, speed ); + DrawTextLine( "on ground: %s | sprint: %s", m_character.m_onGround ? "yes" : "no", m_character.m_sprint ? "yes" : "no" ); + DrawTextLine( "WASD=move Space=jump Shift=sprint T=camera V=debug" ); + } + + void Render() override + { + Sample::Render(); + DrawTransform( m_scene, { { 0.0f, 0.0f, 0.02f }, b3Quat_identity }, 2.0f ); + } + + void UpdateUI() override + { + float fontSize = ImGui::GetFontSize(); + float height = 12.0f * fontSize; + ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 22.0f * fontSize, height ) ); + + ImGui::Begin( "Rigidbody Character", nullptr, ImGuiWindowFlags_NoResize ); + + bool thirdPerson = m_camera->m_thirdPerson; + if ( ImGui::Checkbox( "Third Person (Key: T)", &thirdPerson ) ) + { + ToggleThirdPerson(); + } + + ImGui::Checkbox( "Debug Visualization (Key: V)", &m_showDebug ); + + ImGui::Separator(); + ImGui::Text( "Ground: %s", m_character.m_onGround ? "YES" : "NO" ); + + b3Vec3 vel = b3Body_GetLinearVelocity( m_character.m_bodyId ); + float hSpeed = sqrtf( vel.x * vel.x + vel.z * vel.z ); + ImGui::Text( "Speed: %.2f m/s", hSpeed ); + ImGui::Text( "Vertical: %.2f m/s", vel.y ); + + b3Vec3 mc = m_character.m_massCenterWorld; + b3Vec3 pos = b3Body_GetPosition( m_character.m_bodyId ); + ImGui::Text( "Mass center offset: %.2f", mc.y - pos.y ); + + ImGui::End(); + } + + static Sample* Create( SampleContext* context ) + { + return new RigidBodyCharacter( context ); + } + + RigidbodyCharacter m_character; + b3MeshData* m_levelMesh; + b3MeshData* m_stairs; + b3MeshData* m_building; + b3MeshData* m_voxel01; + b3MeshData* m_voxel02; + b3HeightField* m_heightField; + bool m_showDebug; +}; + +static int sampleRBCharacter = SampleManager::Register( "Character", "Rigid Body", RigidBodyCharacter::Create ); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index 3622e4f..88419cf 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -2571,251 +2571,3 @@ public: }; static int sampleCapsuleCastRay = SampleManager::Register( "Collision", "Capsule Cast Ray", CapsuleCastRay::Create ); - -class Hull2D : public Sample -{ -public: - enum SourceMode - { - e_random = 0, - e_preset = 1, - }; - - static Sample* Create( SampleContext* context ) - { - return new Hull2D( context ); - } - - explicit Hull2D( SampleContext* context ) - : Sample( context ) - { - if ( context->restart == false ) - { - m_camera->SetView( 0.0f, 0.0f, 20.0f, { 0.0f, 0.0f, 0.0f } ); - } - - m_sourceMode = e_preset; - m_inputCount = 0; - m_hullCount = 0; - m_simplifiedCount = 0; - m_targetCount = 6; - m_randomCount = 20; - m_simpleCull = true; - m_drawLabels = true; - - Generate(); - } - - void Generate() - { - if ( m_sourceMode == e_random ) - { - m_inputCount = m_randomCount; - for ( int i = 0; i < m_inputCount; ++i ) - { - m_inputPoints[i].p = { RandomFloatRange( -5.0f, 5.0f ), RandomFloatRange( -5.0f, 5.0f ) }; - m_inputPoints[i].separation = 0.0f; - m_inputPoints[i].originalIndex = (int16_t)i; - m_inputPoints[i].persisted = false; - } - } - else - { -#if 0 - b3Vec2 points[m_pointCapacity]; - int n = b3MinInt(15, m_pointCapacity); - float delta = 2.0f * B3_PI / (float)n; - // This makes a difference when comparing areas - //b3CosSin cs = b3ComputeCosSin( delta ); - b3CosSin cs = { - cosf( delta ), - sinf( delta ), - }; - ; - float x1 = 0.5f, y1 = 0.0f; - - for ( int i = 0; i < n; ++i ) - { - points[i] = {x1, y1}; - float x2 = cs.cosine * x1 - cs.sine * y1; - float y2 = cs.sine * x1 + cs.cosine * y1; - x1 = x2; - y1 = y2; - } - m_drawScale = 1.0f; -#else - b3Vec2 points[] = { - { 0.00000000, 0.00000000 }, { -0.00305632968, 5.54900616e-05 }, { -0.00305632968, 0.0246563386 }, - { -0.00305632968, 0.0246562902 }, { -0.00305632968, 0.0800221488 }, - }; - int n = sizeof( points ) / sizeof( b3Vec2 ); - m_drawScale = 50.0f; -#endif - - m_targetCount = b3MinInt( 6, n ); - m_inputCount = n; - - for ( int i = 0; i < m_inputCount; ++i ) - { - m_inputPoints[i].p = { points[i].x, points[i].y }; - m_inputPoints[i].separation = 0.0f; - m_inputPoints[i].originalIndex = (int16_t)i; - m_inputPoints[i].persisted = false; - } - } - } - - void ComputeHull() - { - // Copy input for hull computation because b3Hull2D may modify the array - - if ( m_simpleCull ) - { - for ( int i = 0; i < m_inputCount; ++i ) - { - m_hullPoints[i] = m_inputPoints[i]; - } - - m_hullCount = b3CullPoints( m_hullPoints, m_inputCount, m_targetCount ); - } - else - { - // Copy input for hull computation because b3Hull2D may modify the array - b3Point2D points[m_pointCapacity]; - for ( int i = 0; i < m_inputCount; ++i ) - { - points[i] = m_inputPoints[i]; - } - - m_hullCount = b3Hull2D( points, m_inputCount, m_hullPoints ); - - // Copy hull for simplification - for ( int i = 0; i < m_hullCount; ++i ) - { - m_simplifiedPoints[i] = m_hullPoints[i]; - } - - int target = m_targetCount < 3 ? 3 : m_targetCount; - m_simplifiedCount = b3SimplifyHull2D( m_simplifiedPoints, m_hullCount, target ); - } - } - - void UpdateUI() override - { - float fontSize = ImGui::GetFontSize(); - float height = 16.0f * fontSize; - ImGui::SetNextWindowPos( ImVec2( 0.5f * fontSize, m_camera->m_height - height - 2.0f * fontSize ), ImGuiCond_Once ); - ImGui::SetNextWindowSize( ImVec2( 19.0f * fontSize, height ) ); - - ImGui::Begin( "Hull 2D", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); - - const char* modes[] = { "Random", "Preset" }; - if ( ImGui::Combo( "Source", (int*)&m_sourceMode, modes, IM_ARRAYSIZE( modes ) ) ) - { - Generate(); - } - - if ( m_sourceMode == e_random ) - { - if ( ImGui::SliderInt( "Point Count", &m_randomCount, 4, m_pointCapacity ) ) - { - Generate(); - } - - if ( ImGui::Button( "Regenerate" ) ) - { - Generate(); - } - } - - ImGui::Checkbox( "Simple Cull", &m_simpleCull ); - ImGui::SliderInt( "Target Count", &m_targetCount, 1, m_inputCount ); - - ImGui::Checkbox( "Draw Labels", &m_drawLabels ); - - ImGui::End(); - } - - void Render() override - { - Sample::Render(); - - DrawTextLine( "Hull 2D: input=%d, hull=%d, simplified=%d", m_inputCount, m_hullCount, m_simplifiedCount ); - - DrawTransform( m_scene, b3Transform_identity, 0.4f ); - - bool onHull[m_pointCapacity] = {}; - for ( int i = 0; i < m_hullCount; ++i ) - { - onHull[m_hullPoints[i].originalIndex] = true; - } - - float s = m_drawScale; - - // Draw input points (small gray) - for ( int i = 0; i < m_inputCount; ++i ) - { - if ( onHull[i] ) - { - continue; - } - - b3Vec3 p = { s * m_inputPoints[i].p.x, s * m_inputPoints[i].p.y, 0.0f }; - DrawPoint( m_scene, p, 4.0f, b3_colorGray ); - - if ( m_drawLabels ) - { - DrawWorldString( m_camera, p, b3_colorGray, " %d", i ); - } - } - - // Draw convex hull edges and vertices (cyan) - for ( int i = 0; i < m_hullCount; ++i ) - { - int next = ( i + 1 ) % m_hullCount; - b3Vec3 p1 = { s * m_hullPoints[i].p.x, s * m_hullPoints[i].p.y, 0.0f }; - b3Vec3 p2 = { s * m_hullPoints[next].p.x, s * m_hullPoints[next].p.y, 0.0f }; - DrawLine( m_scene, p1, p2, b3_colorCyan ); - DrawPoint( m_scene, p1, 6.0f, b3_colorCyan ); - - if ( m_drawLabels ) - { - DrawWorldString( m_camera, p1, b3_colorCyan, " %d : %d", m_hullPoints[i].originalIndex, i ); - } - } - - if ( m_simpleCull == false ) - { - // Draw simplified hull edges and vertices (yellow, slightly offset in z so visible) - for ( int i = 0; i < m_simplifiedCount; ++i ) - { - int next = ( i + 1 ) % m_simplifiedCount; - b3Vec3 p1 = { s * m_simplifiedPoints[i].p.x, s * m_simplifiedPoints[i].p.y, 0.01f }; - b3Vec3 p2 = { s * m_simplifiedPoints[next].p.x, s * m_simplifiedPoints[next].p.y, 0.01f }; - DrawLine( m_scene, p1, p2, b3_colorYellow ); - DrawPoint( m_scene, p1, 10.0f, b3_colorYellow ); - } - } - } - - void Step() override - { - ComputeHull(); - } - - static constexpr int m_pointCapacity = 128; - SourceMode m_sourceMode; - b3Point2D m_inputPoints[m_pointCapacity]; - b3Point2D m_hullPoints[2 * m_pointCapacity]; - b3Point2D m_simplifiedPoints[2 * m_pointCapacity]; - float m_drawScale; - int m_inputCount; - int m_hullCount; - int m_simplifiedCount; - int m_targetCount; - int m_randomCount; - bool m_simpleCull; - bool m_drawLabels; -}; - -static int sampleHull2D = SampleManager::Register( "Collision", "Hull 2D", Hull2D::Create ); diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index 81a11e2..c4c6e16 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -570,7 +570,7 @@ public: bodyDef.type = b3_dynamicBody; b3ShapeDef shapeDef = b3DefaultShapeDef(); - shapeDef.baseMaterial.rollingResistance = 0.1f; + shapeDef.baseMaterial.rollingResistance = m_shapeType == e_capsule ? 0.4f : 0.1f; // Don't allow shapes to collide with each other. This // makes isolating failures easier. @@ -743,7 +743,8 @@ public: m_stepCount += n; } - if ( m_stepCount > 1000 ) + int maxSteps = m_shapeType == e_capsule ? 3000 : 1000; + if ( m_stepCount > maxSteps ) { m_context->pause = true; m_failure = true; diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 879e49a..6e8d6a2 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -3,6 +3,7 @@ #include "camera.h" #include "imgui.h" +#include "mesh_loader.h" #include "renderer.h" #include "sample.h" #include "scene.h" @@ -476,3 +477,74 @@ public: }; static int sampleBoxMesh = SampleManager::Register( "Issues", "s&box mover", SBoxMover::Create ); + +class CapsuleMeshBug : public Sample +{ +public: + explicit CapsuleMeshBug( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 120.0f, 30.0f, 10.0f, { 0.0f, 2.0f, 0.0f } ); + } + + // --- Ground plane --- + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull ground = b3MakeBoxHull( 50.0f, 0.1f, 50.0f ); + b3CreateHullShape( body, &shapeDef, &ground.base ); + } + + // --- Building mesh on top of ground --- + m_building = CreateMeshData( "data/meshes/building.obj", 1.0f, false, false, true, true ); + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, 0.1f, 0.0f }; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( body, &shapeDef, m_building, b3Vec3_one ); + } + + // --- Locked capsule (same setup as player controller body) --- + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 4.0f, 10.0f }; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + bodyDef.enableSleep = false; + bodyDef.enableContactRecycling = false; + b3BodyId body = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.3f; + shapeDef.baseMaterial.customColor = b3_colorMagenta; + + b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, { 0.0f, 0.5f, 0.0f }, 0.3f }; + b3CreateCapsuleShape( body, &shapeDef, &capsule ); + } + } + + ~CapsuleMeshBug() override + { + if ( m_building ) + { + b3DestroyMesh( m_building ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new CapsuleMeshBug( context ); + } + + b3MeshData* m_building = nullptr; +}; + +static int sampleIndex = SampleManager::Register( "Issues", "Capsule Mesh", CapsuleMeshBug::Create ); diff --git a/samples/sample_manifold.cpp b/samples/sample_manifold.cpp index 94b934f..8f800f3 100644 --- a/samples/sample_manifold.cpp +++ b/samples/sample_manifold.cpp @@ -921,7 +921,8 @@ public: b3TransformPoint( xf, m_triangle[2] ), }; - b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle, m_flags, &m_satCache ); + b3CollideHullAndTriangle( &m_manifold, m_pointCapacity, m_hull, localTriangle[0], localTriangle[1], localTriangle[2], + m_flags, &m_satCache ); // Convert manifold from frame B to world space m_manifold.normal = b3RotateVector( m_transformB.q, m_manifold.normal ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 85155ec..b5c175d 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -39,11 +39,11 @@ public: m_gridMesh = b3CreateGridMesh( 20, 20, 1.0f, 0, true ); m_scale = b3Vec3_one; - m_scale = {2.0f, 2.0f, 2.0f}; - //m_scale = { -1.2f, 2.0f, 4.0f }; - //m_scale = { 8.0f, 8.0f, 8.0f }; - //m_scale = { 0.5f, 0.5f, 0.5f }; - //m_scale = { 0.25f, 0.25f, 0.25f }; + m_scale = { 2.0f, 2.0f, 2.0f }; + // m_scale = { -1.2f, 2.0f, 4.0f }; + // m_scale = { 8.0f, 8.0f, 8.0f }; + // m_scale = { 0.5f, 0.5f, 0.5f }; + // m_scale = { 0.25f, 0.25f, 0.25f }; b3ShapeDef shapeDef = b3DefaultShapeDef(); m_gridShapeId = b3CreateMeshShape( groundId, &shapeDef, m_gridMesh, m_scale ); @@ -51,7 +51,7 @@ public: m_shapeType = ShapeType::cylinder; m_bodyId = b3_nullBodyId; m_cylinderHull = b3CreateCylinder( 1.0f, 0.25f, 0.0f, 15 ); - //m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + // m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); Spawn(); @@ -73,9 +73,9 @@ public: b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; - //bodyDef.position = { 0.5f, 2.0f, -0.5f }; + // bodyDef.position = { 0.5f, 2.0f, -0.5f }; bodyDef.position = { 0.1f, 1.0f, -0.1f }; - //bodyDef.enableContactRecycling = m_shapeType != ShapeType::cylinder; + // bodyDef.enableContactRecycling = m_shapeType != ShapeType::cylinder; bodyDef.angularDamping = m_shapeType == ShapeType::cylinder ? 0.1f : 0.0f; // bodyDef.linearVelocity = { 2.0f, 0.0f, 0.0f }; @@ -108,7 +108,7 @@ public: case ShapeType::box: { b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); - //shapeDef.baseMaterial.friction = 0.01f; + // shapeDef.baseMaterial.friction = 0.01f; b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); } break; @@ -225,18 +225,19 @@ public: b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); - m_boxMesh = b3CreateBoxMesh( b3Vec3_zero, { 50.0f, 1.0f, 50.0f }, true ); + m_boxMesh = b3CreateBoxMesh( {0.0f, -1.0f, 0.0f}, { 50.0f, 1.0f, 50.0f }, true ); m_scale = b3Vec3_one; b3ShapeDef shapeDef = b3DefaultShapeDef(); shapeDef.baseMaterial.friction = 0.5f; - //shapeDef.baseMaterial.restitution = 0.5f; - //shapeDef.baseMaterial.rollingResistance = 0.5f; + // shapeDef.baseMaterial.restitution = 0.5f; + // shapeDef.baseMaterial.rollingResistance = 0.5f; m_gridShapeId = b3CreateMeshShape( groundId, &shapeDef, m_boxMesh, m_scale ); m_shapeType = ShapeType::cylinder; m_bodyId = b3_nullBodyId; - m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + //m_cylinderHull = b3CreateCylinder( 0.5f, 0.2f, 0.0f, 15 ); + m_cylinderHull = b3CreateCylinder( 0.3f, 0.15f, 0.0f, 32 ); m_stepWhilePaused = false; @@ -258,7 +259,7 @@ public: b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; - bodyDef.position = { 0.0f, 2.0f, 0.0f }; + bodyDef.position = { 0.5f, 0.0f, 0.0f }; m_bodyId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeDef shapeDef = b3DefaultShapeDef(); @@ -276,8 +277,8 @@ public: case ShapeType::capsule: { b3SurfaceMaterial material = b3DefaultSurfaceMaterial(); - //material.friction = 0.0f; - //material.restitution = 1.0f; + // material.friction = 0.0f; + // material.restitution = 1.0f; material.rollingResistance = 0.1f; shapeDef.materials = &material; shapeDef.materialCount = 1; @@ -289,7 +290,7 @@ public: case ShapeType::box: { b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); - //shapeDef.baseMaterial.friction = 0.01f; + // shapeDef.baseMaterial.friction = 0.01f; b3CreateHullShape( m_bodyId, &shapeDef, &box.base ); } break; @@ -440,7 +441,7 @@ public: b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.type = b3_dynamicBody; bodyDef.position = { 0.0f, 1.5f, 0.0f }; - //bodyDef.position = { -0.2f, 1.5f, -1.25f }; + // bodyDef.position = { -0.2f, 1.5f, -1.25f }; if ( m_shapeType == ShapeType::cylinder ) { @@ -788,7 +789,7 @@ public: m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); } -#if defined(NDEBUG) +#if defined( NDEBUG ) m_rowCount = 400; m_columnCount = 400; #else @@ -1098,7 +1099,10 @@ public: float scale = 0.01f; const char* filesNames[m_meshCount] = { - "voxel_mesh_01.obj", "voxel_mesh_02.obj", "voxel_mesh_03.obj", "voxel_mesh_04.obj", + "voxel_mesh_01.obj", + "voxel_mesh_02.obj", + "voxel_mesh_03.obj", + "voxel_mesh_04.obj", }; char buffer[64] = {}; @@ -1523,3 +1527,94 @@ public: }; static int sampleVoxelMesh = SampleManager::Register( "Mesh", "Voxel", VoxelMesh::Create ); + +// Pause this to check contact manifolds for axis aligned collisions. +class HollowBox : public Sample +{ +public: + explicit HollowBox( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 30.0f, 6.0f, b3Vec3_zero ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + m_mesh = b3CreateHollowBoxMesh( { 0.0f, 0.0f, 0.0f }, { 10.0f, 10.0f, 10.0f } ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( groundId, &shapeDef, m_mesh, b3Vec3_one ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.gravityScale = 0.0f; + bodyDef.enableSleep = false; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + { + b3Hull* cylinderHull = b3CreateCylinder( 1.0f, 0.25f, 0.0f, 8 ); + + b3Vec3 positions[6] = { + { 0.0f, -10.2f, 0.0f }, { 0.0f, 9.2f, 0.0f }, { -9.8f, 0.0f, 0.0f }, + { 9.8f, 0.0f, 0.0f }, { 0.0f, 0.0f, -9.8f }, { 0.0f, 0.0f, 9.8f }, + }; + + for (int i = 0; i < 6; ++i) + { + bodyDef.position = positions[i]; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, cylinderHull ); + } + + b3DestroyHull( cylinderHull ); + } + + { + b3Capsule capsule ={{0.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, 0.25f}; + b3Vec3 positions[8] = { + { 0.0f, -10.2f, 2.0f }, { 0.0f, 9.2f, 2.0f }, + { 0.0f, -9.9f, 4.0f }, { 0.0f, 8.9f, 4.0f }, + { -9.8f, 2.0f, 0.0f }, + { 9.8f, 2.0f, 0.0f }, { 0.0f, 2.0f, -9.8f }, { 0.0f, 2.0f, 9.8f }, + }; + + for (int i = 0; i < 8; ++i) + { + bodyDef.position = positions[i]; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + } + } + + ~HollowBox() override + { + b3DestroyMesh( m_mesh ); + } + + void Render() override + { + Sample::Render(); + + b3Transform transform = { { 0.0f, 0.01f, 0.0f }, b3Quat_identity }; + DrawTransform( m_scene, transform, 1.0f ); + } + + void Step() override + { + Sample::Step(); + } + + static Sample* Create( SampleContext* context ) + { + return new HollowBox( context ); + } + + b3MeshData* m_mesh; +}; + +static int sampleHollowBox = SampleManager::Register( "Mesh", "Hollow Box", HollowBox::Create ); diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp index 0c52dbc..eee0b7b 100644 --- a/samples/sample_stacking.cpp +++ b/samples/sample_stacking.cpp @@ -2,7 +2,6 @@ // SPDX-License-Identifier: MIT #include "camera.h" -#include "human.h" #include "imgui.h" #include "mesh_loader.h" #include "sample.h" @@ -246,6 +245,61 @@ public: static int sampleSphereStack = SampleManager::Register( "Stacking", "Sphere Stack", SphereStack::Create ); +class CapsuleStack : public Sample +{ +public: + explicit CapsuleStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 10.0f, 0.0f } ); + EnableGrid( m_scene, true ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "ground"; + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull hull = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundId, &shapeDef, &hull.base ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.motionLocks.linearZ = true; + bodyDef.motionLocks.angularX = true; + bodyDef.motionLocks.angularY = true; + bodyDef.motionLocks.angularZ = true; + + float r = 0.5f; + b3Capsule capsule = { {-1.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, r }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + float y = 1.5f * r; + + for ( int i = 0; i < 20; ++i ) + { + bodyDef.position.y = y; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + y += 2.0f * r; + } + } + + static Sample* Create( SampleContext* context ) + { + return new CapsuleStack( context ); + } +}; + +static int sampleCapsuleStack = SampleManager::Register( "Stacking", "Capsule Stack", CapsuleStack::Create ); + class SingleBox : public Sample { public: diff --git a/shared/benchmarks.c b/shared/benchmarks.c index e750ad5..4c942af 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -424,7 +424,7 @@ typedef struct StaticFloorData static StaticFloorData g_staticFloorData; -void GetStaticFloorCapacity( b3Capacity* capacity ) +void GetLargeWorldCapacity( b3Capacity* capacity ) { int floorCount = STATIC_FLOOR_GRID * STATIC_FLOOR_GRID; capacity->staticShapeCount = floorCount; @@ -434,7 +434,7 @@ void GetStaticFloorCapacity( b3Capacity* capacity ) capacity->contactCount = b3MaxInt( 1024, 8 * STATIC_FLOOR_SPHERES ); } -void CreateStaticFloor( b3WorldId worldId ) +void CreateLargeWorld( b3WorldId worldId ) { memset( &g_staticFloorData, 0, sizeof( g_staticFloorData ) ); @@ -463,7 +463,7 @@ void CreateStaticFloor( b3WorldId worldId ) } } -void StepStaticFloor( b3WorldId worldId, int stepCount ) +void StepLargeWorld( b3WorldId worldId, int stepCount ) { if ( g_staticFloorData.spheresDropped >= STATIC_FLOOR_SPHERES ) { @@ -661,110 +661,6 @@ void CreateWasher( b3WorldId worldId ) } } -struct -{ - b3MeshData* meshData; -} g_compoundCapsulesData; - -void CreateCompoundCapsules( b3WorldId worldId ) -{ - memset( &g_compoundCapsulesData, 0, sizeof( g_compoundCapsulesData ) ); - - // float tilt = 0.15f * B3_PI; - float tilt = 0.0f * B3_PI; - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = (b3Vec3){ 0.0f, -1.0f, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); - b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); - - int xCount = 200; - int zCount = 400; - - float cellWidth = 1.0f; - float amplitude = 0.4f; - float rowHz = 0.05f; - float columnHz = 0.1f; - - g_compoundCapsulesData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); - - // g_compoundCapsulesData.meshData = b3CreateWaveMesh( xCount, zCount, 1.0f, 2.5f, 0.05f, 0.01f ); - b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateMeshShape( groundId, &shapeDef, g_compoundCapsulesData.meshData, b3Vec3_one ); - - bodyDef.rotation = b3Quat_identity; - bodyDef.type = b3_dynamicBody; - bodyDef.sleepThreshold = 0.2f; - - int bodyCount = BENCHMARK_DEBUG ? 10 : 90; - - shapeDef.baseMaterial.friction = 0.9f; - shapeDef.baseMaterial.rollingResistance = 0.2f; - shapeDef.updateBodyMass = false; - shapeDef.density = 1.0f; - - float angularVelocity = -0.5f; - float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; - b3CosSin cs = b3ComputeCosSin( tilt ); - float yTilt = cs.sine / cs.cosine; - // b3CosSin stubCS = b3ComputeCosSin( 50.0f * B3_PI / 180.0f ); - for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) - { - bodyDef.position = (b3Vec3){ 0.0, 0.5f - z * yTilt, z }; - b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); - - float y = 1.0f; - float r = 0.75f; - float l = 1.5f; - float offset = 0.05f; - // float stubX = 1.4f; - // float stubZ = 0.0f; - for ( int shapeIndex = 0; shapeIndex < 22; ++shapeIndex ) - { - b3Capsule capsule = { { offset, y, 0.0f }, { 0.0f, y + l, -offset }, r }; - b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); - -#if 0 - if ( ( shapeIndex & 1 ) == 0 ) - { - b3Capsule stub = { { 0.0f, y + 0.5f * l, 0.0f }, { 1.4f * r, y + 0.5f * l, 0.0f }, 0.2f }; - b3CreateCapsuleShape( bodyId, &shapeDef, &stub ); - } - else - { - } -#elif 0 - b3Capsule stub = { { 0.0f, y + 0.5f * l, 0.0f }, { stubX * r, y + 0.5f * l, stubZ * r }, 0.2f }; - b3CreateCapsuleShape( bodyId, &shapeDef, &stub ); - stubCS = b3ComputeCosSin( B3_PI * RandomFloat() ); - float sx = stubX * stubCS.cosine - stubZ * stubCS.sine; - float sz = stubX * stubCS.sine + stubZ * stubCS.cosine; - stubX = sx; - stubZ = sz; -#endif - y += l + 2.0f * r; - r = 0.95f * r; - offset = -offset; - } - - float velocityScale = 0.5f + ( 0.5f * bodyIndex ) / bodyCount; - b3Body_ApplyMassFromShapes( bodyId ); - b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); - b3Vec3 omega = { 0.0f, 0.0f, velocityScale * angularVelocity }; - b3Vec3 v = b3Cross( omega, b3Sub( center, bodyDef.position ) ); - b3Body_SetAngularVelocity( bodyId, omega ); - b3Body_SetLinearVelocity( bodyId, v ); - - z += 3.0f; - angularVelocity = -angularVelocity; - } -} - -void DestroyCompoundCapsules( void ) -{ - b3DestroyMesh( g_compoundCapsulesData.meshData ); - memset( &g_compoundCapsulesData, 0, sizeof( g_compoundCapsulesData ) ); -} - struct { b3MeshData* meshData; @@ -878,136 +774,6 @@ void DestroyTrees( void ) memset( &g_treeData, 0, sizeof( g_treeData ) ); } -struct -{ - b3MeshData* meshData; -} g_weldedHullsData; - -void CreateWeldedHulls( b3WorldId worldId ) -{ - memset( &g_weldedHullsData, 0, sizeof( g_weldedHullsData ) ); - - float tilt = 0.0f * B3_PI; - b3BodyDef bodyDef = b3DefaultBodyDef(); - bodyDef.position = (b3Vec3){ 0.0f, 0.0f, 0.0f }; - bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); - b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); - - int xCount = 200; - // int zCount = BENCHMARK_DEBUG ? 100 : 400; - int zCount = 400; - - float cellWidth = 1.0f; - float amplitude = 0.4f; - float rowHz = 0.05f; - float columnHz = 0.1f; - - g_weldedHullsData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); - b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateMeshShape( groundId, &shapeDef, g_weldedHullsData.meshData, b3Vec3_one ); - - int entityCount = BENCHMARK_DEBUG ? 10 : 90; - - int shapeCount = 22; - b3Hull* hulls[22] = { 0 }; - - float y = 1.0f; - float r = 0.75f; - float l = 1.5f; - for ( int i = 0; i < shapeCount; ++i ) - { - hulls[i] = b3CreateCylinder( l + 2.0f * r, r, y - r, 6 ); - y += l + 2.0f * r; - r = 0.95f * r; - } - - b3WeldJointDef jointDef = b3DefaultWeldJointDef(); - jointDef.angularHertz = 6.0f; - jointDef.angularDampingRatio = 2.0f; - - float angularVelocity = -0.5f; - float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; - b3CosSin cs = b3ComputeCosSin( tilt ); - float yTilt = cs.sine / cs.cosine; - - int shapesPerBody = 5; - - shapeDef.baseMaterial.friction = 0.9f; - shapeDef.baseMaterial.rollingResistance = 0.1f; - shapeDef.updateBodyMass = false; - shapeDef.density = 1.0f; - - bodyDef.type = b3_dynamicBody; - bodyDef.sleepThreshold = 0.2f; - - for ( int entityIndex = 0; entityIndex < entityCount; ++entityIndex ) - { - b3BodyId prevBodyId = b3_nullBodyId; - - y = 1.0f; - r = 0.75f; - b3Vec3 base = (b3Vec3){ 0.0, 1.0f - z * yTilt, z }; - bodyDef.position = base; - b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); - float velocityScale = 0.5f + ( 0.5f * entityIndex ) / entityCount; - - for ( int i = 0; i < shapeCount; ++i ) - { - b3CreateHullShape( bodyId, &shapeDef, hulls[i] ); - - if ( ( i + 1 ) % shapesPerBody == 0 || i == shapeCount - 1 ) - { - b3Body_ApplyMassFromShapes( bodyId ); - - b3Vec3 center = b3Body_GetWorldCenterOfMass( bodyId ); - b3Vec3 omega = { 0.0f, 0.0f, angularVelocity * velocityScale }; - b3Vec3 v = b3Cross( omega, b3Sub( center, base ) ); - b3Body_SetAngularVelocity( bodyId, omega ); - b3Body_SetLinearVelocity( bodyId, v ); - - if ( i < shapeCount - 1 ) - { - prevBodyId = bodyId; - - if ( i < shapeCount - 1 ) - { - bodyId = b3CreateBody( worldId, &bodyDef ); - - if ( B3_IS_NON_NULL( prevBodyId ) ) - { - jointDef.base.bodyIdA = prevBodyId; - jointDef.base.bodyIdB = bodyId; - jointDef.base.localFrameA.p = (b3Vec3){ 0.0f, y + l + r, 0.0f }; - jointDef.base.localFrameB.p = (b3Vec3){ 0.0f, y + l + r, 0.0f }; - - b3CreateWeldJoint( worldId, &jointDef ); - } - - velocityScale *= 0.75f; - } - } - } - - y += l + 2.0f * r; - r = 0.95f * r; - } - - z += 3.0f; - angularVelocity = -angularVelocity; - } - - for ( int i = 0; i < shapeCount; ++i ) - { - b3DestroyHull( hulls[i] ); - } -} - -void DestroyWeldedHulls( void ) -{ - b3DestroyMesh( g_weldedHullsData.meshData ); - memset( &g_weldedHullsData, 0, sizeof( g_weldedHullsData ) ); -} - struct JunkyardData { b3BodyId pusherId; diff --git a/shared/benchmarks.h b/shared/benchmarks.h index f25f91a..e8b15f5 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -15,8 +15,6 @@ extern "C" { #endif -void CreateCompoundCapsules( b3WorldId worldId ); -void DestroyCompoundCapsules( void ); void CreateTrees25( b3WorldId worldId ); void CreateTrees50( b3WorldId worldId ); void CreateTrees100( b3WorldId worldId ); @@ -25,22 +23,17 @@ void CreateJointGrid( b3WorldId worldId ); void CreateJunkyard( b3WorldId worldId ); void StepJunkyard( b3WorldId worldId, int stepCount ); void CreateLargePyramid( b3WorldId worldId ); -//void CreateLargeWorld( b3WorldId worldId ); -//void StepLargeWorld( b3WorldId worldId, int stepCount ); -//void DestroyLargeWorld( void ); void CreateWidePyramid( b3WorldId worldId ); void CreateManyPyramids( b3WorldId worldId ); void GetRainCapacity( b3Capacity* capacity ); void CreateRain( b3WorldId worldId ); void StepRain( b3WorldId worldId, int stepCount ); void DestroyRain( void ); -void GetStaticFloorCapacity( b3Capacity* capacity ); -void CreateStaticFloor( b3WorldId worldId ); -void StepStaticFloor( b3WorldId worldId, int stepCount ); +void GetLargeWorldCapacity( b3Capacity* capacity ); +void CreateLargeWorld( b3WorldId worldId ); +void StepLargeWorld( b3WorldId worldId, int stepCount ); void GetWasherCapacity( b3Capacity* capacity ); void CreateWasher( b3WorldId worldId ); -void CreateWeldedHulls( b3WorldId worldId ); -void DestroyWeldedHulls( void ); // void CreateSpinner( b3WorldId worldId ); // float StepSpinner( b3WorldId worldId, int stepCount ); diff --git a/src/body.c b/src/body.c index b71d4bd..8af92ba 100644 --- a/src/body.c +++ b/src/body.c @@ -69,6 +69,21 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) return NULL; } +static void b3SyncBodyFlags(b3World* world, b3Body* body) +{ + // Never sync transient flags + uint32_t flags = body->flags & ~b3_bodyTransientFlags; + + b3BodySim* bodySim = b3GetBodySim( world, body ); + bodySim->flags = flags; + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if (bodyState != NULL) + { + bodyState->flags = flags; + } +} + static void b3CreateIslandForBody( b3World* world, int setIndex, b3Body* body ) { B3_ASSERT( body->islandId == B3_NULL_INDEX ); @@ -1472,6 +1487,8 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) body->flags &= ~b3_dynamicFlag; } + b3SyncBodyFlags( world, body ); + // Body type affects the mass properties b3UpdateBodyMassData( world, body ); world->locked = false; @@ -1626,16 +1643,11 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) b3LinkJoint( world, joint ); } + b3SyncBodyFlags( world, body ); + // Body type affects the mass b3UpdateBodyMassData( world, body ); - b3BodyState* state = b3GetBodyState( world, body ); - if ( state != NULL ) - { - // Ensure flags are in sync (b2_skipSolverWrite) - state->flags = body->flags; - } - b3ValidateSolverSets( world ); b3ValidateIsland( world, body->islandId ); @@ -2143,16 +2155,12 @@ void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks ) body->flags &= ~b3_allLocks; body->flags |= newLocks; - b3BodySim* bodySim = b3GetBodySim( world, body ); - bodySim->flags &= ~b3_allLocks; - bodySim->flags |= newLocks; + b3SyncBodyFlags( world, body ); b3BodyState* state = b3GetBodyState( world, body ); if ( state != NULL ) { - state->flags = body->flags; - if ( locks.linearX ) { state->linearVelocity.x = 0.0f; diff --git a/src/body.h b/src/body.h index cdd6d0a..f4d00b8 100644 --- a/src/body.h +++ b/src/body.h @@ -62,6 +62,9 @@ enum b3BodyFlags // If all these flags are set then the body has fixed rotation b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, + + // These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState. + b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact, }; // Body organizational details that are not used in the solver. diff --git a/src/convex_manifold.c b/src/convex_manifold.c index ff5ee1f..574130a 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -761,8 +761,8 @@ void b3CollideCapsules( b3LocalManifold* manifold, int capacity, const b3Capsule return; } - b3Vec3 normal1 = b3MulSV( 1.0f / distance1, b3Sub( closestPoint1, verticesB[0].position ) ); - b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( closestPoint2, verticesB[1].position ) ); + b3Vec3 normal1 = b3MulSV( 1.0f / distance1, b3Sub( verticesB[0].position, closestPoint1 ) ); + b3Vec3 normal2 = b3MulSV( 1.0f / distance2, b3Sub( verticesB[1].position, closestPoint2 ) ); b3Vec3 normal = b3Normalize( b3Add( normal1, normal2 ) ); float radiusA = capsuleA->radius; float radiusB = capsuleB->radius; diff --git a/src/hull.c b/src/hull.c index 6648b79..bd4716b 100644 --- a/src/hull.c +++ b/src/hull.c @@ -2702,188 +2702,3 @@ b3BoxHull b3MakeScaledBoxHull( b3Vec3 halfWidths, b3Transform transform, b3Vec3 b3ScaleBox( &h, &xf, postScale, 4.0f * B3_LINEAR_SLOP ); return b3MakeTransformedBoxHull( h.x, h.y, h.z, xf ); } - -// ============================================================================= -// 2D hull (Andrew's monotone chain) -// ============================================================================= - -// cross(b - a, c - a) -static inline float b3Cross2D( b3Vec2 a, b3Vec2 b, b3Vec2 c ) -{ - return ( b.x - a.x ) * ( c.y - a.y ) - ( b.y - a.y ) * ( c.x - a.x ); -} - -// Iteratively remove the vertices contributing the least area. -int b3SimplifyHull2D( b3Point2D* hull, int count1, int target ) -{ - B3_ASSERT( target >= 3 ); - - if ( count1 <= 3 ) - { - return count1; - } - - float linearSlop = B3_LINEAR_SLOP; - float areaTol = 0.25f * linearSlop * linearSlop; - int count2 = count1; - - for ( ;; ) - { - float minArea = FLT_MAX; - int minIndex = B3_NULL_INDEX; - - for ( int i = 0; i < count2; i++ ) - { - int prev = ( i + count2 - 1 ) % count2; - int next = ( i + 1 ) % count2; - float area = b3Cross2D( hull[prev].p, hull[i].p, hull[next].p ); - B3_VALIDATE( area >= -FLT_EPSILON ); - - if ( area + areaTol < minArea ) - { - minArea = area; - minIndex = i; - } - } - - if ( minArea > areaTol && count2 <= target ) - { - break; - } - - B3_ASSERT( minIndex != B3_NULL_INDEX ); - - count2 -= 1; - if ( minIndex < count2 ) - { - memmove( hull + minIndex, hull + ( minIndex + 1 ), ( count2 - minIndex ) * sizeof( b3Point2D ) ); - } - } - - return count2; -} - -// Sort comparator: by x, then by y. Insertion since n is typically 20 or less. -static void b3Sort2D( b3Point2D* pts, int count ) -{ - for ( int i = 1; i < count; ++i ) - { - b3Point2D base = pts[i]; - int j = i - 1; - while ( j >= 0 && ( pts[j].p.x > base.p.x || ( pts[j].p.x == base.p.x && pts[j].p.y > base.p.y ) ) ) - { - pts[j + 1] = pts[j]; - j -= 1; - } - pts[j + 1] = base; - } -} - -// Welds points that have been sorted O(n) time -static int b3Weld2D( b3Point2D* pts, int count1 ) -{ - if ( count1 <= 1 ) - { - return count1; - } - - float linearSlop = 0.25f * B3_LINEAR_SLOP; - float tolSqr = linearSlop * linearSlop; - - int count2 = 1; - int baseIndex = 0; - for ( int i = 1; i < count1; i++ ) - { - float distanceSqr = b3DistanceSquared2( pts[i].p, pts[baseIndex].p ); - if ( distanceSqr < tolSqr ) - { - if ( pts[i].originalIndex < pts[baseIndex].originalIndex ) - { - pts[baseIndex] = pts[i]; - } - } - else - { - pts[count2] = pts[i]; - baseIndex = count2; - count2 += 1; - } - } - - return count2; -} - -// Andrew's monotone chain convex hull algorithm -// hull must have space for 2 * count1 points -int b3Hull2D( b3Point2D* pts, int count1, b3Point2D* hull ) -{ - if ( count1 <= 0 ) - { - return 0; - } - - if ( count1 == 1 ) - { - hull[0] = pts[0]; - return 1; - } - - b3Sort2D( pts, count1 ); - count1 = b3Weld2D( pts, count1 ); - - if ( count1 == 1 ) - { - hull[0] = pts[0]; - return 1; - } - - if ( count1 == 2 ) - { - hull[0] = pts[0]; - hull[1] = pts[1]; - return 2; - } - - int count2 = 0; - - for ( int i = 0; i < count1; i++ ) - { - while ( count2 >= 2 ) - { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) - { - break; - } - - count2 -= 1; - } - - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; - } - - int lowerCount = count2 + 1; - - for ( int i = count1 - 2; i >= 0; i-- ) - { - while ( count2 >= lowerCount ) - { - float area = b3Cross2D( hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p ); - if ( area > 0.0f ) - { - break; - } - - count2 -= 1; - } - - B3_VALIDATE( count2 < 2 * count1 ); - hull[count2] = pts[i]; - count2 += 1; - } - - B3_VALIDATE( hull[0].originalIndex == hull[count2 - 1].originalIndex ); - return count2 - 1; -} diff --git a/src/mesh.c b/src/mesh.c index 1384398..c81578d 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -1482,6 +1482,40 @@ b3MeshData* b3CreateBoxMesh( b3Vec3 center, b3Vec3 extent, bool identifyEdges ) return b3CreateMesh( &def, NULL, 0 ); } +b3MeshData* b3CreateHollowBoxMesh(b3Vec3 center, b3Vec3 extent) +{ + float x = extent.x; + float y = extent.y; + float z = extent.z; + b3Vec3 vertices[] = { + { x, y, z }, { -x, y, z }, { -x, -y, z }, { x, -y, z }, { x, y, -z }, { -x, y, -z }, { -x, -y, -z }, { x, -y, -z }, + }; + + for ( int i = 0; i < 8; ++i ) + { + vertices[i] = b3Add( vertices[i], center ); + } + + int indices[] = { + 3, 1, 0, 3, 2, 1, // front + 1, 4, 0, 5, 4, 1, // top + 7, 3, 0, 7, 0, 4, // right + 5, 7, 4, 7, 5, 6, // back + 2, 5, 1, 5, 2, 6, // left + 7, 2, 3, 2, 7, 6, // bottom + }; + + b3MeshDef def = { 0 }; + def.vertexCount = 8; + def.vertices = vertices; + def.triangleCount = 12; + def.indices = indices; + def.useMedianSplit = false; + def.identifyEdges = true; + + return b3CreateMesh( &def, NULL, 0 ); +} + b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, float bottomWidth ) { float hb = 0.5f * bottomWidth; diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 01ec2ff..be1426d 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -13,7 +13,7 @@ // This guards against excessive memory usage and complex collision #define B3_MAX_MESH_CONTACT_TRIANGLES 256 -#define B3_MAX_POINTS_PER_TRIANGLE 16 +#define B3_MAX_POINTS_PER_TRIANGLE 32 #if B3_ENABLE_VALIDATION static bool b3IsSorted( const int* array, int count ) @@ -271,7 +271,24 @@ static inline bool b3AddVertex( b3FoundVertices* vertices, int vertex ) return true; } -int b3CullPoints( b3Point2D* points, int count, int target ) +// Returns true if (score, separation) should replace (bestScore, bestSeparation). +static inline bool b3IsBetterCullCandidate( float score, float separation, float bestScore, float bestSeparation, float scoreTol, + float separationTol ) +{ + if ( score > bestScore + scoreTol ) + { + return true; + } + if ( score < bestScore - scoreTol ) + { + return false; + } + + // Break the tie using separation + return separation < bestSeparation - separationTol; +} + +int b3CullPoints( b3Point2D* points, int count ) { if ( count <= 1 ) { @@ -280,12 +297,14 @@ int b3CullPoints( b3Point2D* points, int count, int target ) float tol = 0.25f * B3_LINEAR_SLOP; float tolSqr = tol * tol; + float separationTol = B3_LINEAR_SLOP; b3Point2D finalPoints[4]; int count1 = count; - // Step 1: the two points with the largest distance + // Step 1: the two points with the largest distance, ties broken by deepest combined separation float bestScore = 0.0f; + float bestSeparation = FLT_MAX; int bestIndex1 = B3_NULL_INDEX; int bestIndex2 = B3_NULL_INDEX; @@ -295,20 +314,35 @@ int b3CullPoints( b3Point2D* points, int count, int target ) for ( int j = i + 1; j < count1; ++j ) { float score = b3DistanceSquared2( p1, points[j].p ); + // Separation sum heuristic + float separation = points[i].separation + points[j].separation; - // Use tolerance for welding and hysteresis - if ( score > bestScore + tolSqr ) + if ( b3IsBetterCullCandidate( score, separation, bestScore, bestSeparation, tolSqr, separationTol ) ) { bestIndex1 = i; bestIndex2 = j; bestScore = score; + bestSeparation = separation; } } } - if ( bestScore < tolSqr || target == 1 ) + if ( bestScore < tolSqr ) { - // All points coincident. Just use first point. + // Choose deepest point + int deepestIndex = 0; + for ( int i = 1; i < count1; ++i ) + { + if ( points[i].separation < points[deepestIndex].separation ) + { + deepestIndex = i; + } + } + + if ( deepestIndex != 0 ) + { + points[0] = points[deepestIndex]; + } return 1; } @@ -320,7 +354,7 @@ int b3CullPoints( b3Point2D* points, int count, int target ) points[bestIndex1] = points[count1 - 2]; count1 -= 2; - if ( count1 == 0 || target == 2 ) + if ( count1 == 0 ) { points[0] = finalPoints[0]; points[1] = finalPoints[1]; @@ -336,8 +370,9 @@ int b3CullPoints( b3Point2D* points, int count, int target ) // float length = b3Length2( ba ); // float areaTol = tol * length; - // Step 2: find the point with the maximum triangular area + // Step 2: find the point with the maximum triangular area, ties broken by deepest separation bestScore = 0.0f; + bestSeparation = FLT_MAX; int bestIndex = B3_NULL_INDEX; float bestSignedArea = 0.0f; for ( int i = 0; i < count1; ++i ) @@ -346,11 +381,11 @@ int b3CullPoints( b3Point2D* points, int count, int target ) float signedArea = b3Cross2( ba, b3Sub2( p, a ) ); float score = b3AbsFloat( signedArea ); - // Use the area tolerance for collinear points and hysteresis - if ( score > bestScore + tolSqr ) + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) { bestSignedArea = signedArea; bestScore = score; + bestSeparation = points[i].separation; bestIndex = i; } } @@ -366,7 +401,7 @@ int b3CullPoints( b3Point2D* points, int count, int target ) // Store best point finalPoints[2] = points[bestIndex]; - if ( count1 == 1 || target == 3 ) + if ( count1 == 1 ) { points[0] = finalPoints[0]; points[1] = finalPoints[1]; @@ -394,6 +429,7 @@ int b3CullPoints( b3Point2D* points, int count, int target ) b3Vec2 ac = b3Sub2( a, c ); bestScore = 0.0f; + bestSeparation = FLT_MAX; bestIndex = B3_NULL_INDEX; for ( int i = 0; i < count1; ++i ) { @@ -404,9 +440,10 @@ int b3CullPoints( b3Point2D* points, int count, int target ) float score = b3MaxFloat( u1, b3MaxFloat( u2, u3 ) ); // Use the area tolerance for collinear points and hysteresis - if ( score > bestScore + tolSqr ) + if ( b3IsBetterCullCandidate( score, points[i].separation, bestScore, bestSeparation, tolSqr, separationTol ) ) { bestScore = score; + bestSeparation = points[i].separation; bestIndex = i; } } @@ -439,7 +476,6 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor return count1; } - // bump MAX_POINTS * MAX_TRI * 16 b3Point2D* pts = b3Bump( &arena, count1 * sizeof( b3Point2D ) ); b3Vec3 u = b3Perp( normal ); b3Vec3 v = b3Cross( normal, u ); @@ -447,27 +483,25 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor for ( int i = 0; i < count1; ++i ) { - pts[i].originalIndex = (int16_t)i; - pts[i].separation = 0.0f; - b3Vec3 d = b3Sub( points[i].point, origin ); pts[i].p = (b3Vec2){ b3Dot( d, u ), b3Dot( d, v ) }; - pts[i].persisted = false; + pts[i].separation = points[i].separation; + pts[i].originalIndex = i; } - int count3 = b3CullPoints( pts, count1, 4 ); - B3_ASSERT( count3 <= B3_MAX_MANIFOLD_POINTS ); + int count2 = b3CullPoints( pts, count1 ); + B3_ASSERT( count2 <= B3_MAX_MANIFOLD_POINTS ); b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; - for ( int i = 0; i < count3; ++i ) + for ( int i = 0; i < count2; ++i ) { int index = pts[i].originalIndex; B3_ASSERT( 0 <= index && index < count1 ); finalPoints[i] = points[index]; } - memcpy( points, finalPoints, count3 * sizeof( b3LocalManifoldPoint ) ); - return count3; + memcpy( points, finalPoints, count2 * sizeof( b3LocalManifoldPoint ) ); + return count2; } typedef struct b3Cluster @@ -573,7 +607,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact cache->satCache = (b3SATCache){ 0 }; } - b3CollideHullAndTriangle( manifold, pointCapacity, shapeB->hull, vertices, triangle.flags, &cache->satCache ); + b3CollideHullAndTriangle( manifold, pointCapacity, shapeB->hull, vertices[0], vertices[1], vertices[2], + triangle.flags, &cache->satCache ); context->satCallCount += 1; context->satCacheHitCount += cache->satCache.hit; break; diff --git a/src/physics_world.c b/src/physics_world.c index a79bdf3..23c268f 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -1995,10 +1995,17 @@ b3Counters b3World_GetCounters( b3WorldId worldId ) s.recycledContactCount = 0; s.arenaCapacity = 0; + s.distanceIterations = 0; + s.pushBackIterations = 0; + s.rootIterations = 0; for ( int i = 0; i < world->workerCount; ++i ) { s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + s.distanceIterations = b3MaxInt( s.distanceIterations, world->taskContexts.data[i].distanceIterations ); + s.pushBackIterations = b3MaxInt( s.pushBackIterations, world->taskContexts.data[i].pushBackIterations ); + s.rootIterations = b3MaxInt( s.rootIterations, world->taskContexts.data[i].rootIterations ); + int peak = world->taskContexts.data[i].arena.shared->peakDemand; if ( peak > s.arenaCapacity ) { @@ -3064,7 +3071,15 @@ void b3ValidateSolverSets( b3World* world ) b3Body* body = bodies + bodyId; B3_ASSERT( body->setIndex == setIndex ); B3_ASSERT( body->localIndex == i ); - B3_ASSERT( body->generation == body->generation ); + + uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags; + B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags ); + + b3BodyState* bodyState = b3GetBodyState( world, body ); + if ( bodyState != NULL ) + { + B3_ASSERT( ( bodyState->flags & syncedFlags ) == syncedFlags ); + } if ( body->type == b3_dynamicBody ) { diff --git a/src/physics_world.h b/src/physics_world.h index c9a929a..56b29cc 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -100,6 +100,12 @@ typedef struct b3TaskContext // Profiling int satCallCount; int satCacheHitCount; + int distanceIterations; + int pushBackIterations; + int rootIterations; + + // Number of contacts recycled this step (collide pass). + int recycledContactCount; b3DebugPoint points[B3_DEBUG_POINT_CAPACITY]; int pointCount; @@ -108,10 +114,6 @@ typedef struct b3TaskContext int lineCount; int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; - - // Number of contacts recycled this step (collide pass). - int recycledContactCount; - // Prevent false sharing char cacheLine[64]; } b3TaskContext; diff --git a/src/solver.c b/src/solver.c index 7effc4c..0e6ea36 100644 --- a/src/solver.c +++ b/src/solver.c @@ -148,14 +148,11 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte // Jacobian derived by Erin Catto, Ph.D. Do not attempt to do this without a Ph.D. // Doubled inertia terms above fold into Iw, e.g. row 2 col 1: i00*w3 - i02*w1 - Iw3. b3Matrix3 J = { - { i00 + h * ( w2 * i02 - w3 * i01 ), - i01 + h * ( w3 * i00 - w1 * i02 - Iw3 ), + { i00 + h * ( w2 * i02 - w3 * i01 ), i01 + h * ( w3 * i00 - w1 * i02 - Iw3 ), i02 + h * ( w1 * i01 - w2 * i00 + Iw2 ) }, - { i01 + h * ( w2 * i12 - w3 * i11 + Iw3 ), - i11 + h * ( w3 * i01 - w1 * i12 ), + { i01 + h * ( w2 * i12 - w3 * i11 + Iw3 ), i11 + h * ( w3 * i01 - w1 * i12 ), i12 + h * ( w1 * i11 - w2 * i01 - Iw1 ) }, - { i02 + h * ( w2 * i22 - w3 * i12 - Iw2 ), - i12 + h * ( w3 * i02 - w1 * i22 + Iw1 ), + { i02 + h * ( w2 * i22 - w3 * i12 - Iw2 ), i12 + h * ( w3 * i02 - w1 * i22 + Iw1 ), i22 + h * ( w1 * i12 - w2 * i02 ) }, }; @@ -326,6 +323,10 @@ typedef struct b3ContinuousContext b3SensorHit sensorHits[B2_MAX_CONTINUOUS_SENSOR_HITS]; float sensorFractions[B2_MAX_CONTINUOUS_SENSOR_HITS]; int sensorCount; + + int distanceIterations; + int pushBackIterations; + int rootIterations; } b3ContinuousContext; // This is called from b3DynamicTree_Query for continuous collision @@ -442,14 +443,9 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con { fastBodySim->flags |= b3_hadTimeOfImpact; continuousContext->fraction = output.fraction; - - // if (output.usedFallback == true) - //{ - // // help out the solver - // b3BodyState* state = b3GetBodyState( world, fastBody ); - // B3_ASSERT( state != NULL ); - // state->angularVelocity = b3Vec3_zero; - // } + continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations ); + continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations ); + continuousContext->rootIterations = b3MaxInt( continuousContext->rootIterations, output.rootIterations ); } } @@ -620,6 +616,10 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* } } + taskContext->distanceIterations = b3MaxInt( taskContext->distanceIterations, context.distanceIterations ); + taskContext->pushBackIterations = b3MaxInt( taskContext->pushBackIterations, context.pushBackIterations ); + taskContext->rootIterations = b3MaxInt( taskContext->rootIterations, context.rootIterations ); + b3TracyCZoneEnd( ccd ); } @@ -681,7 +681,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, // q = [sin(theta/2) * v, cos(theta/2)] // for small angles abs(theta) ~= 2 * length(sin(theta/2) * v) b3Vec3 rotationArc = b3ModifiedCross( b3Abs( localDeltaRotation ), sim->maxExtent ); - float maxDeltaPosition = b3Length( state->deltaPosition ) + 2.0f * b3Length(rotationArc); + float maxDeltaPosition = b3Length( state->deltaPosition ) + 2.0f * b3Length( rotationArc ); // Position correction is not as important for sleep as true velocity. float positionSleepFactor = 0.5f; @@ -705,13 +705,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, sim->force = b3Vec3_zero; sim->torque = b3Vec3_zero; - body->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + body->flags &= ~b3_bodyTransientFlags; body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); - sim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); - state->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + sim->flags &= ~b3_bodyTransientFlags; + state->flags &= ~b3_bodyTransientFlags; - if ( enableSleep == false || (body->flags & b3_enableSleep) == 0 || sleepVelocity > body->sleepThreshold ) + if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold ) { // Body is not sleepy body->sleepTime = 0.0f; @@ -1783,8 +1783,9 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) if ( world->taskCount < B3_MAX_TASKS ) { char buffer[16]; - snprintf( buffer, sizeof(buffer), "solve[%d]", i ); - workerContext[i].userTask = world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); + snprintf( buffer, sizeof( buffer ), "solve[%d]", i ); + workerContext[i].userTask = + world->enqueueTaskFcn( &b3SolverTask, workerContext + i, world->userTaskContext, buffer ); world->taskCount += 1; world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; } @@ -1841,7 +1842,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) } // Finalize bodies. Must happen after the constraint solver and after island splitting. - b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 64, stepContext, "ccd" ); + b3ParallelFor( world, &b3FinalizeBodiesTask, awakeBodyCount, 16, stepContext, "ccd" ); // Free in reverse order b3StackFree( &world->stack, graphBlocks ); diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index ec39d9e..015cb25 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -243,11 +243,12 @@ static void b3BuildTriangleAndCapsuleFaceContact( b3LocalManifold* manifold, con return; } + float radius = capsule->radius; float distance1 = b3PlaneSeparation( plane, segment[0].position ); float distance2 = b3PlaneSeparation( plane, segment[1].position ); float speculativeDistance = B3_SPECULATIVE_DISTANCE; - if ( distance1 > speculativeDistance && distance2 > speculativeDistance ) + if ( distance1 > speculativeDistance + radius && distance2 > speculativeDistance + radius ) { return; } @@ -900,7 +901,7 @@ b3AtomicInt b3_triangleConvexCalls; b3AtomicInt b3_triangleCacheHits; // Computes the manifold in the local space of the hull -void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, const b3Vec3* triangleB, +void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3Hull* hullA, b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, int triangleFlags, b3SATCache* cache ) { manifold->pointCount = 0; @@ -911,7 +912,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 return; } - b3Vec3 v1 = triangleB[0], v2 = triangleB[1], v3 = triangleB[2]; b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); float linearSlop = B3_LINEAR_SLOP; @@ -1222,6 +1222,7 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 // In this fall back to GJK. This is important to prevent tunneling in rare cases. if ( manifold->pointCount == 0 ) { + b3Vec3 triangleB[] = { v1, v2, v3 }; b3DistanceInput input = { 0 }; input.proxyA = (b3ShapeProxy){ .points = triangleB, diff --git a/test/test_determinism.c b/test/test_determinism.c index 547082e..4faf953 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -14,8 +14,8 @@ #define TracyCFrameMark #endif -#define EXPECTED_SLEEP_STEP 300 -#define EXPECTED_HASH 0x2A58F9D0 +#define EXPECTED_SLEEP_STEP 306 +#define EXPECTED_HASH 0x596EA165 static int SingleMultithreadingTest( int workerCount ) {