diff --git a/benchmark/main.c b/benchmark/main.c index 1fc8fd8..d7216af 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -77,15 +77,15 @@ int main( int argc, char** argv ) #endif Benchmark benchmarks[] = { - { "compound_capsules", NULL, CreateCompoundCapsules, DestroyCompoundCapsules, NULL, 500 }, - { "compound_hulls", NULL, CreateCompoundHulls, DestroyCompoundHulls, NULL, 500 }, + { "trees100", NULL, CreateTrees100, DestroyTrees, NULL, 500 }, + { "trees50", NULL, CreateTrees50, DestroyTrees, NULL, 500 }, + { "trees25", NULL, CreateTrees25, DestroyTrees, NULL, 500 }, { "joint_grid", NULL, CreateJointGrid, NULL, NULL, 100 }, { "junkyard", NULL, CreateJunkyard, NULL, StepJunkyard, 500 }, { "large_pyramid", NULL, CreateLargePyramid, NULL, NULL, 200 }, { "many_pyramids", NULL, CreateManyPyramids, NULL, NULL, 100 }, { "rain", GetRainCapacity, CreateRain, DestroyRain, StepRain, 400 }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, - { "welded_hulls", NULL, CreateWeldedHulls, DestroyWeldedHulls, NULL, 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 98dec64..6d09dcc 100644 --- a/include/box3d/collision.h +++ b/include/box3d/collision.h @@ -1430,7 +1430,6 @@ typedef struct b3FeaturePair typedef struct b3LocalManifoldPoint { b3Vec3 point; - b3Vec3 normal; float separation; b3FeaturePair pair; int triangleIndex; @@ -1440,7 +1439,7 @@ typedef struct b3LocalManifoldPoint typedef struct b3LocalManifold { b3Vec3 normal; - b3Plane trianglePlane; + b3Vec3 triangleNormal; b3LocalManifoldPoint* points; int pointCount; int triangleIndex; diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index 13a1092..c845d9d 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -805,53 +805,6 @@ B3_FORCE_INLINE b3Matrix3 b3MakeMatrixFromQuat( b3Quat q ) // https://en.wikipedia.org/wiki/Parallel_axis_theorem B3_API b3Matrix3 b3Steiner( float mass, b3Vec3 origin ); -B3_INLINE b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point ) -{ - return B3_LITERAL( b3Plane ){ normal, b3Dot( normal, point ) }; -} - -B3_INLINE b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) -{ - b3Plane plane; - plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); - plane.normal = b3Normalize( plane.normal ); - plane.offset = b3Dot( plane.normal, point1 ); - return plane; -} - -B3_INLINE b3Plane b3NegatePlane( b3Plane plane ) -{ - return B3_LITERAL( b3Plane ){ { -plane.normal.x, -plane.normal.y, -plane.normal.z }, -plane.offset }; -} - -// normal2 = q * normal1 -// offset2 = dot(normal2, p) + offset1 -B3_INLINE b3Plane b3TransformPlane( b3Transform transform, b3Plane plane ) -{ - b3Vec3 normal = b3RotateVector( transform.q, plane.normal ); - return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) }; -} - -// offset1 = offset2 - dot(normal2, p) -// normal1 = qc * normal2 -B3_INLINE b3Plane b3InvTransformPlane( b3Transform transform, b3Plane plane ) -{ - float offset = plane.offset - b3Dot( plane.normal, transform.p ); - b3Vec3 normal = b3InvRotateVector( transform.q, plane.normal ); - return B3_LITERAL( b3Plane ){ normal, offset }; -} - -/// Signed separation of a point from a plane -B3_INLINE float b3PlaneSeparation( b3Plane plane, b3Vec3 point ) -{ - return b3Dot( plane.normal, point ) - plane.offset; -} - -B3_INLINE b3Plane b3ScalePlane( float scale, b3Plane plane ) -{ - return B3_LITERAL( b3Plane ){ plane.normal, scale * plane.offset }; -} - B3_INLINE b3AABB b3MakeAABB( const b3Vec3* points, int count, float radius ) { B3_ASSERT( count > 0 ); diff --git a/include/box3d/types.h b/include/box3d/types.h index 58bc7fb..3521c65 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -474,6 +474,7 @@ typedef struct b3Counters int jointCount; int islandCount; int stackUsed; + int arenaCapacity; // peak demand across all task arenas in bytes int staticTreeHeight; int treeHeight; int satCallCount; diff --git a/samples/sample.cpp b/samples/sample.cpp index d07b691..8abec41 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -292,12 +292,14 @@ void Sample::CreateWorld( b3Capacity* capacity ) b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = m_context->workerCount; - worldDef.userTaskContext = this; worldDef.enableSleep = m_context->enableSleep; worldDef.createDebugShape = CreateDebugShape; worldDef.destroyDebugShape = DestroyDebugShape; worldDef.userDebugShapeContext = m_context; - worldDef.capacity = *capacity; + if (capacity != nullptr) + { + worldDef.capacity = *capacity; + } m_worldId = b3CreateWorld( &worldDef ); b3World_SetContactRecycleDistance( m_worldId, m_context->recycleDistance ); @@ -718,6 +720,7 @@ void Sample::UpdateUI() 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( "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 ); ImGui::Separator(); @@ -1052,7 +1055,7 @@ void SampleManager::Startup( GLFWwindow* window, int width, int height, int buff // todo_erin testing // m_context.settings.workerCount = 1; - m_context.arena.Create( 100 * 1024 * 1024 ); + m_context.arena.Create( 400 * 1024 * 1024 ); m_context.window = window; m_context.camera.Resize( width, height ); diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 37d9ff4..1fc9cb7 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -4,10 +4,10 @@ #include "benchmarks.h" #include "camera.h" #include "imgui.h" -#include "utils.h" #include "renderer.h" #include "sample.h" #include "scene.h" +#include "utils.h" #include "box3d/box3d.h" @@ -26,7 +26,7 @@ public: explicit BenchmarkLargePyramid( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); EnableGrid( m_scene, true ); @@ -49,7 +49,7 @@ public: explicit BenchmarkWidePyramid( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); EnableGrid( m_scene, true ); @@ -72,7 +72,7 @@ public: explicit BenchmarkManyPyramids( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 0.0f, 5.0f, 80.0f, { 0.0f, 18.0f, 0.0f } ); EnableGrid( m_scene, true ); @@ -111,7 +111,7 @@ public: explicit BenchmarkRain( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 45.0f, 30.0f, 40.0f, b3Vec3_zero ); context->debugDraw.drawJoints = false; @@ -214,7 +214,7 @@ public: explicit BenchmarkJointGrid( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( -25.0f, 25.0f, 94.0f, { 30.0f, -30.0f, 30.0f } ); } @@ -258,7 +258,7 @@ public: explicit FallingBoxes( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 45.0f, 45.0f, 250.0f, b3Vec3_zero ); EnableGrid( m_scene, true ); @@ -282,11 +282,11 @@ public: b3ShapeDef shapeDef = b3DefaultShapeDef(); b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 1.0f ); - for (int i = 0; i < n; ++i) + for ( int i = 0; i < n; ++i ) { - for (int j = 0; j < 8; ++j) + for ( int j = 0; j < 8; ++j ) { - for (int k = 0; k < 8; ++k) + for ( int k = 0; k < 8; ++k ) { bodyDef.position = { -16.0f + 4.0f * j, 4.0f * i + 5.0f, -16.0f + 4.0f * k }; b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); @@ -311,7 +311,7 @@ public: explicit CandyCups( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 45.0f, 35.0f, 40.0f, b3Vec3_zero ); EnableGrid( m_scene, true ); @@ -335,11 +335,11 @@ public: bodyDef.type = b3_dynamicBody; b3ShapeDef shapeDef = b3DefaultShapeDef(); m_convex = CreateConvex( 0.6f, 0.0f, 0.95f, 1.0f, context->arena ); - for (int i = 0; i < n; ++i) + for ( int i = 0; i < n; ++i ) { - for (int j = 0; j < m; ++j) + for ( int j = 0; j < m; ++j ) { - for (int k = 0; k < m; ++k) + for ( int k = 0; k < m; ++k ) { bodyDef.position = { -10.0f + 2.5f * j, 1.0f * i, -10.0f + 2.5f * k }; b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); @@ -364,7 +364,7 @@ public: b3Vec3* vertexBase = static_cast( arena.Allocate( vertexCount * sizeof( b3Vec3 ) ) ); float alpha = 0.0f; - for (int sideIndex = 0; sideIndex < sideCount; ++sideIndex) + for ( int sideIndex = 0; sideIndex < sideCount; ++sideIndex ) { b3CosSin cs = b3ComputeCosSin( alpha ); @@ -397,7 +397,7 @@ public: explicit BenchmarkExplosion( SampleContext* context ) : Sample( context ) { - if (m_context->restart == false) + if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 40.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); } @@ -454,9 +454,9 @@ public: bodyDef.type = b3_dynamicBody; shapeDef.explosionScale = 2.0f; - for (int i = -n; i <= n; ++i) + for ( int i = -n; i <= n; ++i ) { - for (int k = -n; k <= n; ++k) + for ( int k = -n; k <= n; ++k ) { bodyDef.position = { 1.0f * i, 0.0f, 1.0f * k }; @@ -499,7 +499,7 @@ public: ImGui::SliderFloat( "impulse", &m_impulse, 0.0f, 2000.0f, "%.0f" ); - if (ImGui::Button( "explode" )) + if ( ImGui::Button( "explode" ) ) { Explode(); } @@ -556,7 +556,7 @@ public: explicit BenchmarkHeightField( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 45.0f, 30.0f, 20.0f ); } @@ -616,14 +616,14 @@ public: b3Sphere sphere = { b3Vec3_zero, m_radius }; int castCount = 0; - for (float x = -spanX; x <= spanX; x += delta) + for ( float x = -spanX; x <= spanX; x += delta ) { - for (float z = -spanZ; z <= spanZ; z += delta) + for ( float z = -spanZ; z <= spanZ; z += delta ) { b3Vec3 rayOrigin = { x, 2.0f, z }; b3RayResult result = {}; - if (m_radius == 0.0f) + if ( m_radius == 0.0f ) { result = b3World_CastRayClosest( m_worldId, rayOrigin, rayTranslation, b3DefaultQueryFilter() ); } @@ -633,7 +633,7 @@ public: b3ShapeProxy proxy = { &rayOrigin, 1, m_radius }; b3World_CastShape( m_worldId, &proxy, rayTranslation, b3DefaultQueryFilter(), CastCallback, &context ); - if (context.hit) + if ( context.hit ) { result.point = context.point; result.normal = context.normal; @@ -644,16 +644,16 @@ public: castCount += 1; - if (result.hit) + if ( result.hit ) { hitCount += 1; - if (m_isDebug) + if ( m_isDebug ) { b3Vec3 point = result.point; DrawLine( m_scene, point, point + 0.5f * result.normal, b3_colorGreen ); DrawPoint( m_scene, point, 10.0f, b3_colorGreen ); - if (m_radius > 0.0f) + if ( m_radius > 0.0f ) { b3Transform transform = b3Transform_identity; transform.p = rayOrigin + result.fraction * rayTranslation; @@ -666,7 +666,7 @@ public: DrawPoint( m_scene, rayEnd, 2.0f, b3_colorRed ); } } - else if (m_isDebug) + else if ( m_isDebug ) { b3Vec3 rayEnd = rayOrigin + rayTranslation; DrawLine( m_scene, rayOrigin, rayEnd, b3_colorYellow ); @@ -708,7 +708,7 @@ public: explicit BenchmarkCompoundCapsules( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { float distance = 220.0f; float height = 20.0f; @@ -739,34 +739,88 @@ public: static int sampleCompoundCapsules = SampleManager::Register( "Benchmark", "Compounds Capsules", BenchmarkCompoundCapsules::Create ); -class BenchmarkCompoundHulls : public Sample +class BenchmarkFallingTrees : public Sample { public: - explicit BenchmarkCompoundHulls( SampleContext* context ) + explicit BenchmarkFallingTrees( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + 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 } ); } - CreateCompoundHulls( m_worldId ); + m_gridSize = 100; + CreateTrees100( m_worldId ); } - ~BenchmarkCompoundHulls() override + ~BenchmarkFallingTrees() override { - DestroyCompoundHulls(); + DestroyTrees(); + } + + void Generate() + { + ResetProfile(); + + CreateWorld( nullptr ); + + DestroyTrees(); + + if ( m_gridSize == 100 ) + { + CreateTrees100( m_worldId ); + } + else if ( m_gridSize == 50 ) + { + CreateTrees50( m_worldId ); + } + else + { + CreateTrees25( m_worldId ); + } + } + + void UpdateUI() override + { + Sample::UpdateUI(); + + float fontSize = ImGui::GetFontSize(); + float height = 10.0f * fontSize; + ImGui::SetNextWindowPos( { 1.0f * fontSize, m_camera->m_height - height - 3.0f * fontSize }, ImGuiCond_Once ); + ImGui::SetNextWindowSize( { 20.0f * fontSize, height } ); + + ImGui::Begin( "Falling Trees", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::RadioButton( "100cm", &m_gridSize, 100 ) ) + { + Generate(); + } + + if ( ImGui::RadioButton( "50cm", &m_gridSize, 50 ) ) + { + Generate(); + } + + if ( ImGui::RadioButton( "25cm", &m_gridSize, 25 ) ) + { + Generate(); + } + + ImGui::End(); } static Sample* Create( SampleContext* context ) { - return new BenchmarkCompoundHulls( context ); + return new BenchmarkFallingTrees( context ); } + + int m_gridSize; }; -static int sampleCompoundHulls = SampleManager::Register( "Benchmark", "Compounds Hulls", BenchmarkCompoundHulls::Create ); +static int sampleCompoundHulls = SampleManager::Register( "Benchmark", "Falling Trees", BenchmarkFallingTrees::Create ); class WeldedHulls : public Sample { @@ -809,7 +863,7 @@ public: explicit BenchmarkSensor( SampleContext* context ) : Sample( context ) { - if (m_context->restart == false) + if ( m_context->restart == false ) { m_camera->SetView( 5.0f, 18.0f, 180.0f, { 0.0f, 5.0f, 0.0f } ); } @@ -832,7 +886,7 @@ public: float y = 0.0f; float x = -40.0f * gridSize; - for (int i = 0; i < 81; ++i) + for ( int i = 0; i < 81; ++i ) { b3Vec3 extents = { 0.5f * gridSize, 0.5f * gridSize, 0.5f * gridSize }; b3Transform transform = { { x, y, 0.0f }, b3Quat_identity }; @@ -853,14 +907,14 @@ public: float yStart = 10.0f; - for (int j = 0; j < m_rowCount; ++j) + for ( int j = 0; j < m_rowCount; ++j ) { m_passiveSensors[j].row = j; m_passiveSensors[j].active = false; shapeDef.userData = m_passiveSensors + j; float y = j * shift + yStart; - for (int i = 0; i < m_columnCount; ++i) + for ( int i = 0; i < m_columnCount; ++i ) { float x = i * shift - xCenter; b3Vec3 extents = { 0.5f, 0.5f, 0.5f }; @@ -890,7 +944,7 @@ public: shapeDef.enableSensorEvents = true; b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; - for (int i = 0; i < m_columnCount; ++i) + for ( int i = 0; i < m_columnCount; ++i ) { // stagger bodies to avoid bunching up events into a single update float yOffset = RandomFloatRange( -1.0f, 1.0f ); @@ -905,7 +959,7 @@ public: { Sample::Step(); - if (m_stepCount == m_lastStepCount) + if ( m_stepCount == m_lastStepCount ) { return; } @@ -913,14 +967,14 @@ public: std::set zombies; b3SensorEvents events = b3World_GetSensorEvents( m_worldId ); - for (int i = 0; i < events.beginCount; ++i) + for ( int i = 0; i < events.beginCount; ++i ) { b3SensorBeginTouchEvent* event = events.beginEvents + i; // shapes on begin touch are always valid ShapeUserData* userData = static_cast( b3Shape_GetUserData( event->sensorShapeId ) ); - if (userData->active) + if ( userData->active ) { zombies.emplace( b3Shape_GetBody( event->visitorShapeId ) ); } @@ -936,11 +990,11 @@ public: } } - for (int i = 0; i < events.endCount; ++i) + for ( int i = 0; i < events.endCount; ++i ) { b3SensorEndTouchEvent* event = events.endEvents + i; - if (b3Shape_IsValid( event->visitorShapeId ) == false) + if ( b3Shape_IsValid( event->visitorShapeId ) == false ) { continue; } @@ -951,14 +1005,14 @@ public: b3Shape_SetSurfaceMaterial( event->visitorShapeId, surfaceMaterial ); } - for (b3BodyId bodyId : zombies) + for ( b3BodyId bodyId : zombies ) { b3DestroyBody( bodyId ); } int delay = 0x1F; - if (( m_stepCount & delay ) == 0) + if ( ( m_stepCount & delay ) == 0 ) { CreateRow( 10.0f + m_rowCount * 5.0f ); } @@ -974,16 +1028,16 @@ public: bool Filter( b3ShapeId idA, b3ShapeId idB ) { ShapeUserData* userData = nullptr; - if (b3Shape_IsSensor( idA )) + if ( b3Shape_IsSensor( idA ) ) { userData = (ShapeUserData*)b3Shape_GetUserData( idA ); } - else if (b3Shape_IsSensor( idB )) + else if ( b3Shape_IsSensor( idB ) ) { userData = (ShapeUserData*)b3Shape_GetUserData( idB ); } - if (userData != nullptr) + if ( userData != nullptr ) { return userData->active == true || userData->row != m_filterRow; } @@ -1020,7 +1074,7 @@ public: explicit BenchmarkWasher( SampleContext* context ) : Sample( context ) { - if (m_context->restart == false) + if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 20.0f, 60.0, { 0.0f, 15.0f, 0.0f } ); EnableGrid( m_scene, true ); @@ -1047,7 +1101,7 @@ public: explicit BenchmarkHull( SampleContext* context ) : Sample( context ) { - if (m_context->restart == false) + if ( m_context->restart == false ) { m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Vec3_zero ); } @@ -1055,7 +1109,7 @@ public: g_randomSeed = 42; m_count = 64; - for (int i = 0; i < m_count; ++i) + for ( int i = 0; i < m_count; ++i ) { m_points[i] = RandomVec3( { -1.0f, -1.0f, -1.0f }, { 1.0f, 1.0f, 1.0f } ); } @@ -1088,7 +1142,7 @@ public: float area = 0.0f; int trials = m_isDebug ? 200 : 2000; - for (int i = 0; i < trials; ++i) + for ( int i = 0; i < trials; ++i ) { b3Hull* hull = b3CreateHull( m_points, m_count, m_count ); area += hull->surfaceArea; @@ -1098,7 +1152,7 @@ public: float createTime = b3GetMillisecondsAndReset( &startTick ); float scaledArea = 0.0f; - for (int i = 0; i < trials; ++i) + for ( int i = 0; i < trials; ++i ) { b3Hull* hull = b3CloneAndTransformHull( m_hull, b3Transform_identity, m_scale ); scaledArea += hull->surfaceArea; @@ -1135,7 +1189,7 @@ public: explicit BenchmarkChains( SampleContext* context ) : Sample( context ) { - if (context->restart == false) + if ( context->restart == false ) { m_camera->SetView( 0.0f, 15.0f, 50.0f, { 0.0f, 5.0f, 0.0f } ); EnableGrid( context->scene, false ); @@ -1168,12 +1222,12 @@ public: int shapeIndex = 0; float x = -1.0f * m_gridCount; - for (int rowIndex = 0; rowIndex < m_gridCount; ++rowIndex) + for ( int rowIndex = 0; rowIndex < m_gridCount; ++rowIndex ) { float z = -1.0f * m_gridCount; - for (int columnIndex = 0; columnIndex < m_gridCount; ++columnIndex) + for ( int columnIndex = 0; columnIndex < m_gridCount; ++columnIndex ) { - for (int i = 0; i < linkCount; ++i) + for ( int i = 0; i < linkCount; ++i ) { bodyDef.position = { x, ( 1.0f - 2.0f * i ) * linkExtent + 3.0f, z }; @@ -1182,13 +1236,13 @@ public: b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); b3ShapeId shapeId = b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); - if (i == linkCount - 1) + if ( i == linkCount - 1 ) { m_shapeIds[shapeIndex] = shapeId; shapeIndex += 1; } - if (i > 0) + if ( i > 0 ) { jointDef.base.bodyIdB = bodyId; b3CreateSphericalJoint( m_worldId, &jointDef ); @@ -1218,7 +1272,7 @@ public: b3Vec3 direction = b3GetLengthAndNormalize( &speed, baseWind ); b3Vec3 wind = b3MulSV( speed, b3Add( direction, m_noise ) ); - for (int i = 0; i < m_gridCount * m_gridCount; ++i) + for ( int i = 0; i < m_gridCount * m_gridCount; ++i ) { b3Shape_ApplyWind( m_shapeIds[i], wind, 1.0f, 1.0f, 20.0f, false ); } @@ -1248,9 +1302,9 @@ public: explicit BenchmarkDestruction( SampleContext* context ) : Sample( context ) { - if (m_context->restart == false) + if ( m_context->restart == false ) { - if (m_small) + if ( m_small ) { m_camera->SetView( 0.0f, 40.0f, 10.0f, { 0.0f, 0.0f, 0.0f } ); } @@ -1339,7 +1393,7 @@ public: { uint64_t ticks = b3GetTicks(); - for (int i = 0; i < m_bodyCount; ++i) + for ( int i = 0; i < m_bodyCount; ++i ) { b3DestroyBody( m_bodyIds[i] ); } @@ -1361,13 +1415,13 @@ public: int randomRange = m_small ? 3 : 2; m_bodyCount = 0; - for (int i = 0; i < m_gridCount; ++i) + for ( int i = 0; i < m_gridCount; ++i ) { - for (int j = 0; j < m_gridCount; ++j) + for ( int j = 0; j < m_gridCount; ++j ) { - for (int k = 0; k < m_gridCount; ++k) + for ( int k = 0; k < m_gridCount; ++k ) { - if (RandomIntRange( 1, randomRange ) == 1) + if ( RandomIntRange( 1, randomRange ) == 1 ) { continue; } @@ -1411,7 +1465,7 @@ public: Sample::Step(); int spawnStep = m_small ? 80 : 140; - if (m_stepCount % spawnStep == 0) + if ( m_stepCount % spawnStep == 0 ) { DestroyBodies(); Spawn(); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index 7fb9587..2b9871b 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -597,7 +597,7 @@ void CreateCompoundCapsules( b3WorldId worldId ) 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 ); + // 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 }; @@ -607,8 +607,8 @@ void CreateCompoundCapsules( b3WorldId worldId ) float r = 0.75f; float l = 1.5f; float offset = 0.05f; - //float stubX = 1.4f; - //float stubZ = 0.0f; + // 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 }; @@ -659,11 +659,11 @@ void DestroyCompoundCapsules( void ) struct { b3MeshData* meshData; -} g_compoundHullsData; +} g_treeData; -void CreateCompoundHulls( b3WorldId worldId ) +static void CreateTrees( b3WorldId worldId, int scale ) { - memset( &g_compoundHullsData, 0, sizeof( g_compoundHullsData ) ); + memset( &g_treeData, 0, sizeof( g_treeData ) ); // float tilt = 0.15f * B3_PI; float tilt = 0.0f * B3_PI; @@ -672,24 +672,24 @@ void CreateCompoundHulls( b3WorldId worldId ) bodyDef.rotation = b3MakeQuatFromAxisAngle( (b3Vec3){ 1.0f, 0.0f, 0.0 }, tilt ); b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); - int xCount = 200; + int xCount = scale * 150; // int zCount = BENCHMARK_DEBUG ? 100 : 400; - int zCount = 400; + int zCount = scale * 200; - float cellWidth = 1.0f; + float cellWidth = 1.0f / scale; float amplitude = 0.4f; float rowHz = 0.05f; float columnHz = 0.1f; - g_compoundHullsData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); + g_treeData.meshData = b3CreateWaveMesh( xCount, zCount, cellWidth, amplitude, rowHz, columnHz ); b3ShapeDef shapeDef = b3DefaultShapeDef(); - b3CreateMeshShape( groundId, &shapeDef, g_compoundHullsData.meshData, b3Vec3_one ); + b3CreateMeshShape( groundId, &shapeDef, g_treeData.meshData, b3Vec3_one ); bodyDef.type = b3_dynamicBody; bodyDef.sleepThreshold = 0.2f; bodyDef.rotation = b3Quat_identity; - int bodyCount = BENCHMARK_DEBUG ? 10 : 90; + int bodyCount = BENCHMARK_DEBUG ? 10 : 50; shapeDef.baseMaterial.friction = 0.9f; shapeDef.baseMaterial.rollingResistance = 0.05f; @@ -710,7 +710,7 @@ void CreateCompoundHulls( b3WorldId worldId ) } float angularVelocity = -0.5f; - float z = BENCHMARK_DEBUG ? -15.0f : -140.0f; + float z = BENCHMARK_DEBUG ? -15.0f : -70.0f; b3CosSin cs = b3ComputeCosSin( tilt ); float yTilt = cs.sine / cs.cosine; for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) @@ -748,10 +748,25 @@ void CreateCompoundHulls( b3WorldId worldId ) } } -void DestroyCompoundHulls( void ) +void CreateTrees25( b3WorldId worldId ) { - b3DestroyMesh( g_compoundHullsData.meshData ); - memset( &g_compoundHullsData, 0, sizeof( g_compoundHullsData ) ); + CreateTrees( worldId, 4 ); +} + +void CreateTrees50( b3WorldId worldId ) +{ + CreateTrees( worldId, 2 ); +} + +void CreateTrees100( b3WorldId worldId ) +{ + CreateTrees( worldId, 1 ); +} + +void DestroyTrees( void ) +{ + b3DestroyMesh( g_treeData.meshData ); + memset( &g_treeData, 0, sizeof( g_treeData ) ); } struct diff --git a/shared/benchmarks.h b/shared/benchmarks.h index 8f07af5..561e683 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -17,8 +17,10 @@ extern "C" void CreateCompoundCapsules( b3WorldId worldId ); void DestroyCompoundCapsules( void ); -void CreateCompoundHulls( b3WorldId worldId ); -void DestroyCompoundHulls( void ); +void CreateTrees25( b3WorldId worldId ); +void CreateTrees50( b3WorldId worldId ); +void CreateTrees100( b3WorldId worldId ); +void DestroyTrees( void ); void CreateJointGrid( b3WorldId worldId ); void CreateJunkyard( b3WorldId worldId ); void StepJunkyard( b3WorldId worldId, int stepCount ); diff --git a/src/algorithm.h b/src/algorithm.h index 67d4b46..56dac32 100644 --- a/src/algorithm.h +++ b/src/algorithm.h @@ -5,11 +5,7 @@ #include -#ifdef __cplusplus -#define B3_TYPE_OF( A ) decltype( A ) -#else #define B3_TYPE_OF( A ) __typeof__( A ) -#endif #define B3_SWAP( x, y ) \ do \ diff --git a/src/arena_allocator.c b/src/arena_allocator.c index b7031f0..1de2db5 100644 --- a/src/arena_allocator.c +++ b/src/arena_allocator.c @@ -113,15 +113,67 @@ int b3GetMaxStackAllocation( b3Stack* stack ) b3Arena b3CreateArena( int capacity ) { int c = capacity > 8 ? capacity : 8; + b3ArenaSharedState* shared = (b3ArenaSharedState*)b3Alloc( sizeof( b3ArenaSharedState ) ); + *shared = (b3ArenaSharedState){ 0 }; + return (b3Arena){ - .memory = b3Alloc( c ), + .memory = (char*)b3Alloc( c ), .capacity = c, .index = 0, + .shared = shared, }; } -void b3DestroyArena(b3Arena* arena) +void b3DestroyArena( b3Arena* arena ) { + b3ArenaSharedState* shared = arena->shared; + if ( shared != NULL ) + { + for ( int i = 0; i < shared->overflows.count; ++i ) + { + b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size ); + } + b3Array_Destroy( shared->overflows ); + b3Free( shared, sizeof( b3ArenaSharedState ) ); + } b3Free( arena->memory, arena->capacity ); *arena = (b3Arena){ 0 }; } + +void* b3ArenaOverflowAlloc( b3Arena* arena, int size ) +{ + b3ArenaSharedState* shared = arena->shared; + char* data = (char*)b3Alloc( size ); + b3OverflowBlock block = { data, size }; + b3Array_Push( shared->overflows, block ); + shared->overflowBytes += size; + return data; +} + +void b3ArenaSync( b3Arena* arena ) +{ + b3ArenaSharedState* shared = arena->shared; + + for ( int i = 0; i < shared->overflows.count; ++i ) + { + b3Free( shared->overflows.data[i].data, shared->overflows.data[i].size ); + } + b3Array_Clear( shared->overflows ); + + int demand = shared->maxIndex + shared->overflowBytes; + if ( demand > shared->peakDemand ) + { + shared->peakDemand = demand; + } + if ( demand > arena->capacity ) + { + b3Free( arena->memory, arena->capacity ); + int newCapacity = demand + demand / 2; + arena->memory = (char*)b3Alloc( newCapacity ); + arena->capacity = newCapacity; + } + + arena->index = 0; + shared->maxIndex = 0; + shared->overflowBytes = 0; +} diff --git a/src/arena_allocator.h b/src/arena_allocator.h index f5e59f9..7a57e5c 100644 --- a/src/arena_allocator.h +++ b/src/arena_allocator.h @@ -3,6 +3,7 @@ #pragma once #include "box3d/base.h" +#include "container.h" #include #include @@ -35,17 +36,37 @@ typedef struct b3Stack int entryCount; } b3Stack; +// Heap-allocated fallback block tracked when an arena bump overflows. +typedef struct b3OverflowBlock +{ + char* data; + int size; +} b3OverflowBlock; + +b3DeclareArray( b3OverflowBlock ); + +// Shared, heap-allocated state co-owned by every copy of a b3Arena. +// b3Arena is passed by value so its bump pointer auto-restores on +// function return, but overflow tracking and watermarks must persist +// across copies -- hence this pointer-shared block. +typedef struct b3ArenaSharedState +{ + b3ArrayC( b3OverflowBlock ) overflows; + int maxIndex; // high water mark of the bump pointer this step + int overflowBytes; // total bytes in overflow blocks this step + int peakDemand; // all-time peak of (maxIndex + overflowBytes), survives sync +} b3ArenaSharedState; + typedef struct b3Arena { char* memory; int capacity; int index; + b3ArenaSharedState* shared; } b3Arena; -#ifdef __cplusplus -extern "C" -{ -#endif +// 16-byte alignment for SSE2 + typical struct alignment. +#define B3_ARENA_ALIGNMENT 16 b3Stack b3CreateStack( int capacity ); void b3DestroyStack( b3Stack* stack ); @@ -63,24 +84,31 @@ int b3GetMaxStackAllocation( b3Stack* stack ); b3Arena b3CreateArena( int capacity ); void b3DestroyArena( b3Arena* arena ); +// Heap-allocate an overflow block, register it in the shared state, return it. +void* b3ArenaOverflowAlloc( b3Arena* arena, int size ); + +// Call between simulation steps. Frees this step's overflow blocks and grows +// the backing capacity if last step's demand (maxIndex + overflowBytes) exceeded it. +void b3ArenaSync( b3Arena* arena ); + static inline void* b3Bump( b3Arena* arena, int size ) { - if (size == 0) + if ( size == 0 ) { return NULL; } - if ( size + arena->index > arena->capacity ) + int aligned = ( arena->index + ( B3_ARENA_ALIGNMENT - 1 ) ) & ~( B3_ARENA_ALIGNMENT - 1 ); + + if ( aligned + size > arena->capacity ) { - B3_ASSERT( false ); - return NULL; + return b3ArenaOverflowAlloc( arena, size ); } - void* memory = arena->memory + arena->index; - arena->index += size; - return memory; + arena->index = aligned + size; + if ( arena->index > arena->shared->maxIndex ) + { + arena->shared->maxIndex = arena->index; + } + return arena->memory + aligned; } - -#ifdef __cplusplus -} -#endif diff --git a/src/contact.c b/src/contact.c index ac3929f..50b52dc 100644 --- a/src/contact.c +++ b/src/contact.c @@ -823,8 +823,6 @@ bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Sha contact->flags &= ~b3_simEnableHitEvent; } - // B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) - // ); B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) ); } else diff --git a/src/convex_manifold.c b/src/convex_manifold.c index 6639539..ff5ee1f 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -1182,7 +1182,6 @@ static bool b3BuildFaceAContact( b3LocalManifold* manifold, int capacity, const // Old way of pushing onto the reference face. // b3Vec3 point = clipPoint->position - clipPoint->separation * refPlane.normal; - pt->normal = manifold->normal; pt->point = point; pt->separation = clipPoint->separation; pt->pair = clipPoint->pair; diff --git a/src/hull.c b/src/hull.c index 47f0d39..6648b79 100644 --- a/src/hull.c +++ b/src/hull.c @@ -92,9 +92,9 @@ struct b3QHFace typedef struct b3HorizonFrame { b3QHFace* face; - b3QHHalfEdge* startEdge; // ring termination sentinel - b3QHHalfEdge* edge; // next edge to process - bool started; // false until the first edge of this ring has been processed + b3QHHalfEdge* startEdge; // ring termination sentinel + b3QHHalfEdge* edge; // next edge to process + bool started; // false until the first edge of this ring has been processed } b3HorizonFrame; // All working memory for one hull build, carved from a single b3Alloc block. @@ -194,7 +194,7 @@ static inline void b3QHList_PushBack( b3QHListNode* head, b3QHListNode* node ) b3QHList_Insert( node, head->prev ); } -static b3QHVertex* b3HullBuilder_NewVertex( b3HullBuilder* b, b3Vec3 position) +static b3QHVertex* b3HullBuilder_NewVertex( b3HullBuilder* b, b3Vec3 position ) { B3_ASSERT( b->vertexCount < b->vertexCapacity ); b3QHVertex* vertex = b->vertexBase + b->vertexCount++; @@ -813,8 +813,7 @@ static void b3HullBuilder_DrainConflictList( b3HullBuilder* b, b3QHFace* face ) // `entryEdge` is the half-edge in `face` whose twin lies in the just-deleted parent face, or // NULL for the seed. The frame skips `entryEdge` on recursive entries (it would be ignored // anyway since the parent's mark is now DELETE, but skipping saves one iteration). -static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, - b3HorizonFrame* frameOut ) +static void b3HullBuilder_EnterHorizonFace( b3HullBuilder* b, b3QHFace* face, b3QHHalfEdge* entryEdge, b3HorizonFrame* frameOut ) { face->mark = B3_MARK_DELETE; b3HullBuilder_DrainConflictList( b, face ); @@ -930,9 +929,9 @@ static void b3HullBuilder_ConnectEdges( b3HullBuilder* b, b3QHHalfEdge* prev, b3 if ( b3VertexCountOfFace( prev->twin->face ) == 3 ) { // Capture all 3 half-edges of the dead triangle before the rewire overwrites prev->twin. - b3QHHalfEdge* deadEdge0 = prev->twin; // prev->twin (will be rewired below) - b3QHHalfEdge* deadEdge1 = next->twin; // next->twin - b3QHHalfEdge* deadEdge2 = next->twin->prev; // third edge of the dead triangle + b3QHHalfEdge* deadEdge0 = prev->twin; // prev->twin (will be rewired below) + b3QHHalfEdge* deadEdge1 = next->twin; // next->twin + b3QHHalfEdge* deadEdge2 = next->twin->prev; // third edge of the dead triangle twin = deadEdge2->twin; B3_ASSERT( twin->face->mark != B3_MARK_DELETE ); @@ -991,7 +990,6 @@ static void b3HullBuilder_ConnectEdges( b3HullBuilder* b, b3QHHalfEdge* prev, b3 } } - static void b3HullBuilder_AbsorbFaces( b3HullBuilder* b, b3QHFace* face ) { for ( int i = 0; i < b->mergedFacesCount; ++i ) @@ -1449,15 +1447,10 @@ static bool b3HullBuilder_Construct( b3HullBuilder* b, const b3Vec3* points, int return b3HullBuilder_HasHull( b ); } -static inline size_t b3AlignUp8( size_t x ) -{ - return ( x + 7u ) & ~(size_t)7u; -} - typedef struct b3HullWorkSizes { - int N; // pointCount - int M; // clamped maxVertexCount, in [4, B3_HULL_LIMIT] + int N; // pointCount + int M; // clamped maxVertexCount, in [4, B3_HULL_LIMIT] int vertexCapacity; int edgeCapacity; int faceCapacity; @@ -2020,21 +2013,21 @@ b3Hull* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) return NULL; } - if (builder.finalVertexCount >= B3_HULL_LIMIT) + if ( builder.finalVertexCount >= B3_HULL_LIMIT ) { b3Log( "hull final vertex count of %d exceeds limit of %d", builder.finalVertexCount, B3_HULL_LIMIT ); b3Free( work, sizes.totalBytes ); return NULL; } - if (builder.finalFaceCount >= B3_HULL_LIMIT) + if ( builder.finalFaceCount >= B3_HULL_LIMIT ) { b3Log( "hull final face count of %d exceeds limit of %d", builder.finalFaceCount, B3_HULL_LIMIT ); b3Free( work, sizes.totalBytes ); return NULL; } - if (builder.finalHalfEdgeCount >= B3_HULL_LIMIT) + if ( builder.finalHalfEdgeCount >= B3_HULL_LIMIT ) { b3Log( "hull final half edge count of %d exceeds limit of %d", builder.finalHalfEdgeCount, B3_HULL_LIMIT ); b3Free( work, sizes.totalBytes ); @@ -2087,17 +2080,17 @@ b3Hull* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) } // Allocate the hull. Arrays hang off the end. - int byteCount = (int)sizeof( b3Hull ); - int vertexOffset = byteCount; - byteCount += vertexCount * (int)sizeof( b3HullVertex ); - int pointOffset = byteCount; - byteCount += vertexCount * (int)sizeof( b3Vec3 ); - int edgeOffset = byteCount; - byteCount += edgeCount * (int)sizeof( b3HullHalfEdge ); - int faceOffset = byteCount; - byteCount += faceCount * (int)sizeof( b3HullFace ); - int planeOffset = byteCount; - byteCount += faceCount * (int)sizeof( b3Plane ); + size_t byteCount = b3AlignUp8( sizeof( b3Hull ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3HullVertex ) ); + int pointOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * (int)sizeof( b3Vec3 ) ); + int edgeOffset = (int)byteCount; + byteCount += b3AlignUp8( edgeCount * (int)sizeof( b3HullHalfEdge ) ); + int faceOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3HullFace ) ); + int planeOffset = (int)byteCount; + byteCount += b3AlignUp8( faceCount * (int)sizeof( b3Plane ) ); b3Hull* hull = b3Alloc( byteCount ); memset( hull, 0, byteCount ); @@ -2113,7 +2106,7 @@ b3Hull* b3CreateHull( const b3Vec3* points, int pointCount, int maxVertexCount ) hull->edgeCount = edgeCount; hull->faceCount = faceCount; - hull->byteCount = byteCount; + hull->byteCount = (int)byteCount; b3HullVertex* vertices = b3GetHullVerticesWrite( hull ); b3HullHalfEdge* edges = b3GetHullEdgesWrite( hull ); @@ -2550,63 +2543,49 @@ float b3ComputeHullProjectedArea( const b3Hull* hull, b3Vec3 direction ) // Constant template box (vertex/edge/face/topology). b3MakeTransformedBoxHull copies and // fills in the runtime-dependent fields (boxPoints, boxPlanes, aabb, mass properties, hash). static const b3BoxHull s_boxHull = { - .base = { - .version = B3_HULL_VERSION, - .byteCount = sizeof( b3BoxHull ), - .hash = 0, - .vertexCount = 8, - .edgeCount = 24, - .faceCount = 6, - .vertexOffset = offsetof( b3BoxHull, boxVertices ), - .pointOffset = offsetof( b3BoxHull, boxPoints ), - .edgeOffset = offsetof( b3BoxHull, boxEdges ), - .faceOffset = offsetof( b3BoxHull, boxFaces ), - .planeOffset = offsetof( b3BoxHull, boxPlanes ), - }, - .boxVertices = { - [0] = { .edge = 8 }, - [1] = { .edge = 1 }, - [2] = { .edge = 0 }, - [3] = { .edge = 9 }, - [4] = { .edge = 13 }, - [5] = { .edge = 3 }, - [6] = { .edge = 5 }, - [7] = { .edge = 11 }, - }, - .boxEdges = { - [0] = { 2, 1, 2, 0 }, - [1] = { 17, 0, 1, 5 }, - [2] = { 4, 3, 1, 0 }, - [3] = { 20, 2, 5, 3 }, - [4] = { 6, 5, 5, 0 }, - [5] = { 23, 4, 6, 4 }, - [6] = { 0, 7, 6, 0 }, - [7] = { 18, 6, 2, 2 }, - [8] = { 10, 9, 0, 1 }, - [9] = { 21, 8, 3, 5 }, - [10] = { 12, 11, 3, 1 }, - [11] = { 16, 10, 7, 2 }, - [12] = { 14, 13, 7, 1 }, - [13] = { 19, 12, 4, 4 }, - [14] = { 8, 15, 4, 1 }, - [15] = { 22, 14, 0, 3 }, - [16] = { 7, 17, 3, 2 }, - [17] = { 9, 16, 2, 5 }, - [18] = { 11, 19, 6, 2 }, - [19] = { 5, 18, 7, 4 }, - [20] = { 15, 21, 1, 3 }, - [21] = { 1, 20, 0, 5 }, - [22] = { 3, 23, 4, 3 }, - [23] = { 13, 22, 5, 4 }, - }, - .boxFaces = { - [0] = { .edge = 0 }, - [1] = { .edge = 8 }, - [2] = { .edge = 16 }, - [3] = { .edge = 20 }, - [4] = { .edge = 19 }, - [5] = { .edge = 21 }, - }, + .base = + { + .version = B3_HULL_VERSION, + .byteCount = sizeof( b3BoxHull ), + .hash = 0, + .vertexCount = 8, + .edgeCount = 24, + .faceCount = 6, + .vertexOffset = offsetof( b3BoxHull, boxVertices ), + .pointOffset = offsetof( b3BoxHull, boxPoints ), + .edgeOffset = offsetof( b3BoxHull, boxEdges ), + .faceOffset = offsetof( b3BoxHull, boxFaces ), + .planeOffset = offsetof( b3BoxHull, boxPlanes ), + }, + .boxVertices = + { + [0] = { .edge = 8 }, + [1] = { .edge = 1 }, + [2] = { .edge = 0 }, + [3] = { .edge = 9 }, + [4] = { .edge = 13 }, + [5] = { .edge = 3 }, + [6] = { .edge = 5 }, + [7] = { .edge = 11 }, + }, + .boxEdges = + { + [0] = { 2, 1, 2, 0 }, [1] = { 17, 0, 1, 5 }, [2] = { 4, 3, 1, 0 }, [3] = { 20, 2, 5, 3 }, + [4] = { 6, 5, 5, 0 }, [5] = { 23, 4, 6, 4 }, [6] = { 0, 7, 6, 0 }, [7] = { 18, 6, 2, 2 }, + [8] = { 10, 9, 0, 1 }, [9] = { 21, 8, 3, 5 }, [10] = { 12, 11, 3, 1 }, [11] = { 16, 10, 7, 2 }, + [12] = { 14, 13, 7, 1 }, [13] = { 19, 12, 4, 4 }, [14] = { 8, 15, 4, 1 }, [15] = { 22, 14, 0, 3 }, + [16] = { 7, 17, 3, 2 }, [17] = { 9, 16, 2, 5 }, [18] = { 11, 19, 6, 2 }, [19] = { 5, 18, 7, 4 }, + [20] = { 15, 21, 1, 3 }, [21] = { 1, 20, 0, 5 }, [22] = { 3, 23, 4, 3 }, [23] = { 13, 22, 5, 4 }, + }, + .boxFaces = + { + [0] = { .edge = 0 }, + [1] = { .edge = 8 }, + [2] = { .edge = 16 }, + [3] = { .edge = 20 }, + [4] = { .edge = 19 }, + [5] = { .edge = 21 }, + }, }; b3BoxHull b3MakeTransformedBoxHull( float hx, float hy, float hz, b3Transform transform ) diff --git a/src/math_internal.h b/src/math_internal.h index 40b8d54..7ba4337 100644 --- a/src/math_internal.h +++ b/src/math_internal.h @@ -43,11 +43,6 @@ typedef struct b3ShapeExtent b3Vec3 maxExtent; } b3ShapeExtent; -#ifdef __cplusplus -extern "C" -{ -#endif - b3TrianglePoint b3ClosestPointOnTriangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, b3Vec3 q ); float b3IntersectSegmentTriangle( b3Vec3 p, b3Vec3 q, b3Vec3 a, b3Vec3 b, b3Vec3 c ); @@ -69,19 +64,20 @@ int b3GetPointSupport( const b3Vec3* points, int count, b3Vec3 axis ); bool b3IntersectRayAndAABB( b3Vec3 lowerBound, b3Vec3 upperBound, b3Vec3 p1, b3Vec3 p2, float* minFraction, float* maxFraction ); -#ifdef __cplusplus +static inline size_t b3AlignUp8( size_t x ) +{ + return ( x + 7u ) & ~(size_t)7u; } -#endif // https://en.wikipedia.org/wiki/Floor_and_ceiling_functions -B3_INLINE int b3CeilingInt( int numerator, int denominator ) +static inline int b3CeilingInt( int numerator, int denominator ) { B3_VALIDATE( denominator > 0 ); return ( numerator + denominator - 1 ) / denominator; } // Assumes denominator == 2^exponent -B3_INLINE int b3CeilingPow2( int numerator, int denominator, int exponent ) +static inline int b3CeilingPow2( int numerator, int denominator, int exponent ) { B3_VALIDATE( exponent > 0 && ( denominator == 1 << exponent ) ); return ( numerator + denominator - 1 ) >> exponent; @@ -89,22 +85,22 @@ B3_INLINE int b3CeilingPow2( int numerator, int denominator, int exponent ) bool b3IsSweepNormalized( b3Sweep* sweep ); -B3_INLINE float b3Dot2( b3Vec2 v1, b3Vec2 v2 ) +static inline float b3Dot2( b3Vec2 v1, b3Vec2 v2 ) { return v1.x * v2.x + v1.y * v2.y; } -B3_INLINE float b3Length2( b3Vec2 v ) +static inline float b3Length2( b3Vec2 v ) { return sqrtf( b3Dot2( v, v ) ); } -B3_INLINE float b3LengthSquared2( b3Vec2 v ) +static inline float b3LengthSquared2( b3Vec2 v ) { return b3Dot2( v, v ); } -B3_INLINE b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) +static inline b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) { b3Vec2 v; v.x = b3MinFloat( v1.x, v2.x ); @@ -112,7 +108,7 @@ B3_INLINE b3Vec2 b3MinVec2( b3Vec2 v1, b3Vec2 v2 ) return v; } -B3_INLINE b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) +static inline b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) { b3Vec2 v; v.x = b3MaxFloat( v1.x, v2.x ); @@ -120,14 +116,14 @@ B3_INLINE b3Vec2 b3MaxVec2( b3Vec2 v1, b3Vec2 v2 ) return v; } -B3_INLINE void b3Store( float* dst, b3Vec3 src ) +static inline void b3Store( float* dst, b3Vec3 src ) { dst[0] = src.x; dst[1] = src.y; dst[2] = src.z; } -B3_INLINE b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) +static inline b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) { float lengthSq = b3LengthSquared( v ); if ( lengthSq <= maxLength * maxLength ) @@ -140,7 +136,7 @@ B3_INLINE b3Vec3 b3ClampLength( b3Vec3 v, float maxLength ) } // Assume v is a unit vector -B3_INLINE b3Vec3 b3ArbitraryPerp( b3Vec3 v ) +static inline b3Vec3 b3ArbitraryPerp( b3Vec3 v ) { // Suppose vector a has all equal components and is a unit vector: a = (s, s, s) // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component @@ -185,7 +181,7 @@ B3_INLINE b3Vec3 b3ArbitraryPerp( b3Vec3 v ) return b3Normalize( p ); } -B3_INLINE b3Quat b3QuatFromExponentialMap( b3Vec3 v ) +static inline b3Quat b3QuatFromExponentialMap( b3Vec3 v ) { // Exponential map (Grassia) float threshold = 0.018581361f; @@ -208,7 +204,7 @@ B3_INLINE b3Quat b3QuatFromExponentialMap( b3Vec3 v ) /// @param q1 initial rotation /// @param deltaRotation the angular displacement vector in radians (angular velocity multiplied by the time step) /// q2 = q1 + 0.5 * omega * q1 -B3_INLINE b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) +static inline b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) { // https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ b3Quat qd = { b3MulSV( 0.5f, deltaRotation ), 0.0f }; @@ -222,7 +218,7 @@ B3_INLINE b3Quat b3IntegrateRotation( b3Quat q1, b3Vec3 deltaRotation ) // Pseudo angular velocity from a quaternion target // w = 2 * (target - q) * conj(q) -B3_INLINE b3Vec3 b3DeltaQuatToRotation(b3Quat q, b3Quat target) +static inline b3Vec3 b3DeltaQuatToRotation(b3Quat q, b3Quat target) { b3Quat s = q; if (b3DotQuat(q, target) < 0.0f) @@ -236,7 +232,7 @@ B3_INLINE b3Vec3 b3DeltaQuatToRotation(b3Quat q, b3Quat target) return b3MulSV( 2.0f, product.v ); } -B3_INLINE float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) +static inline float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) { b3Vec3 d; d.x = b.y * c.z - b.z * c.y; @@ -246,90 +242,90 @@ B3_INLINE float b3ScalarTripleProduct( b3Vec3 a, b3Vec3 b, b3Vec3 c ) } // Get a value by index. Avoid undefined behavior of code like (&v.x)[2]. -B3_INLINE float b3GetByIndex( b3Vec3 v, int index ) +static inline float b3GetByIndex( b3Vec3 v, int index ) { B3_ASSERT( 0 <= index && index < 3 ); float temp[3] = { v.x, v.y, v.z }; return temp[index]; } -B3_INLINE int b3MajorAxis( b3Vec3 v ) +static inline int b3MajorAxis( b3Vec3 v ) { return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); } -B3_INLINE float b3MinElement( b3Vec3 v ) +static inline float b3MinElement( b3Vec3 v ) { return b3MinFloat( v.x, b3MinFloat( v.y, v.z ) ); } -B3_INLINE float b3MaxElement( b3Vec3 v ) +static inline float b3MaxElement( b3Vec3 v ) { return b3MaxFloat( v.x, b3MaxFloat( v.y, v.z ) ); } -B3_INLINE int b3MaxElementIndex( b3Vec3 v ) +static inline int b3MaxElementIndex( b3Vec3 v ) { return v.x < v.y ? ( v.y < v.z ? 2 : 1 ) : ( v.x < v.z ? 2 : 0 ); } -B3_INLINE b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b ) +static inline b3Vec2 b3Add2( b3Vec2 a, b3Vec2 b ) { b3Vec2 c = { a.x + b.x, a.y + b.y }; return c; } -B3_INLINE b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b ) +static inline b3Vec2 b3Sub2( b3Vec2 a, b3Vec2 b ) { b3Vec2 c = { a.x - b.x, a.y - b.y }; return c; } -B3_INLINE b3Vec2 b3Neg2( b3Vec2 v ) +static inline b3Vec2 b3Neg2( b3Vec2 v ) { b3Vec2 c = { -v.x, -v.y }; return c; } -B3_INLINE b3Vec2 b3MulSV2( float s, b3Vec2 v ) +static inline b3Vec2 b3MulSV2( float s, b3Vec2 v ) { b3Vec2 c = { s * v.x, s * v.y }; return c; } // a + s * b -B3_INLINE b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b ) +static inline b3Vec2 b3MulAdd2( b3Vec2 a, float s, b3Vec2 b ) { b3Vec2 c = { a.x + s * b.x, a.y + s * b.y }; return c; } // a - s * b -B3_INLINE b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b ) +static inline b3Vec2 b3MulSub2( b3Vec2 a, float s, b3Vec2 b ) { b3Vec2 c = { a.x - s * b.x, a.y - s * b.y }; return c; } -B3_INLINE float b3Cross2( b3Vec2 a, b3Vec2 b ) +static inline float b3Cross2( b3Vec2 a, b3Vec2 b ) { return a.x * b.y - a.y * b.x; } -B3_INLINE float b3DistanceSquared2( b3Vec2 a, b3Vec2 b ) +static inline float b3DistanceSquared2( b3Vec2 a, b3Vec2 b ) { float dx = b.x - a.x; float dy = b.y - a.y; return dx * dx + dy * dy; } -B3_INLINE b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a ) +static inline b3Vec2 b3MulMV2( b3Matrix2 m, b3Vec2 a ) { b3Vec2 b = { m.cx.x * a.x + m.cy.x * a.y, m.cx.y * a.x + m.cy.y * a.y }; return b; } -B3_INLINE b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) +static inline b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) { b3Matrix2 out; out.cx = b3MulMV2( m1, m2.cx ); @@ -337,12 +333,12 @@ B3_INLINE b3Matrix2 b3MulMM2( b3Matrix2 m1, b3Matrix2 m2 ) return out; } -B3_INLINE float b3Det2( b3Matrix2 m ) +static inline float b3Det2( b3Matrix2 m ) { return m.cx.x * m.cy.y - m.cx.y * m.cy.x; } -B3_INLINE b3Matrix2 b3Invert2( b3Matrix2 m ) +static inline b3Matrix2 b3Invert2( b3Matrix2 m ) { float det = b3Det2( m ); if ( b3AbsFloat( det ) > 1000.0f * FLT_MIN ) @@ -358,7 +354,7 @@ B3_INLINE b3Matrix2 b3Invert2( b3Matrix2 m ) } // Assumes positive semi-definite -B3_INLINE b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) +static inline b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) { float det = b3Det2( m ); if ( det > 1000.0f * FLT_MIN ) @@ -373,7 +369,7 @@ B3_INLINE b3Vec2 b3Solve2( b3Matrix2 m, b3Vec2 b ) return B3_LITERAL( b3Vec2 ){ 0.0f, 0.0f }; } -B3_INLINE b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b) +static inline b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b) { b3Vec3 c; c.x = a.y * b.z + a.z * b.y; @@ -382,14 +378,48 @@ B3_INLINE b3Vec3 b3ModifiedCross(b3Vec3 a, b3Vec3 b) return c; } -B3_INLINE b3Plane b3NormalizePlane( b3Plane plane ) +static inline b3Plane b3NormalizePlane( b3Plane plane ) { float invLength = 1.0f / b3Length( plane.normal ); return B3_LITERAL( b3Plane ){ b3MulSV( invLength, plane.normal ), invLength * plane.offset }; } +static inline b3Plane b3MakePlaneFromNormalAndPoint( b3Vec3 normal, b3Vec3 point ) +{ + return B3_LITERAL( b3Plane ){ normal, b3Dot( normal, point ) }; +} + +static inline b3Plane b3MakePlaneFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) +{ + b3Plane plane; + plane.normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); + plane.normal = b3Normalize( plane.normal ); + plane.offset = b3Dot( plane.normal, point1 ); + return plane; +} + +static inline b3Vec3 b3MakeNormalFromPoints( b3Vec3 point1, b3Vec3 point2, b3Vec3 point3 ) +{ + b3Vec3 normal = b3Cross( b3Sub( point2, point1 ), b3Sub( point3, point1 ) ); + return b3Normalize( normal ); +} + +// normal2 = q * normal1 +// offset2 = dot(normal2, p) + offset1 +static inline b3Plane b3TransformPlane( b3Transform transform, b3Plane plane ) +{ + b3Vec3 normal = b3RotateVector( transform.q, plane.normal ); + return B3_LITERAL( b3Plane ){ normal, plane.offset + b3Dot( normal, transform.p ) }; +} + +/// Signed separation of a point from a plane +static inline float b3PlaneSeparation( b3Plane plane, b3Vec3 point ) +{ + return b3Dot( plane.normal, point ) - plane.offset; +} + // Negative if p is below the triangle v1-v2-v3 -B3_INLINE float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) +static inline float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) { b3Vec3 e1 = b3Sub( v2, v1 ); b3Vec3 e2 = b3Sub( v3, v1 ); @@ -398,116 +428,21 @@ B3_INLINE float b3SignedVolume( b3Vec3 v1, b3Vec3 v2, b3Vec3 v3, b3Vec3 p ) } // todo eliminate this -B3_INLINE bool b3IsWithinSegments( const b3ClosestApproachResult* result ) +static inline bool b3IsWithinSegments( const b3ClosestApproachResult* result ) { return ( 0.0f <= result->lambda1 && result->lambda1 <= 1.0f ) && ( 0.0f <= result->lambda2 && result->lambda2 <= 1.0f ); } -B3_INLINE b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia ) +static inline b3Matrix3 b3RotateInertia( b3Quat q, b3Matrix3 centralInertia ) { b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( q ); b3Matrix3 inertia = b3MulMM( rotationMatrix, b3MulMM( centralInertia, b3Transpose( rotationMatrix ) ) ); return inertia; } -B3_INLINE b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass ) +static inline b3Matrix3 b3TransformInertia( b3Transform transform, b3Matrix3 centralInertia, float mass ) { b3Matrix3 inertia = b3RotateInertia( transform.q, centralInertia ); inertia = b3AddMM( inertia, b3Steiner( mass, transform.p ) ); return inertia; } - -#ifdef __cplusplus - -B3_INLINE b3Vec2& operator+=( b3Vec2& a, b3Vec2 b ) -{ - a.x += b.x; - a.y += b.y; - return a; -} - -B3_INLINE b3Vec2& operator*=( b3Vec2& a, float s ) -{ - a.x *= s; - a.y *= s; - return a; -} - -B3_INLINE b3Vec2 operator-( b3Vec2 v ) -{ - return { -v.x, -v.y }; -} - -B3_INLINE b3Vec2 operator+( b3Vec2 v1, b3Vec2 v2 ) -{ - b3Vec2 out; - out.x = v1.x + v2.x; - out.y = v1.y + v2.y; - - return out; -} - -B3_INLINE b3Vec2 operator-( b3Vec2 v1, b3Vec2 v2 ) -{ - b3Vec2 out; - out.x = v1.x - v2.x; - out.y = v1.y - v2.y; - - return out; -} - -B3_INLINE b3Vec2 operator*( float f, b3Vec2 v ) -{ - b3Vec2 out; - out.x = f * v.x; - out.y = f * v.y; - - return out; -} - -B3_INLINE b3Vec2 operator*( b3Vec2 v, float f ) -{ - b3Vec2 out; - out.x = v.x * f; - out.y = v.y * f; - - return out; -} - -B3_INLINE b3Matrix2 operator+( b3Matrix2 m1, b3Matrix2 m2 ) -{ - b3Matrix2 out; - out.cx = m1.cx + m2.cx; - out.cy = m1.cy + m2.cy; - - return out; -} - -B3_INLINE b3Matrix2 operator-( b3Matrix2 m1, b3Matrix2 m2 ) -{ - b3Matrix2 out; - out.cx = m1.cx - m2.cx; - out.cy = m1.cy - m2.cy; - - return out; -} - -B3_INLINE b3Matrix2 operator*( float f, b3Matrix2 m ) -{ - b3Matrix2 out; - out.cx = f * m.cx; - out.cy = f * m.cy; - - return out; -} - -B3_INLINE b3Matrix2 operator*( b3Matrix2 m, float f ) -{ - b3Matrix2 out; - out.cx = m.cx * f; - out.cy = m.cy * f; - - return out; -} - -#endif diff --git a/src/mesh.c b/src/mesh.c index 80e7b41..e9507a1 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -423,8 +423,7 @@ static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentInd b3Vec3 other = h->vertices[existingIndex]; // IsEqual inlined: check if vertices are within tolerance - if ( fabsf( vertex.x - other.x ) <= tolerance && - fabsf( vertex.y - other.y ) <= tolerance && + if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance && fabsf( vertex.z - other.z ) <= tolerance ) { // Found duplicate @@ -1316,8 +1315,8 @@ b3MeshData* b3CreateWaveMesh( int xCount, int zCount, float cellWidth, float amp float xWidth = cellWidth * xCount; float zWidth = cellWidth * zCount; - float omegaZ = 2.0f * B3_PI * rowFrequency; - float omegaX = 2.0f * B3_PI * columnFrequency; + float omegaZ = 2.0f * B3_PI * rowFrequency * cellWidth; + float omegaX = 2.0f * B3_PI * columnFrequency * cellWidth; float x = -0.5f * xWidth; for ( int ix = 0; ix <= xCount; ++ix ) @@ -1605,25 +1604,25 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, b3BuildRecursive( &tempNodes, triangleCount, primitives.data, primitives.data, def->useMedianSplit, &treeHeight ); // Allocate the mesh - int byteCount = sizeof( b3MeshData ); - int nodeOffset = byteCount; - byteCount += tempNodes.count * sizeof( b3MeshNode ); - int vertexOffset = byteCount; - byteCount += vertexCount * sizeof( b3Vec3 ); - int triangleOffset = byteCount; - byteCount += triangleCount * sizeof( b3MeshTriangle ); - int materialIndicesOffset = byteCount; - byteCount += triangleCount * sizeof( uint8_t ); - int flagsOffset = byteCount; - byteCount += triangleCount * sizeof( uint8_t ); + size_t byteCount = b3AlignUp8( sizeof( b3MeshData ) ); + int nodeOffset = (int)byteCount; + byteCount += b3AlignUp8( tempNodes.count * sizeof( b3MeshNode ) ); + int vertexOffset = (int)byteCount; + byteCount += b3AlignUp8( vertexCount * sizeof( b3Vec3 ) ); + int triangleOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( b3MeshTriangle ) ); + int materialIndicesOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); + int flagsOffset = (int)byteCount; + byteCount += b3AlignUp8( triangleCount * sizeof( uint8_t ) ); - b3MeshData* mesh = (b3MeshData*)b3Alloc( byteCount ); + b3MeshData* mesh = b3Alloc( byteCount ); // zero initialize for determinism memset( mesh, 0, byteCount ); mesh->version = B3_MESH_VERSION; - mesh->byteCount = byteCount; + mesh->byteCount = (int)byteCount; mesh->bounds = meshBounds; mesh->surfaceArea = surfaceArea; mesh->nodeCount = tempNodes.count; @@ -1759,7 +1758,8 @@ bool b3OverlapMesh( const b3Mesh* shape, b3Transform shapeTransform, const b3Sha if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) { // Shape-triangle overlap test in scaled space. Winding order doesn't matter. - b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), b3Mul( meshScale, vertex3 ) }; + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; input.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; // reset the cache @@ -2201,7 +2201,8 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh { // Compute shape distance in scaled space. Winding order doesn't matter. // todo implement one-sided collision? - b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), b3Mul( meshScale, vertex3 ) }; + b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), + b3Mul( meshScale, vertex3 ) }; distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; // reset the cache diff --git a/src/mesh_contact.c b/src/mesh_contact.c index bcb74b4..122cf2f 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -11,6 +11,10 @@ #include +// This guards against excessive memory usage and complex collision +#define B3_MAX_MESH_CONTACT_TRIANGLES 256 +#define B3_MAX_POINTS_PER_TRIANGLE 16 + #if B3_ENABLE_VALIDATION static bool b3IsSorted( const int* array, int count ) { @@ -70,7 +74,7 @@ static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3Heig return context.count; } -static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transform xfA, const b3AABB* bounds, b3Arena arena ) +static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transform xfA, const b3AABB* bounds ) { B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); @@ -98,8 +102,9 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transfo meshContact->queryBounds.upperBound = b3Add( bounds->upperBound, extension ); // Query triangles - int triangleCapacity = 256; - int* triangleIndices = (int*)b3Bump( &arena, triangleCapacity * sizeof( int ) ); + int triangleCapacity = B3_MAX_MESH_CONTACT_TRIANGLES; + + int triangleIndices[B3_MAX_MESH_CONTACT_TRIANGLES]; // Bounds are in world space. Convert to the local mesh frame. b3AABB localBounds = b3AABB_Transform( b3InvertTransform( xfA ), meshContact->queryBounds ); @@ -116,15 +121,19 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3Transfo if ( triangleCount == triangleCapacity ) { - b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); - B3_VALIDATE( false ); + static bool s_once = false; + if ( s_once == false ) + { + b3Log( "WARNING: complex mesh detected, triangle buffer capacity of %d reached", triangleCapacity ); + s_once = true; + } } // Triangle indices must be sorted to match caches. B3_VALIDATE( b3IsSorted( triangleIndices, triangleCount ) ); // Create new contact cache and match with old one - b3ContactCache* contactCache = (b3ContactCache*)b3Bump( &arena, triangleCount * sizeof( b3ContactCache ) ); + b3ContactCache contactCache[B3_MAX_MESH_CONTACT_TRIANGLES]; int index2 = 0; for ( int index1 = 0; index1 < triangleCount; ++index1 ) @@ -430,7 +439,8 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor return count1; } - b3Point2D* pts = (b3Point2D*)b3Bump( &arena, count1 * sizeof( b3Point2D ) ); + // bump MAX_POINTS * MAX_TRI * 16 + b3Point2D* pts = b3Bump( &arena, count1 * sizeof( b3Point2D ) ); b3Vec3 u = b3Perp( normal ); b3Vec3 v = b3Cross( normal, u ); b3Vec3 origin = points[0].point; @@ -446,8 +456,9 @@ static int b3ReduceCluster( b3LocalManifoldPoint* points, int count1, b3Vec3 nor } int count3 = b3CullPoints( pts, count1, 4 ); + B3_ASSERT( count3 <= B3_MAX_MANIFOLD_POINTS ); - b3LocalManifoldPoint* finalPoints = (b3LocalManifoldPoint*)b3Bump( &arena, count3 * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifoldPoint finalPoints[B3_MAX_MANIFOLD_POINTS]; for ( int i = 0; i < count3; ++i ) { int index = pts[i].originalIndex; @@ -478,19 +489,17 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); - // todo test using the shape local aabb - b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb, arena ); + b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb ); // Collide with triangles and build manifolds b3MeshContact* meshContact = &contact->meshContact; int triangleCount = meshContact->triangleCache.count; - b3LocalManifold** acceptedManifolds = (b3LocalManifold**)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int acceptedManifoldCount = 0; - b3LocalManifold** tentativeManifolds = (b3LocalManifold**)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int tentativeManifoldCount = 0; - b3TentativeTriangle* tentativeTriangles = - (b3TentativeTriangle*)b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); int tentativeTriangleCount = 0; b3FoundEdges foundEdges; @@ -503,12 +512,13 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); float linearSlop = B3_LINEAR_SLOP; - int pointBufferCapacity = 256; - b3LocalManifoldPoint* pointBuffer = - (b3LocalManifoldPoint*)b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + // Make room for clip points + int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; + + b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); int totalPointCount = 0; - b3LocalManifold* manifoldBuffer = (b3LocalManifold*)b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); + b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); int manifoldCount = 0; b3TriangleCache* triangleCaches = meshContact->triangleCache.data; @@ -579,7 +589,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact manifoldCount += 1; totalPointCount += manifoldPointCount; manifold->triangleIndex = triangleIndex; - manifold->trianglePlane = b3MakePlaneFromPoints( vertices[0], vertices[1], vertices[2] ); + manifold->triangleNormal = b3MakeNormalFromPoints( vertices[0], vertices[1], vertices[2] ); manifold->i1 = triangle.i1; manifold->i2 = triangle.i2; manifold->i3 = triangle.i3; @@ -597,7 +607,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } else if ( manifold->feature == b3_featureHullFace ) { - float cosNormalAngle = b3Dot( manifold->trianglePlane.normal, manifold->normal ); + float cosNormalAngle = b3Dot( manifold->triangleNormal, manifold->normal ); if ( cosNormalAngle > 0.5f ) { (void)b3AddEdge( &foundEdges, triangle.i1, triangle.i2 ); @@ -788,20 +798,20 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact // seems appropriate in this case. float restOffset = linearSlop; - b3Cluster* clusters = (b3Cluster*)b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); - int* clusterMemberships = (int*)b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); + b3Cluster* clusters = b3Bump( &arena, acceptedManifoldCount * sizeof( b3Cluster ) ); + int* clusterMemberships = b3Bump( &arena, acceptedManifoldCount * sizeof( int ) ); - int clusterCount = 0; // Cluster tolerance is tighter than the warm starting manifold matching tolerance. These // serve different purposes. const float clusterThreshold = 0.996f; - int pointCount2 = 0; + int clusterCount = 0; + int clusterPointCount = 0; for ( int i = 0; i < acceptedManifoldCount; ++i ) { clusterMemberships[i] = B3_NULL_INDEX; const b3LocalManifold* manifold = acceptedManifolds[i]; - pointCount2 += manifold->pointCount; + clusterPointCount += manifold->pointCount; // Cluster based on the triangle normal and contact normal. // The first cluster found is accepted because the tolerance is tight. @@ -810,7 +820,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact // This bool is here to allow quick testing with and without clustering. bool allowClustering = true; b3Vec3 manifoldNormal = manifold->normal; - b3Vec3 triangleNormal = manifold->trianglePlane.normal; + b3Vec3 triangleNormal = manifold->triangleNormal; int clusterIndex = B3_NULL_INDEX; for ( int j = 0; j < clusterCount && allowClustering; ++j ) { @@ -869,13 +879,13 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact } } - if ( pointCount2 == 0 ) + if ( clusterPointCount == 0 ) { return false; } // Setup clusters - b3LocalManifoldPoint* clusterPoints = (b3LocalManifoldPoint*)b3Bump( &arena, pointCount2 * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); int pointOffset = 0; for ( int i = 0; i < clusterCount; ++i ) @@ -905,8 +915,6 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3LocalManifoldPoint* ap = am->points + j; b3LocalManifoldPoint* cp = cm->points + cm->pointCount; - // The contact normal may be different than the cluster normal (triangle normal) - cp->normal = am->normal; cp->triangleIndex = am->triangleIndex; cp->point = ap->point; cp->separation = ap->separation; @@ -929,7 +937,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3Manifold* oldManifolds = NULL; if ( oldManifoldCount > 0 ) { - oldManifolds = (b3Manifold*)b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); + oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); } @@ -949,7 +957,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact bool* consumed = NULL; if ( oldManifoldCount > 0 ) { - consumed = (bool*)b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); + consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); memset( consumed, 0, oldManifoldCount * sizeof( bool ) ); } diff --git a/src/physics_world.c b/src/physics_world.c index ebe53c5..af13b0c 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -117,7 +117,7 @@ static void b3CreateWorkerContexts( b3World* world ) for ( int i = 0; i < world->workerCount; ++i ) { - world->taskContexts.data[i].arena = b3CreateArena( 32 * 1024 ); + world->taskContexts.data[i].arena = b3CreateArena( 128 * 1024 ); b3Array_Reserve( world->taskContexts.data[i].sensorHits, 8 ); world->taskContexts.data[i].contactStateBitSet = b3CreateBitSet( 1024 ); world->taskContexts.data[i].hitEventBitSet = b3CreateBitSet( 1024 ); @@ -836,6 +836,14 @@ static void b3Collide( b3StepContext* context ) } } + // Release per-step overflow blocks and grow the backing capacity if last + // step's demand exceeded it. Contact processing is the only consumer of + // these arenas and is finished by this point. + for ( int i = 0; i < world->workerCount; ++i ) + { + b3ArenaSync( &world->taskContexts.data[i].arena ); + } + int endEventArrayIndex = world->endEventArrayIndex; const b3Shape* shapes = world->shapes.data; @@ -1981,9 +1989,16 @@ b3Counters b3World_GetCounters( b3WorldId worldId ) s.awakeContactCount += world->solverSets.data[b3_awakeSet].contactIndices.count; s.recycledContactCount = 0; + s.arenaCapacity = 0; for ( int i = 0; i < world->workerCount; ++i ) { s.recycledContactCount += world->taskContexts.data[i].recycledContactCount; + + int peak = world->taskContexts.data[i].arena.shared->peakDemand; + if ( peak > s.arenaCapacity ) + { + s.arenaCapacity = peak; + } } return s; diff --git a/src/triangle_manifold.c b/src/triangle_manifold.c index 8c11b95..38726c9 100644 --- a/src/triangle_manifold.c +++ b/src/triangle_manifold.c @@ -915,7 +915,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 b3Plane trianglePlane = b3MakePlaneFromPoints( v1, v2, v3 ); float linearSlop = B3_LINEAR_SLOP; -#if 1 float offset = b3PlaneSeparation( trianglePlane, hullA->center ); if ( cache->type == b3_backsideAxis ) { @@ -935,7 +934,6 @@ void b3CollideHullAndTriangle( b3LocalManifold* manifold, int capacity, const b3 cache->separation = offset; return; } -#endif b3Vec3 triangleCenter = b3MulSV( 1.0f / 3.0f, b3Add( v1, b3Add( v2, v3 ) ) ); b3Vec3 trianglePoints[] = { v1, v2, v3 };