From b6984de2be5fdd68ce4e33ba07e7e24c0f1fb9c7 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 25 Jan 2025 12:08:20 -0800 Subject: [PATCH] wip --- include/box2d/collision.h | 10 ++-- include/box2d/types.h | 4 +- samples/main.cpp | 13 +----- samples/sample.cpp | 29 +++++++++--- samples/sample.h | 7 +-- samples/sample_benchmark.cpp | 29 +++++++++--- samples/sample_shapes.cpp | 91 +++++++++++++++++++++++++++++++++++- samples/sample_stacking.cpp | 17 +++++-- src/constraint_graph.c | 4 +- src/contact.c | 20 ++++++++ src/contact.h | 3 +- src/contact_solver.c | 33 ++++++++++++- src/contact_solver.h | 4 ++ src/core.h | 3 +- src/shape.c | 2 + src/shape.h | 17 +++++++ src/solver.c | 1 + 17 files changed, 244 insertions(+), 43 deletions(-) diff --git a/include/box2d/collision.h b/include/box2d/collision.h index b71eced8c..c00946cf2 100644 --- a/include/box2d/collision.h +++ b/include/box2d/collision.h @@ -535,14 +535,18 @@ typedef struct b2ManifoldPoint /// @note Box2D uses speculative collision so some contact points may be separated. typedef struct b2Manifold { - /// The manifold points, up to two are possible in 2D - b2ManifoldPoint points[2]; - /// The unit normal vector in world space, points from shape A to bodyB b2Vec2 normal; + /// Angular impulse applied for rolling resistance. N * m * s = kg * m^2 / s + float rollingImpulse; + + /// The manifold points, up to two are possible in 2D + b2ManifoldPoint points[2]; + /// The number of contacts points, will be 0, 1, or 2 int pointCount; + } b2Manifold; /// Compute the contact manifold between two circles diff --git a/include/box2d/types.h b/include/box2d/types.h index dab2cee19..a608a8530 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -346,9 +346,11 @@ typedef struct b2ShapeDef float restitution; /// The rolling resistance usually in the range [0,1]. - /// todo float rollingResistance; + /// The tangent speed for conveyor belts + float tangentSpeed; + /// User material identifier. This is passed with query results and to friction and restitution /// combining functions. It is not used internally. int material; diff --git a/samples/main.cpp b/samples/main.cpp index 50e47df4b..d80c821be 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -640,8 +640,6 @@ int main( int, char** ) float frameTime = 0.0; - // int32_t frame = 0; - while ( !glfwWindowShouldClose( g_mainWindow ) ) { double time1 = glfwGetTime(); @@ -754,21 +752,14 @@ int main( int, char** ) // Limit frame rate to 60Hz double time2 = glfwGetTime(); - double targetTime = time1 + 1.0f / 60.0f; - // int loopCount = 0; + double targetTime = time1 + 1.0 / 60.0; while ( time2 < targetTime ) { b2Yield(); time2 = glfwGetTime(); - //++loopCount; } - frameTime = (float)( time2 - time1 ); - // if (frame % 17 == 0) - //{ - // printf("loop count = %d, frame time = %.1f\n", loopCount, 1000.0f * frameTime); - // } - //++frame; + frameTime = float( time2 - time1 ); } delete s_sample; diff --git a/samples/sample.cpp b/samples/sample.cpp index 2c77b6ae0..2c3e3fcd1 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -101,14 +101,8 @@ Sample::Sample( Settings& settings ) m_threadCount = 1 + settings.workerCount; - b2WorldDef worldDef = b2DefaultWorldDef(); - worldDef.workerCount = settings.workerCount; - worldDef.enqueueTask = EnqueueTask; - worldDef.finishTask = FinishTask; - worldDef.userTaskContext = this; - worldDef.enableSleep = settings.enableSleep; + m_worldId = b2_nullWorldId; - m_worldId = b2CreateWorld( &worldDef ); m_textLine = 30; m_textIncrement = 22; m_mouseJointId = b2_nullJointId; @@ -122,6 +116,9 @@ Sample::Sample( Settings& settings ) g_seed = RAND_SEED; + m_settings = &settings; + + CreateWorld( ); TestMathCpp(); } @@ -134,6 +131,24 @@ Sample::~Sample() delete[] m_tasks; } +void Sample::CreateWorld( ) +{ + if ( B2_IS_NON_NULL( m_worldId ) ) + { + b2DestroyWorld( m_worldId ); + m_worldId = b2_nullWorldId; + } + + b2WorldDef worldDef = b2DefaultWorldDef(); + worldDef.workerCount = m_settings->workerCount; + worldDef.enqueueTask = EnqueueTask; + worldDef.finishTask = FinishTask; + worldDef.userTaskContext = this; + worldDef.enableSleep = m_settings->enableSleep; + + m_worldId = b2CreateWorld( &worldDef ); +} + void Sample::DrawTitle( const char* string ) { g_draw.DrawString( 5, 5, string ); diff --git a/samples/sample.h b/samples/sample.h index e1236d8d0..356b7ec22 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -5,11 +5,10 @@ #include "box2d/id.h" #include "box2d/types.h" +#include "settings.h" #define ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) -struct Settings; - namespace enki { class TaskScheduler; @@ -43,6 +42,8 @@ class Sample explicit Sample( Settings& settings ); virtual ~Sample(); + void CreateWorld( ); + void DrawTitle( const char* string ); virtual void Step( Settings& settings ); virtual void UpdateUI() @@ -66,9 +67,9 @@ class Sample static constexpr int m_maxTasks = 64; static constexpr int m_maxThreads = 64; + const Settings* m_settings; enki::TaskScheduler* m_scheduler; class SampleTask* m_tasks; - int m_taskCount; int m_threadCount; diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 71c3338a6..62dac89a4 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -557,7 +557,7 @@ class BenchmarkLargePyramid : public Sample settings.enableSleep = false; } - CreateLargePyramid(m_worldId); + CreateLargePyramid( m_worldId ); } static Sample* Create( Settings& settings ) @@ -624,6 +624,9 @@ class BenchmarkCreateDestroy : public Sample m_bodies[i] = b2_nullBodyId; } + m_createTime = 0.0f; + m_destroyTime = 0.0f; + m_baseCount = g_sampleDebug ? 40 : 100; m_iterations = g_sampleDebug ? 1 : 10; m_bodyCount = 0; @@ -631,6 +634,8 @@ class BenchmarkCreateDestroy : public Sample void CreateScene() { + uint64_t ticks = b2GetTicks(); + for ( int i = 0; i < e_maxBodyCount; ++i ) { if ( B2_IS_NON_NULL( m_bodies[i] ) ) @@ -640,6 +645,8 @@ class BenchmarkCreateDestroy : public Sample } } + m_destroyTime += b2GetMillisecondsAndReset( &ticks ); + int count = m_baseCount; float rad = 0.5f; float shift = rad * 2.0f; @@ -675,22 +682,28 @@ class BenchmarkCreateDestroy : public Sample } } + m_createTime += b2GetMilliseconds( ticks ); + m_bodyCount = index; + + b2World_Step( m_worldId, 1.0f / 60.0f, 4 ); } void Step( Settings& settings ) override { - uint64_t ticks = b2GetTicks(); + m_createTime = 0.0f; + m_destroyTime = 0.0f; for ( int i = 0; i < m_iterations; ++i ) { CreateScene(); } - float ms = b2GetMilliseconds( ticks ); + DrawTextLine( "total: create = %g ms, destroy = %g ms", m_createTime, m_destroyTime ); - g_draw.DrawString( 5, m_textLine, "milliseconds = %g", ms ); - m_textLine += m_textIncrement; + float createPerBody = 1000.0f * m_createTime / m_iterations / m_bodyCount; + float destroyPerBody = 1000.0f * m_destroyTime / m_iterations / m_bodyCount; + DrawTextLine( "body: create = %g us, destroy = %g us", createPerBody, destroyPerBody ); Sample::Step( settings ); } @@ -700,6 +713,8 @@ class BenchmarkCreateDestroy : public Sample return new BenchmarkCreateDestroy( settings ); } + float m_createTime; + float m_destroyTime; b2BodyId m_bodies[e_maxBodyCount]; int m_bodyCount; int m_baseCount; @@ -1529,14 +1544,14 @@ class BenchmarkRain : public Sample void Step( Settings& settings ) override { - if (settings.pause == false || settings.singleStep == true) + if ( settings.pause == false || settings.singleStep == true ) { StepRain( m_worldId, m_stepCount ); } Sample::Step( settings ); - if (m_stepCount % 1000 == 0) + if ( m_stepCount % 1000 == 0 ) { m_stepCount += 0; } diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index a09d1e5d7..aaa831a2b 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -952,7 +952,96 @@ class Friction : public Sample } }; -static int sampleIndex3 = RegisterSample( "Shapes", "Friction", Friction::Create ); +static int sampleFriction = RegisterSample( "Shapes", "Friction", Friction::Create ); + +class RollingResistance : public Sample +{ +public: + explicit RollingResistance( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 5.0f, 20.0f }; + g_camera.m_zoom = 27.5f; + } + + m_lift = 0.0f; + CreateScene(); + } + + void CreateScene() + { + b2Circle circle = { b2Vec2_zero, 0.5f }; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + + for ( int i = 0; i < 20; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Segment segment = { { -40.0f, 2.0f * i }, { 40.0f, 2.0f * i + m_lift } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -39.5f, 2.0f * i + 0.75f }; + bodyDef.angularVelocity = -10.0f; + bodyDef.linearVelocity = { 5.0f, 0.0f }; + + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + shapeDef.rollingResistance = 0.1f * i; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + } + + void Keyboard( int key ) override + { + switch ( key ) + { + case GLFW_KEY_1: + m_lift = 0.0f; + CreateWorld(); + CreateScene(); + break; + + case GLFW_KEY_2: + m_lift = 5.0f; + CreateWorld(); + CreateScene(); + break; + + case GLFW_KEY_3: + m_lift = -5.0f; + CreateWorld(); + CreateScene(); + break; + + default: + Sample::Keyboard( key ); + break; + } + } + + void Step( Settings& settings ) override + { + Sample::Step( settings ); + + for ( int i = 0; i < 20; ++i ) + { + g_draw.DrawString( { -41.5f, 2.0f * i + 1.0f }, "%.2f", 0.1f * i ); + } + } + + static Sample* Create( Settings& settings ) + { + return new RollingResistance( settings ); + } + + float m_lift; +}; + +static int sampleRollingResistance = RegisterSample( "Shapes", "Rolling Resistance", RollingResistance::Create ); // This sample shows how to modify the geometry on an existing shape. This is only supported on // dynamic and kinematic shapes because static shapes don't look for new collisions. diff --git a/samples/sample_stacking.cpp b/samples/sample_stacking.cpp index 79728327d..235372bb7 100644 --- a/samples/sample_stacking.cpp +++ b/samples/sample_stacking.cpp @@ -32,16 +32,17 @@ class SingleBox : public Sample float groundWidth = 66.0f * extent; b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.friction = 0.5f; + //shapeDef.friction = 0.5f; b2Segment segment = { { -0.5f * 2.0f * groundWidth, 0.0f }, { 0.5f * 2.0f * groundWidth, 0.0f } }; b2CreateSegmentShape( groundId, &shapeDef, &segment ); bodyDef.type = b2_dynamicBody; b2Polygon box = b2MakeBox( extent, extent ); - bodyDef.position = { 0.0f, 4.0f }; - b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); - b2CreatePolygonShape( bodyId, &shapeDef, &box ); + bodyDef.position = { 0.0f, 1.0f }; + bodyDef.linearVelocity = { 5.0f, 0.0f }; + m_bodyId = b2CreateBody( m_worldId, &bodyDef ); + b2CreatePolygonShape( m_bodyId, &shapeDef, &box ); } void Step( Settings& settings ) override @@ -49,12 +50,17 @@ class SingleBox : public Sample Sample::Step( settings ); // g_draw.DrawCircle({0.0f, 2.0f}, 1.0f, b2_colorWhite); + + b2Vec2 position = b2Body_GetPosition( m_bodyId ); + DrawTextLine( "(x, y) = (%.2g, %.2g)", position.x, position.y ); } static Sample* Create( Settings& settings ) { return new SingleBox( settings ); } + + b2BodyId m_bodyId; }; static int sampleSingleBox = RegisterSample( "Stacking", "Single Box", SingleBox::Create ); @@ -425,13 +431,14 @@ class CircleStack : public Sample b2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.enableHitEvents = true; + shapeDef.rollingResistance = 0.2f; b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; float y = 0.5f; - for ( int i = 0; i < 8; ++i ) + for ( int i = 0; i < 1; ++i ) { bodyDef.position.y = y; diff --git a/src/constraint_graph.c b/src/constraint_graph.c index 3fc77763d..fbd843f76 100644 --- a/src/constraint_graph.c +++ b/src/constraint_graph.c @@ -23,7 +23,7 @@ // cause horrible cache stalls. To make this feasible I would need a way to block these writes. // This is used for debugging by making all constraints be assigned to overflow. -#define B2_FORCE_OVERFLOW 0 +#define B2_FORCE_OVERFLOW 1 _Static_assert( B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12" ); @@ -259,6 +259,8 @@ static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyId return i; } } +#else + B2_MAYBE_UNUSED( graph, bodyIdA, bodyIdB ); #endif return B2_OVERFLOW_INDEX; diff --git a/src/contact.c b/src/contact.c index 7911b8fc6..1e812923d 100644 --- a/src/contact.c +++ b/src/contact.c @@ -493,6 +493,21 @@ bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, contactSim->friction = world->frictionCallback( shapeA->friction, shapeA->material, shapeB->friction, shapeB->material ); contactSim->restitution = world->restitutionCallback( shapeA->restitution, shapeA->material, shapeB->restitution, shapeB->material ); + // todo branch improves perf? + if (shapeA->rollingResistance > 0.0f || shapeB->rollingResistance > 0.0f) + { + float radiusA = b2GetShapeRadius( shapeA ); + float radiusB = b2GetShapeRadius( shapeB ); + float maxRadius = b2MaxFloat( radiusA, radiusB ); + contactSim->rollingResistance = b2MaxFloat( shapeA->rollingResistance, shapeB->rollingResistance ) * maxRadius; + } + else + { + contactSim->rollingResistance = 0.0f; + } + + contactSim->tangentSpeed = shapeB->tangentSpeed - shapeA->tangentSpeed; + int pointCount = contactSim->manifold.pointCount; bool touching = pointCount > 0; @@ -536,6 +551,11 @@ bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, contactSim->simFlags &= ~b2_simEnableHitEvent; } + if (pointCount > 0) + { + contactSim->manifold.rollingImpulse = oldManifold.rollingImpulse; + } + // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. int unmatchedCount = 0; diff --git a/src/contact.h b/src/contact.h index b6176a025..76d23dfb5 100644 --- a/src/contact.h +++ b/src/contact.h @@ -121,8 +121,7 @@ typedef struct b2ContactSim // Mixed friction and restitution float friction; float restitution; - - // todo for conveyor belts + float rollingResistance; float tangentSpeed; // b2ContactSimFlags diff --git a/src/contact_solver.c b/src/contact_solver.c index 6c25653b8..4731ddd64 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -62,6 +62,9 @@ void b2PrepareOverflowContacts( b2StepContext* context ) constraint->normal = manifold->normal; constraint->friction = contactSim->friction; constraint->restitution = contactSim->restitution; + constraint->rollingResistance = contactSim->rollingResistance; + constraint->rollingImpulse = warmStartScale * manifold->rollingImpulse; + constraint->tangentSpeed = contactSim->tangentSpeed; constraint->pointCount = pointCount; b2Vec2 vA = b2Vec2_zero; @@ -101,6 +104,11 @@ void b2PrepareOverflowContacts( b2StepContext* context ) constraint->invMassB = mB; constraint->invIB = iB; + { + float k = iA + iB; + constraint->rollingMass = k > 0.0f ? 1.0f / k : 0.0f; + } + b2Vec2 normal = constraint->normal; b2Vec2 tangent = b2RightPerp( constraint->normal ); @@ -195,6 +203,9 @@ void b2WarmStartOverflowContacts( b2StepContext* context ) vB = b2MulAdd( vB, mB, P ); } + wA -= constraint->rollingImpulse; + wB += constraint->rollingImpulse; + stateA->linearVelocity = vA; stateA->angularVelocity = wA; stateB->linearVelocity = vB; @@ -248,7 +259,9 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) b2Softness softness = constraint->softness; int pointCount = constraint->pointCount; + float totalNormalImpulse = 0.0f; + // Non-penetration for ( int j = 0; j < pointCount; ++j ) { b2ContactConstraintPoint* cp = constraint->points + j; @@ -290,6 +303,7 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) impulse = newImpulse - cp->normalImpulse; cp->normalImpulse = newImpulse; cp->maxNormalImpulse = b2MaxFloat( cp->maxNormalImpulse, impulse ); + totalNormalImpulse += newImpulse; // apply normal impulse b2Vec2 P = b2MulSV( impulse, normal ); @@ -300,6 +314,7 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) wB += iB * b2Cross( rB, P ); } + // Friction for ( int j = 0; j < pointCount; ++j ) { b2ContactConstraintPoint* cp = constraint->points + j; @@ -311,7 +326,7 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) // relative tangent velocity at contact b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); - float vt = b2Dot( b2Sub( vrB, vrA ), tangent ); + float vt = b2Dot( b2Sub( vrB, vrA ), tangent ) + constraint->tangentSpeed; // incremental tangent impulse float impulse = cp->tangentMass * ( -vt ); @@ -330,6 +345,20 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) wB += iB * b2Cross( rB, P ); } + // Rolling resistance + { + float deltaLambda = -constraint->rollingMass * ( wB - wA ); + float lambda = constraint->rollingImpulse; + constraint->rollingImpulse = lambda + deltaLambda; + + float maxLambda = constraint->rollingResistance * totalNormalImpulse; + constraint->rollingImpulse = b2ClampFloat( lambda + deltaLambda, -maxLambda, maxLambda ); + deltaLambda = constraint->rollingImpulse - lambda; + + wA -= iA * deltaLambda; + wB += iB * deltaLambda; + } + stateA->linearVelocity = vA; stateA->angularVelocity = wA; stateB->linearVelocity = vB; @@ -461,6 +490,8 @@ void b2StoreOverflowImpulses( b2StepContext* context ) manifold->points[j].maxNormalImpulse = constraint->points[j].maxNormalImpulse; manifold->points[j].normalVelocity = constraint->points[j].relativeVelocity; } + + manifold->rollingImpulse = constraint->rollingImpulse; } b2TracyCZoneEnd( store_impulses ); diff --git a/src/contact_solver.h b/src/contact_solver.h index 911e6a9d8..6faaeb9ff 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -29,6 +29,10 @@ typedef struct b2ContactConstraint float invIA, invIB; float friction; float restitution; + float tangentSpeed; + float rollingResistance; + float rollingMass; + float rollingImpulse; b2Softness softness; int pointCount; } b2ContactConstraint; diff --git a/src/core.h b/src/core.h index 5f540ce18..362f6832e 100644 --- a/src/core.h +++ b/src/core.h @@ -110,7 +110,8 @@ #define B2_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) // Used to prevent the compiler from warning about unused variables -#define B2_MAYBE_UNUSED( x ) ( (void)( x ) ) +//#define B2_MAYBE_UNUSED( x ) ( (void)( x ) ) +#define B2_MAYBE_UNUSED( ... ) (void)( __VA_ARGS__ ) // Use to validate definitions. Do not take my cookie. #define B2_SECRET_COOKIE 1152023 diff --git a/src/shape.c b/src/shape.c index 42dcd4793..fb07822f2 100644 --- a/src/shape.c +++ b/src/shape.c @@ -109,6 +109,8 @@ static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform shape->density = def->density; shape->friction = def->friction; shape->restitution = def->restitution; + shape->rollingResistance = def->rollingResistance; + shape->tangentSpeed = def->tangentSpeed; shape->material = def->material; shape->filter = def->filter; shape->userData = def->userData; diff --git a/src/shape.h b/src/shape.h index 71b0278a1..6437a2b26 100644 --- a/src/shape.h +++ b/src/shape.h @@ -21,6 +21,8 @@ typedef struct b2Shape float density; float friction; float restitution; + float rollingResistance; + float tangentSpeed; int material; b2AABB aabb; @@ -94,5 +96,20 @@ b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape ); b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform ); b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform ); +static inline float b2GetShapeRadius(const b2Shape* shape) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return shape->capsule.radius; + case b2_circleShape: + return shape->circle.radius; + case b2_polygonShape: + return shape->polygon.radius; + default: + return 0.0f; + } +} + B2_ARRAY_INLINE( b2ChainShape, b2ChainShape ); B2_ARRAY_INLINE( b2Shape, b2Shape ); diff --git a/src/solver.c b/src/solver.c index bc37d6174..b12851249 100644 --- a/src/solver.c +++ b/src/solver.c @@ -22,6 +22,7 @@ #include #include +// Compare to SDL_CPUPauseInstruction #if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) static inline void b2Pause( void ) {