diff --git a/benchmark/main.c b/benchmark/main.c index 16df1fea..0d492e81 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -16,6 +16,7 @@ #include #include #include + #include #include @@ -46,6 +47,7 @@ static void MinProfile( b3Profile* p1, const b3Profile* p2 ) p1->step = b3MinFloat( p1->step, p2->step ); p1->pairs = b3MinFloat( p1->pairs, p2->pairs ); p1->collide = b3MinFloat( p1->collide, p2->collide ); + p1->solve = b3MinFloat( p1->solve, p2->solve ); p1->constraints = b3MinFloat( p1->constraints, p2->constraints ); p1->transforms = b3MinFloat( p1->transforms, p2->transforms ); p1->refit = b3MinFloat( p1->refit, p2->refit ); @@ -161,6 +163,7 @@ int main( int argc, char** argv ) { "trees50", NULL, CreateTrees50, DestroyTrees, NULL, 500 }, { "trees25", NULL, CreateTrees25, DestroyTrees, NULL, 500 }, { "washer", GetWasherCapacity, CreateWasher, NULL, NULL, 1000 }, + { "wheel_stack", NULL, CreateWheelStack, NULL, NULL, 500 }, //{ "smash", CreateSmash, NULL, 300 }, //{ "spinner", CreateSpinner, StepSpinner, 1400 }, //{ "tumbler", CreateTumbler, NULL, 750 }, @@ -412,8 +415,13 @@ int main( int argc, char** argv ) } } - printf( "body %d / shape %d / contact %d / joint %d / stack %d\n\n", counters.bodyCount, counters.shapeCount, + printf( "body %d / shape %d / contact %d / joint %d / stack %d\n", counters.bodyCount, counters.shapeCount, counters.contactCount, counters.jointCount, counters.stackUsed ); + { + b3Profile last = profiles[stepCount - 1]; + printf( "profile(min last step): step %.3f / collide %.3f / solve %.3f ms\n\n", last.step, last.collide, + last.solve ); + } char fileName[64] = { 0 }; snprintf( fileName, 64, "%s.csv", benchmarks[benchmarkIndex].name ); diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 3c4eb511..bf018709 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -179,6 +179,7 @@ set(SAMPLE_FILES sample_shapes.cpp sample_stacking.cpp sample_tree.cpp + sample_wheel_stack.cpp sample_world.cpp ) diff --git a/samples/sample_wheel_stack.cpp b/samples/sample_wheel_stack.cpp new file mode 100644 index 00000000..6f1e6418 --- /dev/null +++ b/samples/sample_wheel_stack.cpp @@ -0,0 +1,93 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "gfx/debug_adapter.h" +#include "sample.h" + +#include "box3d/box3d.h" + +#include "metal_wheel1_hulls.h" + +#include + +// 30 metal_wheel1 props (37-piece convex decompositions) stacked. Body-pair contact merging +// lets them settle and sleep instead of wobbling. +class WheelStack : public Sample +{ +public: + explicit WheelStack( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 12.0f, 5.0f, { 0.0f, 0.85f, 0.0f } ); + } + + AddGroundBox( 10.0f ); + + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + const WheelHullSpan& span = s_metalWheel1Hulls[h]; + m_hulls[h] = b3CreateHull( &s_metalWheel1Verts[span.offset], span.count, span.count ); + } + + const float height = 0.171f; + const float spacing = height + 0.006f; + const float startY = 0.5f * height + 0.004f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.6f; + + for ( int i = 0; i < m_wheelCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.name = "wheel"; + bodyDef.position = { 0.0f, startY + i * spacing, 0.0f }; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3CreateHullShape( bodyId, &shapeDef, m_hulls[h] ); + } + } + + b3World_SetContactTuning( m_worldId, 240.0f, 10.0f, 3.0f ); + } + + ~WheelStack() override + { + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3DestroyHull( m_hulls[h] ); + } + } + + void Step() override + { + Sample::Step(); + + b3Profile p = b3World_GetProfile( m_worldId ); + float substepRate = m_context->hertz * m_context->subStepCount; + + DrawTextLine( "wheels %d, hull pieces/wheel %d (%d total shapes)", m_wheelCount, s_metalWheel1HullCount, + m_wheelCount * s_metalWheel1HullCount ); + DrawTextLine( "step %.0f hz, sub-steps %d -> substep rate %.0f hz", m_context->hertz, m_context->subStepCount, + substepRate ); + DrawTextLine( "eff contact hz = min(240, %.0f) = %.0f", 0.125f * substepRate, + b3MinFloat( 240.0f, 0.125f * substepRate ) ); + DrawTextLine( "step %.3f ms | collide %.3f ms (%.0f%%) | solve %.3f ms (%.0f%%)", p.step, p.collide, + p.step > 0.0f ? 100.0f * p.collide / p.step : 0.0f, p.solve, + p.step > 0.0f ? 100.0f * p.solve / p.step : 0.0f ); + } + + static Sample* Create( SampleContext* context ) + { + return new WheelStack( context ); + } + + b3HullData* m_hulls[s_metalWheel1HullCount]; + static constexpr int m_wheelCount = 30; +}; + +static int sampleWheelStack = RegisterSample( "Stacking", "Wheel Stack (PHX)", WheelStack::Create ); diff --git a/shared/benchmarks.c b/shared/benchmarks.c index a04cf9af..30a693db 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -4,6 +4,7 @@ #include "benchmarks.h" #include "human.h" +#include "metal_wheel1_hulls.h" #include "utils.h" #include "box3d/box3d.h" @@ -673,6 +674,54 @@ void CreateWasher( b3WorldId worldId ) } } +// A stack of 30 metal_wheel1 props (37-piece convex decompositions): the contact-reduction stress case. +void CreateWheelStack( b3WorldId worldId ) +{ + b3World_EnableSleeping( worldId, false ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3BoxHull box = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + g_groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + b3HullData* hulls[s_metalWheel1HullCount]; + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + const WheelHullSpan span = s_metalWheel1Hulls[h]; + hulls[h] = b3CreateHull( &s_metalWheel1Verts[span.offset], span.count, span.count ); + } + + const float height = 0.171f; // y extent of the wheel + const float spacing = height + 0.006f; + const float startY = 0.5f * height + 0.004f; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.6f; + + for ( int i = 0; i < 30; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, startY + i * spacing, 0.0f }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3CreateHullShape( bodyId, &shapeDef, hulls[h] ); + } + } + + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3DestroyHull( hulls[h] ); + } +} + struct { b3MeshData* meshData; diff --git a/shared/benchmarks.h b/shared/benchmarks.h index 2dcd3969..62cc7e74 100644 --- a/shared/benchmarks.h +++ b/shared/benchmarks.h @@ -40,6 +40,7 @@ void StepLargeWorld( b3WorldId worldId, int stepCount ); void GetWasherCapacity( b3Capacity* capacity ); void CreateWasher( b3WorldId worldId ); void CreateConvexPile( b3WorldId worldId ); +void CreateWheelStack( b3WorldId worldId ); // void CreateSpinner( b3WorldId worldId ); // float StepSpinner( b3WorldId worldId, int stepCount ); diff --git a/shared/metal_wheel1_hulls.h b/shared/metal_wheel1_hulls.h new file mode 100644 index 00000000..5d71183a --- /dev/null +++ b/shared/metal_wheel1_hulls.h @@ -0,0 +1,372 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT +// +// metal_wheel1.phy collision model: 37 convex hulls (Source IVPS ledge tree), +// extracted from testdata/metal_wheel1.phy. Units are meters. A ~1.0 m diameter, +// ~0.17 m thick wheel (round face in x-z, axis along y), decomposed into 37 convex +// pieces (a 29-vert hub plus 8-vert wedges). Points recentered on the centroid. +#pragma once + +#include "box3d/math_functions.h" + +static const b3Vec3 s_metalWheel1Verts[317] = { + { 0.010279f, 0.086341f, 0.051768f }, + { -0.010314f, -0.084708f, 0.051804f }, + { 0.010279f, -0.084708f, 0.051768f }, + { -0.010314f, 0.086341f, 0.051804f }, + { 0.029138f, -0.084708f, -0.043911f }, + { 0.043724f, -0.084708f, -0.029376f }, + { 0.010098f, -0.084708f, -0.051759f }, + { -0.010494f, -0.084708f, -0.051723f }, + { 0.051638f, -0.084708f, -0.010364f }, + { 0.051674f, -0.084708f, 0.010229f }, + { 0.043827f, -0.084708f, 0.029268f }, + { 0.029291f, -0.084708f, 0.043855f }, + { -0.029353f, -0.084708f, 0.043957f }, + { -0.051889f, -0.084708f, -0.010183f }, + { -0.051853f, -0.084708f, 0.010409f }, + { -0.043939f, -0.084708f, 0.029421f }, + { -0.029506f, -0.084708f, -0.043809f }, + { -0.044042f, -0.084708f, -0.029222f }, + { -0.029353f, 0.086341f, 0.043957f }, + { 0.043724f, 0.086341f, -0.029376f }, + { 0.029138f, 0.086341f, -0.043911f }, + { 0.010098f, 0.086341f, -0.051759f }, + { 0.051638f, 0.086341f, -0.010364f }, + { 0.051674f, 0.086341f, 0.010229f }, + { -0.044042f, 0.086341f, -0.029222f }, + { 0.043827f, 0.086341f, 0.029268f }, + { -0.051889f, 0.086341f, -0.010183f }, + { -0.043939f, 0.086341f, 0.029421f }, + { -0.029506f, 0.086341f, -0.043809f }, + { 0.035354f, 0.070897f, 0.035658f }, + { 0.035354f, -0.069397f, 0.035658f }, + { -0.035498f, 0.070897f, 0.035658f }, + { -0.035498f, -0.069397f, 0.035658f }, + { 0.035354f, -0.069397f, 0.365503f }, + { 0.035354f, 0.070897f, 0.365503f }, + { -0.035498f, 0.070897f, 0.365503f }, + { -0.035498f, -0.069397f, 0.365503f }, + { 0.035354f, 0.070897f, -0.365033f }, + { 0.035354f, -0.069397f, -0.365033f }, + { -0.035498f, 0.070897f, -0.365033f }, + { -0.035498f, -0.069397f, -0.365033f }, + { 0.035354f, -0.069397f, -0.035187f }, + { 0.035354f, 0.070897f, -0.035187f }, + { -0.035498f, 0.070897f, -0.035187f }, + { -0.035498f, -0.069397f, -0.035187f }, + { -0.035494f, -0.069397f, -0.035191f }, + { -0.365340f, -0.069397f, -0.035191f }, + { -0.035494f, 0.070897f, -0.035191f }, + { -0.365340f, 0.070897f, -0.035191f }, + { -0.365340f, -0.069397f, 0.035661f }, + { -0.035494f, -0.069397f, 0.035661f }, + { -0.035494f, 0.070897f, 0.035661f }, + { -0.365340f, 0.070897f, 0.035661f }, + { 0.035350f, 0.070897f, -0.035191f }, + { 0.365196f, 0.070897f, -0.035191f }, + { 0.035350f, -0.069397f, -0.035191f }, + { 0.365196f, -0.069397f, -0.035191f }, + { 0.365196f, 0.070897f, 0.035661f }, + { 0.035350f, 0.070897f, 0.035661f }, + { 0.035350f, -0.069397f, 0.035661f }, + { 0.365196f, -0.069397f, 0.035661f }, + { -0.070802f, 0.086337f, 0.355420f }, + { -0.070802f, -0.084705f, 0.355420f }, + { -0.097081f, 0.086335f, 0.487537f }, + { -0.097081f, -0.084704f, 0.487537f }, + { -0.000108f, 0.086337f, 0.362383f }, + { -0.000108f, 0.086335f, 0.497088f }, + { -0.000108f, -0.084704f, 0.497088f }, + { -0.000108f, -0.084705f, 0.362383f }, + { -0.138787f, -0.084705f, 0.334799f }, + { -0.138787f, 0.086337f, 0.334799f }, + { -0.070810f, -0.084705f, 0.355419f }, + { -0.070810f, 0.086337f, 0.355419f }, + { -0.097090f, 0.086335f, 0.487536f }, + { -0.190336f, 0.086335f, 0.459250f }, + { -0.190336f, -0.084704f, 0.459250f }, + { -0.097090f, -0.084704f, 0.487536f }, + { -0.138795f, 0.086337f, 0.334796f }, + { -0.138795f, -0.084705f, 0.334796f }, + { -0.201442f, 0.086337f, 0.301310f }, + { -0.201442f, -0.084705f, 0.301310f }, + { -0.276280f, -0.084704f, 0.413313f }, + { -0.276280f, 0.086335f, 0.413313f }, + { -0.190344f, 0.086335f, 0.459247f }, + { -0.190344f, -0.084704f, 0.459247f }, + { -0.201450f, -0.084705f, 0.301306f }, + { -0.256361f, -0.084705f, 0.256241f }, + { -0.201450f, 0.086337f, 0.301306f }, + { -0.256361f, 0.086337f, 0.256241f }, + { -0.351612f, 0.086335f, 0.351492f }, + { -0.351612f, -0.084704f, 0.351492f }, + { -0.276288f, 0.086335f, 0.413309f }, + { -0.276288f, -0.084704f, 0.413309f }, + { -0.301431f, -0.084705f, 0.201325f }, + { -0.413435f, -0.084704f, 0.276163f }, + { -0.413435f, 0.086335f, 0.276163f }, + { -0.301431f, 0.086337f, 0.201325f }, + { -0.256367f, -0.084705f, 0.256236f }, + { -0.256367f, 0.086337f, 0.256236f }, + { -0.351618f, 0.086335f, 0.351487f }, + { -0.351618f, -0.084704f, 0.351487f }, + { -0.334922f, -0.084705f, 0.138670f }, + { -0.459374f, -0.084704f, 0.190220f }, + { -0.459374f, 0.086335f, 0.190220f }, + { -0.334922f, 0.086337f, 0.138670f }, + { -0.301436f, 0.086337f, 0.201318f }, + { -0.301436f, -0.084705f, 0.201318f }, + { -0.413440f, 0.086335f, 0.276156f }, + { -0.413440f, -0.084704f, 0.276156f }, + { -0.487663f, -0.084704f, 0.096966f }, + { -0.355546f, -0.084705f, 0.070686f }, + { -0.459377f, -0.084704f, 0.190212f }, + { -0.334926f, -0.084705f, 0.138663f }, + { -0.355546f, 0.086337f, 0.070686f }, + { -0.334926f, 0.086337f, 0.138663f }, + { -0.487663f, 0.086335f, 0.096966f }, + { -0.459377f, 0.086335f, 0.190212f }, + { -0.362511f, -0.084705f, -0.000016f }, + { -0.355549f, -0.084705f, 0.070678f }, + { -0.497217f, -0.084704f, -0.000016f }, + { -0.487665f, -0.084704f, 0.096957f }, + { -0.497217f, 0.086335f, -0.000016f }, + { -0.362511f, 0.086337f, -0.000016f }, + { -0.355549f, 0.086337f, 0.070678f }, + { -0.487665f, 0.086335f, 0.096957f }, + { -0.362512f, 0.086337f, -0.000024f }, + { -0.362512f, -0.084705f, -0.000024f }, + { -0.355549f, 0.086337f, -0.070717f }, + { -0.355549f, -0.084705f, -0.070717f }, + { -0.497217f, -0.084704f, -0.000024f }, + { -0.487666f, -0.084704f, -0.096997f }, + { -0.497217f, 0.086335f, -0.000024f }, + { -0.487666f, 0.086335f, -0.096997f }, + { -0.334927f, -0.084705f, -0.138702f }, + { -0.355548f, 0.086337f, -0.070726f }, + { -0.355548f, -0.084705f, -0.070726f }, + { -0.334927f, 0.086337f, -0.138702f }, + { -0.487665f, -0.084704f, -0.097005f }, + { -0.459379f, -0.084704f, -0.190252f }, + { -0.487665f, 0.086335f, -0.097005f }, + { -0.459379f, 0.086335f, -0.190252f }, + { -0.413442f, -0.084704f, -0.276196f }, + { -0.301439f, -0.084705f, -0.201358f }, + { -0.334925f, -0.084705f, -0.138710f }, + { -0.459376f, -0.084704f, -0.190260f }, + { -0.334925f, 0.086337f, -0.138710f }, + { -0.301439f, 0.086337f, -0.201358f }, + { -0.459376f, 0.086335f, -0.190260f }, + { -0.413442f, 0.086335f, -0.276196f }, + { -0.256370f, -0.084705f, -0.256276f }, + { -0.301434f, 0.086337f, -0.201365f }, + { -0.301434f, -0.084705f, -0.201365f }, + { -0.256370f, 0.086337f, -0.256276f }, + { -0.413438f, 0.086335f, -0.276203f }, + { -0.351621f, 0.086335f, -0.351527f }, + { -0.413438f, -0.084704f, -0.276203f }, + { -0.351621f, -0.084704f, -0.351527f }, + { -0.256364f, -0.084705f, -0.256283f }, + { -0.201453f, 0.086337f, -0.301347f }, + { -0.256364f, 0.086337f, -0.256283f }, + { -0.201453f, -0.084705f, -0.301347f }, + { -0.276292f, -0.084704f, -0.413350f }, + { -0.351615f, -0.084704f, -0.351534f }, + { -0.351615f, 0.086335f, -0.351534f }, + { -0.276292f, 0.086335f, -0.413350f }, + { -0.201447f, -0.084705f, -0.301352f }, + { -0.276285f, -0.084704f, -0.413355f }, + { -0.138799f, -0.084705f, -0.334838f }, + { -0.190349f, -0.084704f, -0.459289f }, + { -0.276285f, 0.086335f, -0.413355f }, + { -0.201447f, 0.086337f, -0.301352f }, + { -0.190349f, 0.086335f, -0.459289f }, + { -0.138799f, 0.086337f, -0.334838f }, + { -0.190341f, 0.086335f, -0.459293f }, + { -0.190341f, -0.084704f, -0.459293f }, + { -0.138791f, -0.084705f, -0.334842f }, + { -0.138791f, 0.086337f, -0.334842f }, + { -0.070815f, 0.086337f, -0.355462f }, + { -0.070815f, -0.084705f, -0.355462f }, + { -0.097095f, 0.086335f, -0.487579f }, + { -0.097095f, -0.084704f, -0.487579f }, + { -0.070807f, 0.086337f, -0.355464f }, + { -0.097086f, 0.086335f, -0.487581f }, + { -0.097086f, -0.084704f, -0.487581f }, + { -0.070807f, -0.084705f, -0.355464f }, + { -0.000113f, -0.084705f, -0.362427f }, + { -0.000113f, 0.086337f, -0.362427f }, + { -0.000113f, 0.086335f, -0.497132f }, + { -0.000113f, -0.084704f, -0.497132f }, + { 0.070588f, 0.086337f, -0.355465f }, + { 0.096868f, -0.084704f, -0.487582f }, + { 0.096868f, 0.086335f, -0.487582f }, + { 0.070588f, -0.084705f, -0.355465f }, + { -0.000105f, 0.086337f, -0.362427f }, + { -0.000105f, 0.086335f, -0.497133f }, + { -0.000105f, -0.084704f, -0.497133f }, + { -0.000105f, -0.084705f, -0.362427f }, + { 0.190123f, -0.084704f, -0.459294f }, + { 0.190123f, 0.086335f, -0.459294f }, + { 0.138573f, -0.084705f, -0.334843f }, + { 0.138573f, 0.086337f, -0.334843f }, + { 0.070597f, 0.086337f, -0.355463f }, + { 0.070597f, -0.084705f, -0.355463f }, + { 0.096876f, -0.084704f, -0.487580f }, + { 0.096876f, 0.086335f, -0.487580f }, + { 0.201229f, -0.084705f, -0.301354f }, + { 0.276067f, -0.084704f, -0.413358f }, + { 0.201229f, 0.086337f, -0.301354f }, + { 0.138581f, -0.084705f, -0.334840f }, + { 0.138581f, 0.086337f, -0.334840f }, + { 0.190131f, 0.086335f, -0.459292f }, + { 0.276067f, 0.086335f, -0.413358f }, + { 0.190131f, -0.084704f, -0.459292f }, + { 0.201236f, 0.086337f, -0.301350f }, + { 0.256147f, -0.084705f, -0.256286f }, + { 0.256147f, 0.086337f, -0.256286f }, + { 0.201236f, -0.084705f, -0.301350f }, + { 0.351398f, -0.084704f, -0.351537f }, + { 0.351398f, 0.086335f, -0.351537f }, + { 0.276074f, -0.084704f, -0.413353f }, + { 0.276074f, 0.086335f, -0.413353f }, + { 0.301218f, -0.084705f, -0.201369f }, + { 0.301218f, 0.086337f, -0.201369f }, + { 0.256154f, -0.084705f, -0.256280f }, + { 0.256154f, 0.086337f, -0.256280f }, + { 0.413221f, 0.086335f, -0.276207f }, + { 0.413221f, -0.084704f, -0.276207f }, + { 0.351405f, 0.086335f, -0.351531f }, + { 0.351405f, -0.084704f, -0.351531f }, + { 0.334709f, 0.086337f, -0.138715f }, + { 0.301223f, 0.086337f, -0.201362f }, + { 0.334709f, -0.084705f, -0.138715f }, + { 0.301223f, -0.084705f, -0.201362f }, + { 0.459160f, -0.084704f, -0.190264f }, + { 0.459160f, 0.086335f, -0.190264f }, + { 0.413226f, 0.086335f, -0.276200f }, + { 0.413226f, -0.084704f, -0.276200f }, + { 0.334713f, -0.084705f, -0.138707f }, + { 0.355333f, -0.084705f, -0.070731f }, + { 0.334713f, 0.086337f, -0.138707f }, + { 0.355333f, 0.086337f, -0.070731f }, + { 0.459164f, 0.086335f, -0.190257f }, + { 0.487450f, 0.086335f, -0.097010f }, + { 0.487450f, -0.084704f, -0.097010f }, + { 0.459164f, -0.084704f, -0.190257f }, + { 0.355335f, -0.084705f, -0.070722f }, + { 0.362298f, -0.084705f, -0.000029f }, + { 0.355335f, 0.086337f, -0.070722f }, + { 0.362298f, 0.086337f, -0.000029f }, + { 0.497003f, -0.084704f, -0.000029f }, + { 0.497003f, 0.086335f, -0.000029f }, + { 0.487452f, 0.086335f, -0.097002f }, + { 0.487452f, -0.084704f, -0.097002f }, + { 0.362298f, -0.084705f, -0.000021f }, + { 0.497003f, -0.084704f, -0.000021f }, + { 0.355336f, -0.084705f, 0.070673f }, + { 0.487453f, -0.084704f, 0.096952f }, + { 0.497003f, 0.086335f, -0.000021f }, + { 0.362298f, 0.086337f, -0.000021f }, + { 0.355336f, 0.086337f, 0.070673f }, + { 0.487453f, 0.086335f, 0.096952f }, + { 0.355334f, -0.084705f, 0.070681f }, + { 0.355334f, 0.086337f, 0.070681f }, + { 0.487451f, 0.086335f, 0.096961f }, + { 0.487451f, -0.084704f, 0.096961f }, + { 0.334714f, -0.084705f, 0.138658f }, + { 0.459165f, -0.084704f, 0.190207f }, + { 0.334714f, 0.086337f, 0.138658f }, + { 0.459165f, 0.086335f, 0.190207f }, + { 0.334711f, 0.086337f, 0.138666f }, + { 0.301225f, 0.086337f, 0.201313f }, + { 0.459163f, 0.086335f, 0.190215f }, + { 0.413229f, 0.086335f, 0.276151f }, + { 0.334711f, -0.084705f, 0.138666f }, + { 0.301225f, -0.084705f, 0.201313f }, + { 0.413229f, -0.084704f, 0.276151f }, + { 0.459163f, -0.084704f, 0.190215f }, + { 0.301221f, -0.084705f, 0.201321f }, + { 0.256157f, -0.084705f, 0.256232f }, + { 0.301221f, 0.086337f, 0.201321f }, + { 0.256157f, 0.086337f, 0.256232f }, + { 0.413224f, -0.084704f, 0.276159f }, + { 0.413224f, 0.086335f, 0.276159f }, + { 0.351408f, 0.086335f, 0.351483f }, + { 0.351408f, -0.084704f, 0.351483f }, + { 0.201240f, -0.084705f, 0.301302f }, + { 0.201240f, 0.086337f, 0.301302f }, + { 0.256151f, -0.084705f, 0.256238f }, + { 0.256151f, 0.086337f, 0.256238f }, + { 0.351402f, 0.086335f, 0.351489f }, + { 0.351402f, -0.084704f, 0.351489f }, + { 0.276078f, 0.086335f, 0.413305f }, + { 0.276078f, -0.084704f, 0.413305f }, + { 0.138586f, -0.084705f, 0.334793f }, + { 0.201233f, -0.084705f, 0.301307f }, + { 0.190135f, -0.084704f, 0.459244f }, + { 0.276071f, -0.084704f, 0.413311f }, + { 0.201233f, 0.086337f, 0.301307f }, + { 0.138586f, 0.086337f, 0.334793f }, + { 0.276071f, 0.086335f, 0.413311f }, + { 0.190135f, 0.086335f, 0.459244f }, + { 0.138578f, -0.084705f, 0.334797f }, + { 0.070602f, -0.084705f, 0.355417f }, + { 0.138578f, 0.086337f, 0.334797f }, + { 0.070602f, 0.086337f, 0.355417f }, + { 0.190127f, 0.086335f, 0.459248f }, + { 0.190127f, -0.084704f, 0.459248f }, + { 0.096881f, 0.086335f, 0.487534f }, + { 0.096881f, -0.084704f, 0.487534f }, + { 0.070593f, -0.084705f, 0.355419f }, + { 0.096873f, 0.086335f, 0.487536f }, + { 0.096873f, -0.084704f, 0.487536f }, + { 0.070593f, 0.086337f, 0.355419f }, + { -0.000100f, 0.086337f, 0.362382f }, + { -0.000100f, 0.086335f, 0.497087f }, + { -0.000100f, -0.084704f, 0.497087f }, + { -0.000100f, -0.084705f, 0.362382f }, +}; + +typedef struct WheelHullSpan { int offset; int count; } WheelHullSpan; +static const WheelHullSpan s_metalWheel1Hulls[37] = { + { 0, 29 }, + { 29, 8 }, + { 37, 8 }, + { 45, 8 }, + { 53, 8 }, + { 61, 8 }, + { 69, 8 }, + { 77, 8 }, + { 85, 8 }, + { 93, 8 }, + { 101, 8 }, + { 109, 8 }, + { 117, 8 }, + { 125, 8 }, + { 133, 8 }, + { 141, 8 }, + { 149, 8 }, + { 157, 8 }, + { 165, 8 }, + { 173, 8 }, + { 181, 8 }, + { 189, 8 }, + { 197, 8 }, + { 205, 8 }, + { 213, 8 }, + { 221, 8 }, + { 229, 8 }, + { 237, 8 }, + { 245, 8 }, + { 253, 8 }, + { 261, 8 }, + { 269, 8 }, + { 277, 8 }, + { 285, 8 }, + { 293, 8 }, + { 301, 8 }, + { 309, 8 }, +}; +#define s_metalWheel1HullCount 37 diff --git a/src/body.c b/src/body.c index b78642c6..9fe21a8e 100644 --- a/src/body.c +++ b/src/body.c @@ -467,8 +467,8 @@ int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capa // Is contact touching? if ( contact->flags & b3_contactTouchingFlag ) { - b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Shape* shapeA = b3Array_Get( world->shapes, contact->sub0.shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->sub0.shapeIdB ); contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation }; contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; diff --git a/src/constraint_graph.c b/src/constraint_graph.c index 9f06e9e4..530a62f1 100644 --- a/src/constraint_graph.c +++ b/src/constraint_graph.c @@ -144,7 +144,16 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact ) } #endif - bool isScalar = ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX; + bool isScalar = ( contact->flags & b3_simMeshContact ) != 0 || + ( contact->subCount > 1 && contact->manifoldCount != 1 ) || colorIndex == B3_OVERFLOW_INDEX; + if ( isScalar ) + { + contact->flags |= b3_contactScalarPlacement; + } + else + { + contact->flags &= ~b3_contactScalarPlacement; + } b3GraphColor* color = graph->colors + colorIndex; contact->colorIndex = colorIndex; @@ -168,7 +177,7 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact ) } } -void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ) +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool scalarPlacement ) { b3ConstraintGraph* graph = &world->constraintGraph; @@ -182,7 +191,7 @@ void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int col b3ClearBit( &color->bodySet, bodyIdB ); } - if ( meshContact || colorIndex == B3_OVERFLOW_INDEX ) + if ( scalarPlacement || colorIndex == B3_OVERFLOW_INDEX ) { int movedIndex = b3Array_RemoveSwap( color->contacts, localIndex ); if ( movedIndex != B3_NULL_INDEX ) diff --git a/src/constraint_graph.h b/src/constraint_graph.h index ab17e9ac..debdbf5d 100644 --- a/src/constraint_graph.h +++ b/src/constraint_graph.h @@ -70,7 +70,7 @@ void b3CreateGraph( b3ConstraintGraph* graph, int bodyCapacity ); void b3DestroyGraph( b3ConstraintGraph* graph ); void b3AddContactToGraph( b3World* world, b3Contact* contact ); -void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool meshContact ); +void b3RemoveContactFromGraph( b3World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex, bool scalarPlacement ); b3JointSim* b3CreateJointInGraph( b3World* world, b3Joint* joint ); void b3AddJointToGraph( b3World* world, b3JointSim* jointSim, b3Joint* joint ); diff --git a/src/contact.c b/src/contact.c index 92e38b01..aaf826d3 100644 --- a/src/contact.c +++ b/src/contact.c @@ -64,8 +64,8 @@ b3ContactData b3Contact_GetData( b3ContactId contactId ) b3World* world = b3GetWorld( contactId.world0 ); b3Contact* contact = b3GetContactFullId( world, contactId ); - const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + const b3Shape* shapeA = b3Array_Get( world->shapes, contact->sub0.shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, contact->sub0.shapeIdB ); b3ContactData data = { 0 }; data.contactId = contactId; @@ -142,6 +142,60 @@ void b3InitializeContactRegisters( void ) } } +static void b3AppendSubContact( b3World* world, b3Contact* contact, b3Shape* shapeA, b3Shape* shapeB, int childIndex ) +{ + B3_ASSERT( contact->subCount < UINT16_MAX ); + int extraIndex = contact->subCount - 1; + if ( extraIndex >= contact->extraCapacity ) + { + int newCapacity = b3MaxInt( 4, 2 * contact->extraCapacity ); + contact->extraSubs = b3GrowAlloc( contact->extraSubs, contact->extraCapacity * (int)sizeof( b3SubContact ), + newCapacity * (int)sizeof( b3SubContact ) ); + contact->extraCapacity = (uint16_t)newCapacity; + } + + b3SubContact* sub = contact->extraSubs + extraIndex; + *sub = ( b3SubContact ){ 0 }; + sub->shapeIdA = shapeA->id; + sub->shapeIdB = shapeB->id; + sub->childIndex = childIndex; + contact->subCount += 1; + + uint64_t pairKey = b3ShapePairKey( shapeA->id, shapeB->id, childIndex ); + b3AddKey( &world->broadPhase.pairSet, pairKey ); + + if ( ( shapeA->flags & b3_enableContactEvents ) || ( shapeB->flags & b3_enableContactEvents ) ) + { + contact->flags |= b3_contactEnableContactEvents; + } + + if ( ( ( shapeA->flags & b3_enableSpeculative ) && ( shapeB->flags & b3_enableSpeculative ) ) == false ) + { + contact->flags &= ~b3_enableSpeculativePoints; + } + + bool meshSub = shapeA->type == b3_meshShape || shapeA->type == b3_heightShape; + if ( meshSub == false && shapeA->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); + meshSub = child.type == b3_meshShape; + } + if ( meshSub ) + { + contact->flags |= b3_simMeshContact; + sub->isMesh = true; + } + + // Re-place a colored contact so the routing can account for the new sub + if ( contact->colorIndex != B3_NULL_INDEX ) + { + bool scalarPlacement = ( contact->flags & b3_contactScalarPlacement ) != 0; + b3RemoveContactFromGraph( world, contact->edges[0].bodyId, contact->edges[1].bodyId, contact->colorIndex, + contact->localIndex, scalarPlacement ); + b3AddContactToGraph( world, contact ); + } +} + void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ) { b3ShapeType typeA = shapeA->type; @@ -169,6 +223,34 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil B3_ASSERT( bodyA->setIndex != b3_disabledSet && bodyB->setIndex != b3_disabledSet ); B3_ASSERT( bodyA->setIndex != b3_staticSet || bodyB->setIndex != b3_staticSet ); + // Merge into the existing body-pair contact when one exists. Two single-shape bodies can + // only ever share one shape pair, so the scan is skipped for them. + if ( bodyA->shapeCount > 1 || bodyB->shapeCount > 1 ) + { + int pairBodyIdA = shapeA->bodyId; + int pairBodyIdB = shapeB->bodyId; + b3Body* walkBody = bodyA->type == b3_dynamicBody ? bodyA : bodyB; + if ( bodyA->type == b3_dynamicBody && bodyB->type == b3_dynamicBody && bodyB->contactCount < bodyA->contactCount ) + { + walkBody = bodyB; + } + + int contactKey = walkBody->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int existingId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b3Contact* existing = b3Array_Get( world->contacts, existingId ); + if ( ( existing->edges[0].bodyId == pairBodyIdA && existing->edges[1].bodyId == pairBodyIdB ) || + ( existing->edges[0].bodyId == pairBodyIdB && existing->edges[1].bodyId == pairBodyIdA ) ) + { + b3AppendSubContact( world, existing, shapeA, shapeB, childIndex ); + return; + } + contactKey = existing->edges[edgeIndex].nextKey; + } + } + int setIndex; if ( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet ) { @@ -207,9 +289,10 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil contact->localIndex = set->contactIndices.count; contact->islandId = B3_NULL_INDEX; contact->islandIndex = B3_NULL_INDEX; - contact->shapeIdA = shapeIdA; - contact->shapeIdB = shapeIdB; - contact->childIndex = childIndex; + contact->subCount = 1; + contact->sub0.shapeIdA = shapeIdA; + contact->sub0.shapeIdB = shapeIdB; + contact->sub0.childIndex = childIndex; // Both bodies must enable recycling if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 ) @@ -220,6 +303,7 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil if ( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ) { contact->flags |= b3_simMeshContact; + contact->sub0.isMesh = true; } else if ( shapeA->type == b3_compoundShape ) { @@ -227,6 +311,7 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil if ( child.type == b3_meshShape ) { contact->flags |= b3_simMeshContact; + contact->sub0.isMesh = true; } } @@ -328,6 +413,146 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil } } +static void b3EmitSubEndTouchEvent( b3World* world, b3Contact* contact, const b3SubContact* sub ) +{ + if ( ( contact->flags & b3_contactEnableContactEvents ) == 0 ) + { + return; + } + + uint16_t worldId = world->worldId; + const b3Shape* shapeA = b3Array_Get( world->shapes, sub->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, sub->shapeIdB ); + b3ContactEndTouchEvent event = { + .shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }, + .shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }, + .contactId = { contact->contactId + 1, world->worldId, 0, contact->generation }, + }; + b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event ); +} + +// Emit begin/end touch events for sub transitions and update the reported state +void b3SyncSubTouchEvents( b3World* world, b3Contact* contact ) +{ + bool eventsEnabled = ( contact->flags & b3_contactEnableContactEvents ) != 0; + uint16_t worldId = world->worldId; + + for ( int i = 0; i < contact->subCount; ++i ) + { + b3SubContact* sub = b3GetSubContact( contact, i ); + if ( sub->touching == sub->reportedTouching ) + { + continue; + } + + if ( eventsEnabled ) + { + const b3Shape* shapeA = b3Array_Get( world->shapes, sub->shapeIdA ); + const b3Shape* shapeB = b3Array_Get( world->shapes, sub->shapeIdB ); + b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + b3ContactId contactFullId = { + .index1 = contact->contactId + 1, + .world0 = worldId, + .padding = 0, + .generation = contact->generation, + }; + + if ( sub->touching ) + { + b3ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactBeginEvents, event ); + } + else + { + b3ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; + b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event ); + } + } + + sub->reportedTouching = sub->touching; + } +} + +static void b3DestroySubContactData( b3World* world, b3SubContact* sub ) +{ + uint64_t pairKey = b3ShapePairKey( sub->shapeIdA, sub->shapeIdB, sub->childIndex ); + b3RemoveKey( &world->broadPhase.pairSet, pairKey ); + + if ( sub->isMesh ) + { + b3Array_Destroy( sub->meshContact.triangleCache ); + } +} + +// Remove one shape pair from a body-pair contact. The contact survives if other subs remain. +void b3RemoveSubContact( b3World* world, b3Contact* contact, int subIndex, bool wakeBodies ) +{ + B3_ASSERT( 0 <= subIndex && subIndex < contact->subCount ); + + if ( contact->subCount == 1 ) + { + b3DestroyContact( world, contact, wakeBodies ); + return; + } + + b3SubContact* sub = b3GetSubContact( contact, subIndex ); + + if ( sub->reportedTouching ) + { + b3EmitSubEndTouchEvent( world, contact, sub ); + } + + b3DestroySubContactData( world, sub ); + + // Cluster manifolds span subs, so surviving points cannot be attributed. Force a fresh + // narrowphase so the removed pair's points never reach the solver. + contact->flags &= ~b3_relativeTransformValid; + + // Swap-remove: last extra fills the hole + b3SubContact* last = b3GetSubContact( contact, contact->subCount - 1 ); + if ( sub != last ) + { + *sub = *last; + } + contact->subCount -= 1; + + bool anyTouching = false; + for ( int i = 0; i < contact->subCount; ++i ) + { + anyTouching = anyTouching || b3GetSubContact( contact, i )->touching; + } + + if ( ( contact->flags & b3_contactTouchingFlag ) != 0 && anyTouching == false ) + { + // The contact is no longer touching: mirror the stopped-touching transition + contact->flags &= ~b3_contactTouchingFlag; + if ( contact->islandId != B3_NULL_INDEX ) + { + b3UnlinkContact( world, contact ); + } + if ( contact->colorIndex != B3_NULL_INDEX ) + { + B3_ASSERT( contact->setIndex == b3_awakeSet ); + bool scalarPlacement = ( contact->flags & b3_contactScalarPlacement ) != 0; + b3RemoveContactFromGraph( world, contact->edges[0].bodyId, contact->edges[1].bodyId, contact->colorIndex, + contact->localIndex, scalarPlacement ); + contact->colorIndex = B3_NULL_INDEX; + b3SolverSet* set = b3Array_Get( world->solverSets, contact->setIndex ); + contact->localIndex = set->contactIndices.count; + b3Array_Push( set->contactIndices, contact->contactId ); + } + } + + if ( wakeBodies ) + { + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + b3WakeBody( world, bodyA ); + b3WakeBody( world, bodyB ); + } +} + // A contact is destroyed when: // - broad-phase proxies stop overlapping // - a body is destroyed @@ -337,9 +562,10 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil // - contact filtering is modified void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) { - // Remove pair from set - uint64_t pairKey = b3ShapePairKey( contact->shapeIdA, contact->shapeIdB, contact->childIndex ); - b3RemoveKey( &world->broadPhase.pairSet, pairKey ); + for ( int i = 0; i < contact->subCount; ++i ) + { + b3DestroySubContactData( world, b3GetSubContact( contact, i ) ); + } b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); contact->manifolds = NULL; @@ -356,29 +582,17 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) uint32_t flags = contact->flags; bool touching = ( flags & b3_contactTouchingFlag ) != 0; - // End touch event - if ( touching && ( flags & b3_contactEnableContactEvents ) != 0 ) + // End touch events + if ( ( flags & b3_contactEnableContactEvents ) != 0 ) { - uint16_t worldId = world->worldId; - const b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - const b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); - b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; - b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; - - b3ContactId contactId = { - .index1 = contact->contactId + 1, - .world0 = world->worldId, - .padding = 0, - .generation = contact->generation, - }; - - b3ContactEndTouchEvent event = { - .shapeIdA = shapeIdA, - .shapeIdB = shapeIdB, - .contactId = contactId, - }; - - b3Array_Push( world->contactEndEvents[world->endEventArrayIndex], event ); + for ( int i = 0; i < contact->subCount; ++i ) + { + b3SubContact* sub = b3GetSubContact( contact, i ); + if ( sub->reportedTouching ) + { + b3EmitSubEndTouchEvent( world, contact, sub ); + } + } } // Remove from body A @@ -429,11 +643,6 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) bodyB->contactCount -= 1; - if ( contact->flags & b3_simMeshContact ) - { - b3Array_Destroy( contact->meshContact.triangleCache ); - } - // Remove contact from the array that owns it if ( contact->islandId != B3_NULL_INDEX ) { @@ -444,8 +653,8 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) { // contact is an active constraint B3_ASSERT( contact->setIndex == b3_awakeSet ); - bool meshContact = contact->flags & b3_simMeshContact; - b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex, meshContact ); + bool scalarPlacement = ( contact->flags & b3_contactScalarPlacement ) != 0; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex, scalarPlacement ); } else { @@ -463,6 +672,14 @@ void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ) } } + if ( contact->extraSubs != NULL ) + { + b3Free( contact->extraSubs, contact->extraCapacity * (int)sizeof( b3SubContact ) ); + contact->extraSubs = NULL; + contact->extraCapacity = 0; + } + contact->subCount = 0; + // Free contact and id (preserve generation) contact->contactId = B3_NULL_INDEX; contact->setIndex = B3_NULL_INDEX; @@ -483,7 +700,7 @@ static bool b3ComputeConvexManifold( b3World* world, int workerIndex, b3Contact* b3ShapeType typeA = shapeA->type; b3ShapeType typeB = shapeB->type; - b3ContactCache* cache = &contact->convexContact.cache; + b3ContactCache* cache = &contact->sub0.convexContact.cache; int pointCapacity = 32; b3LocalManifoldPoint* pointBuffer = (b3LocalManifoldPoint*)b3Bump( &arena, pointCapacity * sizeof( b3LocalManifoldPoint ) ); @@ -734,9 +951,16 @@ bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Sha B3_ASSERT( shapeB->type != b3_compoundShape ); - if ( shapeA->type == b3_compoundShape ) + if ( contact->subCount > 1 ) + { + // Body pair with multiple shape pairs. shapeA/xfA map to edges[0], shapeB/xfB to edges[1]. + touching = b3ComputeMultiSubManifolds( world, workerIndex, contact, xfA, xfB, isFast, arena ); + + B3_ASSERT( ( touching == true && contact->manifoldCount > 0 ) || ( touching == false && contact->manifoldCount == 0 ) ); + } + else if ( shapeA->type == b3_compoundShape ) { - int childIndex = contact->childIndex; + int childIndex = contact->sub0.childIndex; b3ChildShape child = b3GetCompoundChild( shapeA->compound, childIndex ); // Temporary child shape to match existing function signatures diff --git a/src/contact.h b/src/contact.h index ad9fa74d..ca5ecb7f 100644 --- a/src/contact.h +++ b/src/contact.h @@ -70,6 +70,12 @@ enum b3ContactFlags // Enable speculative contact points b3_enableSpeculativePoints = 0x01000000, + + // Contact lives on the graph color's scalar list (multi manifold path) + b3_contactScalarPlacement = 0x02000000, + + // Manifold count no longer fits the wide path, move to the scalar list + b3_simGraphMove = 0x04000000, }; // A contact edge is used to connect bodies and contacts together @@ -95,6 +101,25 @@ typedef struct b3ConvexContact b3ContactCache cache; } b3ConvexContact; +// One narrowphase shape pair within a body-pair contact +typedef struct b3SubContact +{ + int shapeIdA; + int shapeIdB; + int childIndex; + + bool touching; + bool reportedTouching; + bool isMesh; + + // Usage determined by b3_simMeshContact style dispatch on the shapes + union + { + b3ConvexContact convexContact; + b3MeshContact meshContact; + }; +} b3SubContact; + // Represents the persistent interaction between two shapes typedef struct b3Contact { @@ -112,9 +137,6 @@ typedef struct b3Contact int localIndex; b3ContactEdge edges[2]; - int shapeIdA; - int shapeIdB; - int childIndex; // A contact only belongs to an island if touching, otherwise B3_NULL_INDEX. int islandId; @@ -144,13 +166,6 @@ typedef struct b3Contact // Mixed friction and restitution float friction; - // Usage determined by b3_simMeshContact in simFlags - union - { - b3ConvexContact convexContact; - b3MeshContact meshContact; - }; - float restitution; float rollingResistance; b3Vec3 tangentVelocity; @@ -158,6 +173,11 @@ typedef struct b3Contact // This is monotonically advanced when a contact is allocated in this slot // Used to check for invalid b3ContactId uint32_t generation; + + uint16_t subCount; + uint16_t extraCapacity; + b3SubContact* extraSubs; + b3SubContact sub0; } b3Contact; typedef struct b3ContactSpec @@ -171,13 +191,23 @@ typedef struct b3ContactSpec b3DeclareArray( b3ContactSpec ); +static inline b3SubContact* b3GetSubContact( b3Contact* contact, int index ) +{ + return index == 0 ? &contact->sub0 : contact->extraSubs + ( index - 1 ); +} + void b3InitializeContactRegisters( void ); void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int childIndex ); void b3DestroyContact( b3World* world, b3Contact* contact, bool wakeBodies ); +void b3RemoveSubContact( b3World* world, b3Contact* contact, int subIndex, bool wakeBodies ); +void b3SyncSubTouchEvents( b3World* world, b3Contact* contact ); bool b3UpdateContact( b3World* world, int workerIndex, b3Contact* contact, b3Shape* shapeA, b3Vec3 localCenterA, b3WorldTransform xfA, b3Shape* shapeB, b3Vec3 localCenterB, b3WorldTransform xfB, bool isFast, b3Arena arena ); bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ); + +bool b3ComputeMultiSubManifolds( b3World* world, int workerIndex, b3Contact* contact, b3WorldTransform xfA, b3WorldTransform xfB, + bool isFast, b3Arena arena ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index a7163477..18352d97 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -74,21 +74,19 @@ static int b3QueryHeightFieldTriangles( int* indices, int capacity, const b3Heig return context.count; } -static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds ) +static void b3RefreshCache( b3MeshContact* meshContact, const b3Shape* shapeA, b3WorldTransform xfA, const b3AABB* bounds ) { B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); - b3MeshContact* meshContact = &contact->meshContact; - // If the dynamic body didn't move out of the cached query bounds we are done! if ( b3AABB_Contains( meshContact->queryBounds, *bounds ) ) { if ( shapeA->type == b3_meshShape ) { - for ( int i = 0; i < contact->meshContact.triangleCache.count; ++i ) + for ( int i = 0; i < meshContact->triangleCache.count; ++i ) { - B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && - contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + B3_ASSERT( 0 <= meshContact->triangleCache.data[i].triangleIndex && + meshContact->triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); } } @@ -142,29 +140,29 @@ static void b3RefreshCache( b3Contact* contact, const b3Shape* shapeA, b3WorldTr { contactCache[index1] = (b3ContactCache){ 0 }; - while ( index2 < contact->meshContact.triangleCache.count && - contact->meshContact.triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) + while ( index2 < meshContact->triangleCache.count && + meshContact->triangleCache.data[index2].triangleIndex < triangleIndices[index1] ) { index2 += 1; } - if ( index2 < contact->meshContact.triangleCache.count && - contact->meshContact.triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) + if ( index2 < meshContact->triangleCache.count && + meshContact->triangleCache.data[index2].triangleIndex == triangleIndices[index1] ) { - contactCache[index1] = contact->meshContact.triangleCache.data[index2].cache; + contactCache[index1] = meshContact->triangleCache.data[index2].cache; } } // Save new cache - b3Array_Resize( contact->meshContact.triangleCache, triangleCount ); + b3Array_Resize( meshContact->triangleCache, triangleCount ); for ( int i = 0; i < triangleCount; ++i ) { - contact->meshContact.triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; + meshContact->triangleCache.data[i] = (b3TriangleCache){ triangleIndices[i], contactCache[i] }; if ( shapeA->type == b3_meshShape ) { - B3_ASSERT( 0 <= contact->meshContact.triangleCache.data[i].triangleIndex && - contact->meshContact.triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); + B3_ASSERT( 0 <= meshContact->triangleCache.data[i].triangleIndex && + meshContact->triangleCache.data[i].triangleIndex < shapeA->mesh.data->triangleCount ); } } } @@ -522,27 +520,18 @@ typedef struct b3Cluster int pointCount; } b3Cluster; -bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, - b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ) +// Collide a mesh or height-field sub against shapeB. Produces accepted local manifolds with points in the +// shapeB frame. All buffers are caller owned so the results persist. The cache must already be refreshed. +static int b3GatherMeshSubManifolds( b3World* world, int workerIndex, b3MeshContact* meshContact, bool enableSpeculative, + const b3Shape* shapeA, b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, + bool isFast, b3LocalManifold** acceptedManifolds, b3LocalManifold** tentativeManifolds, + b3TentativeTriangle* tentativeTriangles, b3LocalManifold* manifoldBuffer, + b3LocalManifoldPoint* pointBuffer, int pointBufferCapacity, int triangleCount ) { - B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); - B3_UNUSED( workerIndex ); - B3_UNUSED( isFast ); - B3_UNUSED( materialMap ); - b3TaskContext* context = b3Array_Get( world->taskContexts, workerIndex ); - b3RefreshCache( contact, shapeA, xfA, &shapeB->aabb ); - - // Collide with triangles and build manifolds - b3MeshContact* meshContact = &contact->meshContact; - int triangleCount = meshContact->triangleCache.count; - - b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int acceptedManifoldCount = 0; - b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); int tentativeManifoldCount = 0; - b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); int tentativeTriangleCount = 0; b3FoundEdges foundEdges; @@ -555,21 +544,7 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact b3Matrix3 relativeMatrix = b3MakeMatrixFromQuat( transformAtoB.q ); float linearSlop = B3_LINEAR_SLOP; - // This should push apart shapes after a time of impact event. - // In the past I've called this `polygon skin`, but PhysX and Unreal - // call it `rest offset` which seems appropriate in this case. - // It leads to a small visual gap but seems to improve the quality of mesh - // collision, especially for hull versus mesh. - float restOffset = B3_MESH_REST_OFFSET; - bool enableSpeculative = contact->flags & b3_enableSpeculativePoints; - - // 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 = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); int manifoldCount = 0; b3TriangleCache* triangleCaches = meshContact->triangleCache.data; @@ -834,6 +809,35 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact B3_ASSERT( acceptedManifoldCount <= triangleCount ); + return acceptedManifoldCount; +} + +bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact, const b3Shape* shapeA, const int* materialMap, + b3WorldTransform xfA, const b3Shape* shapeB, b3WorldTransform xfB, bool isFast, b3Arena arena ) +{ + B3_ASSERT( shapeA->type == b3_meshShape || shapeA->type == b3_heightShape ); + + b3MeshContact* meshContact = &contact->sub0.meshContact; + b3RefreshCache( meshContact, shapeA, xfA, &shapeB->aabb ); + + int triangleCount = meshContact->triangleCache.count; + + b3LocalManifold** acceptedManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** tentativeManifolds = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold* ) ); + b3TentativeTriangle* tentativeTriangles = b3Bump( &arena, triangleCount * sizeof( b3TentativeTriangle ) ); + + int pointBufferCapacity = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; + b3LocalManifoldPoint* pointBuffer = b3Bump( &arena, pointBufferCapacity * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifold* manifoldBuffer = b3Bump( &arena, triangleCount * sizeof( b3LocalManifold ) ); + + bool enableSpeculative = ( contact->flags & b3_enableSpeculativePoints ) != 0; + + int acceptedManifoldCount = b3GatherMeshSubManifolds( world, workerIndex, meshContact, enableSpeculative, shapeA, xfA, shapeB, + xfB, isFast, acceptedManifolds, tentativeManifolds, tentativeTriangles, + manifoldBuffer, pointBuffer, pointBufferCapacity, triangleCount ); + + float restOffset = B3_MESH_REST_OFFSET; + if ( acceptedManifoldCount == 0 ) { if ( contact->manifoldCount > 0 ) @@ -1184,3 +1188,685 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact contact->tangentVelocity = b3Sub( tangentVelocityA, tangentVelocityB ); return true; } + +// Per sub material data cached during gathering, consumed in the final averaging pass. +typedef struct b3SubMaterial +{ + bool isMesh; + bool isHeightField; + + // Convex mixed values + float friction; + float restitution; + + // Mesh material lookup + const b3SurfaceMaterial* materialsA; + const b3SurfaceMaterial* materialB; + const uint8_t* triMaterialIndices; + const int* materialMap; + int materialCount; + + b3Vec3 tangentVelocity; + float rollingResistance; +} b3SubMaterial; + +static inline float b3SubShapeRadius( b3ShapeType type, const b3Sphere* sphere, const b3Capsule* capsule, const b3HullData* hull ) +{ + if ( type == b3_sphereShape ) + { + return sphere->radius; + } + if ( type == b3_capsuleShape ) + { + return capsule->radius; + } + return 0.25f * hull->innerRadius; +} + +// Narrowphase for a body pair with more than one shape pair. Each sub is collided in its own frame, then +// all local manifolds are expressed in a single canonical frame (world axes relative to body A origin, +// normal from body A to body B), clustered by normal, reduced, and emitted like the mesh path. +bool b3ComputeMultiSubManifolds( b3World* world, int workerIndex, b3Contact* contact, b3WorldTransform xfA, b3WorldTransform xfB, + bool isFast, b3Arena arena ) +{ + b3Shape* shapes = world->shapes.data; + b3TaskContext* taskContext = b3Array_Get( world->taskContexts, workerIndex ); + int edges0BodyId = contact->edges[0].bodyId; + int subCount = contact->subCount; + bool enableSpeculative = ( contact->flags & b3_enableSpeculativePoints ) != 0; + float restOffset = B3_MESH_REST_OFFSET; + + // Size the shared buffers. Refresh mesh caches now so triangle counts are known. + int maxLocalManifolds = 0; + int maxLocalPoints = 0; + for ( int s = 0; s < subCount; ++s ) + { + b3SubContact* sub = b3GetSubContact( contact, s ); + b3Shape* primShape = shapes + sub->shapeIdA; + b3Shape* secShape = shapes + sub->shapeIdB; + + bool meshSub = primShape->type == b3_meshShape || primShape->type == b3_heightShape; + b3ChildShape child = { 0 }; + bool isCompound = primShape->type == b3_compoundShape; + if ( isCompound ) + { + child = b3GetCompoundChild( primShape->compound, sub->childIndex ); + meshSub = child.type == b3_meshShape; + } + + if ( meshSub ) + { + int primBodyId = primShape->bodyId; + b3WorldTransform xfPrimBody = ( primBodyId == edges0BodyId ) ? xfA : xfB; + + b3Shape meshShapeStorage; + const b3Shape* meshShapeP = primShape; + b3WorldTransform xfMesh = xfPrimBody; + if ( isCompound ) + { + memcpy( &meshShapeStorage, primShape, sizeof( b3Shape ) ); + meshShapeStorage.type = b3_meshShape; + meshShapeStorage.mesh = child.mesh; + meshShapeP = &meshShapeStorage; + xfMesh = b3MulWorldTransforms( xfPrimBody, child.transform ); + } + + b3RefreshCache( &sub->meshContact, meshShapeP, xfMesh, &secShape->aabb ); + int triangleCount = sub->meshContact.triangleCache.count; + maxLocalManifolds += triangleCount; + maxLocalPoints += triangleCount * B3_MAX_POINTS_PER_TRIANGLE; + } + else + { + maxLocalManifolds += 1; + maxLocalPoints += B3_MAX_POINTS_PER_TRIANGLE; + } + } + + b3LocalManifold* localManifolds = b3Bump( &arena, maxLocalManifolds * sizeof( b3LocalManifold ) ); + b3LocalManifoldPoint* localPoints = b3Bump( &arena, maxLocalPoints * sizeof( b3LocalManifoldPoint ) ); + b3SubMaterial* subInfo = b3Bump( &arena, subCount * sizeof( b3SubMaterial ) ); + int localCount = 0; + int localPointCount = 0; + + uint32_t hitEventFlag = 0; + + // Gather local manifolds from every sub into the canonical frame. + for ( int s = 0; s < subCount; ++s ) + { + b3SubContact* sub = b3GetSubContact( contact, s ); + b3Shape* primShape = shapes + sub->shapeIdA; + b3Shape* secShape = shapes + sub->shapeIdB; + int subStartPoints = localPointCount; + + if ( ( primShape->flags & b3_enableHitEvents ) || ( secShape->flags & b3_enableHitEvents ) ) + { + hitEventFlag = b3_simEnableHitEvent; + } + + int primBodyId = primShape->bodyId; + bool bodyFlip = primBodyId != edges0BodyId; + b3WorldTransform xfPrimBody = bodyFlip ? xfB : xfA; + b3WorldTransform xfSecBody = bodyFlip ? xfA : xfB; + + bool meshSub = primShape->type == b3_meshShape || primShape->type == b3_heightShape; + b3ChildShape child = { 0 }; + bool isCompound = primShape->type == b3_compoundShape; + if ( isCompound ) + { + child = b3GetCompoundChild( primShape->compound, sub->childIndex ); + meshSub = child.type == b3_meshShape; + } + + const b3SurfaceMaterial* materialB = b3GetShapeMaterials( secShape ); + + if ( meshSub ) + { + b3Shape meshShapeStorage; + const b3Shape* meshShapeP = primShape; + b3WorldTransform xfMesh = xfPrimBody; + const int* materialMap = NULL; + if ( isCompound ) + { + memcpy( &meshShapeStorage, primShape, sizeof( b3Shape ) ); + meshShapeStorage.type = b3_meshShape; + meshShapeStorage.mesh = child.mesh; + meshShapeP = &meshShapeStorage; + xfMesh = b3MulWorldTransforms( xfPrimBody, child.transform ); + materialMap = child.materialIndices; + } + + int triangleCount = sub->meshContact.triangleCache.count; + if ( triangleCount > 0 ) + { + b3Arena scratch = arena; + b3LocalManifold** accepted = b3Bump( &scratch, triangleCount * sizeof( b3LocalManifold* ) ); + b3LocalManifold** tentative = b3Bump( &scratch, triangleCount * sizeof( b3LocalManifold* ) ); + b3TentativeTriangle* tentTri = b3Bump( &scratch, triangleCount * sizeof( b3TentativeTriangle ) ); + int pcap = B3_MAX_POINTS_PER_TRIANGLE * triangleCount; + b3LocalManifoldPoint* pbuf = b3Bump( &scratch, pcap * sizeof( b3LocalManifoldPoint ) ); + b3LocalManifold* mbuf = b3Bump( &scratch, triangleCount * sizeof( b3LocalManifold ) ); + + int accCount = b3GatherMeshSubManifolds( world, workerIndex, &sub->meshContact, enableSpeculative, meshShapeP, + xfMesh, secShape, xfSecBody, isFast, accepted, tentative, tentTri, mbuf, + pbuf, pcap, triangleCount ); + + b3Matrix3 matSec = b3MakeMatrixFromQuat( xfSecBody.q ); + b3Vec3 secOriginRelA = b3SubPos( xfSecBody.p, xfA.p ); + + for ( int a = 0; a < accCount; ++a ) + { + b3LocalManifold* am = accepted[a]; + b3Vec3 normalWorld = b3MulMV( matSec, am->normal ); + b3Vec3 triNormalWorld = b3MulMV( matSec, am->triangleNormal ); + int tag = ( am->triangleIndex & 0x003FFFFF ) | ( s << 22 ); + + b3LocalManifold* lm = localManifolds + localCount; + lm->normal = bodyFlip ? b3Neg( normalWorld ) : normalWorld; + lm->triangleNormal = bodyFlip ? b3Neg( triNormalWorld ) : triNormalWorld; + lm->points = localPoints + localPointCount; + lm->pointCount = am->pointCount; + lm->triangleIndex = tag; + + for ( int j = 0; j < am->pointCount; ++j ) + { + b3LocalManifoldPoint* src = am->points + j; + b3LocalManifoldPoint* dst = localPoints + localPointCount; + dst->point = b3Add( secOriginRelA, b3MulMV( matSec, src->point ) ); + dst->separation = src->separation - restOffset; + dst->pair = src->pair; + dst->triangleIndex = tag; + localPointCount += 1; + } + localCount += 1; + } + } + + const b3SurfaceMaterial* materialsA = b3GetShapeMaterials( meshShapeP ); + subInfo[s].isMesh = true; + subInfo[s].isHeightField = meshShapeP->type == b3_heightShape; + subInfo[s].materialsA = materialsA; + subInfo[s].materialB = materialB; + subInfo[s].materialMap = materialMap; + subInfo[s].materialCount = meshShapeP->materialCount; + if ( meshShapeP->type == b3_meshShape ) + { + subInfo[s].triMaterialIndices = b3GetMeshMaterialIndices( meshShapeP->mesh.data ); + } + else + { + subInfo[s].triMaterialIndices = b3GetHeightFieldMaterialIndices( meshShapeP->heightField ); + } + + b3Vec3 tvMesh = b3RotateVector( xfMesh.q, materialsA[0].tangentVelocity ); + b3Vec3 tvSec = b3RotateVector( xfSecBody.q, materialB->tangentVelocity ); + subInfo[s].tangentVelocity = bodyFlip ? b3Sub( tvSec, tvMesh ) : b3Sub( tvMesh, tvSec ); + + // Mesh path convention: full hull inner radius for the secondary. + float radiusB = 0.0f; + if ( secShape->type == b3_sphereShape ) + { + radiusB = secShape->sphere.radius; + } + else if ( secShape->type == b3_capsuleShape ) + { + radiusB = secShape->capsule.radius; + } + else if ( secShape->type == b3_hullShape ) + { + radiusB = secShape->hull->innerRadius; + } + subInfo[s].rollingResistance = materialB->rollingResistance * radiusB; + } + else + { + // Effective primary geometry (possibly a compound child) + b3ShapeType primType; + b3Sphere primSphere = { 0 }; + b3Capsule primCap = { 0 }; + const b3HullData* primHull = NULL; + b3WorldTransform xfPrim; + if ( isCompound ) + { + primType = child.type; + xfPrim = b3MulWorldTransforms( xfPrimBody, child.transform ); + if ( child.type == b3_capsuleShape ) + { + primCap = child.capsule; + } + else if ( child.type == b3_hullShape ) + { + primHull = child.hull; + } + else + { + primSphere = child.sphere; + } + } + else + { + primType = primShape->type; + xfPrim = xfPrimBody; + if ( primType == b3_capsuleShape ) + { + primCap = primShape->capsule; + } + else if ( primType == b3_hullShape ) + { + primHull = primShape->hull; + } + else + { + primSphere = primShape->sphere; + } + } + + b3ShapeType secType = secShape->type; + b3Sphere secSphere = secType == b3_sphereShape ? secShape->sphere : (b3Sphere){ 0 }; + b3Capsule secCap = secType == b3_capsuleShape ? secShape->capsule : (b3Capsule){ 0 }; + const b3HullData* secHull = secType == b3_hullShape ? secShape->hull : NULL; + + int rankPrim = primType == b3_hullShape ? 2 : ( primType == b3_capsuleShape ? 1 : 0 ); + int rankSec = secType == b3_hullShape ? 2 : ( secType == b3_capsuleShape ? 1 : 0 ); + bool collideFlip = rankSec > rankPrim; + + b3ShapeType aType, bType; + const b3Sphere *aSphere, *bSphere; + const b3Capsule *aCap, *bCap; + const b3HullData *aHull, *bHull; + b3WorldTransform xfCA, xfCB; + int collideABodyId; + if ( collideFlip == false ) + { + aType = primType; + bType = secType; + aSphere = &primSphere; + aCap = &primCap; + aHull = primHull; + bSphere = &secSphere; + bCap = &secCap; + bHull = secHull; + xfCA = xfPrim; + xfCB = xfSecBody; + collideABodyId = primBodyId; + } + else + { + aType = secType; + bType = primType; + aSphere = &secSphere; + aCap = &secCap; + aHull = secHull; + bSphere = &primSphere; + bCap = &primCap; + bHull = primHull; + xfCA = xfSecBody; + xfCB = xfPrim; + collideABodyId = secShape->bodyId; + } + + b3ContactCache* cache = &sub->convexContact.cache; + b3Transform btoa = b3InvMulWorldTransforms( xfCA, xfCB ); + + b3LocalManifoldPoint geomPoints[B3_MAX_POINTS_PER_TRIANGLE]; + b3LocalManifold geom = { 0 }; + geom.points = geomPoints; + + if ( aType == b3_hullShape ) + { + if ( bType == b3_hullShape ) + { + b3CollideHulls( &geom, B3_MAX_POINTS_PER_TRIANGLE, aHull, bHull, btoa, &cache->satCache ); + taskContext->satCallCount += 1; + taskContext->satCacheHitCount += cache->satCache.hit; + } + else if ( bType == b3_capsuleShape ) + { + b3CollideHullAndCapsule( &geom, B3_MAX_POINTS_PER_TRIANGLE, aHull, bCap, btoa, &cache->simplexCache ); + } + else + { + b3CollideHullAndSphere( &geom, B3_MAX_POINTS_PER_TRIANGLE, aHull, bSphere, btoa, &cache->simplexCache ); + } + } + else if ( aType == b3_capsuleShape ) + { + if ( bType == b3_capsuleShape ) + { + b3CollideCapsules( &geom, B3_MAX_POINTS_PER_TRIANGLE, aCap, bCap, btoa ); + } + else + { + b3CollideCapsuleAndSphere( &geom, B3_MAX_POINTS_PER_TRIANGLE, aCap, bSphere, btoa ); + } + } + else + { + b3CollideSpheres( &geom, B3_MAX_POINTS_PER_TRIANGLE, aSphere, bSphere, btoa ); + } + + if ( geom.pointCount > 0 ) + { + b3Matrix3 matCA = b3MakeMatrixFromQuat( xfCA.q ); + b3Vec3 normalWorld = b3MulMV( matCA, geom.normal ); + bool normalFlip = collideABodyId != edges0BodyId; + b3Vec3 caOriginRelA = b3SubPos( xfCA.p, xfA.p ); + int tag = -( s + 2 ); + + b3LocalManifold* lm = localManifolds + localCount; + lm->normal = normalFlip ? b3Neg( normalWorld ) : normalWorld; + lm->triangleNormal = lm->normal; + lm->points = localPoints + localPointCount; + lm->pointCount = geom.pointCount; + lm->triangleIndex = tag; + + for ( int j = 0; j < geom.pointCount; ++j ) + { + b3LocalManifoldPoint* src = geom.points + j; + b3LocalManifoldPoint* dst = localPoints + localPointCount; + dst->point = b3Add( caOriginRelA, b3MulMV( matCA, src->point ) ); + dst->separation = src->separation; + dst->pair = src->pair; + dst->triangleIndex = tag; + localPointCount += 1; + } + localCount += 1; + } + + const b3SurfaceMaterial* materialA = b3GetShapeMaterials( primShape ); + subInfo[s].isMesh = false; + subInfo[s].friction = world->frictionCallback( materialA[0].friction, materialA[0].userMaterialId, + materialB[0].friction, materialB[0].userMaterialId ); + subInfo[s].restitution = world->restitutionCallback( materialA[0].restitution, materialA[0].userMaterialId, + materialB[0].restitution, materialB[0].userMaterialId ); + + b3Vec3 tvPrim = b3RotateVector( xfPrimBody.q, materialA[0].tangentVelocity ); + b3Vec3 tvSec = b3RotateVector( xfSecBody.q, materialB[0].tangentVelocity ); + subInfo[s].tangentVelocity = bodyFlip ? b3Sub( tvSec, tvPrim ) : b3Sub( tvPrim, tvSec ); + + if ( materialA[0].rollingResistance > 0.0f || materialB[0].rollingResistance > 0.0f ) + { + float radiusPrim = b3SubShapeRadius( primType, &primSphere, &primCap, primHull ); + float radiusSec = b3SubShapeRadius( secType, &secSphere, &secCap, secHull ); + float maxRadius = b3MaxFloat( radiusPrim, radiusSec ); + subInfo[s].rollingResistance = b3MaxFloat( materialA[0].rollingResistance, materialB[0].rollingResistance ) * maxRadius; + } + else + { + subInfo[s].rollingResistance = 0.0f; + } + } + + sub->touching = localPointCount > subStartPoints; + } + + if ( localCount == 0 || localPointCount == 0 ) + { + if ( contact->manifoldCount > 0 ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = NULL; + contact->manifoldCount = 0; + } + return false; + } + + // Cluster all local manifolds together by normal. + b3Cluster* clusters = b3Bump( &arena, localCount * sizeof( b3Cluster ) ); + int* clusterMemberships = b3Bump( &arena, localCount * sizeof( int ) ); + + const float clusterThreshold = 0.996f; + int clusterCount = 0; + int clusterPointCount = 0; + for ( int i = 0; i < localCount; ++i ) + { + clusterMemberships[i] = B3_NULL_INDEX; + const b3LocalManifold* manifold = localManifolds + i; + clusterPointCount += manifold->pointCount; + + b3Vec3 manifoldNormal = manifold->normal; + b3Vec3 triangleNormal = manifold->triangleNormal; + int clusterIndex = B3_NULL_INDEX; + for ( int j = 0; j < clusterCount; ++j ) + { + float cosManifoldAngle = b3Dot( clusters[j].manifoldNormal, manifoldNormal ); + float cosTriangleAngle = b3Dot( clusters[j].triangleNormal, triangleNormal ); + if ( cosManifoldAngle <= clusterThreshold || cosTriangleAngle <= clusterThreshold ) + { + continue; + } + clusterIndex = j; + break; + } + + if ( clusterIndex != B3_NULL_INDEX ) + { + clusterMemberships[i] = clusterIndex; + clusters[clusterIndex].pointCapacity += manifold->pointCount; + } + else + { + clusters[clusterCount].manifoldNormal = manifoldNormal; + clusters[clusterCount].triangleNormal = triangleNormal; + clusters[clusterCount].pointCapacity = manifold->pointCount; + clusterMemberships[i] = clusterCount; + clusterCount += 1; + } + } + + b3LocalManifoldPoint* clusterPoints = b3Bump( &arena, clusterPointCount * sizeof( b3LocalManifoldPoint ) ); + int pointOffset = 0; + for ( int i = 0; i < clusterCount; ++i ) + { + clusters[i].points = clusterPoints + pointOffset; + clusters[i].pointCount = 0; + pointOffset += clusters[i].pointCapacity; + } + + for ( int i = 0; i < localCount; ++i ) + { + int clusterIndex = clusterMemberships[i]; + B3_ASSERT( 0 <= clusterIndex && clusterIndex < clusterCount ); + + b3LocalManifold* am = localManifolds + i; + b3Cluster* cm = clusters + clusterIndex; + for ( int j = 0; j < am->pointCount; ++j ) + { + B3_ASSERT( cm->pointCount < cm->pointCapacity ); + b3LocalManifoldPoint* ap = am->points + j; + b3LocalManifoldPoint* cp = cm->points + cm->pointCount; + cp->triangleIndex = ap->triangleIndex; + cp->point = ap->point; + cp->separation = ap->separation; + cp->pair = ap->pair; + cm->pointCount += 1; + } + } + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + int reducedCount = b3ReduceCluster( cm->points, cm->pointCount, cm->triangleNormal, arena ); + cm->pointCount = reducedCount; + } + + // Match to previous manifolds for warm starting + int oldManifoldCount = contact->manifoldCount; + b3Manifold* oldManifolds = NULL; + if ( oldManifoldCount > 0 ) + { + oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) ); + memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) ); + } + + if ( oldManifoldCount != clusterCount ) + { + b3FreeManifolds( world, contact->manifolds, contact->manifoldCount ); + contact->manifolds = b3AllocateManifolds( world, clusterCount ); + contact->manifoldCount = (uint16_t)clusterCount; + } + else + { + memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) ); + } + + bool* consumed = NULL; + if ( oldManifoldCount > 0 ) + { + consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) ); + memset( consumed, 0, oldManifoldCount * sizeof( bool ) ); + } + + b3Vec3 offsetBA = b3SubPos( xfA.p, xfB.p ); + const float normalMatchTolerance = 0.995f; + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Cluster* cm = clusters + i; + int pointCount = cm->pointCount; + B3_ASSERT( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); + + b3Manifold* manifold = contact->manifolds + i; + manifold->pointCount = pointCount; + manifold->normal = cm->manifoldNormal; + + float bestDot = normalMatchTolerance; + int bestIndex = B3_NULL_INDEX; + for ( int j = 0; j < oldManifoldCount; ++j ) + { + if ( consumed[j] ) + { + continue; + } + float dot = b3Dot( oldManifolds[j].normal, cm->manifoldNormal ); + if ( dot > bestDot ) + { + bestIndex = j; + bestDot = dot; + } + } + + b3Manifold* matchedManifold = NULL; + if ( bestIndex != B3_NULL_INDEX ) + { + matchedManifold = oldManifolds + bestIndex; + manifold->frictionImpulse = matchedManifold->frictionImpulse; + manifold->rollingImpulse = matchedManifold->rollingImpulse; + manifold->twistImpulse = matchedManifold->twistImpulse; + consumed[bestIndex] = true; + } + + for ( int j = 0; j < pointCount; ++j ) + { + const b3LocalManifoldPoint* source = cm->points + j; + b3ManifoldPoint* target = manifold->points + j; + + target->anchorA = source->point; + target->anchorB = b3Add( target->anchorA, offsetBA ); + target->separation = source->separation; + target->featureId = b3MakeFeatureId( source->pair ); + target->triangleIndex = source->triangleIndex; + + if ( matchedManifold != NULL ) + { + int oldPointCount = matchedManifold->pointCount; + for ( int k = 0; k < oldPointCount; ++k ) + { + b3ManifoldPoint* oldPt = matchedManifold->points + k; + if ( target->featureId == oldPt->featureId && target->triangleIndex == oldPt->triangleIndex ) + { + target->normalImpulse = oldPt->normalImpulse; + target->persisted = true; + oldPt->triangleIndex = B3_NULL_INDEX; + break; + } + } + } + } + } + + // Average friction and restitution across all emitted points. + float frictionSum = 0.0f; + float restitutionSum = 0.0f; + b3Vec3 tangentVelocitySum = b3Vec3_zero; + float sampleCount = 0.0f; + float rollingResistance = 0.0f; + for ( int s = 0; s < subCount; ++s ) + { + rollingResistance = b3MaxFloat( rollingResistance, subInfo[s].rollingResistance ); + } + + for ( int i = 0; i < clusterCount; ++i ) + { + b3Manifold* manifold = contact->manifolds + i; + for ( int j = 0; j < manifold->pointCount; ++j ) + { + int tag = manifold->points[j].triangleIndex; + int subIndex; + float friction; + float restitution; + if ( tag < 0 ) + { + subIndex = -tag - 2; + friction = subInfo[subIndex].friction; + restitution = subInfo[subIndex].restitution; + } + else + { + subIndex = tag >> 22; + int triangleIndex = tag & 0x003FFFFF; + b3SubMaterial* si = subInfo + subIndex; + int materialIndex = 0; + if ( si->materialCount > 0 ) + { + if ( si->isHeightField ) + { + materialIndex = si->triMaterialIndices[triangleIndex >> 1]; + } + else + { + materialIndex = si->triMaterialIndices[triangleIndex]; + if ( si->materialMap != NULL ) + { + materialIndex = si->materialMap[materialIndex]; + } + } + materialIndex = b3ClampInt( materialIndex, 0, si->materialCount - 1 ); + } + b3SurfaceMaterial material = si->materialsA[materialIndex]; + friction = world->frictionCallback( material.friction, material.userMaterialId, si->materialB->friction, + si->materialB->userMaterialId ); + restitution = world->restitutionCallback( material.restitution, material.userMaterialId, + si->materialB->restitution, si->materialB->userMaterialId ); + } + + frictionSum += friction; + restitutionSum += restitution; + tangentVelocitySum = b3Add( tangentVelocitySum, subInfo[subIndex].tangentVelocity ); + sampleCount += 1.0f; + } + } + + if ( sampleCount > 0.0f ) + { + float invCount = 1.0f / sampleCount; + contact->friction = invCount * frictionSum; + contact->restitution = invCount * restitutionSum; + contact->tangentVelocity = b3MulSV( invCount, tangentVelocitySum ); + } + contact->rollingResistance = rollingResistance; + + B3_ASSERT( b3IsValidFloat( contact->friction ) && contact->friction >= 0.0f ); + B3_ASSERT( b3IsValidFloat( contact->restitution ) && contact->restitution >= 0.0f ); + + if ( hitEventFlag ) + { + contact->flags |= b3_simEnableHitEvent; + } + else + { + contact->flags &= ~b3_simEnableHitEvent; + } + + return true; +} diff --git a/src/physics_world.c b/src/physics_world.c index f0dc19e2..b78b963b 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -463,9 +463,17 @@ void b3DestroyWorld( b3WorldId worldId ) b3Contact* contact = contacts + i; if ( contact->contactId != B3_NULL_INDEX ) { - if ( contact->flags & b3_simMeshContact ) + for ( int s = 0; s < contact->subCount; ++s ) { - b3Array_Destroy( contact->meshContact.triangleCache ); + b3SubContact* sub = b3GetSubContact( contact, s ); + if ( sub->isMesh ) + { + b3Array_Destroy( sub->meshContact.triangleCache ); + } + } + if ( contact->extraSubs != NULL ) + { + b3Free( contact->extraSubs, contact->extraCapacity * (int)sizeof( b3SubContact ) ); } } } @@ -540,7 +548,7 @@ int b3GetMaxWorldCount( void ) return b3_maxWorldCount; } -// Issues T0 prefetches across the cache lines of a b3Contact (216 B / 4 lines). +// Issues T0 prefetches across the cache lines of a b3Contact (232 B / 4 lines). // Used to hide the random-access latency of contact lookups while we work on an // earlier index. static inline void b3PrefetchContact( const b3Contact* contact ) @@ -592,11 +600,25 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3Contact* contact = contacts + contactIndex; B3_VALIDATE( contact->contactId == contactIndex ); - b3Shape* shapeA = shapes + contact->shapeIdA; - b3Shape* shapeB = shapes + contact->shapeIdB; + b3Shape* shapeA = shapes + contact->sub0.shapeIdA; + b3Shape* shapeB = shapes + contact->sub0.shapeIdB; - // Do proxies still overlap? + // Do proxies still overlap? A multi-sub contact is only disjoint when every shape pair is disjoint. bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false && contact->subCount > 1 ) + { + for ( int s = 1; s < contact->subCount; ++s ) + { + b3SubContact* sub = b3GetSubContact( contact, s ); + b3Shape* subShapeA = shapes + sub->shapeIdA; + b3Shape* subShapeB = shapes + sub->shapeIdB; + if ( b3AABB_Overlaps( subShapeA->fatAABB, subShapeB->fatAABB ) ) + { + overlap = true; + break; + } + } + } if ( overlap == false ) { // This contact will be destroyed @@ -746,16 +768,29 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* taskContext->manifoldCounts[bucketIndex - 1] += 1; } - // Update the mesh contact spec - if ( touching == true && wasTouching == true && ( contact->flags & b3_simMeshContact ) ) + // Update the scalar contact spec + if ( touching == true && wasTouching == true && ( contact->flags & b3_contactScalarPlacement ) != 0 && + contact->colorIndex != B3_NULL_INDEX ) { - B3_ASSERT( contact->colorIndex != B3_NULL_INDEX ); B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); b3GraphColor* color = graph->colors + contact->colorIndex; b3ContactSpec* spec = b3Array_Get( color->contacts, contact->localIndex ); spec->manifoldCount = (uint16_t)contact->manifoldCount; } + // A wide-placed contact that no longer has exactly one manifold must move to the scalar list + if ( touching == true && wasTouching == true && contact->colorIndex != B3_NULL_INDEX && + ( contact->flags & b3_contactScalarPlacement ) == 0 && contact->manifoldCount != 1 ) + { + contact->flags |= b3_simGraphMove; + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + } + + if ( contact->subCount == 1 ) + { + contact->sub0.touching = touching; + } + // State changes that affect island connectivity. Also affects contact events. if ( touching == true && wasTouching == false ) { @@ -767,6 +802,26 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* contact->flags |= b3_simStoppedTouching; b3SetBit( &taskContext->contactStateBitSet, contactIndex ); } + else if ( contact->flags & b3_contactEnableContactEvents ) + { + for ( int subIndex = 0; subIndex < contact->subCount; ++subIndex ) + { + b3SubContact* sub = b3GetSubContact( contact, subIndex ); + if ( sub->touching != sub->reportedTouching ) + { + b3SetBit( &taskContext->contactStateBitSet, contactIndex ); + break; + } + } + } + else + { + for ( int subIndex = 0; subIndex < contact->subCount; ++subIndex ) + { + b3SubContact* sub = b3GetSubContact( contact, subIndex ); + sub->reportedTouching = sub->touching; + } + } for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) { @@ -809,6 +864,7 @@ static void b3RemoveNonTouchingContact( b3World* world, int setIndex, int localI } } + // Narrow-phase collision static void b3Collide( b3StepContext* context ) { @@ -885,6 +941,7 @@ static void b3Collide( b3StepContext* context ) int minRange = 20; b3ParallelFor( world, b3CollideTask, contactCount, minRange, context, "collide" ); + b3StackFree( &world->stack, contactIndices ); context->awakeContactIndices = NULL; contactIndices = NULL; @@ -921,10 +978,6 @@ static void b3Collide( b3StepContext* context ) b3ArenaSync( &world->taskContexts.data[i].arena ); } - int endEventArrayIndex = world->endEventArrayIndex; - - const b3Shape* shapes = world->shapes.data; - uint16_t worldId = world->worldId; // Process contact state changes. Iterate over set bits for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) @@ -938,16 +991,6 @@ static void b3Collide( b3StepContext* context ) b3Contact* contact = b3Array_Get( world->contacts, contactId ); B3_ASSERT( contact->setIndex == b3_awakeSet ); - const b3Shape* shapeA = shapes + contact->shapeIdA; - const b3Shape* shapeB = shapes + contact->shapeIdB; - b3ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; - b3ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; - b3ContactId contactFullId = { - .index1 = contactId + 1, - .world0 = worldId, - .padding = 0, - .generation = contact->generation, - }; uint32_t flags = contact->flags; if ( flags & b3_simDisjoint ) @@ -960,11 +1003,7 @@ static void b3Collide( b3StepContext* context ) { B3_ASSERT( contact->islandId == B3_NULL_INDEX ); - if ( flags & b3_contactEnableContactEvents ) - { - b3ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; - b3Array_Push( world->contactBeginEvents, event ); - } + b3SyncSubTouchEvents( world, contact ); B3_ASSERT( contact->manifoldCount > 0 ); B3_ASSERT( contact->setIndex == b3_awakeSet ); @@ -991,11 +1030,7 @@ static void b3Collide( b3StepContext* context ) contact->flags &= ~b3_simStoppedTouching; contact->flags &= ~b3_contactTouchingFlag; - if ( contact->flags & b3_contactEnableContactEvents ) - { - b3ContactEndTouchEvent event = { shapeIdA, shapeIdB, contactFullId }; - b3Array_Push( world->contactEndEvents[endEventArrayIndex], event ); - } + b3SyncSubTouchEvents( world, contact ); B3_ASSERT( contact->manifoldCount == 0 ); @@ -1009,10 +1044,26 @@ static void b3Collide( b3StepContext* context ) b3AddNonTouchingContact( world, contact ); - bool isMeshContact = contact->flags & b3_simMeshContact; - b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex, isMeshContact ); + bool scalarPlacement = ( contact->flags & b3_contactScalarPlacement ) != 0; + b3RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex, scalarPlacement ); contact = NULL; } + else + { + if ( ( contact->flags & b3_simGraphMove ) != 0 ) + { + contact->flags &= ~b3_simGraphMove; + if ( contact->colorIndex != B3_NULL_INDEX ) + { + bool scalarPlacement = ( contact->flags & b3_contactScalarPlacement ) != 0; + b3RemoveContactFromGraph( world, contact->edges[0].bodyId, contact->edges[1].bodyId, + contact->colorIndex, contact->localIndex, scalarPlacement ); + b3AddContactToGraph( world, contact ); + } + } + + b3SyncSubTouchEvents( world, contact ); + } // Clear the smallest set bit bits = bits & ( bits - 1 ); @@ -3578,6 +3629,7 @@ void b3ValidateSolverSets( b3World* world ) int totalBodyCount = 0; int totalJointCount = 0; int totalContactCount = 0; + int totalSubCount = 0; int totalIslandCount = 0; // Validate all solver sets @@ -3736,6 +3788,7 @@ void b3ValidateSolverSets( b3World* world ) { int contactIndex = set->contactIndices.data[i]; b3Contact* contact = b3Array_Get( world->contacts, contactIndex ); + totalSubCount += contact->subCount; if ( setIndex == b3_awakeSet ) { // contact should be non-touching if awake @@ -3806,6 +3859,7 @@ void b3ValidateSolverSets( b3World* world ) { int contactId = color->convexContacts.data[i]; b3Contact* contact = b3Array_Get( world->contacts, contactId ); + totalSubCount += contact->subCount; // contact should be touching in the constraint graph or awaiting transfer to non-touching B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); B3_ASSERT( contact->setIndex == b3_awakeSet ); @@ -3832,6 +3886,7 @@ void b3ValidateSolverSets( b3World* world ) { int contactId = color->contacts.data[i].contactId; b3Contact* contact = b3Array_Get( world->contacts, contactId ); + totalSubCount += contact->subCount; // contact should be touching in the constraint graph or awaiting transfer to non-touching B3_ASSERT( contact->manifoldCount > 0 || ( contact->flags & ( b3_simStoppedTouching | b3_simDisjoint ) ) != 0 ); B3_ASSERT( contact->setIndex == b3_awakeSet ); @@ -3884,7 +3939,8 @@ void b3ValidateSolverSets( b3World* world ) int contactIdCount = b3GetIdCount( &world->contactIdPool ); B3_ASSERT( totalContactCount == contactIdCount ); - B3_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count ); + // Each contact is a body pair holding subCount shape pairs; the pair set counts shape pairs. + B3_ASSERT( totalSubCount == (int)world->broadPhase.pairSet.count ); int jointIdCount = b3GetIdCount( &world->jointIdPool ); B3_ASSERT( totalJointCount == jointIdCount ); @@ -3965,8 +4021,8 @@ void b3ValidateContacts( b3World* world ) { B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); // Validate body sim indices - b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Shape* shapeA = b3Array_Get( world->shapes, contact->sub0.shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->sub0.shapeIdB ); b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); @@ -3989,7 +4045,7 @@ void b3ValidateContacts( b3World* world ) B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex ); } - if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) + if ( ( contact->flags & b3_contactScalarPlacement ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) { b3GraphColor* color = graph->colors + contact->colorIndex; int contactId = b3Array_Get( color->contacts, contact->localIndex )->contactId; @@ -4033,41 +4089,44 @@ void b3ValidateContacts( b3World* world ) if ( contact->flags & b3_simMeshContact ) { - int cacheCount = contact->meshContact.triangleCache.count; - if ( cacheCount > 0 ) + // The mesh cache lives on the mesh subs. Other subs share the union with a convex cache. + for ( int s = 0; s < contact->subCount; ++s ) { - B3_ASSERT( contact->meshContact.triangleCache.data != NULL ); - B3_ASSERT( contact->meshContact.triangleCache.capacity >= cacheCount ); + b3SubContact* sub = b3GetSubContact( contact, s ); + b3Shape* shapeA = b3Array_Get( world->shapes, sub->shapeIdA ); - b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); + int triangleCount = B3_NULL_INDEX; if ( shapeA->type == b3_meshShape ) { - int triangleCount = shapeA->mesh.data->triangleCount; - for ( int i = 0; i < cacheCount; ++i ) - { - int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; - B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); - } + triangleCount = shapeA->mesh.data->triangleCount; } else if ( shapeA->type == b3_heightShape ) { - int triangleCount = b3GetHeightFieldTriangleCount( shapeA->heightField ); - for ( int i = 0; i < cacheCount; ++i ) + triangleCount = b3GetHeightFieldTriangleCount( shapeA->heightField ); + } + else if ( shapeA->type == b3_compoundShape ) + { + b3ChildShape child = b3GetCompoundChild( shapeA->compound, sub->childIndex ); + if ( child.type == b3_meshShape ) { - int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; - B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); + triangleCount = child.mesh.data->triangleCount; } } - else + + if ( triangleCount == B3_NULL_INDEX ) + { + continue; + } + + int cacheCount = sub->meshContact.triangleCache.count; + if ( cacheCount > 0 ) { - B3_ASSERT( shapeA->type == b3_compoundShape ); - b3ChildShape child = b3GetCompoundChild( shapeA->compound, contact->childIndex ); - B3_ASSERT( child.type == b3_meshShape ); + B3_ASSERT( sub->meshContact.triangleCache.data != NULL ); + B3_ASSERT( sub->meshContact.triangleCache.capacity >= cacheCount ); - int triangleCount = child.mesh.data->triangleCount; for ( int i = 0; i < cacheCount; ++i ) { - int triangleIndex = contact->meshContact.triangleCache.data[i].triangleIndex; + int triangleIndex = sub->meshContact.triangleCache.data[i].triangleIndex; B3_ASSERT( 0 <= triangleIndex && triangleIndex < triangleCount ); } } diff --git a/src/shape.c b/src/shape.c index e4d9d9e0..201fa781 100644 --- a/src/shape.c +++ b/src/shape.c @@ -478,9 +478,13 @@ static void b3DestroyShapeInternal( b3World* world, b3Shape* shape, b3Body* body b3Contact* contact = b3Array_Get( world->contacts, contactId ); contactKey = contact->edges[edgeIndex].nextKey; - if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + for ( int i = contact->subCount - 1; i >= 0; --i ) { - b3DestroyContact( world, contact, wakeBodies ); + b3SubContact* sub = b3GetSubContact( contact, i ); + if ( sub->shapeIdA == shapeId || sub->shapeIdB == shapeId ) + { + b3RemoveSubContact( world, contact, i, wakeBodies ); + } } } @@ -1329,9 +1333,13 @@ static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool b3Contact* contact = b3Array_Get( world->contacts, contactId ); contactKey = contact->edges[edgeIndex].nextKey; - if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + for ( int i = contact->subCount - 1; i >= 0; --i ) { - b3DestroyContact( world, contact, wakeBodies ); + b3SubContact* sub = b3GetSubContact( contact, i ); + if ( sub->shapeIdA == shapeId || sub->shapeIdB == shapeId ) + { + b3RemoveSubContact( world, contact, i, wakeBodies ); + } } } @@ -1702,11 +1710,11 @@ int b3Shape_GetContactData( b3ShapeId shapeId, b3ContactData* contactData, int c b3Contact* contact = b3Array_Get( world->contacts, contactId ); // Does contact involve this shape and is it touching? - if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + if ( ( contact->sub0.shapeIdA == shapeId.index1 - 1 || contact->sub0.shapeIdB == shapeId.index1 - 1 ) && ( contact->flags & b3_contactTouchingFlag ) != 0 ) { - b3Shape* shapeA = world->shapes.data + contact->shapeIdA; - b3Shape* shapeB = world->shapes.data + contact->shapeIdB; + b3Shape* shapeA = world->shapes.data + contact->sub0.shapeIdA; + b3Shape* shapeB = world->shapes.data + contact->sub0.shapeIdB; contactData[index].contactId = (b3ContactId){ contact->contactId + 1, shapeId.world0, 0, contact->generation }; contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, shapeId.world0, shapeA->generation }; diff --git a/src/solver.c b/src/solver.c index a2b1be97..a1b35daf 100644 --- a/src/solver.c +++ b/src/solver.c @@ -1441,6 +1441,15 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) int awakeBodyCount = awakeSet->bodySims.count; if ( awakeBodyCount == 0 ) { + // The rebuild task queued in b3UpdateBroadPhasePairs clears enlarged nodes. It must finish before + // validating, matching the non-early-return path that finishes it before refit. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + b3ValidateNoEnlarged( &world->broadPhase ); return; } @@ -2002,8 +2011,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) b3Contact* contact = contactArray + contactId; B3_ASSERT( contact->setIndex == b3_awakeSet && contact->colorIndex != B3_NULL_INDEX ); - b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); + b3Shape* shapeA = b3Array_Get( world->shapes, contact->sub0.shapeIdA ); + b3Shape* shapeB = b3Array_Get( world->shapes, contact->sub0.shapeIdB ); b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); b3BodySim* simA = b3GetBodySim( world, bodyA ); @@ -2051,7 +2060,7 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) // shapeB is never a compound today (asserted in b3CreateContact), so the // childIndex argument is irrelevant for it. shapeA carries the compound. - event.userMaterialIdA = b3GetShapeUserMaterialId( shapeA, contact->childIndex, triangleIndex ); + event.userMaterialIdA = b3GetShapeUserMaterialId( shapeA, contact->sub0.childIndex, triangleIndex ); event.userMaterialIdB = b3GetShapeUserMaterialId( shapeB, 0, triangleIndex ); b3Array_Push( world->contactHitEvents, event ); diff --git a/src/solver_set.c b/src/solver_set.c index f050a869..0a3a0b35 100644 --- a/src/solver_set.c +++ b/src/solver_set.c @@ -324,7 +324,7 @@ void b3TrySleepIsland( b3World* world, int islandId ) b3Array_Push( sleepSet->contactIndices, contactId ); int localIndex = contact->localIndex; - if ( ( contact->flags & b3_simMeshContact ) || colorIndex == B3_OVERFLOW_INDEX ) + if ( ( contact->flags & b3_contactScalarPlacement ) || colorIndex == B3_OVERFLOW_INDEX ) { int movedLocalIndex = b3Array_RemoveSwap( color->contacts, localIndex ); if ( movedLocalIndex != B3_NULL_INDEX ) diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 9efaee9f..0ada07f4 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -32,8 +32,9 @@ #include // Snapshot image magic 'BNS3' and version +// Version 3 changed the contact image to body-pair contacts with sub contacts #define B3_SNAP_MAGIC 0x33534E42u -#define B3_SNAP_VERSION 2u +#define B3_SNAP_VERSION 3u #define B3_SNAP_FLAG_VALIDATION 0x1u #define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u @@ -797,11 +798,13 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world ) copy.manifolds = NULL; copy.bodySimIndexA = B3_NULL_INDEX; copy.bodySimIndexB = B3_NULL_INDEX; - if ( copy.flags & b3_simMeshContact ) + copy.extraSubs = NULL; + copy.extraCapacity = 0; + if ( copy.sub0.isMesh ) { - copy.meshContact.triangleCache.data = NULL; - copy.meshContact.triangleCache.count = 0; - copy.meshContact.triangleCache.capacity = 0; + copy.sub0.meshContact.triangleCache.data = NULL; + copy.sub0.meshContact.triangleCache.count = 0; + copy.sub0.meshContact.triangleCache.capacity = 0; } b3SnapW_Bytes( buf, ©, sizeof( b3Contact ) ); @@ -809,7 +812,6 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world ) { // Free slot: no heap data b3SnapW_I32( buf, 0 ); // manifoldCount - // No triangleCache continue; } @@ -820,14 +822,31 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world ) b3SnapW_Bytes( buf, c->manifolds, c->manifoldCount * (int)sizeof( b3Manifold ) ); } - // Mesh triangleCache - if ( c->flags & b3_simMeshContact ) + // Extra subs with per-sub cache pointers zeroed + for ( int subIndex = 1; subIndex < c->subCount; ++subIndex ) { - b3SnapW_I32( buf, c->meshContact.triangleCache.count ); - if ( c->meshContact.triangleCache.count > 0 ) + b3SubContact subCopy = c->extraSubs[subIndex - 1]; + if ( subCopy.isMesh ) { - b3SnapW_Bytes( buf, c->meshContact.triangleCache.data, - c->meshContact.triangleCache.count * (int)sizeof( b3TriangleCache ) ); + subCopy.meshContact.triangleCache.data = NULL; + subCopy.meshContact.triangleCache.count = 0; + subCopy.meshContact.triangleCache.capacity = 0; + } + b3SnapW_Bytes( buf, &subCopy, sizeof( b3SubContact ) ); + } + + // Per-sub mesh triangleCache + for ( int subIndex = 0; subIndex < c->subCount; ++subIndex ) + { + const b3SubContact* sub = subIndex == 0 ? &c->sub0 : c->extraSubs + ( subIndex - 1 ); + if ( sub->isMesh ) + { + b3SnapW_I32( buf, sub->meshContact.triangleCache.count ); + if ( sub->meshContact.triangleCache.count > 0 ) + { + b3SnapW_Bytes( buf, sub->meshContact.triangleCache.data, + sub->meshContact.triangleCache.count * (int)sizeof( b3TriangleCache ) ); + } } } } @@ -855,14 +874,20 @@ static void b3DesContacts( b3SnapReader* r, b3World* world ) dst->manifolds = NULL; dst->bodySimIndexA = B3_NULL_INDEX; dst->bodySimIndexB = B3_NULL_INDEX; - if ( dst->flags & b3_simMeshContact ) + dst->extraSubs = NULL; + dst->extraCapacity = 0; + if ( dst->sub0.isMesh ) { - dst->meshContact.triangleCache.data = NULL; - dst->meshContact.triangleCache.count = 0; - dst->meshContact.triangleCache.capacity = 0; + dst->sub0.meshContact.triangleCache.data = NULL; + dst->sub0.meshContact.triangleCache.count = 0; + dst->sub0.meshContact.triangleCache.capacity = 0; } bool isLive = ( dst->contactId == i ); + if ( isLive == false ) + { + dst->subCount = 0; + } int manifoldCount = b3SnapR_I32( r ); @@ -888,9 +913,32 @@ static void b3DesContacts( b3SnapReader* r, b3World* world ) dst->manifoldCount = 0; } - // Mesh triangleCache - if ( isLive && ( dst->flags & b3_simMeshContact ) ) + if ( isLive && dst->subCount > 1 ) + { + int extraCount = dst->subCount - 1; + if ( b3SnapCheckCount( r, extraCount, (int)sizeof( b3SubContact ), (int)sizeof( b3SubContact ) ) == false ) + { + r->ok = false; + break; + } + dst->extraSubs = b3Alloc( extraCount * sizeof( b3SubContact ) ); + dst->extraCapacity = (uint16_t)extraCount; + b3SnapR_Bytes( r, dst->extraSubs, extraCount * (int)sizeof( b3SubContact ) ); + } + + // Per-sub mesh triangleCache + for ( int subIndex = 0; isLive && subIndex < dst->subCount && r->ok; ++subIndex ) { + b3SubContact* sub = subIndex == 0 ? &dst->sub0 : dst->extraSubs + ( subIndex - 1 ); + if ( sub->isMesh == false ) + { + continue; + } + + sub->meshContact.triangleCache.data = NULL; + sub->meshContact.triangleCache.count = 0; + sub->meshContact.triangleCache.capacity = 0; + int cacheCount = b3SnapR_I32( r ); if ( !r->ok ) { @@ -903,8 +951,8 @@ static void b3DesContacts( b3SnapReader* r, b3World* world ) r->ok = false; break; } - b3Array_Resize( dst->meshContact.triangleCache, cacheCount ); - b3SnapR_Bytes( r, dst->meshContact.triangleCache.data, cacheCount * (int)sizeof( b3TriangleCache ) ); + b3Array_Resize( sub->meshContact.triangleCache, cacheCount ); + b3SnapR_Bytes( r, sub->meshContact.triangleCache.data, cacheCount * (int)sizeof( b3TriangleCache ) ); } } } @@ -951,9 +999,19 @@ static void b3FreeLiveSimElements( b3World* world ) c->manifolds = NULL; c->manifoldCount = 0; } - if ( c->flags & b3_simMeshContact ) + for ( int subIndex = 0; subIndex < c->subCount; ++subIndex ) + { + b3SubContact* sub = subIndex == 0 ? &c->sub0 : c->extraSubs + ( subIndex - 1 ); + if ( sub->isMesh ) + { + b3Array_Destroy( sub->meshContact.triangleCache ); + } + } + if ( c->extraSubs != NULL ) { - b3Array_Destroy( c->meshContact.triangleCache ); + b3Free( c->extraSubs, c->extraCapacity * (int)sizeof( b3SubContact ) ); + c->extraSubs = NULL; + c->extraCapacity = 0; } } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 92dcbb10..28871b13 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -9,6 +9,7 @@ set(BOX3D_TEST_FILES test_collision.c test_compound.c test_container.c + test_compound_contact.c test_determinism.c test_distance.c test_height_field.c diff --git a/test/main.c b/test/main.c index 8edcf966..606a9ae8 100644 --- a/test/main.c +++ b/test/main.c @@ -31,6 +31,7 @@ extern int BodyTest( void ); extern int BodyQueryTest( void ); extern int CollisionTest( void ); extern int CompoundTest( void ); +extern int CompoundContactTest( void ); extern int ContainerTest( void ); extern int DeterminismTest( void ); extern int DistanceTest( void ); @@ -103,6 +104,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( BodyQueryTest ); MAYBE_RUN_TEST( CollisionTest ); MAYBE_RUN_TEST( CompoundTest ); + MAYBE_RUN_TEST( CompoundContactTest ); MAYBE_RUN_TEST( ContainerTest ); MAYBE_RUN_TEST( DeterminismTest ); MAYBE_RUN_TEST( DistanceTest ); diff --git a/test/test_compound_contact.c b/test/test_compound_contact.c new file mode 100644 index 00000000..8062382d --- /dev/null +++ b/test/test_compound_contact.c @@ -0,0 +1,332 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/math_functions.h" +#include "test_macros.h" + +// A multi-shape body, so a body pair carries many sub-contacts and clustering engages. +static b3BodyId CreateSlab( b3WorldId worldId, float x, float y, float z, b3BodyType type, float restitution, float density ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = type; + bodyDef.position = ( b3Pos ){ x, y, z }; + b3BodyId body = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = density; + shapeDef.baseMaterial.friction = 0.5f; + shapeDef.baseMaterial.restitution = restitution; + + for ( int i = -1; i <= 1; ++i ) + { + for ( int k = -1; k <= 1; ++k ) + { + b3Vec3 offset = { (float)i, 0.0f, (float)k }; + b3BoxHull box = b3MakeOffsetBoxHull( 0.5f, 0.5f, 0.5f, offset ); + b3CreateHullShape( body, &shapeDef, &box.base ); + } + } + + return body; +} + +static void AddGround( b3WorldId worldId ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = ( b3Pos ){ 0.0f, -1.0f, 0.0f }; + b3BodyId ground = b3CreateBody( worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( ground, &shapeDef, &box.base ); +} + +static void StepN( b3WorldId worldId, int steps ) +{ + for ( int i = 0; i < steps; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + } +} + +static float BodySpeed( b3BodyId body ) +{ + return b3Length( b3Body_GetLinearVelocity( body ) ) + b3Length( b3Body_GetAngularVelocity( body ) ); +} + +static void RunStack( float* topY, float* maxSpeed ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + + b3BodyId slabs[3]; + for ( int i = 0; i < 3; ++i ) + { + slabs[i] = CreateSlab( worldId, 0.0f, 0.55f + i * 1.05f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + } + + StepN( worldId, 600 ); + + float ms = 0.0f; + for ( int i = 0; i < 3; ++i ) + { + float s = BodySpeed( slabs[i] ); + ms = s > ms ? s : ms; + } + + *maxSpeed = ms; + *topY = (float)b3Body_GetPosition( slabs[2] ).y; + + b3DestroyWorld( worldId ); +} + +// A stack of multi-shape slabs must settle without gaining energy or sinking through itself. +static int CompoundStackTest( void ) +{ + float topY, maxSpeed; + RunStack( &topY, &maxSpeed ); + + ENSURE( maxSpeed < 0.1f ); + ENSURE( topY > 2.4f && topY < 2.6f ); + + return 0; +} + +static float RunBounce( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + b3BodyId slab = CreateSlab( worldId, 0.0f, 3.0f, 0.0f, b3_dynamicBody, 0.7f, 1.0f ); + + float peak = 0.0f; + bool contacted = false; + for ( int i = 0; i < 250; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + float y = (float)b3Body_GetPosition( slab ).y; + contacted = contacted || y < 0.7f; + if ( contacted && y > peak ) + { + peak = y; + } + } + + b3DestroyWorld( worldId ); + return peak; +} + +// Restitution stays physical: the slab bounces up but never above its drop height. +static int CompoundRestitutionTest( void ) +{ + float peak = RunBounce(); + + ENSURE( peak > 1.0f ); + ENSURE( peak < 3.0f ); + + return 0; +} + +// Compound on mesh contacts: the slab rests on the mesh without sinking through. +static int CompoundMeshTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef meshBodyDef = b3DefaultBodyDef(); + b3BodyId meshBody = b3CreateBody( worldId, &meshBodyDef ); + b3MeshData* mesh = b3CreateGridMesh( 10, 10, 1.0f, 0, false ); + ENSURE( mesh != NULL ); + b3ShapeDef meshShapeDef = b3DefaultShapeDef(); + b3CreateMeshShape( meshBody, &meshShapeDef, mesh, ( b3Vec3 ){ 1.0f, 1.0f, 1.0f } ); + + b3BodyId slab = CreateSlab( worldId, 0.0f, 2.0f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + + StepN( worldId, 400 ); + + float y = (float)b3Body_GetPosition( slab ).y; + ENSURE( y > 0.3f && y < 0.7f ); + ENSURE( BodySpeed( slab ) < 0.1f ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + + return 0; +} + +static float RunMassRatio( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + CreateSlab( worldId, 0.0f, 0.55f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + b3BodyId heavy = CreateSlab( worldId, 0.0f, 1.6f, 0.0f, b3_dynamicBody, 0.0f, 200.0f ); + + StepN( worldId, 600 ); + + float y = (float)b3Body_GetPosition( heavy ).y; + b3DestroyWorld( worldId ); + return y; +} + +// A heavy body on a light one must not crush through the reduced contact set. +static int CompoundMassRatioTest( void ) +{ + float y = RunMassRatio(); + + ENSURE( y > 1.35f && y < 1.65f ); + + return 0; +} + +// A spinning body must not gain energy from contact point churn. +static int CompoundRollingTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + b3BodyId slab = CreateSlab( worldId, 0.0f, 0.5f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + b3Body_SetAngularVelocity( slab, ( b3Vec3 ){ 0.0f, 8.0f, 0.0f } ); + + float maxSpeed = 0.0f; + for ( int i = 0; i < 300; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + float s = BodySpeed( slab ); + maxSpeed = s > maxSpeed ? s : maxSpeed; + } + + float finalSpeed = BodySpeed( slab ); + float finalY = (float)b3Body_GetPosition( slab ).y; + b3DestroyWorld( worldId ); + + ENSURE( maxSpeed < 8.5f ); + ENSURE( finalSpeed < 8.0f ); + ENSURE( finalY > 0.3f && finalY < 0.7f ); + + return 0; +} + +// Touch and hit events still fire with sane data for merged contacts. +static int CompoundEventsTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = ( b3Pos ){ 0.0f, -1.0f, 0.0f }; + b3BodyId ground = b3CreateBody( worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 20.0f, 1.0f, 20.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.enableContactEvents = true; + shapeDef.enableHitEvents = true; + b3CreateHullShape( ground, &shapeDef, &box.base ); + } + + b3BodyId slab = CreateSlab( worldId, 0.0f, 4.0f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + b3Body_SetLinearVelocity( slab, ( b3Vec3 ){ 0.0f, -10.0f, 0.0f } ); + + int beginTotal = 0; + int hitTotal = 0; + bool hitDataValid = true; + for ( int i = 0; i < 120; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + b3ContactEvents events = b3World_GetContactEvents( worldId ); + beginTotal += events.beginCount; + hitTotal += events.hitCount; + for ( int e = 0; e < events.hitCount; ++e ) + { + const b3ContactHitEvent* h = events.hitEvents + e; + b3Vec3 pt = { (float)h->point.x, (float)h->point.y, (float)h->point.z }; + if ( b3IsValidVec3( pt ) == false || b3IsValidVec3( h->normal ) == false || h->approachSpeed <= 0.0f ) + { + hitDataValid = false; + } + } + } + + b3DestroyWorld( worldId ); + + ENSURE( beginTotal > 0 ); + ENSURE( hitTotal > 0 ); + ENSURE( hitDataValid ); + + return 0; +} + +// Spread normals on a sphere must stay finite and bounded (no NaN or tunnelling). +static int CompoundCurvedTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = ( b3Pos ){ 0.0f, 2.0f, 0.0f }; + b3BodyId sphereBody = b3CreateBody( worldId, &bodyDef ); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 2.0f }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateSphereShape( sphereBody, &shapeDef, &sphere ); + } + + b3BodyId slab = CreateSlab( worldId, 0.0f, 6.0f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + + for ( int i = 0; i < 300; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + b3Pos p = b3Body_GetPosition( slab ); + b3Vec3 v = { (float)p.x, (float)p.y, (float)p.z }; + ENSURE( b3IsValidVec3( v ) ); + ENSURE( v.y > -1.0f && v.y < 12.0f ); + } + + b3DestroyWorld( worldId ); + + return 0; +} + +// Friction on the merged contacts must brake a sliding body. +static int CompoundSlidingTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + AddGround( worldId ); + b3BodyId slab = CreateSlab( worldId, 0.0f, 0.5f, 0.0f, b3_dynamicBody, 0.0f, 1.0f ); + b3Body_SetLinearVelocity( slab, ( b3Vec3 ){ 5.0f, 0.0f, 0.0f } ); + + StepN( worldId, 300 ); + + float speed = BodySpeed( slab ); + float y = (float)b3Body_GetPosition( slab ).y; + b3DestroyWorld( worldId ); + + ENSURE( speed < 0.2f ); + ENSURE( y > 0.3f && y < 0.7f ); + + return 0; +} + +int CompoundContactTest( void ) +{ + RUN_SUBTEST( CompoundStackTest ); + RUN_SUBTEST( CompoundRestitutionTest ); + RUN_SUBTEST( CompoundMeshTest ); + RUN_SUBTEST( CompoundMassRatioTest ); + RUN_SUBTEST( CompoundRollingTest ); + RUN_SUBTEST( CompoundEventsTest ); + RUN_SUBTEST( CompoundCurvedTest ); + RUN_SUBTEST( CompoundSlidingTest ); + + return 0; +} diff --git a/test/test_determinism.c b/test/test_determinism.c index 888a0d67..821ec28d 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -3,6 +3,7 @@ #include "box3d/box3d.h" #include "determinism.h" +#include "metal_wheel1_hulls.h" #include "test_macros.h" #include @@ -19,9 +20,11 @@ #if defined( BOX3D_DOUBLE_PRECISION ) #define EXPECTED_SLEEP_STEP 301 #define EXPECTED_HASH 0xE4844A97 +#define EXPECTED_WHEEL_HASH 0xEC7CDF3B #else #define EXPECTED_SLEEP_STEP 269 #define EXPECTED_HASH 0x50313037 +#define EXPECTED_WHEEL_HASH 0x1A105A82 #endif static int SingleMultithreadingTest( int workerCount ) @@ -111,10 +114,100 @@ static int CrossPlatformTest( void ) return 0; } +// Step the wheel stack and hash the body transforms. +static uint32_t RunWheelStackHash( int workerCount, int stepCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = ( b3Pos ){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull box = b3MakeBoxHull( 10.0f, 1.0f, 10.0f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( groundId, &shapeDef, &box.base ); + } + + b3HullData* hulls[s_metalWheel1HullCount]; + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + const WheelHullSpan span = s_metalWheel1Hulls[h]; + hulls[h] = b3CreateHull( &s_metalWheel1Verts[span.offset], span.count, span.count ); + } + + const float height = 0.171f; + const float spacing = height + 0.006f; + const float startY = 0.5f * height + 0.004f; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.6f; + + const int wheelCount = 10; + b3BodyId wheelBodies[10]; + for ( int i = 0; i < wheelCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = ( b3Pos ){ 0.0f, startY + i * spacing, 0.0f }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3CreateHullShape( bodyId, &shapeDef, hulls[h] ); + } + wheelBodies[i] = bodyId; + } + for ( int h = 0; h < s_metalWheel1HullCount; ++h ) + { + b3DestroyHull( hulls[h] ); + } + + float timeStep = 1.0f / 60.0f; + for ( int i = 0; i < stepCount; ++i ) + { + b3World_Step( worldId, timeStep, 8 ); + } + + uint32_t hash = B3_HASH_INIT; + for ( int i = 0; i < wheelCount; ++i ) + { + b3WorldTransform xf = b3Body_GetTransform( wheelBodies[i] ); + hash = b3Hash( hash, (uint8_t*)( &xf ), sizeof( b3WorldTransform ) ); + } + + b3DestroyWorld( worldId ); + return hash; +} + +// Compound contacts must be deterministic: identical hash at every worker count and against the reference. +static int WheelStackDeterminismTest( void ) +{ + uint32_t base = RunWheelStackHash( 1, 200 ); + + for ( int workerCount = 2; workerCount <= 4; ++workerCount ) + { + uint32_t hash = RunWheelStackHash( workerCount, 200 ); + if ( hash != base ) + { + printf( " wheel stack: workers=%d hash=0x%08X base=0x%08X\n", workerCount, hash, base ); + } + ENSURE( hash == base ); + } + + if ( base != EXPECTED_WHEEL_HASH ) + { + printf( " wheel stack cross-platform: hash=0x%08X expected=0x%08X\n", base, EXPECTED_WHEEL_HASH ); + } + ENSURE( base == EXPECTED_WHEEL_HASH ); + + return 0; +} + int DeterminismTest( void ) { RUN_SUBTEST( MultithreadingTest ); RUN_SUBTEST( CrossPlatformTest ); + RUN_SUBTEST( WheelStackDeterminismTest ); return 0; }