From 577c62a29925cd7f0f67c4776af01f31e11ea422 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Mon, 14 Oct 2024 00:20:17 -0700 Subject: [PATCH] aabb margin experiments spinner benchmark --- benchmark/CMakeLists.txt | 1 + benchmark/main.c | 4 + benchmark/spinner.c | 112 ++++++++++++++++++ samples/sample_benchmark.cpp | 213 ++++++++++++++++++++++++++++++++++ samples/sample_joints.cpp | 219 ----------------------------------- src/aabb.h | 12 +- src/core.h | 5 +- src/solver.c | 84 ++++++++++++++ 8 files changed, 420 insertions(+), 230 deletions(-) create mode 100644 benchmark/spinner.c diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index af1c842f6..b42adc59e 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -6,6 +6,7 @@ add_executable(benchmark large_pyramid.c many_pyramids.c smash.c + spinner.c tumbler.c ) diff --git a/benchmark/main.c b/benchmark/main.c index a3ff97930..585b60f31 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -30,6 +30,7 @@ extern b2WorldId JointGrid( b2WorldDef* worldDef ); extern b2WorldId LargePyramid( b2WorldDef* worldDef ); extern b2WorldId ManyPyramids( b2WorldDef* worldDef ); extern b2WorldId Smash( b2WorldDef* worldDef ); +extern b2WorldId Spinner( b2WorldDef* worldDef ); extern b2WorldId Tumbler( b2WorldDef* worldDef ); typedef struct Benchmark @@ -115,6 +116,8 @@ static void FinishTask( void* userTask, void* userContext ) } // Box2D benchmark application. On Windows I recommend running this in an administrator command prompt. Don't use Windows Terminal. +// Or use affinity. [0x01 0x02 0x04 0x08 0x10 0x20 0x40 0x80] +// start /affinity 0x5555 .\build\bin\Release\benchmark.exe -t=4 -w=4 int main( int argc, char** argv ) { Benchmark benchmarks[] = { @@ -122,6 +125,7 @@ int main( int argc, char** argv ) { "large_pyramid", LargePyramid, 500 }, { "many_pyramids", ManyPyramids, 200 }, { "smash", Smash, 300 }, + { "spinner", Spinner, 1400 }, { "tumbler", Tumbler, 750 }, }; diff --git a/benchmark/spinner.c b/benchmark/spinner.c new file mode 100644 index 000000000..c14cd1fc0 --- /dev/null +++ b/benchmark/spinner.c @@ -0,0 +1,112 @@ +// SPDX-FileCopyrightText: 2024 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/box2d.h" +#include "box2d/math_functions.h" + +enum +{ + e_count = 3038, + e_pointCount = 360, +}; + +b2WorldId Spinner( b2WorldDef* worldDef ) +{ + b2WorldId worldId = b2CreateWorld( worldDef ); + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( worldId, &bodyDef ); + + b2Vec2 points[e_pointCount]; + + b2Rot q = b2MakeRot( -2.0f * b2_pi / e_pointCount ); + b2Vec2 p = { 40.0f, 0.0f }; + for ( int i = 0; i < e_pointCount; ++i ) + { + points[i] = (b2Vec2){ p.x, p.y + 32.0f }; + p = b2RotateVector( q, p ); + } + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = e_pointCount; + chainDef.isLoop = true; + chainDef.friction = 0.1f; + + b2CreateChain( groundId, &chainDef ); + } + + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = (b2Vec2){ 0.0, 12.0f }; + bodyDef.enableSleep = false; + + b2BodyId spinnerId = b2CreateBody( worldId, &bodyDef ); + + b2Polygon box = b2MakeRoundedBox( 0.4f, 20.0f, 0.2f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.0f; + b2CreatePolygonShape( spinnerId, &shapeDef, &box ); + + float motorSpeed = 5.0f; + float maxMotorTorque = 40000.0f; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = spinnerId; + jointDef.localAnchorA = bodyDef.position; + jointDef.enableMotor = true; + jointDef.motorSpeed = motorSpeed; + jointDef.maxMotorTorque = maxMotorTorque; + } + + b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; + b2Circle circle = { { 0.0f, 0.0f }, 0.35f }; + b2Polygon square = b2MakeSquare( 0.35f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.1f; + shapeDef.restitution = 0.1f; + shapeDef.density = 0.25f; + +#ifdef NDEBUG + int bodyCount = e_count; +#else + int bodyCount = 499; +#endif + + float x = -24.0f, y = 2.0f; + for ( int i = 0; i < bodyCount; ++i ) + { + bodyDef.position = (b2Vec2){ x, y }; + b2BodyId bodyId = b2CreateBody( worldId, &bodyDef ); + + int remainder = i % 3; + if ( remainder == 0 ) + { + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else if ( remainder == 1 ) + { + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + else if ( remainder == 2 ) + { + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + + x += 1.0f; + + if ( x > 24.0f ) + { + x = -24.0f; + y += 1.0f; + } + } + + return worldId; +} diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 46cbecc1c..e9c67ac57 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -1975,3 +1975,216 @@ class BenchmarkCast : public Sample }; static int sampleCast = RegisterSample( "Benchmark", "Cast", BenchmarkCast::Create ); + + +class BenchmarkSpinner : public Sample +{ +public: + enum + { + e_count = 3038, + e_pointCount = 360, + }; + + explicit BenchmarkSpinner( Settings& settings ) + : Sample( settings ) + { + if ( settings.restart == false ) + { + g_camera.m_center = { 0.0f, 32.0f }; + g_camera.m_zoom = 42.0f; + } + + b2BodyId groundId; + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody( m_worldId, &bodyDef ); + + b2Vec2 points[e_pointCount]; + + b2Rot q = b2MakeRot( -2.0f * b2_pi / e_pointCount ); + b2Vec2 p = { 40.0f, 0.0f }; + for ( int i = 0; i < e_pointCount; ++i ) + { + points[i] = { p.x, p.y + 32.0f }; + p = b2RotateVector( q, p ); + } + + b2ChainDef chainDef = b2DefaultChainDef(); + chainDef.points = points; + chainDef.count = e_pointCount; + chainDef.isLoop = true; + chainDef.friction = 0.1f; + chainDef.customColor = b2_colorWhite; + + b2CreateChain( groundId, &chainDef ); + } + + { + m_bodyType = b2_dynamicBody; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = m_bodyType; + bodyDef.position = { 0.0, 12.0f }; + bodyDef.enableSleep = false; + + m_spinnerId = b2CreateBody( m_worldId, &bodyDef ); + + b2Polygon box = b2MakeRoundedBox( 0.4f, 20.0f, 0.2f ); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.0f; + shapeDef.customColor = b2_colorAliceBlue; + b2CreatePolygonShape( m_spinnerId, &shapeDef, &box ); + + m_motorSpeed = 5.0f; + m_maxMotorTorque = 40000.0f; + b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_spinnerId; + jointDef.localAnchorA = bodyDef.position; + jointDef.enableMotor = true; + jointDef.motorSpeed = m_motorSpeed; + jointDef.maxMotorTorque = m_maxMotorTorque; + + m_jointId = b2CreateRevoluteJoint( m_worldId, &jointDef ); + } + + b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; + b2Circle circle = { { 0.0f, 0.0f }, 0.35f }; + b2Polygon square = b2MakeSquare( 0.35f ); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.friction = 0.1f; + shapeDef.restitution = 0.1f; + shapeDef.density = 0.25f; + + int bodyCount = g_sampleDebug ? 499 : e_count; + + float x = -24.0f, y = 2.0f; + for ( int i = 0; i < bodyCount; ++i ) + { + bodyDef.position = { x, y }; + b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); + + int remainder = i % 3; + if ( remainder == 0 ) + { + shapeDef.customColor = b2_colorYellow; + b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + } + else if ( remainder == 1 ) + { + shapeDef.customColor = b2_colorYellowGreen; + b2CreateCircleShape( bodyId, &shapeDef, &circle ); + } + else if ( remainder == 2 ) + { + shapeDef.customColor = b2_colorGreenYellow; + b2CreatePolygonShape( bodyId, &shapeDef, &square ); + } + + x += 1.0f; + + if ( x > 24.0f ) + { + x = -24.0f; + y += 1.0f; + } + } + + m_acceleration = 0.0f; + } + + void UpdateUI() override + { + float height = 160.0f; + ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); + ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); + + ImGui::Begin( "Spinner", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); + + if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -5.0f, 5.0f ) ) + { + b2RevoluteJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + + if ( m_bodyType == b2_kinematicBody ) + { + b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); + } + } + + if ( ImGui::SliderFloat( "Torque", &m_maxMotorTorque, 0.0f, 200000.0f ) ) + { + b2RevoluteJoint_SetMaxMotorTorque( m_jointId, m_maxMotorTorque ); + } + + if ( ImGui::RadioButton( "Static", m_bodyType == b2_staticBody ) ) + { + m_bodyType = b2_staticBody; + b2Body_SetType( m_spinnerId, b2_staticBody ); + } + + if ( ImGui::RadioButton( "Kinematic", m_bodyType == b2_kinematicBody ) ) + { + m_bodyType = b2_kinematicBody; + b2Body_SetType( m_spinnerId, b2_kinematicBody ); + b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); + } + + if ( ImGui::RadioButton( "Dynamic", m_bodyType == b2_dynamicBody ) ) + { + m_bodyType = b2_dynamicBody; + b2Body_SetType( m_spinnerId, b2_dynamicBody ); + } + + ImGui::End(); + } + + void Step( Settings& settings ) override + { + if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) + { + m_acceleration = -0.2f; + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_S ) == GLFW_PRESS ) + { + m_acceleration = 0.0f; + m_motorSpeed = 0.0f; + } + + if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) + { + m_acceleration = 0.2f; + } + + if ( settings.hertz > 0.0f ) + { + m_motorSpeed = b2ClampFloat( m_motorSpeed + m_acceleration / settings.hertz, -5.0f, 5.0f ); + b2RevoluteJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); + + if ( m_bodyType == b2_kinematicBody ) + { + b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); + } + } + + Sample::Step( settings ); + } + + static Sample* Create( Settings& settings ) + { + return new BenchmarkSpinner( settings ); + } + + b2BodyId m_spinnerId; + b2JointId m_jointId; + float m_acceleration; + float m_motorSpeed; + float m_maxMotorTorque; + b2BodyType m_bodyType; +}; + +static int sampleSpinner = RegisterSample( "Benchmark", "Spinner", BenchmarkSpinner::Create ); diff --git a/samples/sample_joints.cpp b/samples/sample_joints.cpp index 028db6303..61759eb85 100644 --- a/samples/sample_joints.cpp +++ b/samples/sample_joints.cpp @@ -2486,222 +2486,3 @@ class ScissorLift : public Sample }; static int sampleScissorLift = RegisterSample( "Joints", "Scissor Lift", ScissorLift::Create ); - -class Spinner : public Sample -{ -public: - enum - { - e_count = 3038, - e_pointCount = 360, - }; - - explicit Spinner( Settings& settings ) - : Sample( settings ) - { - if ( settings.restart == false ) - { - g_camera.m_center = { 0.0f, 32.0f }; - g_camera.m_zoom = 42.0f; - } - - b2BodyId groundId; - { - b2BodyDef bodyDef = b2DefaultBodyDef(); - groundId = b2CreateBody( m_worldId, &bodyDef ); - - b2Vec2 points[e_pointCount]; - - b2Rot q = b2MakeRot( -2.0f * b2_pi / e_pointCount ); - b2Vec2 p = { 40.0f, 0.0f }; - for ( int i = 0; i < e_pointCount; ++i ) - { - points[i] = { p.x, p.y + 32.0f }; - p = b2RotateVector( q, p ); - } - - b2ChainDef chainDef = b2DefaultChainDef(); - chainDef.points = points; - chainDef.count = e_pointCount; - chainDef.isLoop = true; - chainDef.friction = 0.1f; - chainDef.customColor = b2_colorWhite; - - b2CreateChain( groundId, &chainDef ); - } - - { - m_bodyType = b2_dynamicBody; - - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = m_bodyType; - bodyDef.position = { 0.0, 12.0f }; - bodyDef.enableSleep = false; - - m_spinnerId = b2CreateBody( m_worldId, &bodyDef ); - - b2Polygon box = b2MakeRoundedBox( 0.4f, 20.0f, 0.2f ); - b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.friction = 0.0f; - shapeDef.customColor = b2_colorAliceBlue; - b2CreatePolygonShape( m_spinnerId, &shapeDef, &box ); - - m_motorSpeed = 5.0f; - m_maxMotorTorque = 40000.0f; - b2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); - jointDef.bodyIdA = groundId; - jointDef.bodyIdB = m_spinnerId; - jointDef.localAnchorA = bodyDef.position; - jointDef.enableMotor = true; - jointDef.motorSpeed = m_motorSpeed; - jointDef.maxMotorTorque = m_maxMotorTorque; - - m_jointId = b2CreateRevoluteJoint( m_worldId, &jointDef ); - } - - b2Capsule capsule = { { -0.25f, 0.0f }, { 0.25f, 0.0f }, 0.25f }; - b2Circle circle = { { 0.0f, 0.0f }, 0.35f }; - b2Polygon square = b2MakeSquare( 0.35f ); - - b2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.type = b2_dynamicBody; - b2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.friction = 0.1f; - shapeDef.restitution = 0.1f; - shapeDef.density = 0.25f; - - int bodyCount = g_sampleDebug ? 499 : e_count; - - float x = -24.0f, y = 2.0f; - for ( int i = 0; i < bodyCount; ++i ) - { - bodyDef.position = { x, y }; - b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef ); - - int remainder = i % 3; - if ( remainder == 0 ) - { - shapeDef.customColor = b2_colorYellow; - b2CreateCapsuleShape( bodyId, &shapeDef, &capsule ); - } - else if ( remainder == 1 ) - { - shapeDef.customColor = b2_colorYellowGreen; - b2CreateCircleShape( bodyId, &shapeDef, &circle ); - } - else if ( remainder == 2 ) - { - shapeDef.customColor = b2_colorGreenYellow; - b2CreatePolygonShape( bodyId, &shapeDef, &square ); - } - else - { - b2Polygon poly = RandomPolygon( 0.75f ); - poly.radius = 0.1f; - shapeDef.customColor = b2_colorLightGoldenrodYellow; - b2CreatePolygonShape( bodyId, &shapeDef, &poly ); - } - - x += 1.0f; - - if ( x > 24.0f ) - { - x = -24.0f; - y += 1.0f; - } - } - - m_acceleration = 0.0f; - } - - void UpdateUI() override - { - float height = 160.0f; - ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once ); - ImGui::SetNextWindowSize( ImVec2( 240.0f, height ) ); - - ImGui::Begin( "Spinner", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize ); - - if ( ImGui::SliderFloat( "Speed", &m_motorSpeed, -5.0f, 5.0f ) ) - { - b2RevoluteJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); - - if ( m_bodyType == b2_kinematicBody ) - { - b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); - } - } - - if ( ImGui::SliderFloat( "Torque", &m_maxMotorTorque, 0.0f, 200000.0f ) ) - { - b2RevoluteJoint_SetMaxMotorTorque( m_jointId, m_maxMotorTorque ); - } - - if ( ImGui::RadioButton( "Static", m_bodyType == b2_staticBody ) ) - { - m_bodyType = b2_staticBody; - b2Body_SetType( m_spinnerId, b2_staticBody ); - } - - if ( ImGui::RadioButton( "Kinematic", m_bodyType == b2_kinematicBody ) ) - { - m_bodyType = b2_kinematicBody; - b2Body_SetType( m_spinnerId, b2_kinematicBody ); - b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); - } - - if ( ImGui::RadioButton( "Dynamic", m_bodyType == b2_dynamicBody ) ) - { - m_bodyType = b2_dynamicBody; - b2Body_SetType( m_spinnerId, b2_dynamicBody ); - } - - ImGui::End(); - } - - void Step( Settings& settings ) override - { - if ( glfwGetKey( g_mainWindow, GLFW_KEY_A ) == GLFW_PRESS ) - { - m_acceleration = -0.2f; - } - - if ( glfwGetKey( g_mainWindow, GLFW_KEY_S ) == GLFW_PRESS ) - { - m_acceleration = 0.0f; - m_motorSpeed = 0.0f; - } - - if ( glfwGetKey( g_mainWindow, GLFW_KEY_D ) == GLFW_PRESS ) - { - m_acceleration = 0.2f; - } - - if ( settings.hertz > 0.0f ) - { - m_motorSpeed = b2ClampFloat( m_motorSpeed + m_acceleration / settings.hertz, -5.0f, 5.0f ); - b2RevoluteJoint_SetMotorSpeed( m_jointId, m_motorSpeed ); - - if ( m_bodyType == b2_kinematicBody ) - { - b2Body_SetAngularVelocity( m_spinnerId, m_motorSpeed ); - } - } - - Sample::Step( settings ); - } - - static Sample* Create( Settings& settings ) - { - return new Spinner( settings ); - } - - b2BodyId m_spinnerId; - b2JointId m_jointId; - float m_acceleration; - float m_motorSpeed; - float m_maxMotorTorque; - b2BodyType m_bodyType; -}; - -static int sampleBodyMove = RegisterSample( "Joints", "Spinner", Spinner::Create ); diff --git a/src/aabb.h b/src/aabb.h index 6fa99c4ea..b41f13e96 100644 --- a/src/aabb.h +++ b/src/aabb.h @@ -51,14 +51,6 @@ static inline bool b2EnlargeAABB( b2AABB* a, b2AABB b ) /// Do a and b overlap static inline bool b2AABB_Overlaps( b2AABB a, b2AABB b ) { - b2Vec2 d1 = { b.lowerBound.x - a.upperBound.x, b.lowerBound.y - a.upperBound.y }; - b2Vec2 d2 = { a.lowerBound.x - b.upperBound.x, a.lowerBound.y - b.upperBound.y }; - - if ( d1.x > 0.0f || d1.y > 0.0f ) - return false; - - if ( d2.x > 0.0f || d2.y > 0.0f ) - return false; - - return true; + return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || a.lowerBound.x > b.upperBound.x || + a.lowerBound.y > b.upperBound.y ); } diff --git a/src/core.h b/src/core.h index c7d755e9e..803873d3f 100644 --- a/src/core.h +++ b/src/core.h @@ -162,7 +162,10 @@ extern float b2_lengthUnitsPerMeter; // to move by a small amount without triggering a tree adjustment. // This is in meters. // @warning modifying this can have a significant impact on performance -#define b2_aabbMargin ( 0.1f * b2_lengthUnitsPerMeter ) +#define b2_aabbMargin ( 0.02f * b2_lengthUnitsPerMeter ) + +// todo testing +#define b2_aabbVelocityScale 0.0f // The time that a body must be still before it will go to sleep. In seconds. #define b2_timeToSleep 0.5f diff --git a/src/solver.c b/src/solver.c index 0603f2a31..4caedde1b 100644 --- a/src/solver.c +++ b/src/solver.c @@ -355,11 +355,39 @@ static void b2FinalizeBodiesTask( int startIndex, int endIndex, uint32_t threadI if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) { +#if 1 + float deltax = b2_aabbVelocityScale * v.x * timeStep; + float deltay = b2_aabbVelocityScale * v.y * timeStep; + + b2AABB fatAABB = aabb; + if (deltax > 0.0f) + { + fatAABB.lowerBound.x -= aabbMargin; + fatAABB.upperBound.x += deltax + aabbMargin; + } + else + { + fatAABB.lowerBound.x += deltax - aabbMargin; + fatAABB.upperBound.x += aabbMargin; + } + + if (deltay > 0.0f) + { + fatAABB.lowerBound.y -= aabbMargin; + fatAABB.upperBound.y += deltay + aabbMargin; + } + else + { + fatAABB.lowerBound.y += deltay - aabbMargin; + fatAABB.upperBound.y += aabbMargin; + } +#else b2AABB fatAABB; fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; +#endif shape->fatAABB = fatAABB; shape->enlargedAABB = true; @@ -942,6 +970,8 @@ static void b2SolveContinuous( b2World* world, int bodySimIndex ) b2BodySim* fastBodySim = b2BodySimArray_Get( &awakeSet->bodySims, bodySimIndex ); B2_ASSERT( fastBodySim->isFast ); + b2BodyState* bodyState = b2BodyStateArray_Get( &awakeSet->bodyStates, bodySimIndex ); + b2Sweep sweep = b2MakeSweep( fastBodySim ); b2Transform xf1; @@ -1004,6 +1034,10 @@ static void b2SolveContinuous( b2World* world, int bodySimIndex ) const float speculativeDistance = b2_speculativeDistance; const float aabbMargin = b2_aabbMargin; + b2Vec2 v = bodyState->linearVelocity; + + float deltax = b2_aabbVelocityScale * v.x * 1.0f / 60.0f; + float deltay = b2_aabbVelocityScale * v.y * 1.0f / 60.0f; if ( context.fraction < 1.0f ) { @@ -1035,11 +1069,36 @@ static void b2SolveContinuous( b2World* world, int bodySimIndex ) if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) { +#if 1 + b2AABB fatAABB = aabb; + if ( deltax > 0.0f ) + { + fatAABB.lowerBound.x -= aabbMargin; + fatAABB.upperBound.x += deltax + aabbMargin; + } + else + { + fatAABB.lowerBound.x += deltax - aabbMargin; + fatAABB.upperBound.x += aabbMargin; + } + + if ( deltay > 0.0f ) + { + fatAABB.lowerBound.y -= aabbMargin; + fatAABB.upperBound.y += deltay + aabbMargin; + } + else + { + fatAABB.lowerBound.y += deltay - aabbMargin; + fatAABB.upperBound.y += aabbMargin; + } +#else b2AABB fatAABB; fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; +#endif shape->fatAABB = fatAABB; shape->enlargedAABB = true; @@ -1067,11 +1126,36 @@ static void b2SolveContinuous( b2World* world, int bodySimIndex ) if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) { +#if 1 + b2AABB fatAABB = shape->aabb; + if ( deltax > 0.0f ) + { + fatAABB.lowerBound.x -= aabbMargin; + fatAABB.upperBound.x += deltax + aabbMargin; + } + else + { + fatAABB.lowerBound.x += deltax - aabbMargin; + fatAABB.upperBound.x += aabbMargin; + } + + if ( deltay > 0.0f ) + { + fatAABB.lowerBound.y -= aabbMargin; + fatAABB.upperBound.y += deltay + aabbMargin; + } + else + { + fatAABB.lowerBound.y += deltay - aabbMargin; + fatAABB.upperBound.y += aabbMargin; + } +#else b2AABB fatAABB; fatAABB.lowerBound.x = shape->aabb.lowerBound.x - aabbMargin; fatAABB.lowerBound.y = shape->aabb.lowerBound.y - aabbMargin; fatAABB.upperBound.x = shape->aabb.upperBound.x + aabbMargin; fatAABB.upperBound.y = shape->aabb.upperBound.y + aabbMargin; +#endif shape->fatAABB = fatAABB; shape->enlargedAABB = true;