diff --git a/benchmark/amd7950x_avx2/joint_grid.csv b/benchmark/amd7950x_avx2/joint_grid.csv index 113289ac2..a052e4730 100644 --- a/benchmark/amd7950x_avx2/joint_grid.csv +++ b/benchmark/amd7950x_avx2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,2565.3 -2,1533.85 -3,1053.01 -4,865.702 -5,748.989 -6,658.623 -7,613.633 -8,578.973 +1,2528.51 +2,1460.58 +3,1004.38 +4,822.484 +5,712.691 +6,638.201 +7,583.016 +8,540.158 diff --git a/benchmark/amd7950x_avx2/large_pyramid.csv b/benchmark/amd7950x_avx2/large_pyramid.csv index 516f542c5..23f526fe1 100644 --- a/benchmark/amd7950x_avx2/large_pyramid.csv +++ b/benchmark/amd7950x_avx2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,1584.03 -2,841.862 -3,598.747 -4,471.697 -5,396.306 -6,347.983 -7,309.086 -8,308.832 +1,1579.54 +2,849.264 +3,591.984 +4,466.064 +5,393.207 +6,347.593 +7,305.863 +8,305.205 diff --git a/benchmark/amd7950x_avx2/many_pyramids.csv b/benchmark/amd7950x_avx2/many_pyramids.csv index 6a0f4358b..82f73dfbf 100644 --- a/benchmark/amd7950x_avx2/many_pyramids.csv +++ b/benchmark/amd7950x_avx2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,2663.34 -2,1407.85 -3,934.506 -4,725.271 -5,590.288 -6,502.513 -7,422.693 -8,395.369 +1,2592.03 +2,1412.01 +3,953.71 +4,731.394 +5,601.527 +6,511.144 +7,441.201 +8,393.566 diff --git a/benchmark/amd7950x_avx2/rain.csv b/benchmark/amd7950x_avx2/rain.csv index 849ad421d..a44f60e83 100644 --- a/benchmark/amd7950x_avx2/rain.csv +++ b/benchmark/amd7950x_avx2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,6526.9 -2,4017.74 -3,3052.28 -4,2493.5 -5,2149.12 -6,1911.45 -7,1735.97 -8,1625.71 +1,6462.19 +2,3806.63 +3,2907 +4,2413.23 +5,2069.53 +6,1852.14 +7,1671.47 +8,1547.16 diff --git a/benchmark/amd7950x_avx2/smash.csv b/benchmark/amd7950x_avx2/smash.csv index 3dd380467..a91426f99 100644 --- a/benchmark/amd7950x_avx2/smash.csv +++ b/benchmark/amd7950x_avx2/smash.csv @@ -1,9 +1,9 @@ threads,ms -1,1562.06 -2,1020.55 -3,781.193 -4,661.318 -5,580.115 -6,530.502 -7,489.672 -8,467.998 +1,1533.42 +2,981.749 +3,763.27 +4,642.928 +5,574.053 +6,527.394 +7,492.883 +8,468.971 diff --git a/benchmark/amd7950x_avx2/spinner.csv b/benchmark/amd7950x_avx2/spinner.csv index 091eb9040..921025140 100644 --- a/benchmark/amd7950x_avx2/spinner.csv +++ b/benchmark/amd7950x_avx2/spinner.csv @@ -1,9 +1,9 @@ threads,ms -1,4106.89 -2,2635.21 -3,1986.32 -4,1614.21 -5,1420.63 -6,1272.06 -7,1157.16 -8,1091.25 +1,4126.1 +2,2491.09 +3,1918.78 +4,1543.11 +5,1360.84 +6,1216.53 +7,1110.95 +8,1043.96 diff --git a/benchmark/amd7950x_avx2/tumbler.csv b/benchmark/amd7950x_avx2/tumbler.csv index 1f68a4ca1..f0b75fb86 100644 --- a/benchmark/amd7950x_avx2/tumbler.csv +++ b/benchmark/amd7950x_avx2/tumbler.csv @@ -1,9 +1,9 @@ threads,ms -1,1613.69 -2,1061.88 -3,810.738 -4,670.874 -5,578.959 -6,519.473 -7,479.842 -8,437.851 +1,1624.23 +2,1028.59 +3,796.538 +4,651.347 +5,563.763 +6,510.42 +7,468.899 +8,445.968 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..c7b2d5c1e 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; @@ -394,6 +396,35 @@ typedef struct b2ShapeDef /// @ingroup shape B2_API b2ShapeDef b2DefaultShapeDef( void ); +/// Surface materials allow chain shapes to have per segment surface properties. +/// @ingroup shape +typedef struct b2SurfaceMaterial +{ + /// The Coulomb (dry) friction coefficient, usually in the range [0,1]. + float friction; + + /// The coefficient of restitution (bounce) usually in the range [0,1]. + /// https://en.wikipedia.org/wiki/Coefficient_of_restitution + float restitution; + + /// The rolling resistance usually in the range [0,1]. + 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; + + /// Custom debug draw color. + uint32_t customColor; +} b2SurfaceMaterial; + +/// Use this to initialize your surface material +/// @ingroup shape +B2_API b2SurfaceMaterial b2DefaultSurfaceMaterial( void ); + /// Used to create a chain of line segments. This is designed to eliminate ghost collisions with some limitations. /// - chains are one-sided /// - chains have no mass and should be used on static bodies @@ -420,28 +451,19 @@ typedef struct b2ChainDef /// The point count, must be 4 or more. int count; - /// The friction coefficient, usually in the range [0,1]. - float friction; - - /// The restitution (elasticity) usually in the range [0,1]. - float restitution; + /// Surface materials for each segment. These are cloned. + const b2SurfaceMaterial* materials; - /// User material identifier. This is passed with query results and to friction and restitution - /// combining functions. It is not used internally. - int material; + /// The material count. Must be 1 or count. This allows you to provide one + /// material for all segments or a unique material per segment. + int materialCount; /// Contact filtering data. b2Filter filter; - /// Custom debug draw color. - uint32_t customColor; - /// Indicates a closed chain formed by connecting the first and last points bool isLoop; - /// Generate events when a sensor overlaps this chain - bool enableSensorEvents; - /// Used internally to detect a valid definition. DO NOT SET. int internalValue; } b2ChainDef; diff --git a/samples/car.cpp b/samples/car.cpp index cb7109b07..d9e7f34e0 100644 --- a/samples/car.cpp +++ b/samples/car.cpp @@ -53,6 +53,7 @@ void Car::Spawn( b2WorldId worldId, b2Vec2 position, float scale, float hertz, f shapeDef.density = 2.0f / scale; shapeDef.friction = 1.5f; + shapeDef.rollingResistance = 0.1f; bodyDef.position = b2Add( { -1.0f * scale, 0.35f * scale }, position ); bodyDef.allowFastRotation = true; diff --git a/samples/data/ramp.svg b/samples/data/ramp.svg new file mode 100644 index 000000000..5529d242c --- /dev/null +++ b/samples/data/ramp.svg @@ -0,0 +1,66 @@ + + + + + + + + + + + + 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..fe3c14108 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -1,6 +1,8 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#define _CRT_SECURE_NO_WARNINGS + #include "sample.h" #include "TaskScheduler.h" @@ -14,7 +16,7 @@ #include #include -#include +#include class SampleTask : public enki::ITaskSet { @@ -101,14 +103,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 +118,9 @@ Sample::Sample( Settings& settings ) g_seed = RAND_SEED; + m_settings = &settings; + + CreateWorld(); TestMathCpp(); } @@ -134,6 +133,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 ); @@ -488,6 +505,144 @@ void Sample::ShiftOrigin( b2Vec2 newOrigin ) // m_world->ShiftOrigin(newOrigin); } +// Parse an SVG path element with only straight lines. Example: +// "M 47.625004,185.20833 H 161.39585 l 29.10417,-2.64583 26.45834,-7.9375 26.45833,-13.22917 23.81251,-21.16666 h " +// "13.22916 v 44.97916 H 592.66669 V 0 h 21.16671 v 206.375 l -566.208398,-1e-5 z" +int Sample::ParsePath( const char* svgPath, b2Vec2 offset, b2Vec2* points, int capacity, float scale, bool reverseOrder ) +{ + int pointCount = 0; + b2Vec2 currentPoint = {}; + const char* ptr = svgPath; + char command = *ptr; + + while ( *ptr != '\0' ) + { + if ( isdigit( *ptr ) == 0 && *ptr != '-' ) + { + // note: command can be implicitly repeated + command = *ptr; + + if ( command == 'M' || command == 'L' || command == 'H' || command == 'V' || command == 'm' || command == 'l' || + command == 'h' || command == 'v' ) + { + ptr += 2; // Skip the command character and space + } + + if ( command == 'z' ) + { + break; + } + } + + assert( isdigit( *ptr ) != 0 || *ptr == '-' ); + + float x = 0.0f, y = 0.0f; + switch ( command ) + { + case 'M': + case 'L': + if ( sscanf( ptr, "%f,%f", &x, &y ) == 2 ) + { + currentPoint.x = x; + currentPoint.y = y; + } + else + { + assert( false ); + } + break; + case 'H': + if ( sscanf( ptr, "%f", &x ) == 1 ) + { + currentPoint.x = x; + } + else + { + assert( false ); + } + break; + case 'V': + if ( sscanf( ptr, "%f", &y ) == 1 ) + { + currentPoint.y = y; + } + else + { + assert( false ); + } + break; + case 'm': + case 'l': + if ( sscanf( ptr, "%f,%f", &x, &y ) == 2 ) + { + currentPoint.x += x; + currentPoint.y += y; + } + else + { + assert( false ); + } + break; + case 'h': + if ( sscanf( ptr, "%f", &x ) == 1 ) + { + currentPoint.x += x; + } + else + { + assert( false ); + } + break; + case 'v': + if ( sscanf( ptr, "%f", &y ) == 1 ) + { + currentPoint.y += y; + } + else + { + assert( false ); + } + break; + + default: + assert( false ); + break; + } + + points[pointCount] = { scale * ( currentPoint.x + offset.x ), -scale * ( currentPoint.y + offset.y ) }; + pointCount += 1; + if ( pointCount == capacity ) + { + break; + } + + // Move to the next space or end of string + while ( *ptr != '\0' && isspace( *ptr ) == 0 ) + { + ptr++; + } + + // Skip contiguous spaces + while ( isspace( *ptr ) ) + { + ptr++; + } + + ptr += 0; + } + + if ( pointCount == 0 ) + { + return 0; + } + + if ( reverseOrder ) + { + + } + return pointCount; +} + SampleEntry g_sampleEntries[MAX_SAMPLES] = {}; int g_sampleCount = 0; diff --git a/samples/sample.h b/samples/sample.h index e1236d8d0..259b88a04 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() @@ -59,6 +60,8 @@ class Sample void ResetProfile(); void ShiftOrigin( b2Vec2 newOrigin ); + static int ParsePath( const char* svgPath, b2Vec2 offset, b2Vec2* points, int capacity, float scale, bool reverseOrder ); + friend class DestructionListener; friend class BoundaryListener; friend class ContactListener; @@ -66,9 +69,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..c02dbf64a 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -221,6 +221,7 @@ class BenchmarkBarrel : public Sample { m_bodies[index] = b2CreateBody( m_worldId, &bodyDef ); circle.radius = RandomFloatRange( 0.25f, 0.75f ); + shapeDef.rollingResistance = 0.2f; b2CreateCircleShape( m_bodies[index], &shapeDef, &circle ); } else if ( m_shapeType == e_capsuleShape ) @@ -230,6 +231,7 @@ class BenchmarkBarrel : public Sample float length = RandomFloatRange( 0.25f, 1.0f ); capsule.center1 = { 0.0f, -0.5f * length }; capsule.center2 = { 0.0f, 0.5f * length }; + shapeDef.rollingResistance = 0.2f; b2CreateCapsuleShape( m_bodies[index], &shapeDef, &capsule ); } else if ( m_shapeType == e_mixShape ) @@ -557,7 +559,7 @@ class BenchmarkLargePyramid : public Sample settings.enableSleep = false; } - CreateLargePyramid(m_worldId); + CreateLargePyramid( m_worldId ); } static Sample* Create( Settings& settings ) @@ -624,6 +626,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 +636,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 +647,8 @@ class BenchmarkCreateDestroy : public Sample } } + m_destroyTime += b2GetMillisecondsAndReset( &ticks ); + int count = m_baseCount; float rad = 0.5f; float shift = rad * 2.0f; @@ -675,22 +684,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 +715,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 +1546,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_continuous.cpp b/samples/sample_continuous.cpp index 86387043a..3d1749e81 100644 --- a/samples/sample_continuous.cpp +++ b/samples/sample_continuous.cpp @@ -667,11 +667,15 @@ class GhostBumps : public Sample points[18] = b2Add( points[17], { -2.0f * hx, 0.0f } ); points[19] = b2Add( points[18], { -2.0f * hx, 0.0f } ); + b2SurfaceMaterial material = {}; + material.friction = m_friction; + b2ChainDef chainDef = b2DefaultChainDef(); chainDef.points = points; chainDef.count = 20; chainDef.isLoop = true; - chainDef.friction = m_friction; + chainDef.materials = &material; + chainDef.materialCount = 1; b2CreateChain( m_groundId, &chainDef ); } diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp index 27bae39eb..789cdccde 100644 --- a/samples/sample_events.cpp +++ b/samples/sample_events.cpp @@ -92,11 +92,15 @@ class SensorFunnel : public Sample // } // printf("};\n"); + b2SurfaceMaterial material = {}; + material.friction = 0.2f; + b2ChainDef chainDef = b2DefaultChainDef(); chainDef.points = points; chainDef.count = count; chainDef.isLoop = true; - chainDef.friction = 0.2f; + chainDef.materials = &material; + chainDef.materialCount = 1; b2CreateChain( groundId, &chainDef ); float sign = 1.0f; diff --git a/samples/sample_joints.cpp b/samples/sample_joints.cpp index 43b88c72e..3dd06d951 100644 --- a/samples/sample_joints.cpp +++ b/samples/sample_joints.cpp @@ -2072,11 +2072,11 @@ class Driving : public Sample m_throttle = 0.0f; m_speed = 35.0f; - m_torque = 2.5f; + m_torque = 5.0f; m_hertz = 5.0f; m_dampingRatio = 0.7f; - m_car.Spawn( m_worldId, { 0.0f, 0.0f }, 1.0f, m_hertz, m_dampingRatio, m_torque, NULL ); + m_car.Spawn( m_worldId, { 0.0f, 0.0f }, 1.0f, m_hertz, m_dampingRatio, m_torque, nullptr ); } void UpdateUI() override @@ -2103,7 +2103,7 @@ class Driving : public Sample m_car.SetSpeed( m_throttle * m_speed ); } - if ( ImGui::SliderFloat( "Torque", &m_torque, 0.0f, 5.0f, "%.1f" ) ) + if ( ImGui::SliderFloat( "Torque", &m_torque, 0.0f, 10.0f, "%.1f" ) ) { m_car.SetTorque( m_torque ); } @@ -2491,7 +2491,7 @@ class ScissorLift : public Sample m_liftJointId = b2CreateDistanceJoint( m_worldId, &distanceDef ); Car car; - car.Spawn( m_worldId, { 0.0f, y + 2.0f }, 1.0f, 3.0f, 0.7f, 0.0f, NULL ); + car.Spawn( m_worldId, { 0.0f, y + 2.0f }, 1.0f, 3.0f, 0.7f, 0.0f, nullptr ); } void UpdateUI() override diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index a09d1e5d7..6d4ffc6b8 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -112,13 +112,17 @@ class ChainShape : public Sample // } // printf("};\n"); + b2SurfaceMaterial material = {}; + material.friction = 0.2f; + material.customColor = b2_colorSteelBlue; + material.material = 42; + b2ChainDef chainDef = b2DefaultChainDef(); chainDef.points = points; chainDef.count = count; - chainDef.customColor = b2_colorSteelBlue; + chainDef.materials = &material; + chainDef.materialCount = 1; chainDef.isLoop = true; - chainDef.friction = 0.2f; - chainDef.material = 42; b2BodyDef bodyDef = b2DefaultBodyDef(); m_groundId = b2CreateBody( m_worldId, &bodyDef ); @@ -952,7 +956,257 @@ 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; + m_resistScale = 0.02f; + 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 = m_resistScale * 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", m_resistScale * i ); + } + } + + static Sample* Create( Settings& settings ) + { + return new RollingResistance( settings ); + } + + float m_resistScale; + float m_lift; +}; + +static int sampleRollingResistance = RegisterSample( "Shapes", "Rolling Resistance", RollingResistance::Create ); + +class ConveyorBelt : public Sample +{ +public: + explicit ConveyorBelt( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 2.0f, 7.5f }; + g_camera.m_zoom = 12.0f; + } + + // Ground + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment segment = { { -20.0f, 0.0f }, { 20.0f, 0.0f } }; + b2CreateSegmentShape( groundId, &shapeDef, &segment ); + } + + // Platform + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = { -5.0f, 5.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeRoundedBox( 10.0f, 0.25f, 0.25f ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.8f; + shapeDef.tangentSpeed = 2.0f; + + b2CreatePolygonShape( bodyId, &shapeDef, &box ); + } + + // Boxes + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Polygon cube = b2MakeSquare( 0.5f ); + for ( int i = 0; i < 5; ++i ) + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { -10.0f + 2.0f * i, 7.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2CreatePolygonShape( bodyId, &shapeDef, &cube ); + } + } + + static Sample* Create( Settings& settings ) + { + return new ConveyorBelt( settings ); + } +}; + +static int sampleConveyorBelt = RegisterSample( "Shapes", "Conveyor Belt", ConveyorBelt::Create ); + +class TangentSpeed : public Sample +{ +public: + explicit TangentSpeed( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 60.0f, -15.0f }; + g_camera.m_zoom = 38.0f; + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef ); + + //const char* path = "M 613.8334,185.20833 H 500.06255 L 470.95838,182.5625 444.50004,174.625 418.04171,161.39583 " + // "394.2292,140.22917 h " + // "-13.22916 v 44.97916 H 68.791712 V 0 h -21.16671 v 206.375 l 566.208398,-1e-5 z"; + + const char* path = "m 613.8334,185.20833 -42.33338,0 h -37.04166 l -34.39581,0 -29.10417,-2.64583 -26.45834,-7.9375 " + "-26.45833,-13.22917 -23.81251,-21.16666 h -13.22916 v 44.97916 H 68.791712 V 0 h -21.16671 v " + "206.375 l 566.208398,-1e-5 z"; + + b2Vec2 offset = { -47.375002f, 0.25f }; + + float scale = 0.2f; + b2Vec2 points[20] = {}; + int count = ParsePath( path, offset, points, 20, scale, true ); + + b2SurfaceMaterial materials[20] = {}; + for ( int i = 0; i < 20; ++i ) + { + materials[i].friction = 0.6f; + } + + materials[0].tangentSpeed = -10.0; + materials[0].customColor = b2_colorDarkBlue; + materials[1].tangentSpeed = -20.0; + materials[1].customColor = b2_colorDarkCyan; + materials[2].tangentSpeed = -30.0; + materials[2].customColor = b2_colorDarkGoldenRod; + materials[3].tangentSpeed = -40.0; + materials[3].customColor = b2_colorDarkGray; + materials[4].tangentSpeed = -50.0; + materials[4].customColor = b2_colorDarkGreen; + materials[5].tangentSpeed = -60.0; + materials[5].customColor = b2_colorDarkKhaki; + materials[6].tangentSpeed = -70.0; + materials[6].customColor = b2_colorDarkMagenta; + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = count; + chainDef.isLoop = true; + chainDef.materials = materials; + chainDef.materialCount = count; + + b2CreateChain( groundId, &chainDef ); + } + } + + b2BodyId DropBall() + { + b2Circle circle = { { 0.0f, 0.0f }, 0.5f }; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = { 110.0f, -30.0f }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.rollingResistance = 0.3f; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + return bodyId; + } + + void Step( Settings& settings ) override + { + if ( m_stepCount % 25 == 0 && m_count < m_totalCount && settings.pause == false) + { + DropBall(); + m_count += 1; + } + + Sample::Step( settings ); + + } + + static Sample* Create( Settings& settings ) + { + return new TangentSpeed( settings ); + } + + static constexpr int m_totalCount = 200; + int m_count = 0; +}; + +static int sampleTangentSpeed = RegisterSample( "Shapes", "Tangent Speed", TangentSpeed::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/samples/settings.cpp b/samples/settings.cpp index 7eadcebd1..18b5ecc18 100644 --- a/samples/settings.cpp +++ b/samples/settings.cpp @@ -35,7 +35,7 @@ static bool ReadFile( char*& data, int& size, const char* filename ) data = (char*)malloc( size + 1 ); size_t count = fread( data, size, 1, file ); - B2_MAYBE_UNUSED( count ); + B2_UNUSED( count ); fclose( file ); data[size] = 0; diff --git a/samples/shader.cpp b/samples/shader.cpp index b17218a31..61fdcba17 100644 --- a/samples/shader.cpp +++ b/samples/shader.cpp @@ -153,7 +153,7 @@ static GLuint sCreateShaderFromFile( const char* filename, GLenum type ) char* source = static_cast( malloc( size + 1 ) ); size_t count = fread( source, size, 1, file ); - B2_MAYBE_UNUSED( count ); + B2_UNUSED( count ); fclose( file ); source[size] = 0; diff --git a/shared/benchmarks.c b/shared/benchmarks.c index 123afcb49..908e5b50e 100644 --- a/shared/benchmarks.c +++ b/shared/benchmarks.c @@ -356,11 +356,15 @@ void CreateSpinner( b2WorldId worldId ) p = b2RotateVector( q, p ); } + b2SurfaceMaterial material = {0}; + material.friction = 0.1f; + b2ChainDef chainDef = b2DefaultChainDef(); chainDef.points = points; chainDef.count = SPINNER_POINT_COUNT; chainDef.isLoop = true; - chainDef.friction = 0.1f; + chainDef.materials = &material; + chainDef.materialCount = 1; b2CreateChain( groundId, &chainDef ); } diff --git a/src/body.c b/src/body.c index 6d7cd297c..e53f87c56 100644 --- a/src/body.c +++ b/src/body.c @@ -325,7 +325,7 @@ b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ) bool b2IsBodyAwake( b2World* world, b2Body* body ) { - B2_MAYBE_UNUSED( world ); + B2_UNUSED( world ); return body->setIndex == b2_awakeSet; } @@ -425,7 +425,7 @@ void b2DestroyBody( b2BodyId bodyId ) if ( body->setIndex == b2_awakeSet ) { int result = b2BodyStateArray_RemoveSwap( &set->bodyStates, body->localIndex ); - B2_MAYBE_UNUSED( result ); + B2_UNUSED( result ); B2_ASSERT( result == movedIndex ); } else if ( set->setIndex >= b2_firstSleepingSet && set->bodySims.count == 0 ) diff --git a/src/broad_phase.c b/src/broad_phase.c index 5801ad10b..ebfff392e 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -313,7 +313,7 @@ static void b2FindPairsTask( int startIndex, int endIndex, uint32_t threadIndex, { b2TracyCZoneNC( pair_task, "Pair", b2_colorMediumSlateBlue, true ); - B2_MAYBE_UNUSED( threadIndex ); + B2_UNUSED( threadIndex ); b2World* world = context; b2BroadPhase* bp = &world->broadPhase; @@ -517,6 +517,6 @@ void b2ValidateNoEnlarged( const b2BroadPhase* bp ) b2DynamicTree_ValidateNoEnlarged( tree ); } #else - B2_MAYBE_UNUSED( bp ); + B2_UNUSED( bp ); #endif } diff --git a/src/constraint_graph.c b/src/constraint_graph.c index 3fc77763d..c29066b85 100644 --- a/src/constraint_graph.c +++ b/src/constraint_graph.c @@ -259,6 +259,8 @@ static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyId return i; } } +#else + B2_UNUSED( graph, bodyIdA, bodyIdB ); #endif return B2_OVERFLOW_INDEX; diff --git a/src/contact.c b/src/contact.c index 7911b8fc6..01566ec30 100644 --- a/src/contact.c +++ b/src/contact.c @@ -69,70 +69,70 @@ static bool s_initialized = false; static b2Manifold b2CircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideCircles( &shapeA->circle, xfA, &shapeB->circle, xfB ); } static b2Manifold b2CapsuleAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideCapsuleAndCircle( &shapeA->capsule, xfA, &shapeB->circle, xfB ); } static b2Manifold b2CapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideCapsules( &shapeA->capsule, xfA, &shapeB->capsule, xfB ); } static b2Manifold b2PolygonAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollidePolygonAndCircle( &shapeA->polygon, xfA, &shapeB->circle, xfB ); } static b2Manifold b2PolygonAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollidePolygonAndCapsule( &shapeA->polygon, xfA, &shapeB->capsule, xfB ); } static b2Manifold b2PolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollidePolygons( &shapeA->polygon, xfA, &shapeB->polygon, xfB ); } static b2Manifold b2SegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideSegmentAndCircle( &shapeA->segment, xfA, &shapeB->circle, xfB ); } static b2Manifold b2SegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideSegmentAndCapsule( &shapeA->segment, xfA, &shapeB->capsule, xfB ); } static b2Manifold b2SegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideSegmentAndPolygon( &shapeA->segment, xfA, &shapeB->polygon, xfB ); } static b2Manifold b2ChainSegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, b2SimplexCache* cache ) { - B2_MAYBE_UNUSED( cache ); + B2_UNUSED( cache ); return b2CollideChainSegmentAndCircle( &shapeA->chainSegment, xfA, &shapeB->circle, xfB ); } @@ -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 = shapeA->tangentSpeed + shapeB->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; @@ -575,7 +595,7 @@ bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, unmatchedCount += mp2->persisted ? 0 : 1; } - B2_MAYBE_UNUSED( unmatchedCount ); + B2_UNUSED( unmatchedCount ); #if 0 // todo I haven't found an improvement from this yet 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..e6f131b14 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 -= iA * constraint->rollingImpulse; + wB += iB * 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,11 @@ 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 ); + + // vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent) + // = dot(vrB - vrA, tangent) - (sA + sB) + + float vt = b2Dot( b2Sub( vrB, vrA ), tangent ) - constraint->tangentSpeed; // incremental tangent impulse float impulse = cp->tangentMass * ( -vt ); @@ -330,6 +349,18 @@ void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) wB += iB * b2Cross( rB, P ); } + // Rolling resistance + { + float deltaLambda = -constraint->rollingMass * ( wB - wA ); + float lambda = constraint->rollingImpulse; + 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 +492,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 ); @@ -559,6 +592,13 @@ static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) return _mm256_max_ps( a, b ); } +// a = clamp(a, -b, b) +static inline b2FloatW b2ClampSymW( b2FloatW a, b2FloatW b ) +{ + b2FloatW nb = _mm256_sub_ps( _mm256_setzero_ps(), b ); + return _mm256_max_ps(nb, _mm256_min_ps( a, b )); +} + static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) { return _mm256_or_ps( a, b ); @@ -633,6 +673,13 @@ static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) return vmaxq_f32( a, b ); } +// a = clamp(a, -b, b) +static inline b2FloatW b2ClampSymW( b2FloatW a, b2FloatW b ) +{ + b2FloatW nb = vnegq_f32( b ); + return vmaxq_f32( nb, vminq_f32( a, b ) ); +} + static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) { return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); @@ -741,6 +788,18 @@ static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) return _mm_max_ps( a, b ); } +// a = clamp(a, -b, b) +static inline b2FloatW b2ClampSymW( b2FloatW a, b2FloatW b ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + __m128 nb = _mm_xor_ps( b, mask ); + + return _mm_max_ps( nb, _mm_min_ps( a, b ) ); +} + static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) { return _mm_or_ps( a, b ); @@ -839,6 +898,17 @@ static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) return r; } +// a = clamp(a, -b, b) +static inline b2FloatW b2ClampSymW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = b2ClampFloat(a.x, -b.x, b.x); + r.y = b2ClampFloat(a.y, -b.y, b.y); + r.z = b2ClampFloat(a.z, -b.z, b.z); + r.w = b2ClampFloat(a.w, -b.w, b.w); + return r; +} + static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) { b2FloatW r; @@ -911,6 +981,10 @@ typedef struct b2ContactConstraintSIMD b2FloatW invIA, invIB; b2Vec2W normal; b2FloatW friction; + b2FloatW tangentSpeed; + b2FloatW rollingResistance; + b2FloatW rollingMass; + b2FloatW rollingImpulse; b2FloatW biasRate; b2FloatW massScale; b2FloatW impulseScale; @@ -936,20 +1010,20 @@ int b2GetContactConstraintSIMDByteCount( void ) } // wide version of b2BodyState -typedef struct b2SimdBody +typedef struct b2BodyStateW { b2Vec2W v; b2FloatW w; b2FloatW flags; b2Vec2W dp; b2RotW dq; -} b2SimdBody; +} b2BodyStateW; // Custom gather/scatter for each SIMD type #if defined( B2_SIMD_AVX2 ) // This is a load and 8x8 transpose -static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -981,7 +1055,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); - b2SimdBody simdBody; + b2BodyStateW simdBody; simdBody.v.X = _mm256_permute2f128_ps( tt0, tt4, 0x20 ); simdBody.v.Y = _mm256_permute2f128_ps( tt1, tt5, 0x20 ); simdBody.w = _mm256_permute2f128_ps( tt2, tt6, 0x20 ); @@ -994,7 +1068,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 } // This writes everything back to the solver bodies but only the velocities change -static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -1038,7 +1112,7 @@ static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT i #elif defined( B2_SIMD_NEON ) // This is a load and transpose -static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -1071,7 +1145,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 // [w2 w4 f2 f4] b2FloatW t4a = b2UnpackHiW( b2a, b4a ); - b2SimdBody simdBody; + b2BodyStateW simdBody; simdBody.v.X = b2UnpackLoW( t1a, t2a ); simdBody.v.Y = b2UnpackHiW( t1a, t2a ); simdBody.w = b2UnpackLoW( t3a, t4a ); @@ -1092,7 +1166,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 // This writes only the velocities back to the solver bodies // https://developer.arm.com/documentation/102107a/0100/Floating-point-4x4-matrix-transposition -static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -1144,7 +1218,7 @@ static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT i #elif defined( B2_SIMD_SSE2 ) // This is a load and transpose -static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -1176,7 +1250,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 // [w2 w4 f2 f4] b2FloatW t4a = b2UnpackHiW( b2a, b4a ); - b2SimdBody simdBody; + b2BodyStateW simdBody; simdBody.v.X = b2UnpackLoW( t1a, t2a ); simdBody.v.Y = b2UnpackHiW( t1a, t2a ); simdBody.w = b2UnpackLoW( t3a, t4a ); @@ -1196,7 +1270,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 } // This writes only the velocities back to the solver bodies -static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) { _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); @@ -1240,7 +1314,7 @@ static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT i #else // This is a load and transpose -static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) { b2BodyState identity = b2_identityBodyState; @@ -1249,7 +1323,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 b2BodyState s3 = indices[2] == B2_NULL_INDEX ? identity : states[indices[2]]; b2BodyState s4 = indices[3] == B2_NULL_INDEX ? identity : states[indices[3]]; - b2SimdBody simdBody; + b2BodyStateW simdBody; simdBody.v.X = ( b2FloatW ){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; simdBody.v.Y = ( b2FloatW ){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; simdBody.w = ( b2FloatW ){ s1.angularVelocity, s2.angularVelocity, s3.angularVelocity, s4.angularVelocity }; @@ -1263,7 +1337,7 @@ static b2SimdBody b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2 } // This writes only the velocities back to the solver bodies -static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2SimdBody* B2_RESTRICT simdBody ) +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) { if ( indices[0] != B2_NULL_INDEX ) { @@ -1371,6 +1445,11 @@ void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ( (float*)&constraint->invIA )[j] = iA; ( (float*)&constraint->invIB )[j] = iB; + { + float k = iA + iB; + ( (float*)&constraint->rollingMass )[j] = k > 0.0f ? 1.0f / k : 0.0f; + } + b2Softness soft = ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX ) ? staticSoftness : contactSoftness; b2Vec2 normal = manifold->normal; @@ -1378,7 +1457,11 @@ void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ( (float*)&constraint->normal.Y )[j] = normal.y; ( (float*)&constraint->friction )[j] = contactSim->friction; + ( (float*)&constraint->tangentSpeed )[j] = contactSim->tangentSpeed; ( (float*)&constraint->restitution )[j] = contactSim->restitution; + ( (float*)&constraint->rollingResistance )[j] = contactSim->rollingResistance; + ( (float*)&constraint->rollingImpulse )[j] = warmStartScale * manifold->rollingImpulse; + ( (float*)&constraint->biasRate )[j] = soft.biasRate; ( (float*)&constraint->massScale )[j] = soft.massScale; ( (float*)&constraint->impulseScale )[j] = soft.impulseScale; @@ -1484,6 +1567,10 @@ void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ( (float*)&constraint->normal.X )[j] = 0.0f; ( (float*)&constraint->normal.Y )[j] = 0.0f; ( (float*)&constraint->friction )[j] = 0.0f; + ( (float*)&constraint->tangentSpeed )[j] = 0.0f; + ( (float*)&constraint->rollingResistance )[j] = 0.0f; + ( (float*)&constraint->rollingMass )[j] = 0.0f; + ( (float*)&constraint->rollingImpulse )[j] = 0.0f; ( (float*)&constraint->biasRate )[j] = 0.0f; ( (float*)&constraint->massScale )[j] = 0.0f; ( (float*)&constraint->impulseScale )[j] = 0.0f; @@ -1530,8 +1617,8 @@ void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* conte for ( int i = startIndex; i < endIndex; ++i ) { b2ContactConstraintSIMD* c = constraints + i; - b2SimdBody bA = b2GatherBodies( states, c->indexA ); - b2SimdBody bB = b2GatherBodies( states, c->indexB ); + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); b2FloatW tangentX = c->normal.Y; b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X ); @@ -1568,6 +1655,9 @@ void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* conte bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y ); } + bA.w = b2MulSubW(bA.w, c->invIA, c->rollingImpulse); + bB.w = b2MulAddW(bB.w, c->invIB, c->rollingImpulse); + b2ScatterBodies( states, c->indexA, &bA ); b2ScatterBodies( states, c->indexB, &bB ); } @@ -1588,8 +1678,8 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, { b2ContactConstraintSIMD* c = constraints + i; - b2SimdBody bA = b2GatherBodies( states, c->indexA ); - b2SimdBody bB = b2GatherBodies( states, c->indexB ); + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); b2FloatW biasRate, massScale, impulseScale; if ( useBias ) @@ -1605,6 +1695,8 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, impulseScale = b2ZeroW(); } + b2FloatW totalNormalImpulse = b2ZeroW(); + b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) }; // point1 non-penetration constraint @@ -1644,6 +1736,8 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, c->normalImpulse1 = newImpulse; c->maxNormalImpulse1 = b2MaxW( c->maxNormalImpulse1, newImpulse ); + totalNormalImpulse = b2AddW( totalNormalImpulse, newImpulse ); + // Apply contact impulse b2FloatW Px = b2MulW( impulse, c->normal.X ); b2FloatW Py = b2MulW( impulse, c->normal.Y ); @@ -1691,6 +1785,8 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, c->normalImpulse2 = newImpulse; c->maxNormalImpulse2 = b2MaxW( c->maxNormalImpulse2, newImpulse ); + totalNormalImpulse = b2AddW( totalNormalImpulse, newImpulse ); + // Apply contact impulse b2FloatW Px = b2MulW( impulse, c->normal.X ); b2FloatW Py = b2MulW( impulse, c->normal.Y ); @@ -1718,6 +1814,9 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + // Tangent speed (conveyor belt) + vt = b2SubW( vt, c->tangentSpeed ); + // Compute tangent force b2FloatW negImpulse = b2MulW( c->tangentMass1, vt ); @@ -1752,6 +1851,9 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + // Tangent speed (conveyor belt) + vt = b2SubW( vt, c->tangentSpeed ); + // Compute tangent force b2FloatW negImpulse = b2MulW( c->tangentMass2, vt ); @@ -1775,6 +1877,18 @@ void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); } + // Rolling resistance + { + b2FloatW deltaLambda = b2MulW( c->rollingMass, b2SubW( bA.w, bB.w )); + b2FloatW lambda = c->rollingImpulse; + b2FloatW maxLambda = b2MulW( c->rollingResistance, totalNormalImpulse ); + c->rollingImpulse = b2ClampSymW( b2AddW(lambda, deltaLambda), maxLambda ); + deltaLambda = b2SubW(c->rollingImpulse, lambda); + + bA.w = b2MulSubW( bA.w, c->invIA, deltaLambda ); + bB.w = b2MulAddW( bB.w, c->invIB, deltaLambda ); + } + b2ScatterBodies( states, c->indexA, &bA ); b2ScatterBodies( states, c->indexB, &bB ); } @@ -1795,8 +1909,8 @@ void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* contex { b2ContactConstraintSIMD* c = constraints + i; - b2SimdBody bA = b2GatherBodies( states, c->indexA ); - b2SimdBody bB = b2GatherBodies( states, c->indexB ); + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); // first point non-penetration constraint { @@ -1881,9 +1995,6 @@ void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* contex b2TracyCZoneEnd( restitution ); } -#if B2_SIMD_WIDTH == 8 - -// todo try making an inner loop on B2_SIMD_WIDTH to have a single implementation of this function void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) { b2TracyCZoneNC( store_impulses, "Store", b2_colorFireBrick, true ); @@ -1893,9 +2004,10 @@ void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) b2Manifold dummy = { 0 }; - for ( int i = startIndex; i < endIndex; ++i ) + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) { - const b2ContactConstraintSIMD* c = constraints + i; + const b2ContactConstraintSIMD* c = constraints + constraintIndex; + const float* rollingImpulse = (float*)&c->rollingImpulse; const float* normalImpulse1 = (float*)&c->normalImpulse1; const float* normalImpulse2 = (float*)&c->normalImpulse2; const float* tangentImpulse1 = (float*)&c->tangentImpulse1; @@ -1905,171 +2017,24 @@ void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) const float* normalVelocity1 = (float*)&c->relativeVelocity1; const float* normalVelocity2 = (float*)&c->relativeVelocity2; - int base = 8 * i; - b2Manifold* m0 = contacts[base + 0] == NULL ? &dummy : &contacts[base + 0]->manifold; - b2Manifold* m1 = contacts[base + 1] == NULL ? &dummy : &contacts[base + 1]->manifold; - b2Manifold* m2 = contacts[base + 2] == NULL ? &dummy : &contacts[base + 2]->manifold; - b2Manifold* m3 = contacts[base + 3] == NULL ? &dummy : &contacts[base + 3]->manifold; - b2Manifold* m4 = contacts[base + 4] == NULL ? &dummy : &contacts[base + 4]->manifold; - b2Manifold* m5 = contacts[base + 5] == NULL ? &dummy : &contacts[base + 5]->manifold; - b2Manifold* m6 = contacts[base + 6] == NULL ? &dummy : &contacts[base + 6]->manifold; - b2Manifold* m7 = contacts[base + 7] == NULL ? &dummy : &contacts[base + 7]->manifold; - - m0->points[0].normalImpulse = normalImpulse1[0]; - m0->points[0].tangentImpulse = tangentImpulse1[0]; - m0->points[0].maxNormalImpulse = maxNormalImpulse1[0]; - m0->points[0].normalVelocity = normalVelocity1[0]; - - m0->points[1].normalImpulse = normalImpulse2[0]; - m0->points[1].tangentImpulse = tangentImpulse2[0]; - m0->points[1].maxNormalImpulse = maxNormalImpulse2[0]; - m0->points[1].normalVelocity = normalVelocity2[0]; - - m1->points[0].normalImpulse = normalImpulse1[1]; - m1->points[0].tangentImpulse = tangentImpulse1[1]; - m1->points[0].maxNormalImpulse = maxNormalImpulse1[1]; - m1->points[0].normalVelocity = normalVelocity1[1]; - - m1->points[1].normalImpulse = normalImpulse2[1]; - m1->points[1].tangentImpulse = tangentImpulse2[1]; - m1->points[1].maxNormalImpulse = maxNormalImpulse2[1]; - m1->points[1].normalVelocity = normalVelocity2[1]; - - m2->points[0].normalImpulse = normalImpulse1[2]; - m2->points[0].tangentImpulse = tangentImpulse1[2]; - m2->points[0].maxNormalImpulse = maxNormalImpulse1[2]; - m2->points[0].normalVelocity = normalVelocity1[2]; - - m2->points[1].normalImpulse = normalImpulse2[2]; - m2->points[1].tangentImpulse = tangentImpulse2[2]; - m2->points[1].maxNormalImpulse = maxNormalImpulse2[2]; - m2->points[1].normalVelocity = normalVelocity2[2]; - - m3->points[0].normalImpulse = normalImpulse1[3]; - m3->points[0].tangentImpulse = tangentImpulse1[3]; - m3->points[0].maxNormalImpulse = maxNormalImpulse1[3]; - m3->points[0].normalVelocity = normalVelocity1[3]; - - m3->points[1].normalImpulse = normalImpulse2[3]; - m3->points[1].tangentImpulse = tangentImpulse2[3]; - m3->points[1].maxNormalImpulse = maxNormalImpulse2[3]; - m3->points[1].normalVelocity = normalVelocity2[3]; - - m4->points[0].normalImpulse = normalImpulse1[4]; - m4->points[0].tangentImpulse = tangentImpulse1[4]; - m4->points[0].maxNormalImpulse = maxNormalImpulse1[4]; - m4->points[0].normalVelocity = normalVelocity1[4]; - - m4->points[1].normalImpulse = normalImpulse2[4]; - m4->points[1].tangentImpulse = tangentImpulse2[4]; - m4->points[1].maxNormalImpulse = maxNormalImpulse2[4]; - m4->points[1].normalVelocity = normalVelocity2[4]; - - m5->points[0].normalImpulse = normalImpulse1[5]; - m5->points[0].tangentImpulse = tangentImpulse1[5]; - m5->points[0].maxNormalImpulse = maxNormalImpulse1[5]; - m5->points[0].normalVelocity = normalVelocity1[5]; - - m5->points[1].normalImpulse = normalImpulse2[5]; - m5->points[1].tangentImpulse = tangentImpulse2[5]; - m5->points[1].maxNormalImpulse = maxNormalImpulse2[5]; - m5->points[1].normalVelocity = normalVelocity2[5]; - - m6->points[0].normalImpulse = normalImpulse1[6]; - m6->points[0].tangentImpulse = tangentImpulse1[6]; - m6->points[0].maxNormalImpulse = maxNormalImpulse1[6]; - m6->points[0].normalVelocity = normalVelocity1[6]; - - m6->points[1].normalImpulse = normalImpulse2[6]; - m6->points[1].tangentImpulse = tangentImpulse2[6]; - m6->points[1].maxNormalImpulse = maxNormalImpulse2[6]; - m6->points[1].normalVelocity = normalVelocity2[6]; - - m7->points[0].normalImpulse = normalImpulse1[7]; - m7->points[0].tangentImpulse = tangentImpulse1[7]; - m7->points[0].maxNormalImpulse = maxNormalImpulse1[7]; - m7->points[0].normalVelocity = normalVelocity1[7]; - - m7->points[1].normalImpulse = normalImpulse2[7]; - m7->points[1].tangentImpulse = tangentImpulse2[7]; - m7->points[1].maxNormalImpulse = maxNormalImpulse2[7]; - m7->points[1].normalVelocity = normalVelocity2[7]; - } - - b2TracyCZoneEnd( store_impulses ); -} - -#else - -void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) -{ - b2TracyCZoneNC( store_impulses, "Store", b2_colorFirebrick, true ); - - b2ContactSim** contacts = context->contacts; - const b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + int baseIndex = B2_SIMD_WIDTH * constraintIndex; - b2Manifold dummy = { 0 }; - - for ( int i = startIndex; i < endIndex; ++i ) - { - const b2ContactConstraintSIMD* c = constraints + i; - const float* normalImpulse1 = (float*)&c->normalImpulse1; - const float* normalImpulse2 = (float*)&c->normalImpulse2; - const float* tangentImpulse1 = (float*)&c->tangentImpulse1; - const float* tangentImpulse2 = (float*)&c->tangentImpulse2; - const float* maxNormalImpulse1 = (float*)&c->maxNormalImpulse1; - const float* maxNormalImpulse2 = (float*)&c->maxNormalImpulse2; - const float* normalVelocity1 = (float*)&c->relativeVelocity1; - const float* normalVelocity2 = (float*)&c->relativeVelocity2; - - int base = 4 * i; - b2Manifold* m0 = contacts[base + 0] == NULL ? &dummy : &contacts[base + 0]->manifold; - b2Manifold* m1 = contacts[base + 1] == NULL ? &dummy : &contacts[base + 1]->manifold; - b2Manifold* m2 = contacts[base + 2] == NULL ? &dummy : &contacts[base + 2]->manifold; - b2Manifold* m3 = contacts[base + 3] == NULL ? &dummy : &contacts[base + 3]->manifold; - - m0->points[0].normalImpulse = normalImpulse1[0]; - m0->points[0].tangentImpulse = tangentImpulse1[0]; - m0->points[0].maxNormalImpulse = maxNormalImpulse1[0]; - m0->points[0].normalVelocity = normalVelocity1[0]; - - m0->points[1].normalImpulse = normalImpulse2[0]; - m0->points[1].tangentImpulse = tangentImpulse2[0]; - m0->points[1].maxNormalImpulse = maxNormalImpulse2[0]; - m0->points[1].normalVelocity = normalVelocity2[0]; - - m1->points[0].normalImpulse = normalImpulse1[1]; - m1->points[0].tangentImpulse = tangentImpulse1[1]; - m1->points[0].maxNormalImpulse = maxNormalImpulse1[1]; - m1->points[0].normalVelocity = normalVelocity1[1]; - - m1->points[1].normalImpulse = normalImpulse2[1]; - m1->points[1].tangentImpulse = tangentImpulse2[1]; - m1->points[1].maxNormalImpulse = maxNormalImpulse2[1]; - m1->points[1].normalVelocity = normalVelocity2[1]; - - m2->points[0].normalImpulse = normalImpulse1[2]; - m2->points[0].tangentImpulse = tangentImpulse1[2]; - m2->points[0].maxNormalImpulse = maxNormalImpulse1[2]; - m2->points[0].normalVelocity = normalVelocity1[2]; - - m2->points[1].normalImpulse = normalImpulse2[2]; - m2->points[1].tangentImpulse = tangentImpulse2[2]; - m2->points[1].maxNormalImpulse = maxNormalImpulse2[2]; - m2->points[1].normalVelocity = normalVelocity2[2]; - - m3->points[0].normalImpulse = normalImpulse1[3]; - m3->points[0].tangentImpulse = tangentImpulse1[3]; - m3->points[0].maxNormalImpulse = maxNormalImpulse1[3]; - m3->points[0].normalVelocity = normalVelocity1[3]; - - m3->points[1].normalImpulse = normalImpulse2[3]; - m3->points[1].tangentImpulse = tangentImpulse2[3]; - m3->points[1].maxNormalImpulse = maxNormalImpulse2[3]; - m3->points[1].normalVelocity = normalVelocity2[3]; + for ( int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex ) + { + b2Manifold* m = contacts[baseIndex + laneIndex] == NULL ? &dummy : &contacts[baseIndex + laneIndex]->manifold; + m->rollingImpulse = rollingImpulse[laneIndex]; + + m->points[0].normalImpulse = normalImpulse1[laneIndex]; + m->points[0].tangentImpulse = tangentImpulse1[laneIndex]; + m->points[0].maxNormalImpulse = maxNormalImpulse1[laneIndex]; + m->points[0].normalVelocity = normalVelocity1[laneIndex]; + + m->points[1].normalImpulse = normalImpulse2[laneIndex]; + m->points[1].tangentImpulse = tangentImpulse2[laneIndex]; + m->points[1].maxNormalImpulse = maxNormalImpulse2[laneIndex]; + m->points[1].normalVelocity = normalVelocity2[laneIndex]; + } } b2TracyCZoneEnd( store_impulses ); } - -#endif 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..cb7a5b4d8 100644 --- a/src/core.h +++ b/src/core.h @@ -110,7 +110,7 @@ #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_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) ) // Use to validate definitions. Do not take my cookie. #define B2_SECRET_COOKIE 1152023 diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c index 7bfa2f066..22e884153 100644 --- a/src/dynamic_tree.c +++ b/src/dynamic_tree.c @@ -1025,7 +1025,7 @@ void b2DynamicTree_Validate( const b2DynamicTree* tree ) B2_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); #else - B2_MAYBE_UNUSED( tree ); + B2_UNUSED( tree ); #endif } @@ -1043,7 +1043,7 @@ void b2DynamicTree_ValidateNoEnlarged(const b2DynamicTree* tree) } } #else - B2_MAYBE_UNUSED( tree ); + B2_UNUSED( tree ); #endif } diff --git a/src/id_pool.c b/src/id_pool.c index 11b542d7b..bca24faff 100644 --- a/src/id_pool.c +++ b/src/id_pool.c @@ -64,8 +64,8 @@ void b2ValidateFreeId( b2IdPool* pool, int id ) void b2ValidateFreeId( b2IdPool* pool, int id ) { - B2_MAYBE_UNUSED( pool ); - B2_MAYBE_UNUSED( id ); + B2_UNUSED( pool ); + B2_UNUSED( id ); } #endif diff --git a/src/island.c b/src/island.c index 13b299bd4..3a3a87610 100644 --- a/src/island.c +++ b/src/island.c @@ -843,9 +843,7 @@ void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void { b2TracyCZoneNC( split, "Split Island", b2_colorOlive, true ); - B2_MAYBE_UNUSED( startIndex ); - B2_MAYBE_UNUSED( endIndex ); - B2_MAYBE_UNUSED( threadIndex ); + B2_UNUSED( startIndex, endIndex, threadIndex ); uint64_t ticks = b2GetTicks(); b2World* world = context; @@ -966,7 +964,7 @@ void b2ValidateIsland( b2World* world, int islandId ) void b2ValidateIsland( b2World* world, int islandId ) { - B2_MAYBE_UNUSED( world ); - B2_MAYBE_UNUSED( islandId ); + B2_UNUSED( world ); + B2_UNUSED( islandId ); } #endif diff --git a/src/joint.c b/src/joint.c index 41cb00cb1..c7b321624 100644 --- a/src/joint.c +++ b/src/joint.c @@ -121,7 +121,7 @@ b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ) b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ) { - B2_MAYBE_UNUSED( type ); + B2_UNUSED( type ); b2World* world = b2GetWorld( jointId.world0 ); B2_ASSERT( world->locked == false ); diff --git a/src/motor_joint.c b/src/motor_joint.c index 4dd5defab..d6610d62e 100644 --- a/src/motor_joint.c +++ b/src/motor_joint.c @@ -186,7 +186,7 @@ void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ) void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias ) { - B2_MAYBE_UNUSED( useBias ); + B2_UNUSED( useBias ); B2_ASSERT( base->type == b2_motorJoint ); float mA = base->invMassA; diff --git a/src/sensor.c b/src/sensor.c index 98d7ebc43..8b66b6f6e 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -46,7 +46,7 @@ struct b2SensorQueryContext static bool b2SensorQueryCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); struct b2SensorQueryContext* queryContext = context; b2Shape* sensorShape = queryContext->sensorShape; diff --git a/src/shape.c b/src/shape.c index 42dcd4793..a3bad3f8a 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; @@ -141,7 +143,7 @@ static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform body->headShapeId = shapeId; body->shapeCount += 1; - if (def->isSensor) + if ( def->isSensor ) { shape->sensorIndex = world->sensors.count; b2Sensor sensor = { @@ -269,7 +271,7 @@ static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body } } - if (shape->sensorIndex != B2_NULL_INDEX) + if ( shape->sensorIndex != B2_NULL_INDEX ) { b2Sensor* sensor = b2SensorArray_Get( &world->sensors, shape->sensorIndex ); for ( int i = 0; i < sensor->overlaps2.count; ++i ) @@ -339,9 +341,8 @@ void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass ) b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) { B2_CHECK_DEF( def ); - B2_ASSERT( b2IsValidFloat( def->friction ) && def->friction >= 0.0f ); - B2_ASSERT( b2IsValidFloat( def->restitution ) && def->restitution >= 0.0f ); B2_ASSERT( def->count >= 4 ); + B2_ASSERT( def->materialCount == 1 || def->materialCount == def->count ); b2World* world = b2GetWorldLocked( bodyId.world0 ); if ( world == NULL ) @@ -369,29 +370,37 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) chainShape->bodyId = body->id; chainShape->nextChainId = body->headChainId; chainShape->generation += 1; - chainShape->friction = def->friction; - chainShape->restitution = def->restitution; - chainShape->material = def->material; + + int materialCount = def->materialCount; + chainShape->materialCount = materialCount; + chainShape->materials = b2Alloc( materialCount * sizeof( b2SurfaceMaterial ) ); + + for ( int i = 0; i < materialCount; ++i ) + { + const b2SurfaceMaterial* material = def->materials + i; + B2_ASSERT( b2IsValidFloat( material->friction ) && material->friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->restitution ) && material->restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->rollingResistance ) && material->rollingResistance >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->tangentSpeed ) ); + + chainShape->materials[i] = *material; + } body->headChainId = chainId; b2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.userData = def->userData; - shapeDef.friction = def->friction; - shapeDef.restitution = def->restitution; - shapeDef.material = def->material; shapeDef.filter = def->filter; - shapeDef.customColor = def->customColor; shapeDef.enableContactEvents = false; shapeDef.enableHitEvents = false; - int n = def->count; const b2Vec2* points = def->points; + int n = def->count; if ( def->isLoop ) { chainShape->count = n; - chainShape->shapeIndices = b2Alloc( n * sizeof( int ) ); + chainShape->shapeIndices = b2Alloc( chainShape->count * sizeof( int ) ); b2ChainSegment chainSegment; @@ -405,6 +414,15 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) chainSegment.chainId = chainId; prevIndex = i; + int materialIndex = materialCount == 1 ? 0 : i; + const b2SurfaceMaterial* material = def->materials + materialIndex; + shapeDef.friction = material->friction; + shapeDef.restitution = material->restitution; + shapeDef.rollingResistance = material->rollingResistance; + shapeDef.tangentSpeed = material->tangentSpeed; + shapeDef.customColor = material->customColor; + shapeDef.material = material->material; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); chainShape->shapeIndices[i] = shape->id; } @@ -415,6 +433,16 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) chainSegment.segment.point2 = points[n - 1]; chainSegment.ghost2 = points[0]; chainSegment.chainId = chainId; + + int materialIndex = materialCount == 1 ? 0 : n - 2; + const b2SurfaceMaterial* material = def->materials + materialIndex; + shapeDef.friction = material->friction; + shapeDef.restitution = material->restitution; + shapeDef.rollingResistance = material->rollingResistance; + shapeDef.tangentSpeed = material->tangentSpeed; + shapeDef.customColor = material->customColor; + shapeDef.material = material->material; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); chainShape->shapeIndices[n - 2] = shape->id; } @@ -425,6 +453,16 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) chainSegment.segment.point2 = points[0]; chainSegment.ghost2 = points[1]; chainSegment.chainId = chainId; + + int materialIndex = materialCount == 1 ? 0 : n - 1; + const b2SurfaceMaterial* material = def->materials + materialIndex; + shapeDef.friction = material->friction; + shapeDef.restitution = material->restitution; + shapeDef.rollingResistance = material->rollingResistance; + shapeDef.tangentSpeed = material->tangentSpeed; + shapeDef.customColor = material->customColor; + shapeDef.material = material->material; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); chainShape->shapeIndices[n - 1] = shape->id; } @@ -432,7 +470,7 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) else { chainShape->count = n - 3; - chainShape->shapeIndices = b2Alloc( n * sizeof( int ) ); + chainShape->shapeIndices = b2Alloc( chainShape->count * sizeof( int ) ); b2ChainSegment chainSegment; @@ -444,6 +482,16 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) chainSegment.ghost2 = points[i + 3]; chainSegment.chainId = chainId; + // Material is associated with leading point of solid segment + int materialIndex = materialCount == 1 ? 0 : i + 1; + const b2SurfaceMaterial* material = def->materials + materialIndex; + shapeDef.friction = material->friction; + shapeDef.restitution = material->restitution; + shapeDef.rollingResistance = material->rollingResistance; + shapeDef.tangentSpeed = material->tangentSpeed; + shapeDef.customColor = material->customColor; + shapeDef.material = material->material; + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); chainShape->shapeIndices[i] = shape->id; } @@ -453,6 +501,15 @@ b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) return id; } +void b2FreeChainData(b2ChainShape* chain) +{ + b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + chain->shapeIndices = NULL; + + b2Free( chain->materials, chain->materialCount * sizeof( b2SurfaceMaterial ) ); + chain->materials = NULL; +} + void b2DestroyChain( b2ChainId chainId ) { b2World* world = b2GetWorldLocked( chainId.world0 ); @@ -495,8 +552,7 @@ void b2DestroyChain( b2ChainId chainId ) b2DestroyShapeInternal( world, shape, body, wakeBodies ); } - b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); - chain->shapeIndices = NULL; + b2FreeChainData( chain ); // Return chain to free list. b2FreeId( &world->chainIdPool, chain->id ); @@ -1361,7 +1417,7 @@ b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ) void b2Chain_SetFriction( b2ChainId chainId, float friction ) { - B2_ASSERT( b2IsValidFloat( friction ) ); + B2_ASSERT( b2IsValidFloat( friction ) && friction >= 0.0f ); b2World* world = b2GetWorldLocked( chainId.world0 ); if ( world == NULL ) @@ -1370,7 +1426,12 @@ void b2Chain_SetFriction( b2ChainId chainId, float friction ) } b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - chainShape->friction = friction; + + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].friction = friction; + } int count = chainShape->count; @@ -1386,7 +1447,7 @@ float b2Chain_GetFriction( b2ChainId chainId ) { b2World* world = b2GetWorld( chainId.world0 ); b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - return chainShape->friction; + return chainShape->materials[0].friction; } void b2Chain_SetRestitution( b2ChainId chainId, float restitution ) @@ -1400,7 +1461,12 @@ void b2Chain_SetRestitution( b2ChainId chainId, float restitution ) } b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - chainShape->restitution = restitution; + + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].restitution = restitution; + } int count = chainShape->count; @@ -1416,7 +1482,7 @@ float b2Chain_GetRestitution( b2ChainId chainId ) { b2World* world = b2GetWorld( chainId.world0 ); b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - return chainShape->restitution; + return chainShape->materials[0].restitution; } void b2Chain_SetMaterial( b2ChainId chainId, int material ) @@ -1428,7 +1494,11 @@ void b2Chain_SetMaterial( b2ChainId chainId, int material ) } b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - chainShape->material = material; + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].material = material; + } int count = chainShape->count; @@ -1444,7 +1514,7 @@ int b2Chain_GetMaterial( b2ChainId chainId ) { b2World* world = b2GetWorld( chainId.world0 ); b2ChainShape* chainShape = b2GetChainShape( world, chainId ); - return chainShape->material; + return chainShape->materials[0].material; } int b2Shape_GetContactCapacity( b2ShapeId shapeId ) @@ -1514,7 +1584,7 @@ int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int c return index; } -int b2Shape_GetSensorCapacity(b2ShapeId shapeId) +int b2Shape_GetSensorCapacity( b2ShapeId shapeId ) { b2World* world = b2GetWorldLocked( shapeId.world0 ); if ( world == NULL ) @@ -1574,7 +1644,7 @@ b2AABB b2Shape_GetAABB( b2ShapeId shapeId ) return shape->aabb; } -b2MassData b2Shape_GetMassData(b2ShapeId shapeId) +b2MassData b2Shape_GetMassData( b2ShapeId shapeId ) { b2World* world = b2GetWorld( shapeId.world0 ); if ( world == NULL ) diff --git a/src/shape.h b/src/shape.h index 71b0278a1..6d986f1dc 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; @@ -54,10 +56,9 @@ typedef struct b2ChainShape int bodyId; int nextChainId; int count; + int materialCount; int* shapeIndices; - float friction; - float restitution; - int material; + b2SurfaceMaterial* materials; uint16_t generation; } b2ChainShape; @@ -82,6 +83,8 @@ typedef struct void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation ); void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp ); +void b2FreeChainData( b2ChainShape* chain ); + b2MassData b2ComputeShapeMass( const b2Shape* shape ); b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter ); b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform transform ); @@ -94,5 +97,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..587f1cc98 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 ) { @@ -206,7 +207,7 @@ struct b2ContinuousContext // This is called from b2DynamicTree_Query for continuous collision static bool b2ContinuousQueryCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); struct b2ContinuousContext* continuousContext = context; b2Shape* fastShape = continuousContext->fastShape; @@ -915,11 +916,9 @@ static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, ui } // This should not use the thread index because thread 0 can be called twice by enkiTS. -static void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexDontUse, void* taskContext ) +static void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexIgnore, void* taskContext ) { - B2_MAYBE_UNUSED( startIndex ); - B2_MAYBE_UNUSED( endIndex ); - B2_MAYBE_UNUSED( threadIndexDontUse ); + B2_UNUSED( startIndex, endIndex, threadIndexIgnore ); b2WorkerContext* workerContext = taskContext; int workerIndex = workerContext->workerIndex; @@ -1145,7 +1144,7 @@ static void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexDont static void b2BulletBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) { - B2_MAYBE_UNUSED( threadIndex ); + B2_UNUSED( threadIndex ); b2TracyCZoneNC( bullet_body_task, "Bullet", b2_colorLightSkyBlue, true ); diff --git a/src/table.c b/src/table.c index 00e238066..918b2db8e 100644 --- a/src/table.c +++ b/src/table.c @@ -107,7 +107,7 @@ static void b2AddKeyHaveCapacity( b2HashSet* set, uint64_t key, uint32_t hash ) static void b2GrowTable( b2HashSet* set ) { uint32_t oldCount = set->count; - B2_MAYBE_UNUSED( oldCount ); + B2_UNUSED( oldCount ); uint32_t oldCapacity = set->capacity; b2SetItem* oldItems = set->items; diff --git a/src/types.c b/src/types.c index 9b92defcf..8be9402ed 100644 --- a/src/types.c +++ b/src/types.c @@ -63,10 +63,18 @@ b2ShapeDef b2DefaultShapeDef( void ) return def; } +b2SurfaceMaterial b2DefaultSurfaceMaterial( void ) +{ + b2SurfaceMaterial material = { + .friction = 0.6f, + }; + + return material; +} + b2ChainDef b2DefaultChainDef( void ) { b2ChainDef def = { 0 }; - def.friction = 0.6f; def.filter = b2DefaultFilter(); def.internalValue = B2_SECRET_COOKIE; return def; @@ -74,79 +82,51 @@ b2ChainDef b2DefaultChainDef( void ) static void b2EmptyDrawPolygon( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( vertices ); - B2_MAYBE_UNUSED( vertexCount ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( vertices, vertexCount, color, context ); } -static void b2EmptyDrawSolidPolygon( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, b2HexColor color, - void* context ) +static void b2EmptyDrawSolidPolygon( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, + b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( transform ); - B2_MAYBE_UNUSED( vertices ); - B2_MAYBE_UNUSED( vertexCount ); - B2_MAYBE_UNUSED( radius ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( transform, vertices, vertexCount, radius, color, context ); } static void b2EmptyDrawCircle( b2Vec2 center, float radius, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( center ); - B2_MAYBE_UNUSED( radius ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( center, radius, color, context ); } static void b2EmptyDrawSolidCircle( b2Transform transform, float radius, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( transform ); - B2_MAYBE_UNUSED( radius ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( transform, radius, color, context ); } static void b2EmptyDrawSolidCapsule( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( p1 ); - B2_MAYBE_UNUSED( p2 ); - B2_MAYBE_UNUSED( radius ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( p1, p2, radius, color, context ); } static void b2EmptyDrawSegment( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( p1 ); - B2_MAYBE_UNUSED( p2 ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( p1, p2, color, context ); } static void b2EmptyDrawTransform( b2Transform transform, void* context ) { - B2_MAYBE_UNUSED( transform ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( transform, context ); } static void b2EmptyDrawPoint( b2Vec2 p, float size, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( p ); - B2_MAYBE_UNUSED( size ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( p, size, color, context ); } static void b2EmptyDrawString( b2Vec2 p, const char* s, b2HexColor color, void* context ) { - B2_MAYBE_UNUSED( p ); - B2_MAYBE_UNUSED( s ); - B2_MAYBE_UNUSED( color ); - B2_MAYBE_UNUSED( context ); + B2_UNUSED( p, s, color, context ); } -b2DebugDraw b2DefaultDebugDraw(void) +b2DebugDraw b2DefaultDebugDraw( void ) { b2DebugDraw draw = { 0 }; diff --git a/src/world.c b/src/world.c index 3e7eeb36c..7d8ad1e93 100644 --- a/src/world.c +++ b/src/world.c @@ -75,29 +75,25 @@ b2World* b2GetWorldLocked( int index ) static void* b2DefaultAddTaskFcn( b2TaskCallback* task, int count, int minRange, void* taskContext, void* userContext ) { - B2_MAYBE_UNUSED( minRange ); - B2_MAYBE_UNUSED( userContext ); + B2_UNUSED( minRange, userContext ); task( 0, count, 0, taskContext ); return NULL; } static void b2DefaultFinishTaskFcn( void* userTask, void* userContext ) { - B2_MAYBE_UNUSED( userTask ); - B2_MAYBE_UNUSED( userContext ); + B2_UNUSED( userTask, userContext ); } static float b2DefaultFrictionCallback( float frictionA, int materialA, float frictionB, int materialB ) { - B2_MAYBE_UNUSED( materialA ); - B2_MAYBE_UNUSED( materialB ); + B2_UNUSED( materialA, materialB ); return sqrtf( frictionA * frictionB ); } static float b2DefaultRestitutionCallback( float restitutionA, int materialA, float restitutionB, int materialB ) { - B2_MAYBE_UNUSED( materialA ); - B2_MAYBE_UNUSED( materialB ); + B2_UNUSED( materialA, materialB ); return b2MaxFloat( restitutionA, restitutionB ); } @@ -201,7 +197,7 @@ b2WorldId b2CreateWorld( const b2WorldDef* def ) world->jointHertz = def->jointHertz; world->jointDampingRatio = def->jointDampingRatio; - if (def->frictionCallback == NULL) + if ( def->frictionCallback == NULL ) { world->frictionCallback = b2DefaultFrictionCallback; } @@ -300,11 +296,12 @@ void b2DestroyWorld( b2WorldId worldId ) b2ChainShape* chain = world->chainShapes.data + i; if ( chain->id != B2_NULL_INDEX ) { - b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + b2FreeChainData( chain ); } else { B2_ASSERT( chain->shapeIndices == NULL ); + B2_ASSERT( chain->materials == NULL ); } } @@ -436,9 +433,9 @@ static void b2CollideTask( int startIndex, int endIndex, uint32_t threadIndex, v static void b2UpdateTreesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) { - B2_MAYBE_UNUSED( startIndex ); - B2_MAYBE_UNUSED( endIndex ); - B2_MAYBE_UNUSED( threadIndex ); + B2_UNUSED( startIndex ); + B2_UNUSED( endIndex ); + B2_UNUSED( threadIndex ); b2TracyCZoneNC( tree_task, "Rebuild BVH", b2_colorFireBrick, true ); @@ -863,7 +860,7 @@ struct DrawContext static bool DrawQueryCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); struct DrawContext* drawContext = context; b2World* world = drawContext->world; @@ -993,7 +990,7 @@ static void b2DrawWithBounds( b2World* world, b2DebugDraw* draw ) b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); - if (draw->drawBodyNames && body->name[0] != 0) + if ( draw->drawBodyNames && body->name[0] != 0 ) { b2Vec2 offset = { 0.1f, 0.1f }; b2BodySim* bodySim = b2GetBodySim( world, body ); @@ -1299,7 +1296,7 @@ void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw ) continue; } - if ( body->name[0] == 0) + if ( body->name[0] == 0 ) { continue; } @@ -1849,15 +1846,15 @@ void* b2World_GetUserData( b2WorldId worldId ) return world->userData; } -void b2World_SetFrictionCallback(b2WorldId worldId, b2FrictionCallback* callback) +void b2World_SetFrictionCallback( b2WorldId worldId, b2FrictionCallback* callback ) { b2World* world = b2GetWorldFromId( worldId ); - if (world->locked) + if ( world->locked ) { return; } - if (callback != NULL) + if ( callback != NULL ) { world->frictionCallback = callback; } @@ -1870,12 +1867,12 @@ void b2World_SetFrictionCallback(b2WorldId worldId, b2FrictionCallback* callback void b2World_SetRestitutionCallback( b2WorldId worldId, b2RestitutionCallback* callback ) { b2World* world = b2GetWorldFromId( worldId ); - if (world->locked) + if ( world->locked ) { return; } - if (callback != NULL) + if ( callback != NULL ) { world->restitutionCallback = callback; } @@ -1996,7 +1993,7 @@ typedef struct WorldQueryContext static bool TreeQueryCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); WorldQueryContext* worldContext = context; b2World* world = worldContext->world; @@ -2055,7 +2052,7 @@ typedef struct WorldOverlapContext static bool TreeOverlapCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); WorldOverlapContext* worldContext = context; b2World* world = worldContext->world; @@ -2207,7 +2204,7 @@ typedef struct WorldRayCastContext static float RayCastCallback( const b2RayCastInput* input, int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); WorldRayCastContext* worldContext = context; b2World* world = worldContext->world; @@ -2328,7 +2325,7 @@ b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 tra static float ShapeCastCallback( const b2ShapeCastInput* input, int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); WorldRayCastContext* worldContext = context; b2World* world = worldContext->world; @@ -2618,7 +2615,7 @@ struct ExplosionContext static bool ExplosionCallback( int proxyId, int shapeId, void* context ) { - B2_MAYBE_UNUSED( proxyId ); + B2_UNUSED( proxyId ); struct ExplosionContext* explosionContext = context; b2World* world = explosionContext->world; @@ -3233,17 +3230,17 @@ void b2ValidateContacts( b2World* world ) void b2ValidateConnectivity( b2World* world ) { - B2_MAYBE_UNUSED( world ); + B2_UNUSED( world ); } void b2ValidateSolverSets( b2World* world ) { - B2_MAYBE_UNUSED( world ); + B2_UNUSED( world ); } void b2ValidateContacts( b2World* world ) { - B2_MAYBE_UNUSED( world ); + B2_UNUSED( world ); } #endif