From 8f3b2e12eb6d84c92eb0b8d6cf78726a06969adf Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Tue, 17 Nov 2026 13:51:59 -0800 Subject: [PATCH] Manifold allocation cap (#227) Degenerate scenarios can lead to a huge number of manifolds per contact. This creates memory and performance problems. So I'm capping them to 8 using cheap heuristics for culling. --- benchmark/mesh_soak.c | 94 +++++-- benchmark/soak_baseline.csv | 4 + include/box3d/constants.h | 25 +- include/box3d/types.h | 76 ++--- samples/main.cpp | 14 +- samples/sample.cpp | 13 +- samples/sample.h | 5 + samples/sample_continuous.cpp | 49 +++- samples/sample_issues.cpp | 512 ++++++++++++++++++++++++++++++++++ samples/sample_mesh.cpp | 246 ++++++++++++++++ samples/sample_replay.cpp | 6 + shared/mesh_drop.c | 92 ++++-- shared/mesh_drop.h | 32 ++- src/constraint_graph.c | 2 +- src/mesh_contact.c | 105 ++++++- src/physics_world.c | 11 +- src/physics_world.h | 8 +- src/world_snapshot.c | 3 +- 18 files changed, 1160 insertions(+), 137 deletions(-) diff --git a/benchmark/mesh_soak.c b/benchmark/mesh_soak.c index f696682..dd41e89 100644 --- a/benchmark/mesh_soak.c +++ b/benchmark/mesh_soak.c @@ -6,7 +6,8 @@ // or the pile is still awake at the step limit. Tunnels are rare, so the soak also counts bodies that // sink into the wave past a few depth levels and can compare those counts against a baseline. // Generations run in parallel, one single threaded world per thread. The printed seed reproduces a -// generation in the sample. +// generation in the sample. The rough wave has bumps smaller than the bodies, so mesh contacts reach +// the manifold cap. It is far slower per generation, so run it with fewer of them. #define _CRT_SECURE_NO_WARNINGS @@ -75,8 +76,12 @@ typedef struct Soak int completedCount; int failureCount; - long long totalSteps; + int64_t totalSteps; int maxSteps; + + // Mesh contact updates by manifold count, rough wave only + int64_t manifoldCounts[B3_MAX_MANIFOLDS]; + ShapeStats stats[e_meshDropShapeCount]; Failure failures[MAX_REPORTED_FAILURES]; } Soak; @@ -100,7 +105,7 @@ static void RecordFailure( Soak* soak, const Failure* failure ) // Mirrors MeshDrop::Generate and the auto generate step loop. Returns the step count. static int RunGeneration( Soak* soak, int generation, uint32_t seed, MeshDropShape shape, b3BodyId* bodies, - MeshDropDepth* depth, int* failureKind ) + MeshDropDepth* depth, int* failureKind, int64_t* manifoldCounts ) { b3WorldDef worldDef = b3DefaultWorldDef(); worldDef.workerCount = 1; @@ -115,7 +120,7 @@ static int RunGeneration( Soak* soak, int generation, uint32_t seed, MeshDropSha b3World_SetContactRecycleDistance( worldId, 0.0f ); } - CreateMeshDropGround( worldId, soak->groundMesh ); + CreateMeshDropGround( worldId, soak->groundMesh, soak->settings ); MeshDropSettings settings = soak->settings; settings.shape = shape; @@ -135,6 +140,16 @@ static int RunGeneration( Soak* soak, int generation, uint32_t seed, MeshDropSha UpdateMeshDropDepth( worldId, depth ); step += 1; + // Counters walk the world, so only pay for them where the cap matters + if ( soak->settings.rough ) + { + b3Counters counters = b3World_GetCounters( worldId ); + for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i ) + { + manifoldCounts[i] += counters.manifoldCounts[i]; + } + } + for ( int i = 0; i < MESH_DROP_BODY_COUNT; ++i ) { b3Pos center = b3Body_GetWorldCenter( bodies[i] ); @@ -203,7 +218,8 @@ static void SoakWorker( void* context ) soak->shapeFilter >= 0 ? (MeshDropShape)soak->shapeFilter : (MeshDropShape)( generation % e_meshDropShapeCount ); int failureKind = -1; - int steps = RunGeneration( soak, generation, seed, shape, bodies, depth, &failureKind ); + int64_t manifoldCounts[B3_MAX_MANIFOLDS] = { 0 }; + int steps = RunGeneration( soak, generation, seed, shape, bodies, depth, &failureKind, manifoldCounts ); int depthCounts[MESH_DROP_DEPTH_LEVEL_COUNT]; GetMeshDropDepthCounts( depth, depthCounts ); @@ -212,6 +228,10 @@ static void SoakWorker( void* context ) soak->completedCount += 1; soak->totalSteps += steps; soak->maxSteps = b3MaxInt( soak->maxSteps, steps ); + for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i ) + { + soak->manifoldCounts[i] += manifoldCounts[i]; + } ShapeStats* stats = soak->stats + shape; stats->generationCount += 1; @@ -247,15 +267,24 @@ static void SoakWorker( void* context ) free( bodies ); } -static const char* GetFilterName( int shapeFilter ) +// The rough wave gets its own names so its baseline rows never match a run on the broad wave +static const char* GetFilterName( int shapeFilter, bool rough ) { - return shapeFilter >= 0 ? g_meshDropShapeNames[shapeFilter] : "all"; + static const char* roughNames[e_meshDropShapeCount] = { "rough_box", "rough_capsule", "rough_cylinder", "rough_sphere" }; + + if ( shapeFilter < 0 ) + { + return rough ? "rough_all" : "all"; + } + + return rough ? roughNames[shapeFilter] : g_meshDropShapeNames[shapeFilter]; } // A baseline row starts with the run that made it, so one file can hold several runs static void GetBaselineKey( const Soak* soak, char* key, int size ) { - snprintf( key, size, "%s,%u,%d,", GetFilterName( soak->shapeFilter ), soak->baseSeed, soak->generationCount ); + snprintf( key, size, "%s,%u,%d,", GetFilterName( soak->shapeFilter, soak->settings.rough ), soak->baseSeed, + soak->generationCount ); } // Replaces the rows of this run and keeps the rest @@ -327,8 +356,8 @@ static void WriteBaseline( const Soak* soak, const char* path ) continue; } - fprintf( file, "%s,%u,%d,%s,%d,%d", GetFilterName( soak->shapeFilter ), soak->baseSeed, soak->generationCount, - g_meshDropShapeNames[shape], stats->tunnelCount, stats->awakeCount ); + fprintf( file, "%s,%u,%d,%s,%d,%d", GetFilterName( soak->shapeFilter, soak->settings.rough ), soak->baseSeed, + soak->generationCount, g_meshDropShapeNames[shape], stats->tunnelCount, stats->awakeCount ); for ( int i = 0; i < MESH_DROP_DEPTH_LEVEL_COUNT; ++i ) { fprintf( file, ",%d", stats->depthCounts[i] ); @@ -378,7 +407,7 @@ static int CompareBaseline( const Soak* soak, const char* path ) continue; } - if ( strcmp( filter, GetFilterName( soak->shapeFilter ) ) != 0 || base != soak->baseSeed || + if ( strcmp( filter, GetFilterName( soak->shapeFilter, soak->settings.rough ) ) != 0 || base != soak->baseSeed || generations != soak->generationCount ) { continue; @@ -440,8 +469,8 @@ static int CompareBaseline( const Soak* soak, const char* path ) if ( matchedCount == 0 ) { - printf( "%s has no rows for -s=%s -b=%u -g=%d\n", path, GetFilterName( soak->shapeFilter ), soak->baseSeed, - soak->generationCount ); + printf( "%s has no rows for %s-s=%s -b=%u -g=%d\n", path, soak->settings.rough ? "--rough " : "", + GetFilterName( soak->shapeFilter, false ), soak->baseSeed, soak->generationCount ); return 2; } @@ -474,6 +503,7 @@ int main( int argc, char** argv ) uint32_t seed = (uint32_t)b3GetTicks(); bool fixedSeed = false; bool recycling = true; + bool amplitudeSet = false; const char* writePath = NULL; const char* comparePath = NULL; MeshDropSettings settings = GetDefaultMeshDropSettings(); @@ -525,6 +555,11 @@ int main( int argc, char** argv ) else if ( ( value = MatchValue( arg, "-a=", "--amplitude=" ) ) != NULL ) { settings.amplitude = (float)atof( value ); + amplitudeSet = true; + } + else if ( strcmp( arg, "--rough" ) == 0 ) + { + settings.rough = true; } else if ( ( value = MatchValue( arg, "-w=", "--write=" ) ) != NULL ) { @@ -549,9 +584,10 @@ int main( int argc, char** argv ) " -s, --shape= box, capsule, cylinder, sphere or all (default box)\n" " -b, --base-seed= base seed, each generation hashes its own seed from it (default random)\n" " --seed= run every generation with this exact seed (replay)\n" - " -a, --amplitude= wave amplitude (default 0.5)\n" + " -a, --amplitude= wave amplitude (default 0.5, rough 0.015)\n" + " --rough fine bumps smaller than the bodies, reaches the manifold cap\n" " -w, --write= write the counts as a baseline, replacing only the rows of this run\n" - " -c, --compare= compare the counts with the baseline rows from the same -s, -b and -g\n" + " -c, --compare= compare the counts with the baseline rows from the same wave, -s, -b and -g\n" " --no-recycling disable contact recycling\n" "Exit code is 0 when every generation passes and no count grew past noise, 1 otherwise,\n" "2 for bad arguments or a baseline from a different run.\n", @@ -560,6 +596,11 @@ int main( int argc, char** argv ) } } + if ( settings.rough && amplitudeSet == false ) + { + settings.amplitude = GetMeshDropDefaultAmplitude( true ); + } + if ( fixedSeed && ( writePath != NULL || comparePath != NULL ) ) { printf( "a baseline needs a base seed, not --seed\n" ); @@ -570,7 +611,7 @@ int main( int argc, char** argv ) soak.mutex = b3CreateMutex(); soak.settings = settings; soak.recycling = recycling; - soak.groundMesh = CreateMeshDropMesh( settings.amplitude ); + soak.groundMesh = CreateMeshDropMesh( settings ); soak.cylinder = CreateMeshDropCylinder(); soak.baseSeed = seed; soak.fixedSeed = fixedSeed; @@ -583,8 +624,8 @@ int main( int argc, char** argv ) soak.stats[i].maxGeneration = INT32_MAX; } - printf( "mesh soak: %d generations, %d threads, shape %s, %s seed %u\n", generationCount, threadCount, - GetFilterName( shapeFilter ), fixedSeed ? "fixed" : "base", seed ); + printf( "mesh soak: %d generations, %d threads, %sshape %s, %s seed %u\n", generationCount, threadCount, + settings.rough ? "rough wave, " : "", GetFilterName( shapeFilter, false ), fixedSeed ? "fixed" : "base", seed ); fflush( stdout ); uint64_t ticks = b3GetTicks(); @@ -641,6 +682,23 @@ int main( int argc, char** argv ) printf( "\nsteps to sleep: average %.0f, max %d, time %.1f s\n", averageSteps, soak.maxSteps, seconds ); + if ( settings.rough ) + { + int64_t total = 0; + for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i ) + { + total += soak.manifoldCounts[i]; + } + + // The last bucket is the cap, so it holds every contact that was reduced + printf( "contact updates by manifold count:" ); + for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i ) + { + printf( " %d: %.1f%%", i + 1, total > 0 ? 100.0 * soak.manifoldCounts[i] / total : 0.0 ); + } + printf( "\n" ); + } + int reported = b3MinInt( soak.failureCount, MAX_REPORTED_FAILURES ); for ( int i = 0; i < reported; ++i ) { diff --git a/benchmark/soak_baseline.csv b/benchmark/soak_baseline.csv index ab4be06..f364117 100644 --- a/benchmark/soak_baseline.csv +++ b/benchmark/soak_baseline.csv @@ -7,3 +7,7 @@ all,12345,1000,box,0,0,1503,218,3,0 all,12345,1000,capsule,0,0,14507,3488,28,0 all,12345,1000,cylinder,0,0,10859,2193,36,0 all,12345,1000,sphere,0,0,56,0,0,0 +rough_all,12345,1000,box,0,0,2107,535,6,0 +rough_all,12345,1000,capsule,0,0,3603,177,1,0 +rough_all,12345,1000,cylinder,0,0,3239,726,21,0 +rough_all,12345,1000,sphere,0,0,0,0,0,0 diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 8e8c3e3..8906a12 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -14,9 +14,9 @@ B3_API void b3SetLengthUnitsPerMeter( float lengthUnits ); /// Get the current length units per meter. B3_API float b3GetLengthUnitsPerMeter( void ); -// Used to detect bad values. In float mode positions greater than about 16km have precision -// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther -// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it. +/// Used to detect bad values. In float mode positions greater than about 16km have precision +/// problems, so 100km is a safe limit. Large world mode keeps coordinates accurate much farther +/// from the origin, so the sanity limit widens to keep valid far-field positions from tripping it. #if defined( BOX3D_DOUBLE_PRECISION ) #define B3_HUGE ( 1.0e9f * b3GetLengthUnitsPerMeter() ) #else @@ -32,18 +32,19 @@ B3_API float b3GetLengthUnitsPerMeter( void ); /// size array for Box3D task, which may help with creating stable user task pointers. #define B3_MAX_TASKS 256 -// Maximum number of colors in the constraint graph. Constraints that cannot -// find a color are added to the overflow set which are solved single-threaded. -// The compound barrel benchmark has minor overflow with 24 colors +/// Maximum number of colors in the constraint graph. Constraints that cannot +/// find a color are added to the overflow set which are solved single-threaded. +/// The compound barrel benchmark has minor overflow with 24 colors #define B3_GRAPH_COLOR_COUNT 24 -// Number of contact point buckets for counting the number of contact points per -// shape contact pair. This is just for reporting and doesn't affect simulation. -#define B3_CONTACT_MANIFOLD_COUNT_BUCKETS 8 +/// The maximum number of contact manifolds between shape pairs. Excess manifolds are dropped using heuristics until +/// this number is reached. Culling can lead to contact dither across time steps. Contact recycling mitigates the +/// dithering. +#define B3_MAX_MANIFOLDS 8 -// A small length used as a collision and constraint tolerance. Usually it is -// chosen to be numerically significant, but visually insignificant. In meters. -// @warning modifying this can have a significant impact on stability +/// A small length used as a collision and constraint tolerance. Usually it is +/// chosen to be numerically significant, but visually insignificant. In meters. +/// @warning modifying this can have a significant impact on stability #define B3_LINEAR_SLOP ( 0.005f * b3GetLengthUnitsPerMeter() ) /// The minimum length of a capsules. Very short capsules should be created as spheres diff --git a/include/box3d/types.h b/include/box3d/types.h index b850b7a..df2f1ad 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -573,50 +573,50 @@ B3_API b3ShapeDef b3DefaultShapeDef( void ); /// @ingroup world typedef struct b3Profile { - float step; // - float pairs; // - float collide; // - float solve; // - float solverSetup; // - float constraints; // - float prepareConstraints; // - float integrateVelocities; // - float warmStart; // - float solveImpulses; // - float integratePositions; // - float relaxImpulses; // - float restitution; // - float storeImpulses; // - float splitIslands; // - float transforms; // - float sensorHits; // - float jointEvents; // - float hitEvents; // - float refit; // - float bullets; // - float sleepIslands; // - float sensors; // + float step; ///< + float pairs; ///< + float collide; ///< + float solve; ///< + float solverSetup; ///< + float constraints; ///< + float prepareConstraints; ///< + float integrateVelocities; ///< + float warmStart; ///< + float solveImpulses; ///< + float integratePositions; ///< + float relaxImpulses; ///< + float restitution; ///< + float storeImpulses; ///< + float splitIslands; ///< + float transforms; ///< + float sensorHits; ///< + float jointEvents; ///< + float hitEvents; ///< + float refit; ///< + float bullets; ///< + float sleepIslands; ///< + float sensors; ///< } b3Profile; /// Counters that give details of the simulation size. /// @ingroup world typedef struct b3Counters { - int64_t byteCount; // - int bodyCount; // - int shapeCount; // - int contactCount; // - int jointCount; // - int islandCount; // - int stackUsed; // - int arenaCapacity; // - int staticTreeHeight; // - int treeHeight; // - int satCallCount; // - int satCacheHitCount; // - int taskCount; // - int colorCounts[24]; // - int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; // + int64_t byteCount; ///< + int bodyCount; ///< + int shapeCount; ///< + int contactCount; ///< + int jointCount; ///< + int islandCount; ///< + int stackUsed; ///< + int arenaCapacity; ///< + int staticTreeHeight; ///< + int treeHeight; ///< + int satCallCount; ///< + int satCacheHitCount; ///< + int taskCount; ///< + int colorCounts[24]; ///< + int manifoldCounts[B3_MAX_MANIFOLDS]; ///< /// Number of contacts touched by the collide pass /// graph contacts + awake-set non-touching diff --git a/samples/main.cpp b/samples/main.cpp index 9cc23f1..0fe8a6b 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -49,6 +49,7 @@ static SampleContext s_context; static int s_frame = 0; static int s_frameLimit = -1; static int s_sampleOverride = -1; +static bool s_openReplay = false; static int s_exitCode = 0; static int CompareSamples( const void* a, const void* b ) @@ -135,6 +136,12 @@ static void OnInit( void ) index = s_sampleOverride; } + // Resolved here rather than at parse time because sorting moves the slot. + if ( s_openReplay && g_replayIndex >= 0 ) + { + index = g_replayIndex; + } + SelectSample( &s_context, index, false ); } @@ -535,11 +542,8 @@ static sapp_desc BuildAppDesc( int argc, char** argv ) { const char* path = argv[++i]; - if ( g_replayIndex >= 0 ) - { - snprintf( s_context.replayFile, sizeof( s_context.replayFile ), "%s", path ); - s_sampleOverride = g_replayIndex; - } + snprintf( s_context.replayFile, sizeof( s_context.replayFile ), "%s", path ); + s_openReplay = true; } else if ( strcmp( argv[i], "--zup" ) == 0 ) { diff --git a/samples/sample.cpp b/samples/sample.cpp index 334b8ae..0d16f9a 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -1043,8 +1043,9 @@ void Sample::DrawMetrics() if ( ImGui::BeginTabItem( "Counters" ) ) { - b3Counters s = b3World_GetCounters( m_worldId ); - b3Capacity c = b3World_GetMaxCapacity( m_worldId ); + b3WorldId worldId = GetMetricsWorldId(); + b3Counters s = b3World_GetCounters( worldId ); + b3Capacity c = b3World_GetMaxCapacity( worldId ); constexpr int colorCount = sizeof( s.colorCounts ) / sizeof( s.colorCounts[0] ); constexpr int overflowIndex = colorCount - 1; @@ -1087,9 +1088,8 @@ void Sample::DrawMetrics() ImGui::Text( "toi %d / %d / %d", s.distanceIterations, s.pushBackIterations, s.rootIterations ); ImGui::SetItemTooltip( "TOI distance / push back / root iterations" ); - // The world clamps into the second to last bucket, so the last is always empty and the - // second to last means that many or more - constexpr int manifoldBarCount = manifoldBucketCount - 1; + // One bucket per manifold count, up to the cap + constexpr int manifoldBarCount = manifoldBucketCount; int totalManifolds = 0; int maxManifolds = 0; ImU32 manifoldColors[manifoldBarCount]; @@ -1106,8 +1106,7 @@ void Sample::DrawMetrics() DrawBarChart( "##manifolds", s.manifoldCounts, manifoldColors, manifoldBarCount, maxManifolds, manifoldSize ); if ( hoveredBucket >= 0 ) { - const char* plus = hoveredBucket == manifoldBarCount - 1 ? "+" : ""; - ImGui::SetTooltip( "%d%s manifolds: %d of %d pairs", hoveredBucket + 1, plus, s.manifoldCounts[hoveredBucket], + ImGui::SetTooltip( "%d manifolds: %d of %d pairs", hoveredBucket + 1, s.manifoldCounts[hoveredBucket], totalManifolds ); } diff --git a/samples/sample.h b/samples/sample.h index d6027ff..5e8ff18 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -206,6 +206,11 @@ public: // Bottom metrics drawer (Frame Time / Counters / Renderer). void DrawMetrics(); + virtual b3WorldId GetMetricsWorldId() const + { + return m_worldId; + } + // Append extra tabs to the metrics drawer's tab bar (the Replay viewer adds Timeline). virtual void DrawMetricsTab() { diff --git a/samples/sample_continuous.cpp b/samples/sample_continuous.cpp index fc4d506..770e6f2 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -388,19 +388,23 @@ public: static int sampleNeedleMesh = RegisterSample( "Continuous", "Needle Mesh", NeedleMesh::Create ); +// The rough wave has bumps smaller than the bodies, so a mesh contact reaches the manifold cap. Registered +// a second time as Rough Mesh Drop with that wave on. class MeshDrop : public Sample { public: - explicit MeshDrop( SampleContext* context ) + MeshDrop( SampleContext* context, bool rough ) : Sample( context ) { if ( context->restart == false ) { - m_camera->SetView( 0.0f, 30.0f, 20.0f, b3Pos_zero ); + m_camera->SetView( 0.0f, 30.0f, rough ? 12.0f : 20.0f, b3Pos_zero ); GetGuiDraw()->forceScale = 0.1f; } m_settings = GetDefaultMeshDropSettings(); + m_settings.rough = rough; + m_settings.amplitude = GetMeshDropDefaultAmplitude( rough ); m_groundId = b3_nullBodyId; m_groundMesh = nullptr; CreateGround(); @@ -440,8 +444,8 @@ public: m_groundMesh = nullptr; } - m_groundMesh = CreateMeshDropMesh( m_settings.amplitude ); - m_groundId = CreateMeshDropGround( m_worldId, m_groundMesh ); + m_groundMesh = CreateMeshDropMesh( m_settings ); + m_groundId = CreateMeshDropGround( m_worldId, m_groundMesh, m_settings ); } void Generate() @@ -484,7 +488,17 @@ public: Generate(); } - if ( ImGui::SliderFloat( "Amplitude", &m_settings.amplitude, 0.0f, 1.0f ) ) + if ( ImGui::Checkbox( "Rough", &m_settings.rough ) ) + { + m_settings.amplitude = GetMeshDropDefaultAmplitude( m_settings.rough ); + CreateGround(); + Generate(); + } + + // The rough mesh is slow to build, so wait for the slider to settle + float maxAmplitude = m_settings.rough ? 0.05f : 1.0f; + bool amplitudeChanged = ImGui::SliderFloat( "Amplitude", &m_settings.amplitude, 0.0f, maxAmplitude, "%.3f" ); + if ( m_settings.rough ? ImGui::IsItemDeactivatedAfterEdit() : amplitudeChanged ) { CreateGround(); Generate(); @@ -531,7 +545,8 @@ public: { PickRay pickRay = m_camera->BuildPickRay( m_context->mouseX, m_context->mouseY ); - b3RayResult result = b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); + b3RayResult result = + b3World_CastRayClosest( m_worldId, pickRay.origin, pickRay.translation, b3DefaultQueryFilter() ); if ( result.hit ) { @@ -546,11 +561,21 @@ public: DrawTextLine( "seed %u", m_seed ); + // The last bucket is the cap, so it holds every contact that was reduced + b3Counters counters = b3World_GetCounters( m_worldId ); + int contactCount = 0; + for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i ) + { + contactCount += counters.manifoldCounts[i]; + } + DrawTextLine( "contacts at the manifold cap: %d of %d", counters.manifoldCounts[B3_MAX_MANIFOLDS - 1], + contactCount ); + if ( m_depth.maxIndex >= 0 ) { int index = m_depth.maxIndex; - DrawTextLine( "deepest: (%d, %d) %.1f slop at step %d", index / MESH_DROP_GRID_COUNT, index % MESH_DROP_GRID_COUNT, - m_depth.maxDepth / B3_LINEAR_SLOP, m_depth.maxStep ); + DrawTextLine( "deepest: (%d, %d) %.1f slop at step %d", index / MESH_DROP_GRID_COUNT, + index % MESH_DROP_GRID_COUNT, m_depth.maxDepth / B3_LINEAR_SLOP, m_depth.maxStep ); } float tunnelHeight = GetMeshDropTunnelHeight(); @@ -612,7 +637,12 @@ public: static Sample* Create( SampleContext* context ) { - return new MeshDrop( context ); + return new MeshDrop( context, false ); + } + + static Sample* CreateRough( SampleContext* context ) + { + return new MeshDrop( context, true ); } MeshDropSettings m_settings; @@ -630,6 +660,7 @@ public: }; static int sampleMeshDrop = RegisterSample( "Continuous", "Mesh Drop", MeshDrop::Create ); +static int sampleRoughMeshDrop = RegisterSample( "Continuous", "Rough Mesh Drop", MeshDrop::CreateRough ); // This sample shows that clustering based on the manifold normal can lead to clipping and/or tunneling. class HumpMesh : public Sample diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index 84e5f22..3bc4c5c 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -11,6 +11,7 @@ #include "box3d/box3d.h" #include +#include #include #include #include @@ -2581,3 +2582,514 @@ public: static int sampleRaycastVehicle = RegisterSample( "Vehicle", "Raycast Vehicle", RaycastVehicle::Create ); + +// A user wants the load on a static body C when a box A lands on it, sometimes through a thin sleeping +// plate B. Hit events report approach speed, and a resting plate has none against C, so C gets no hit +// event when B is in the middle. Summing the contact impulses on C each step gives the load either way. +class StaticLoad : public Sample +{ +public: + static constexpr float m_boxHalf = 1.0f; + static constexpr float m_tableTop = 3.0f; + static constexpr float m_dropX = 1.5f; + static constexpr int m_settleSteps = 10; + static constexpr int m_historyCapacity = 240; + static constexpr int m_maxHits = 12; + + explicit StaticLoad( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 15.0f, 22.0f, { 0.0f, 5.0f, 0.0f } ); + } + + // The weld to the wall lives in game code, so Box3D only sees two static bodies + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "wall"; + bodyDef.position = { -4.25f, 4.0f, 0.0f }; + b3BodyId wallId = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 0.25f, 4.0f, 3.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( wallId, &shapeDef, &box.base ); + } + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "C"; + bodyDef.position = { 0.0f, m_tableTop - 1.0f, 0.0f }; + m_bodyC = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 4.0f, 1.0f, 2.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableHitEvents = true; + m_shapeC = b3CreateHullShape( m_bodyC, &shapeDef, &box.base ); + } + + Reset(); + } + + void Reset() + { + if ( B3_IS_NON_NULL( m_bodyA ) ) + { + b3DestroyBody( m_bodyA ); + } + + if ( B3_IS_NON_NULL( m_bodyB ) ) + { + b3DestroyBody( m_bodyB ); + } + + m_bodyA = b3_nullBodyId; + m_bodyB = b3_nullBodyId; + m_shapeA = b3_nullShapeId; + m_shapeB = b3_nullShapeId; + + m_resetStep = m_stepCount; + m_lastStep = m_stepCount; + m_dropped = false; + m_massA = 0.0f; + m_massB = 0.0f; + m_impactStep = -1; + m_impactSpeed = 0.0f; + m_excessImpulse = 0.0f; + m_stepImpulseC = 0.0f; + m_stepImpulseAB = 0.0f; + m_stepImpulseAC = 0.0f; + m_stepImpulseBC = 0.0f; + m_peakForceC = 0.0f; + m_peakForceAB = 0.0f; + m_peakForceAC = 0.0f; + m_peakForceBC = 0.0f; + m_peakStep = -1; + m_hitCountC = 0; + m_hitCountB = 0; + m_hitCount = 0; + m_historyCount = 0; + m_historyIndex = 0; + + if ( m_includeB ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "B"; + bodyDef.position = { m_dropX, m_tableTop + m_plateHalfHeight, 0.0f }; + m_bodyB = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 1.5f, m_plateHalfHeight, 1.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = m_densityB; + shapeDef.enableHitEvents = true; + m_shapeB = b3CreateHullShape( m_bodyB, &shapeDef, &box.base ); + } + else + { + DropA(); + } + } + + // B rests on C long enough to build its contact, then sleeps the way a settled game object would + void DropA() + { + float baseY = m_tableTop; + if ( m_includeB ) + { + if ( m_sleepB ) + { + b3Body_SetAwake( m_bodyB, false ); + } + + baseY += 2.0f * m_plateHalfHeight; + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "A"; + bodyDef.position = { m_dropX, baseY + m_dropHeight + m_boxHalf, 0.0f }; + m_bodyA = b3CreateBody( m_worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( m_boxHalf, m_boxHalf, m_boxHalf ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = m_densityA; + shapeDef.enableHitEvents = true; + m_shapeA = b3CreateHullShape( m_bodyA, &shapeDef, &box.base ); + + m_massA = b3Body_GetMass( m_bodyA ); + m_massB = m_includeB ? b3Body_GetMass( m_bodyB ) : 0.0f; + m_dropped = true; + } + + const char* Label( b3ShapeId shapeId ) const + { + if ( B3_ID_EQUALS( shapeId, m_shapeA ) ) + { + return "A"; + } + + if ( B3_ID_EQUALS( shapeId, m_shapeB ) ) + { + return "B"; + } + + if ( B3_ID_EQUALS( shapeId, m_shapeC ) ) + { + return "C"; + } + + return "?"; + } + + // Total normal impulse a contact applied over the step, sub-steps and restitution included. + // A speculative point that never pushed contributes zero. + static float ContactImpulse( const b3ContactData& data ) + { + float impulse = 0.0f; + for ( int i = 0; i < data.manifoldCount; ++i ) + { + const b3Manifold& manifold = data.manifolds[i]; + for ( int j = 0; j < manifold.pointCount; ++j ) + { + impulse += manifold.points[j].totalNormalImpulse; + } + } + + return impulse; + } + + // Sum the impulse on a shape from every touching contact, or only the one against a given shape + float ShapeImpulse( b3ShapeId shapeId, b3ShapeId otherId, bool drawArrows ) + { + if ( B3_IS_NULL( shapeId ) ) + { + return 0.0f; + } + + int capacity = b3Shape_GetContactCapacity( shapeId ); + m_contactData.resize( capacity ); + int count = b3Shape_GetContactData( shapeId, m_contactData.data(), capacity ); + + float hertz = m_context->hertz; + float arrowScale = m_restingWeight > 0.0f ? 1.0f / m_restingWeight : 0.0f; + + float impulse = 0.0f; + for ( int i = 0; i < count; ++i ) + { + const b3ContactData& data = m_contactData[i]; + bool isShapeA = B3_ID_EQUALS( data.shapeIdA, shapeId ); + b3ShapeId other = isShapeA ? data.shapeIdB : data.shapeIdA; + if ( B3_IS_NON_NULL( otherId ) && B3_ID_EQUALS( other, otherId ) == false ) + { + continue; + } + + impulse += ContactImpulse( data ); + + if ( drawArrows == false ) + { + continue; + } + + // The normal points from shape A to shape B, so it pushes on B and pulls on A + b3Pos center = b3Body_GetWorldCenter( b3Shape_GetBody( data.shapeIdA ) ); + for ( int j = 0; j < data.manifoldCount; ++j ) + { + const b3Manifold& manifold = data.manifolds[j]; + b3Vec3 direction = isShapeA ? -manifold.normal : manifold.normal; + for ( int k = 0; k < manifold.pointCount; ++k ) + { + const b3ManifoldPoint& mp = manifold.points[k]; + if ( mp.totalNormalImpulse <= 0.0f ) + { + continue; + } + + // One meter per resting weight, clamped so an impact spike stays on screen + float length = b3MinFloat( mp.totalNormalImpulse * hertz * arrowScale, 4.0f ); + b3Pos p = b3OffsetPos( center, mp.anchorA ); + DrawArrow( b3OffsetPos( p, -length * direction ), p, MakeColor( b3_colorRed ) ); + } + } + } + + return impulse; + } + + void Step() override + { + b3Vec3 velocityA = B3_IS_NON_NULL( m_bodyA ) ? b3Body_GetLinearVelocity( m_bodyA ) : b3Vec3_zero; + + Sample::Step(); + + float gravity = -b3World_GetGravity( m_worldId ).y; + m_restingWeight = ( m_massA + m_massB ) * gravity; + + // A thin B leaves A within speculative reach of C, so part of the load can skip B entirely + m_stepImpulseC = ShapeImpulse( m_shapeC, b3_nullShapeId, true ); + m_stepImpulseAC = ShapeImpulse( m_shapeC, m_shapeA, false ); + m_stepImpulseBC = ShapeImpulse( m_shapeC, m_shapeB, false ); + m_stepImpulseAB = ShapeImpulse( m_shapeA, m_shapeB, false ); + float impulseA = ShapeImpulse( m_shapeA, b3_nullShapeId, false ); + + // Pausing keeps the last step's events, so only consume a step once + if ( m_stepCount != m_lastStep ) + { + m_lastStep = m_stepCount; + ProcessStep( impulseA, velocityA ); + } + + DrawHits(); + DrawStats(); + } + + void ProcessStep( float impulseA, b3Vec3 velocityA ) + { + if ( m_dropped == false ) + { + if ( m_stepCount - m_resetStep >= m_settleSteps ) + { + DropA(); + } + return; + } + + float hertz = m_context->hertz; + float timeStep = hertz > 0.0f ? 1.0f / hertz : 0.0f; + + if ( m_impactStep < 0 && impulseA > 0.0f ) + { + m_impactStep = m_stepCount; + m_impactSpeed = -velocityA.y; + + if ( m_pauseOnImpact ) + { + m_context->pause = true; + } + } + + // From impact on, the impulse on C beyond the resting weight removes the momentum A brought in + if ( m_impactStep >= 0 ) + { + m_excessImpulse += m_stepImpulseC - m_restingWeight * timeStep; + } + + float forceC = m_stepImpulseC * hertz; + float forceAB = m_stepImpulseAB * hertz; + if ( forceC > m_peakForceC ) + { + m_peakForceC = forceC; + m_peakStep = m_stepCount; + } + + m_peakForceAB = b3MaxFloat( m_peakForceAB, forceAB ); + m_peakForceAC = b3MaxFloat( m_peakForceAC, m_stepImpulseAC * hertz ); + m_peakForceBC = b3MaxFloat( m_peakForceBC, m_stepImpulseBC * hertz ); + + m_historyC[m_historyIndex] = forceC; + m_historyAB[m_historyIndex] = forceAB; + m_historyIndex = ( m_historyIndex + 1 ) % m_historyCapacity; + m_historyCount = b3MinInt( m_historyCount + 1, m_historyCapacity ); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + for ( int i = 0; i < events.hitCount; ++i ) + { + const b3ContactHitEvent& event = events.hitEvents[i]; + bool touchesC = B3_ID_EQUALS( event.shapeIdA, m_shapeC ) || B3_ID_EQUALS( event.shapeIdB, m_shapeC ); + bool touchesB = B3_ID_EQUALS( event.shapeIdA, m_shapeB ) || B3_ID_EQUALS( event.shapeIdB, m_shapeB ); + m_hitCountC += touchesC ? 1 : 0; + m_hitCountB += touchesB ? 1 : 0; + + HitRecord record = {}; + record.event = event; + record.step = m_stepCount; + if ( b3Contact_IsValid( event.contactId ) ) + { + record.impulse = ContactImpulse( b3Contact_GetData( event.contactId ) ); + } + + // Newest first, dropping the oldest when full + int count = b3MinInt( m_hitCount, m_maxHits - 1 ); + for ( int j = count; j > 0; --j ) + { + m_hits[j] = m_hits[j - 1]; + } + + m_hits[0] = record; + m_hitCount = count + 1; + } + } + + void DrawHits() + { + for ( int i = 0; i < m_hitCount; ++i ) + { + const b3ContactHitEvent& event = m_hits[i].event; + b3Pos p1 = event.point; + b3Pos p2 = b3OffsetPos( p1, -event.approachSpeed * event.normal ); + DrawPoint( p1, 10.0f, MakeColor( b3_colorYellow ) ); + DrawLine( p1, p2, MakeColor( b3_colorYellow ) ); + DrawString3D( p1, MakeColor( b3_colorWhite ), "%s-%s %.1f m/s", Label( event.shapeIdA ), + Label( event.shapeIdB ), event.approachSpeed ); + } + } + + void DrawStats() + { + float hertz = m_context->hertz; + + if ( m_dropped == false ) + { + DrawTextLine( "B settling on C" ); + return; + } + + DrawTextLine( "mass A = %.2f kg, mass B = %.2f kg, resting weight on C = %.1f N", m_massA, m_massB, + m_restingWeight ); + + if ( m_includeB ) + { + DrawTextLine( "B is %s", b3Body_IsAwake( m_bodyB ) ? "awake" : "asleep" ); + } + + if ( m_impactStep < 0 ) + { + DrawTextLine( "A falling" ); + return; + } + + float momentum = m_massA * m_impactSpeed; + DrawTextLine( "impact at step %d, speed %.2f m/s, momentum %.1f N*s", m_impactStep - m_resetStep, + m_impactSpeed, momentum ); + + // No continuous collision between dynamic bodies, so A can sink a full step of travel into B before + // they get a contact. If that reaches within speculative distance of C, A loads C directly. + if ( m_includeB ) + { + DrawTextLine( "A travels %.2f m per step, B is %.2f m thick", m_impactSpeed / hertz, + 2.0f * m_plateHalfHeight ); + } + DrawTextLine( "step load on C = %.1f N (impulse %.3f N*s)", m_stepImpulseC * hertz, m_stepImpulseC ); + DrawTextLine( "peak load on C = %.1f N at step %d, %.1fx resting weight", m_peakForceC, m_peakStep - m_resetStep, + m_restingWeight > 0.0f ? m_peakForceC / m_restingWeight : 0.0f ); + + if ( m_includeB ) + { + DrawTextLine( " from B = %.1f N, peak %.1f N", m_stepImpulseBC * hertz, m_peakForceBC ); + DrawTextLine( " from A directly = %.1f N, peak %.1f N", m_stepImpulseAC * hertz, m_peakForceAC ); + DrawTextLine( "load A on B = %.1f N, peak %.1f N", m_stepImpulseAB * hertz, m_peakForceAB ); + } + + DrawTextLine( "impulse on C beyond resting weight = %.1f N*s (A brought %.1f N*s)", m_excessImpulse, + momentum ); + DrawTextLine( "hit events touching C = %d, touching B = %d", m_hitCountC, m_hitCountB ); + + for ( int i = 0; i < m_hitCount; ++i ) + { + const HitRecord& record = m_hits[i]; + DrawTextLine( " step %d: %s-%s approach %.2f m/s, contact impulse %.3f N*s", record.step - m_resetStep, + Label( record.event.shapeIdA ), Label( record.event.shapeIdB ), record.event.approachSpeed, + record.impulse ); + } + } + + bool DrawControls() override + { + bool reset = false; + reset |= ImGui::Checkbox( "Include B", &m_includeB ); + reset |= ImGui::Checkbox( "B Asleep", &m_sleepB ); + reset |= ImGui::SliderFloat( "B Half Thickness", &m_plateHalfHeight, 0.02f, 0.5f, "%.2f m" ); + reset |= ImGui::SliderFloat( "B Density", &m_densityB, 100.0f, 10000.0f, "%.0f kg/m^3", + ImGuiSliderFlags_Logarithmic ); + reset |= ImGui::SliderFloat( "A Density", &m_densityA, 100.0f, 10000.0f, "%.0f kg/m^3", + ImGuiSliderFlags_Logarithmic ); + reset |= ImGui::SliderFloat( "Drop Height", &m_dropHeight, 0.0f, 10.0f, "%.1f m" ); + + float threshold = b3World_GetHitEventThreshold( m_worldId ); + if ( ImGui::SliderFloat( "Hit Threshold", &threshold, 0.0f, 5.0f, "%.2f m/s" ) ) + { + b3World_SetHitEventThreshold( m_worldId, threshold ); + } + + ImGui::Checkbox( "Pause On Impact", &m_pauseOnImpact ); + + if ( ImGui::Button( "Drop Again" ) ) + { + reset = true; + } + + if ( reset ) + { + Reset(); + } + + int offset = m_historyCount < m_historyCapacity ? 0 : m_historyIndex; + ImGui::PlotLines( "##loadC", m_historyC, m_historyCount, offset, "load on C (N)", 0.0f, FLT_MAX, + ImVec2( 0.0f, 80.0f ) ); + ImGui::PlotLines( "##loadAB", m_historyAB, m_historyCount, offset, "load A on B (N)", 0.0f, FLT_MAX, + ImVec2( 0.0f, 80.0f ) ); + + return true; + } + + static Sample* Create( SampleContext* context ) + { + return new StaticLoad( context ); + } + + struct HitRecord + { + b3ContactHitEvent event; + float impulse; + int step; + }; + + std::vector m_contactData; + + b3BodyId m_bodyA = {}; + b3BodyId m_bodyB = {}; + b3BodyId m_bodyC = {}; + b3ShapeId m_shapeA = {}; + b3ShapeId m_shapeB = {}; + b3ShapeId m_shapeC = {}; + + HitRecord m_hits[m_maxHits] = {}; + float m_historyC[m_historyCapacity] = {}; + float m_historyAB[m_historyCapacity] = {}; + + float m_dropHeight = 3.0f; + float m_plateHalfHeight = 0.15f; + float m_densityA = 1000.0f; + float m_densityB = 1000.0f; + float m_massA = 0.0f; + float m_massB = 0.0f; + float m_restingWeight = 0.0f; + float m_impactSpeed = 0.0f; + float m_excessImpulse = 0.0f; + float m_stepImpulseC = 0.0f; + float m_stepImpulseAB = 0.0f; + float m_stepImpulseAC = 0.0f; + float m_stepImpulseBC = 0.0f; + float m_peakForceC = 0.0f; + float m_peakForceAB = 0.0f; + float m_peakForceAC = 0.0f; + float m_peakForceBC = 0.0f; + + int m_resetStep = 0; + int m_lastStep = 0; + int m_impactStep = -1; + int m_peakStep = -1; + int m_hitCountC = 0; + int m_hitCountB = 0; + int m_hitCount = 0; + int m_historyCount = 0; + int m_historyIndex = 0; + + bool m_includeB = true; + bool m_sleepB = true; + bool m_pauseOnImpact = false; + bool m_dropped = false; +}; + +static int sampleStaticLoad = RegisterSample( "Issues", "Static Load", StaticLoad::Create ); diff --git a/samples/sample_mesh.cpp b/samples/sample_mesh.cpp index 983d802..38f5c67 100644 --- a/samples/sample_mesh.cpp +++ b/samples/sample_mesh.cpp @@ -1626,3 +1626,249 @@ public: static int sampleHollowBox = RegisterSample( "Mesh", "Hollow Box", HollowBox::Create ); +// A sphere fitted inside an inward facing sphere mesh. Every triangle is in range and every contact +// normal is different, so the manifolds cannot be merged. From a voxel cavity filled with wedged debris. +// The sphere should stay inside and go to sleep. +class WedgedSphere : public Sample +{ +public: + explicit WedgedSphere( SampleContext* context ) + : Sample( context ) + { + if ( m_context->restart == false ) + { + m_camera->SetView( 45.0f, 20.0f, 4.0f, b3Pos_zero ); + } + + m_mesh = nullptr; + m_groundId = b3_nullBodyId; + m_bodyId = b3_nullBodyId; + m_subdivisions = 3; + m_fit = 5.0f; + + Build(); + } + + ~WedgedSphere() override + { + b3DestroyMesh( m_mesh ); + } + + static void Subdivide( std::vector& vertices, b3Vec3 a, b3Vec3 b, b3Vec3 c, int depth ) + { + if ( depth == 0 ) + { + vertices.push_back( a ); + vertices.push_back( b ); + vertices.push_back( c ); + return; + } + + b3Vec3 ab = b3Normalize( b3Add( a, b ) ); + b3Vec3 bc = b3Normalize( b3Add( b, c ) ); + b3Vec3 ca = b3Normalize( b3Add( c, a ) ); + Subdivide( vertices, a, ab, ca, depth - 1 ); + Subdivide( vertices, ab, b, bc, depth - 1 ); + Subdivide( vertices, ca, bc, c, depth - 1 ); + Subdivide( vertices, ab, bc, ca, depth - 1 ); + } + + void Build() + { + if ( B3_IS_NON_NULL( m_bodyId ) ) + { + b3DestroyBody( m_bodyId ); + b3DestroyBody( m_groundId ); + b3DestroyMesh( m_mesh ); + } + + // Unit icosphere as a triangle soup, welded by the mesh builder + float t = 0.5f * ( 1.0f + sqrtf( 5.0f ) ); + b3Vec3 corners[12] = { + { -1.0f, t, 0.0f }, { 1.0f, t, 0.0f }, { -1.0f, -t, 0.0f }, { 1.0f, -t, 0.0f }, + { 0.0f, -1.0f, t }, { 0.0f, 1.0f, t }, { 0.0f, -1.0f, -t }, { 0.0f, 1.0f, -t }, + { t, 0.0f, -1.0f }, { t, 0.0f, 1.0f }, { -t, 0.0f, -1.0f }, { -t, 0.0f, 1.0f }, + }; + + int faces[20][3] = { + { 0, 11, 5 }, { 0, 5, 1 }, { 0, 1, 7 }, { 0, 7, 10 }, { 0, 10, 11 }, { 1, 5, 9 }, { 5, 11, 4 }, + { 11, 10, 2 }, { 10, 7, 6 }, { 7, 1, 8 }, { 3, 9, 4 }, { 3, 4, 2 }, { 3, 2, 6 }, { 3, 6, 8 }, + { 3, 8, 9 }, { 4, 9, 5 }, { 2, 4, 11 }, { 6, 2, 10 }, { 8, 6, 7 }, { 9, 8, 1 }, + }; + + std::vector vertices; + for ( int i = 0; i < 20; ++i ) + { + Subdivide( vertices, b3Normalize( corners[faces[i][0]] ), b3Normalize( corners[faces[i][1]] ), + b3Normalize( corners[faces[i][2]] ), m_subdivisions ); + } + + m_triangleCount = (int)vertices.size() / 3; + + // The flat faces sit inside the unit sphere. The closest one bounds the largest sphere that fits. + m_inscribedRadius = 1.0f; + std::vector indices( vertices.size() ); + for ( int i = 0; i < m_triangleCount; ++i ) + { + b3Vec3 a = vertices[3 * i + 0]; + b3Vec3 b = vertices[3 * i + 1]; + b3Vec3 c = vertices[3 * i + 2]; + b3Vec3 normal = b3Normalize( b3Cross( b3Sub( b, a ), b3Sub( c, a ) ) ); + m_inscribedRadius = b3MinFloat( m_inscribedRadius, b3AbsFloat( b3Dot( normal, a ) ) ); + + // Reverse the winding so the triangles face the center + indices[3 * i + 0] = 3 * i + 0; + indices[3 * i + 1] = 3 * i + 2; + indices[3 * i + 2] = 3 * i + 1; + } + + b3MeshDef meshDef = {}; + meshDef.vertices = vertices.data(); + meshDef.indices = indices.data(); + meshDef.vertexCount = (int)vertices.size(); + meshDef.triangleCount = m_triangleCount; + meshDef.weldVertices = true; + meshDef.weldTolerance = 1.0e-4f; + meshDef.identifyEdges = true; + m_mesh = b3CreateMesh( &meshDef, nullptr, 0 ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + m_groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( m_groundId, &shapeDef, m_mesh, b3Vec3_one ); + + bodyDef.type = b3_dynamicBody; + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + m_radius = m_inscribedRadius + 0.001f * m_fit; + b3Sphere sphere = { b3Vec3_zero, m_radius }; + b3CreateSphereShape( m_bodyId, &shapeDef, &sphere ); + + m_buildStep = m_stepCount; + m_sleepStep = -1; + m_maxManifoldCount = 0; + m_maxOffset = 0.0f; + m_maxSpeed = 0.0f; + } + + bool DrawControls() override + { + bool rebuild = false; + + if ( ImGui::RadioButton( "320 triangles", m_subdivisions == 2 ) ) + { + m_subdivisions = 2; + rebuild = true; + } + + if ( ImGui::RadioButton( "1280 triangles", m_subdivisions == 3 ) ) + { + m_subdivisions = 3; + rebuild = true; + } + + if ( ImGui::RadioButton( "5120 triangles", m_subdivisions == 4 ) ) + { + m_subdivisions = 4; + rebuild = true; + } + + // Positive is a sphere larger than the cavity + ImGui::SliderFloat( "Fit (mm)", &m_fit, -20.0f, 20.0f, "%.0f" ); + rebuild = rebuild || ImGui::IsItemDeactivatedAfterEdit(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + Build(); + } + + if ( ImGui::Button( "Kick" ) ) + { + b3Body_SetLinearVelocity( m_bodyId, { 6.0f, 7.0f, 3.9f } ); + b3Body_SetAwake( m_bodyId, true ); + m_sleepStep = -1; + } + + return true; + } + + void Step() override + { + Sample::Step(); + + int manifoldCount = 0; + b3ContactData contactData; + if ( b3Body_GetContactData( m_bodyId, &contactData, 1 ) == 1 ) + { + manifoldCount = contactData.manifoldCount; + } + + // Surface speed counts spin + b3Vec3 v = b3Body_GetLinearVelocity( m_bodyId ); + b3Vec3 w = b3Body_GetAngularVelocity( m_bodyId ); + float speed = b3Length( v ) + m_radius * b3Length( w ); + float offset = b3Length( b3SubPos( b3Body_GetPosition( m_bodyId ), b3Pos_zero ) ); + bool awake = b3Body_IsAwake( m_bodyId ); + + m_maxManifoldCount = b3MaxInt( m_maxManifoldCount, manifoldCount ); + m_maxOffset = b3MaxFloat( m_maxOffset, offset ); + m_maxSpeed = b3MaxFloat( m_maxSpeed, speed ); + + if ( awake ) + { + m_sleepStep = -1; + } + else if ( m_sleepStep < 0 ) + { + m_sleepStep = m_stepCount - m_buildStep; + } + + DrawTextLine( "triangles = %d, sphere radius = %.4f m, cavity radius = %.4f m", m_triangleCount, m_radius, + m_inscribedRadius ); + DrawTextLine( "manifolds = %d, max %d", manifoldCount, m_maxManifoldCount ); + DrawTextLine( "speed = %.3f m/s, max %.3f", speed, m_maxSpeed ); + DrawTextLine( "center offset = %.1f mm, max %.1f", 1000.0f * offset, 1000.0f * m_maxOffset ); + + if ( awake ) + { + DrawTextLine( "awake for %d steps", m_stepCount - m_buildStep ); + } + else + { + DrawTextLine( "asleep after %d steps", m_sleepStep ); + } + + // The center cannot leave the cavity unless the sphere tunnels + if ( m_maxOffset > m_inscribedRadius ) + { + DrawTextLine( "FAIL: tunneled" ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new WedgedSphere( context ); + } + + b3MeshData* m_mesh; + b3BodyId m_groundId; + b3BodyId m_bodyId; + int m_subdivisions; + int m_triangleCount; + int m_buildStep; + int m_sleepStep; + int m_maxManifoldCount; + float m_fit; + float m_radius; + float m_inscribedRadius; + float m_maxOffset; + float m_maxSpeed; +}; + +static int sampleWedgedSphere = RegisterSample( "Mesh", "Wedged Sphere", WedgedSphere::Create ); diff --git a/samples/sample_replay.cpp b/samples/sample_replay.cpp index 4879c1d..2d3b96f 100644 --- a/samples/sample_replay.cpp +++ b/samples/sample_replay.cpp @@ -704,6 +704,12 @@ public: return false; } + // The base world before a recording is open + b3WorldId GetMetricsWorldId() const override + { + return B3_IS_NON_NULL( m_replayWorldId ) ? m_replayWorldId : m_worldId; + } + // Wider than the default so the detail pane, hosted in the info panel, has room for ids and vectors. float InfoPanelWidthEm() const override { diff --git a/shared/mesh_drop.c b/shared/mesh_drop.c index bf5df6e..0abdc7b 100644 --- a/shared/mesh_drop.c +++ b/shared/mesh_drop.c @@ -8,9 +8,19 @@ #include #include +#define WAVE_COUNT 40 #define WAVE_CELL_WIDTH 1.0f #define WAVE_ROW_FREQUENCY 0.1f #define WAVE_COLUMN_FREQUENCY 0.2f +#define WAVE_AMPLITUDE 0.5f + +// Bumps about 20 cm across, close to the curvature of the bodies, on cells a few times smaller. The cell +// width is a power of two so the mesh builder steps to the same vertices the depth tracker computes. +#define ROUGH_WAVE_COUNT 640 +#define ROUGH_CELL_WIDTH 0.03125f +#define ROUGH_ROW_FREQUENCY 4.7f +#define ROUGH_COLUMN_FREQUENCY 5.9f +#define ROUGH_AMPLITUDE 0.015f #define DROP_SPACING 0.5f #define DROP_HEIGHT 5.0f #define BODY_RADIUS 0.05f @@ -53,6 +63,27 @@ static uint32_t HashSeed( uint32_t x ) return x != 0 ? x : 1; } +typedef struct WaveGrid +{ + int count; + float cellWidth; + float rowFrequency; + float columnFrequency; +} WaveGrid; + +static WaveGrid GetWaveGrid( bool rough ) +{ + _Static_assert( ROUGH_WAVE_COUNT <= MESH_DROP_MAX_WAVE_COUNT && WAVE_COUNT <= MESH_DROP_MAX_WAVE_COUNT, + "wave count exceeds the depth tracker tables" ); + + if ( rough ) + { + return (WaveGrid){ ROUGH_WAVE_COUNT, ROUGH_CELL_WIDTH, ROUGH_ROW_FREQUENCY, ROUGH_COLUMN_FREQUENCY }; + } + + return (WaveGrid){ WAVE_COUNT, WAVE_CELL_WIDTH, WAVE_ROW_FREQUENCY, WAVE_COLUMN_FREQUENCY }; +} + static b3BoxHull MakeDropBox( void ) { // The soft contact solver cannot always resolve a smaller box at 4 sub-steps, and its size gets @@ -64,13 +95,19 @@ MeshDropSettings GetDefaultMeshDropSettings( void ) { MeshDropSettings settings = { .shape = e_meshDropBox, - .amplitude = 0.5f, + .amplitude = WAVE_AMPLITUDE, + .rough = false, .collide = false, .soloIndex = -1, }; return settings; } +float GetMeshDropDefaultAmplitude( bool rough ) +{ + return rough ? ROUGH_AMPLITUDE : WAVE_AMPLITUDE; +} + uint32_t GetMeshDropSeed( uint32_t baseSeed, int generation ) { return HashSeed( HashSeed( baseSeed ) + (uint32_t)generation ); @@ -86,10 +123,11 @@ float GetMeshDropTunnelHeight( void ) return -2.0f; } -b3MeshData* CreateMeshDropMesh( float amplitude ) +b3MeshData* CreateMeshDropMesh( MeshDropSettings settings ) { - return b3CreateWaveMesh( MESH_DROP_WAVE_COUNT, MESH_DROP_WAVE_COUNT, WAVE_CELL_WIDTH, amplitude, WAVE_ROW_FREQUENCY, - WAVE_COLUMN_FREQUENCY ); + WaveGrid grid = GetWaveGrid( settings.rough ); + return b3CreateWaveMesh( grid.count, grid.count, grid.cellWidth, settings.amplitude, grid.rowFrequency, + grid.columnFrequency ); } b3HullData* CreateMeshDropCylinder( void ) @@ -97,7 +135,7 @@ b3HullData* CreateMeshDropCylinder( void ) return b3CreateCylinder( 0.4f, BODY_RADIUS, 0.0f, 6 ); } -b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh ) +b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh, MeshDropSettings settings ) { b3BodyDef bodyDef = b3DefaultBodyDef(); b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); @@ -106,7 +144,8 @@ b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh ) shapeDef.filter.categoryBits = 1; b3CreateMeshShape( groundId, &shapeDef, mesh, b3Vec3_one ); - float extent = 0.5f * MESH_DROP_WAVE_COUNT * WAVE_CELL_WIDTH; + WaveGrid grid = GetWaveGrid( settings.rough ); + float extent = 0.5f * grid.count * grid.cellWidth; float halfHeight = 1.0f; b3Transform transforms[4] = { @@ -202,19 +241,18 @@ int GetMeshDropBodyIndex( void* userData ) void ResetMeshDropDepth( MeshDropDepth* depth, MeshDropSettings settings, const b3HullData* cylinder ) { // Same heights as b3CreateWaveMesh - float omegaZ = 2.0f * B3_PI * WAVE_ROW_FREQUENCY * WAVE_CELL_WIDTH; - float omegaX = 2.0f * B3_PI * WAVE_COLUMN_FREQUENCY * WAVE_CELL_WIDTH; - int index = 0; - for ( int ix = 0; ix <= MESH_DROP_WAVE_COUNT; ++ix ) + WaveGrid grid = GetWaveGrid( settings.rough ); + float omegaZ = 2.0f * B3_PI * grid.rowFrequency * grid.cellWidth; + float omegaX = 2.0f * B3_PI * grid.columnFrequency * grid.cellWidth; + for ( int i = 0; i <= grid.count; ++i ) { - float rowHeight = b3ComputeCosSin( omegaX * ix ).sine; - for ( int iz = 0; iz <= MESH_DROP_WAVE_COUNT; ++iz ) - { - float columnHeight = b3ComputeCosSin( omegaZ * iz ).sine; - depth->heights[index++] = settings.amplitude * rowHeight * columnHeight; - } + depth->rowHeights[i] = b3ComputeCosSin( omegaX * i ).sine; + depth->columnHeights[i] = b3ComputeCosSin( omegaZ * i ).sine; } + depth->waveCount = grid.count; + depth->cellWidth = grid.cellWidth; + depth->waveAmplitude = settings.amplitude; depth->amplitude = fabsf( settings.amplitude ); depth->probeCount = 0; depth->radius = 0.0f; @@ -272,20 +310,20 @@ void ResetMeshDropDepth( MeshDropDepth* depth, MeshDropSettings settings, const // it reads a little deep on a crest and shallow in a valley. static float GetWaveDistance( const MeshDropDepth* depth, b3Vec3 p ) { - float origin = -0.5f * MESH_DROP_WAVE_COUNT * WAVE_CELL_WIDTH; - float u = ( p.x - origin ) / WAVE_CELL_WIDTH; - float v = ( p.z - origin ) / WAVE_CELL_WIDTH; - int ix = b3ClampInt( (int)floorf( u ), 0, MESH_DROP_WAVE_COUNT - 1 ); - int iz = b3ClampInt( (int)floorf( v ), 0, MESH_DROP_WAVE_COUNT - 1 ); + float w = depth->cellWidth; + float origin = -0.5f * depth->waveCount * w; + float u = ( p.x - origin ) / w; + float v = ( p.z - origin ) / w; + int ix = b3ClampInt( (int)floorf( u ), 0, depth->waveCount - 1 ); + int iz = b3ClampInt( (int)floorf( v ), 0, depth->waveCount - 1 ); - int stride = MESH_DROP_WAVE_COUNT + 1; - float ha = depth->heights[ix * stride + iz]; - float hb = depth->heights[ix * stride + iz + 1]; - float hc = depth->heights[( ix + 1 ) * stride + iz + 1]; - float hd = depth->heights[( ix + 1 ) * stride + iz]; + float amplitude = depth->waveAmplitude; + float ha = amplitude * depth->rowHeights[ix] * depth->columnHeights[iz]; + float hb = amplitude * depth->rowHeights[ix] * depth->columnHeights[iz + 1]; + float hc = amplitude * depth->rowHeights[ix + 1] * depth->columnHeights[iz + 1]; + float hd = amplitude * depth->rowHeights[ix + 1] * depth->columnHeights[iz]; // The cell diagonal runs from a to c, matching b3CreateWaveMesh - float w = WAVE_CELL_WIDTH; b3Vec3 a = { origin + ix * w, ha, origin + iz * w }; b3Vec3 ac = { w, hc - ha, w }; b3Vec3 side = v - iz >= u - ix ? (b3Vec3){ 0.0f, hb - ha, w } : (b3Vec3){ w, hd - ha, 0.0f }; diff --git a/shared/mesh_drop.h b/shared/mesh_drop.h index 9c19dc0..8e23a9a 100644 --- a/shared/mesh_drop.h +++ b/shared/mesh_drop.h @@ -17,7 +17,8 @@ extern "C" // Mesh Drop scene, shared by the Continuous / Mesh Drop sample and the headless mesh soak. A generation // drops a grid of small bodies with random velocities on a wave mesh and steps until they sleep. A body // that falls out of the world tunneled. The depth tracker measures how far each body sinks into the -// wave, which flags contact trouble far more often than tunnels do. +// wave, which flags contact trouble far more often than tunnels do. The rough wave has bumps smaller than +// the bodies, so a body touches many triangles with different normals. typedef enum MeshDropShape { @@ -28,12 +29,19 @@ typedef enum MeshDropShape e_meshDropShapeCount } MeshDropShape; +// Bodies per axis. A debug build drops a smaller patch at the same spacing so the samples app stays +// responsive. The soak tools run in release and keep the full grid. +#ifdef NDEBUG #define MESH_DROP_GRID_COUNT 32 +#else +#define MESH_DROP_GRID_COUNT 16 +#endif + #define MESH_DROP_BODY_COUNT ( MESH_DROP_GRID_COUNT * MESH_DROP_GRID_COUNT ) #define MESH_DROP_DEPTH_LEVEL_COUNT 4 -// Wave mesh cells along x and z -#define MESH_DROP_WAVE_COUNT 40 +// Most wave mesh cells along x and z, reached by the rough wave +#define MESH_DROP_MAX_WAVE_COUNT 640 // Enough for the cylinder hull #define MESH_DROP_MAX_PROBES 16 @@ -48,6 +56,9 @@ typedef struct MeshDropSettings MeshDropShape shape; float amplitude; + // Fine bumps instead of the broad wave. The amplitude should be a few centimeters. + bool rough; + // Let the bodies collide with each other. Off isolates mesh collision. bool collide; @@ -57,6 +68,8 @@ typedef struct MeshDropSettings MeshDropSettings GetDefaultMeshDropSettings( void ); +float GetMeshDropDefaultAmplitude( bool rough ); + // Hashes the base before adding the generation, so nearby base seeds share no generations uint32_t GetMeshDropSeed( uint32_t baseSeed, int generation ); @@ -66,11 +79,11 @@ int GetMeshDropStepLimit( MeshDropShape shape ); // A body below this height tunneled float GetMeshDropTunnelHeight( void ); -b3MeshData* CreateMeshDropMesh( float amplitude ); +b3MeshData* CreateMeshDropMesh( MeshDropSettings settings ); b3HullData* CreateMeshDropCylinder( void ); // Creates a static body with the wave mesh and four walls -b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh ); +b3BodyId CreateMeshDropGround( b3WorldId worldId, const b3MeshData* mesh, MeshDropSettings settings ); // Creates the bodies of one generation in grid order. Bodies left out by the solo index are null. The // body user data encodes the grid index, see GetMeshDropBodyIndex. @@ -82,8 +95,13 @@ int GetMeshDropBodyIndex( void* userData ); typedef struct MeshDropDepth { - // Wave heights at the mesh vertices - float heights[( MESH_DROP_WAVE_COUNT + 1 ) * ( MESH_DROP_WAVE_COUNT + 1 )]; + // Wave height factors at the mesh vertices along x and z. A vertex height is their product times + // the wave amplitude. + float rowHeights[MESH_DROP_MAX_WAVE_COUNT + 1]; + float columnHeights[MESH_DROP_MAX_WAVE_COUNT + 1]; + int waveCount; + float cellWidth; + float waveAmplitude; // Collision points in the body frame, rounded by the radius b3Vec3 probes[MESH_DROP_MAX_PROBES]; diff --git a/src/constraint_graph.c b/src/constraint_graph.c index c5e3d3e..749d0f4 100644 --- a/src/constraint_graph.c +++ b/src/constraint_graph.c @@ -154,7 +154,7 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact ) if ( isScalar ) { - B3_ASSERT( contact->manifoldCount < UINT16_MAX ); + B3_ASSERT( contact->manifoldCount <= B3_MAX_MANIFOLDS ); b3ContactSpec spec = { .contactId = contact->contactId, .manifoldStart = 0, diff --git a/src/mesh_contact.c b/src/mesh_contact.c index c7437e5..35c880f 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -1154,8 +1154,8 @@ static void b3CollideBoundaryEdge( b3LocalManifold* manifold, const b3Shape* sha static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shapeA, const b3Shape* shapeB, b3Transform transformAtoB, b3TriangleCache* triangleCaches, int triangleCount, - bool enableSpeculative, bool handleGhosts, bool isFast, b3TaskContext* context, - b3Arena* arena ) + bool enableSpeculative, bool handleGhosts, bool isFast, int manifoldCapacity, + b3TaskContext* context, b3Arena* arena ) { *clustersOut = NULL; @@ -1647,6 +1647,100 @@ static int b3CollideMeshTriangles( b3Cluster** clustersOut, const b3Shape* shape return 0; } + if ( clusterCount > manifoldCapacity ) + { + float* clusterSeparations = b3Bump( arena, clusterCount * sizeof( float ) ); + int* clusterRemap = b3Bump( arena, clusterCount * sizeof( int ) ); + for ( int i = 0; i < clusterCount; ++i ) + { + clusterSeparations[i] = FLT_MAX; + clusterRemap[i] = B3_NULL_INDEX; + } + + // Compute cluster separations. + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + const b3LocalManifold* manifold = acceptedManifolds[i]; + int clusterIndex = clusterMemberships[i]; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + clusterSeparations[clusterIndex] = b3MinFloat( clusterSeparations[clusterIndex], manifold->points[j].separation ); + } + } + + int keptCount = 0; + + // If there are 8 or more manifolds then find the best per octant region. + if ( manifoldCapacity >= 8 ) + { + int octantClusters[8] = { B3_NULL_INDEX, B3_NULL_INDEX, B3_NULL_INDEX, B3_NULL_INDEX, + B3_NULL_INDEX, B3_NULL_INDEX, B3_NULL_INDEX, B3_NULL_INDEX }; + for ( int i = 0; i < clusterCount; ++i ) + { + b3Vec3 normal = b3InvRotateVector( transformAtoB.q, clusters[i].manifoldNormal ); + int octant = ( normal.x > 0.0f ? 1 : 0 ) | ( normal.y > 0.0f ? 2 : 0 ) | ( normal.z > 0.0f ? 4 : 0 ); + int best = octantClusters[octant]; + if ( best == B3_NULL_INDEX || clusterSeparations[i] < clusterSeparations[best] ) + { + octantClusters[octant] = i; + } + } + + for ( int i = 0; i < 8; ++i ) + { + if ( octantClusters[i] != B3_NULL_INDEX ) + { + // This cluster wins the octant. + clusterRemap[octantClusters[i]] = 0; + keptCount += 1; + } + } + } + + // If the octant culling left some capacity then find the deepest remaining clusters. + while ( keptCount < manifoldCapacity ) + { + int best = B3_NULL_INDEX; + for ( int i = 0; i < clusterCount; ++i ) + { + if ( clusterRemap[i] != B3_NULL_INDEX ) + { + continue; + } + + if ( best == B3_NULL_INDEX || clusterSeparations[i] < clusterSeparations[best] ) + { + best = i; + } + } + + B3_ASSERT( best != B3_NULL_INDEX ); + + // This cluster wins the separation test. + clusterRemap[best] = 0; + keptCount += 1; + } + + // Remap the winning clusters. + keptCount = 0; + for ( int i = 0; i < clusterCount; ++i ) + { + if ( clusterRemap[i] != B3_NULL_INDEX ) + { + clusters[keptCount] = clusters[i]; + clusterRemap[i] = keptCount; + keptCount += 1; + } + } + + for ( int i = 0; i < acceptedManifoldCount; ++i ) + { + clusterMemberships[i] = clusterRemap[clusterMemberships[i]]; + } + + clusterCount = keptCount; + } + // Setup clusters b3LocalManifoldPoint* clusterPoints = b3Bump( arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); int pointOffset = 0; @@ -1718,7 +1812,8 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3Cluster* clusters; int clusterCount = b3CollideMeshTriangles( &clusters, shapeA, shapeB, transformAtoB, meshContact->triangleCache.data, meshContact->triangleCache.count, - enableSpeculative, handleGhosts, isFast, context, &arena ); + enableSpeculative, handleGhosts, isFast, B3_MAX_MANIFOLDS, context, + &arena ); if ( clusterCount == 0 ) { @@ -1985,8 +2080,10 @@ static int b3CollideMeshAndShape( b3Manifold* manifolds, int capacity, const b3M b3Transform transformAtoB = b3InvertTransform( transformBtoA ); b3Cluster* clusters; + int manifoldCapacity = b3MinInt( capacity, B3_MAX_MANIFOLDS ); int clusterCount = b3CollideMeshTriangles( &clusters, &meshShape, shapeB, transformAtoB, triangleCaches, - triangleCount, enableSpeculative, handleGhosts, false, NULL, &arena ); + triangleCount, enableSpeculative, handleGhosts, false, manifoldCapacity, + NULL, &arena ); int count = b3MinInt( clusterCount, capacity ); b3Matrix3 matrixB = b3MakeMatrixFromQuat( transformBtoA.q ); diff --git a/src/physics_world.c b/src/physics_world.c index 7216420..e24ae9d 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -724,7 +724,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* // Diagnostics taskContext->recycledContactCount += 1; - int bucketIndex = b3MinInt( manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + int bucketIndex = b3MinInt( manifoldCount, B3_MAX_MANIFOLDS ); if ( bucketIndex > 0 ) { taskContext->manifoldCounts[bucketIndex - 1] += 1; @@ -748,7 +748,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* transformA, shapeB, bodySimB->localCenter, transformB, isFast, taskContext->arena ); - int bucketIndex = b3MinInt( contact->manifoldCount, B3_CONTACT_MANIFOLD_COUNT_BUCKETS - 1 ); + int bucketIndex = b3MinInt( contact->manifoldCount, B3_MAX_MANIFOLDS ); if ( bucketIndex > 0 ) { taskContext->manifoldCounts[bucketIndex - 1] += 1; @@ -910,15 +910,14 @@ static void b3Collide( b3StepContext* context ) b3BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; world->satCallCount = satMultiplier * world->taskContexts.data[0].satCallCount; world->satCacheHitCount = satMultiplier * world->taskContexts.data[0].satCacheHitCount; - memcpy( world->manifoldCounts, world->taskContexts.data[0].manifoldCounts, - B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + memcpy( world->manifoldCounts, world->taskContexts.data[0].manifoldCounts, B3_MAX_MANIFOLDS * sizeof( int ) ); for ( int i = 1; i < world->workerCount; ++i ) { b3InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); world->satCallCount += world->taskContexts.data[i].satCallCount; world->satCacheHitCount += world->taskContexts.data[i].satCacheHitCount; - for ( int j = 0; j < B3_CONTACT_MANIFOLD_COUNT_BUCKETS; ++j ) + for ( int j = 0; j < B3_MAX_MANIFOLDS; ++j ) { world->manifoldCounts[j] += world->taskContexts.data[i].manifoldCounts[j]; } @@ -2642,7 +2641,7 @@ b3Counters b3World_GetCounters( b3WorldId worldId ) s.satCallCount = world->satCallCount; s.satCacheHitCount = world->satCacheHitCount; - memcpy( s.manifoldCounts, world->manifoldCounts, B3_CONTACT_MANIFOLD_COUNT_BUCKETS * sizeof( int ) ); + memcpy( s.manifoldCounts, world->manifoldCounts, B3_MAX_MANIFOLDS * sizeof( int ) ); s.stackUsed = world->stack.maxAllocation; s.byteCount = (int64_t)b3GetWorldByteCount( world, false ); s.taskCount = world->taskCount; diff --git a/src/physics_world.h b/src/physics_world.h index 2586922..64d363e 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -115,7 +115,7 @@ typedef struct b3TaskContext b3DebugLine lines[B3_DEBUG_LINE_CAPACITY]; int lineCount; - int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + int manifoldCounts[B3_MAX_MANIFOLDS]; // Prevent false sharing char cacheLine[64]; } b3TaskContext; @@ -254,7 +254,7 @@ typedef struct b3World b3Profile profile; int satCallCount; int satCacheHitCount; - int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + int manifoldCounts[B3_MAX_MANIFOLDS]; b3Capacity maxCapacity; @@ -353,6 +353,8 @@ static inline b3Manifold* b3AllocateManifolds( b3World* world, int count ) return NULL; } + B3_ASSERT( count <= B3_MAX_MANIFOLDS ); + int index = count - 1; // Need lock because this is called from the parallel narrow phase @@ -378,6 +380,8 @@ static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int c return; } + B3_ASSERT( count <= B3_MAX_MANIFOLDS ); + int index = count - 1; b3LockMutex( world->manifoldAllocatorMutex ); b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index ); diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 4ce5240..1faf2d1 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -925,7 +925,8 @@ static void b3DesContacts( b3SnapReader* r, b3World* world ) if ( isLive && manifoldCount > 0 ) { - if ( b3SnapCheckCount( r, manifoldCount, (int)sizeof( b3Manifold ), (int)sizeof( b3Manifold ) ) == false ) + if ( manifoldCount > B3_MAX_MANIFOLDS || + b3SnapCheckCount( r, manifoldCount, (int)sizeof( b3Manifold ), (int)sizeof( b3Manifold ) ) == false ) { r->ok = false; break;