diff --git a/CMakeLists.txt b/CMakeLists.txt index 60d1b477..16c9f514 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,6 +26,10 @@ if (MSVC AND WIN32) # add_link_options($<$:/INCREMENTAL>) # add_compile_options($<$:/ZI>) # add_compile_options(/fsanitize=address) + # set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/GS- /Gy /O2 /Oi /Ot") + # set(CMAKE_CXX_FLAGS_RELEASE "/GS- /Gy /O2 /Oi /Ot") + # set(CMAKE_C_FLAGS_RELWITHDEBINFO "/GS- /Gy /O2 /Oi /Ot") + # set(CMAKE_C_FLAGS_RELEASE "/GS- /Gy /O2 /Oi /Ot") endif() SET(ENKITS_BUILD_EXAMPLES OFF CACHE BOOL "Build enkiTS examples") diff --git a/include/box2d/aabb.h b/include/box2d/aabb.h index 7985612f..efa99e8f 100644 --- a/include/box2d/aabb.h +++ b/include/box2d/aabb.h @@ -57,10 +57,10 @@ static inline b2AABB b2AABB_Union(b2AABB a, b2AABB b) static inline b2AABB b2AABB_Extend(b2AABB a) { b2AABB c; - c.lowerBound.x = a.lowerBound.x - b2_aabbExtension; - c.lowerBound.y = a.lowerBound.y - b2_aabbExtension; - c.upperBound.x = a.upperBound.x + b2_aabbExtension; - c.upperBound.y = a.upperBound.y + b2_aabbExtension; + c.lowerBound.x = a.lowerBound.x - b2_aabbMargin; + c.lowerBound.y = a.lowerBound.y - b2_aabbMargin; + c.upperBound.x = a.upperBound.x + b2_aabbMargin; + c.upperBound.y = a.upperBound.y + b2_aabbMargin; return c; } diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index 5ca96706..353c2e3b 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -9,6 +9,7 @@ #include "box2d/timer.h" #include "box2d/types.h" +typedef struct b2Capsule b2Capsule; typedef struct b2Circle b2Circle; typedef struct b2Polygon b2Polygon; typedef struct b2DebugDraw b2DebugDraw; @@ -64,8 +65,9 @@ BOX2D_API void b2Body_Wake(b2BodyId bodyId); /// Create a shape and attach it to a body. Contacts are not created until the next time step. /// @warning This function is locked during callbacks. BOX2D_API b2ShapeId b2Body_CreateCircle(b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle); -BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); BOX2D_API b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment); +BOX2D_API b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule); +BOX2D_API b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon); BOX2D_API b2BodyId b2Shape_GetBody(b2ShapeId shapeId); BOX2D_API bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point); diff --git a/include/box2d/color.h b/include/box2d/color.h index 3c666952..eed9e4a8 100644 --- a/include/box2d/color.h +++ b/include/box2d/color.h @@ -9,7 +9,7 @@ typedef struct b2Color float r, g, b, a; } b2Color; -enum b2Colors +enum b2HexColor { b2_colorSnow = 0xfffafa, b2_colorGhostWhite = 0xf8f8ff, @@ -578,3 +578,22 @@ enum b2Colors b2_colorSilver = 0xc0c0c0, b2_colorTeal = 0x008080 }; + +#ifdef __cplusplus +extern "C" +{ +#endif + +static inline b2Color b2MakeColor(enum b2HexColor hexCode, float alpha) +{ + b2Color color; + color.r = ((hexCode >> 16) & 0xFF) / 255.0f; + color.g = ((hexCode >> 8) & 0xFF) / 255.0f; + color.b = (hexCode & 0xFF) / 255.0f; + color.a = alpha; + return color; +} + +#ifdef __cplusplus +} +#endif diff --git a/include/box2d/constants.h b/include/box2d/constants.h index f9b532df..0f3d2b1f 100644 --- a/include/box2d/constants.h +++ b/include/box2d/constants.h @@ -25,7 +25,7 @@ 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_aabbExtension (0.1f * b2_lengthUnitsPerMeter) +#define b2_aabbMargin (0.1f * b2_lengthUnitsPerMeter) /// A small length used as a collision and constraint tolerance. Usually it is /// chosen to be numerically significant, but visually insignificant. In meters. diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h index 487644fe..2c54aa95 100644 --- a/include/box2d/debug_draw.h +++ b/include/box2d/debug_draw.h @@ -23,6 +23,12 @@ typedef struct b2DebugDraw /// Draw a solid circle. void (*DrawSolidCircle)(b2Vec2 center, float radius, b2Vec2 axis, b2Color color, void* context); + /// Draw a capsule. + void (*DrawCapsule)(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color, void* context); + + /// Draw a solid capsule. + void (*DrawSolidCapsule)(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color, void* context); + /// Draw a line segment. void (*DrawSegment)(b2Vec2 p1, b2Vec2 p2, b2Color color, void* context); diff --git a/include/box2d/timer.h b/include/box2d/timer.h index 4ab7a1d5..f848d271 100644 --- a/include/box2d/timer.h +++ b/include/box2d/timer.h @@ -16,6 +16,7 @@ typedef struct b2Profile float buildIslands; float solveIslands; float broadphase; + float continuous; } b2Profile; static const b2Profile b2_emptyProfile = {0}; @@ -28,7 +29,6 @@ typedef struct b2Statistics int32_t jointCount; int32_t proxyCount; int32_t treeHeight; - int32_t contactPointCount; int32_t stackCapacity; int32_t stackUsed; int32_t byteCount; diff --git a/include/box2d/types.h b/include/box2d/types.h index c07f7f8b..a77aa0ea 100644 --- a/include/box2d/types.h +++ b/include/box2d/types.h @@ -74,12 +74,21 @@ typedef struct b2RayCastOutput } b2RayCastOutput; /// Task interface -/// This is prototype for a Box2D task -typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t threadIndex, void* context); +/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments. +/// The task spans a range of the parallel-for: [startIndex, endIndex) +/// The thread index must correctly identify each thread in the user thread pool, expected in [0, workerCount) +/// The task context is the context pointer sent from Box2D when it is enqueued. +typedef void b2TaskCallback(int32_t startIndex, int32_t endIndex, uint32_t threadIndex, void* taskContext); -/// These functions can be provided to Box2D to invoke a task system -typedef void b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext); -typedef void b2FinishTasksCallback(void* userContext); +/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS. +/// Returns a pointer to the user's task object. May be nullptr. +typedef void* b2EnqueueTaskCallback(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext); + +/// Finishes a user task object that wraps a Box2D task. +typedef void b2FinishTaskCallback(void* userTask, void* userContext); + +/// Finishes all tasks. +typedef void b2FinishAllTasksCallback(void* userContext); typedef struct b2WorldDef { @@ -113,7 +122,8 @@ typedef struct b2WorldDef /// task system hookup uint32_t workerCount; b2EnqueueTaskCallback* enqueueTask; - b2FinishTasksCallback* finishTasks; + b2FinishTaskCallback* finishTask; + b2FinishAllTasksCallback* finishAllTasks; void* userTaskContext; } b2WorldDef; diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index bb83a859..109e4335 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -66,6 +66,7 @@ set(BOX2D_SAMPLES collection/benchmark_barrel.cpp collection/benchmark_create_destroy.cpp collection/benchmark_joint_grid.cpp + collection/benchmark_many_tumblers.cpp collection/benchmark_pyramid.cpp collection/benchmark_tumbler.cpp @@ -75,6 +76,7 @@ set(BOX2D_SAMPLES collection/sample_hull.cpp collection/sample_manifold.cpp collection/sample_ray_cast.cpp + collection/sample_restitution.cpp collection/sample_revolute_joint.cpp collection/sample_shape_cast.cpp collection/sample_tilted_stacks.cpp diff --git a/samples/collection/benchmark_many_tumblers.cpp b/samples/collection/benchmark_many_tumblers.cpp new file mode 100644 index 00000000..05b922c4 --- /dev/null +++ b/samples/collection/benchmark_many_tumblers.cpp @@ -0,0 +1,229 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sample.h" + +#include "box2d/box2d.h" +#include "box2d/geometry.h" + +#include +#include + +class BenchmarkManyTumblers : public Sample +{ + public: + BenchmarkManyTumblers(const Settings& settings) + : Sample(settings) + { + b2BodyDef bd = b2DefaultBodyDef(); + m_groundId = b2World_CreateBody(m_worldId, &bd); + + m_rowCount = g_sampleDebug ? 1 : 19; + m_columnCount = g_sampleDebug ? 1 : 19; + + m_tumblerIds = nullptr; + m_jointIds = nullptr; + m_positions = nullptr; + m_tumblerCount = 0; + + m_bodyIds = nullptr; + m_bodyCount = 0; + m_bodyIndex = 0; + + m_motorSpeed = 0.0f; + m_shapeType = 0; + + CreateScene(); + } + + ~BenchmarkManyTumblers() + { + free(m_jointIds); + free(m_tumblerIds); + free(m_positions); + free(m_bodyIds); + } + + void CreateTumbler(b2Vec2 position, int index) + { + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.enableSleep = false; + bd.position = {position.x, position.y}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bd); + m_tumblerIds[index] = bodyId; + + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 50.0f; + + b2Polygon polygon; + polygon = b2MakeOffsetBox(0.25f, 2.0f, {2.0f, 0.0f}, 0.0); + b2Body_CreatePolygon(bodyId, &sd, &polygon); + polygon = b2MakeOffsetBox(0.25f, 2.0f, {-2.0f, 0.0f}, 0.0); + b2Body_CreatePolygon(bodyId, &sd, &polygon); + polygon = b2MakeOffsetBox(2.0f, 0.25f, {0.0f, 2.0f}, 0.0); + b2Body_CreatePolygon(bodyId, &sd, &polygon); + polygon = b2MakeOffsetBox(2.0f, 0.25f, {0.0f, -2.0f}, 0.0); + b2Body_CreatePolygon(bodyId, &sd, &polygon); + + b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); + jd.bodyIdA = m_groundId; + jd.bodyIdB = bodyId; + jd.localAnchorA = position; + jd.localAnchorB = {0.0f, 0.0f}; + jd.referenceAngle = 0.0f; + jd.motorSpeed = (b2_pi / 180.0f) * m_motorSpeed; + jd.maxMotorTorque = 1e8f; + jd.enableMotor = true; + + m_jointIds[index] = b2World_CreateRevoluteJoint(m_worldId, &jd); + } + + void CreateScene() + { + for (int32_t i = 0; i < m_bodyCount; ++i) + { + if (B2_NON_NULL(m_bodyIds[i])) + { + b2World_DestroyBody(m_bodyIds[i]); + } + } + + for (int32_t i = 0; i < m_tumblerCount; ++i) + { + b2World_DestroyJoint(m_jointIds[i]); + b2World_DestroyBody(m_tumblerIds[i]); + } + + free(m_jointIds); + free(m_tumblerIds); + free(m_positions); + + m_tumblerCount = m_rowCount * m_columnCount; + m_tumblerIds = static_cast(malloc(m_tumblerCount * sizeof(b2BodyId))); + m_jointIds = static_cast(malloc(m_tumblerCount * sizeof(b2JointId))); + m_positions = static_cast(malloc(m_tumblerCount * sizeof(b2Vec2))); + + int32_t index = 0; + float x = -4.0f * m_rowCount; + for (int32_t i = 0; i < m_rowCount; ++i) + { + float y = -4.0f * m_columnCount; + for (int32_t j = 0; j < m_columnCount; ++j) + { + m_positions[index] = {x, y}; + CreateTumbler(m_positions[index], index); + ++index; + y += 8.0f; + } + + x += 8.0f; + } + + free(m_bodyIds); + + int32_t bodiesPerTumbler = g_sampleDebug ? 1 : 50; + m_bodyCount = bodiesPerTumbler * m_tumblerCount; + + m_bodyIds = static_cast(malloc(m_bodyCount * sizeof(b2BodyId))); + + // 0xFF is a fast way to make all bodies satisfy B2_IS_NULL + memset(m_bodyIds, 0XFF, m_bodyCount * sizeof(b2BodyId)); + m_bodyIndex = 0; + + m_shapeType = 0; + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(240.0f, 230.0f)); + ImGui::Begin("Tumbler", nullptr, ImGuiWindowFlags_NoResize); + + bool changed = false; + changed = changed || ImGui::SliderInt("Row Count", &m_rowCount, 1, 32); + changed = changed || ImGui::SliderInt("Column Count", &m_columnCount, 1, 32); + + if (changed) + { + CreateScene(); + } + + if (ImGui::SliderFloat("Speed", &m_motorSpeed, 0.0f, 100.0f, "%.f")) + { + for (int i = 0; i < m_tumblerCount; ++i) + { + b2RevoluteJoint_SetMotorSpeed(m_jointIds[i], (b2_pi / 180.0f) * m_motorSpeed); + } + } + + ImGui::End(); + } + + void Step(Settings& settings) override + { + Sample::Step(settings); + + if (m_bodyIndex < m_bodyCount && (m_stepCount & 0x7) == 0) + { + b2ShapeDef sd = b2DefaultShapeDef(); + sd.density = 1.0f; + + b2Circle circle = {{0.0f, 0.0f}, 0.125f}; + b2Polygon polygon = b2MakeBox(0.125f, 0.125f); + b2Capsule capsule = {{-0.1f, 0.0f}, {0.1f, 0.0f}, 0.075f}; + int j = m_shapeType % 3; + + for (int i = 0; i < m_tumblerCount; ++i) + { + assert(m_bodyIndex < m_bodyCount); + + b2BodyDef bd = b2DefaultBodyDef(); + bd.type = b2_dynamicBody; + bd.position = m_positions[i]; + m_bodyIds[m_bodyIndex] = b2World_CreateBody(m_worldId, &bd); + + //if (j == 0) + //{ + // b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &sd, &polygon); + //} + //else if (j == 1) + { + b2Body_CreateCapsule(m_bodyIds[m_bodyIndex], &sd, &capsule); + } + //else + //{ + // b2Body_CreateCircle(m_bodyIds[m_bodyIndex], &sd, &circle); + //} + + m_bodyIndex += 1; + } + + m_shapeType += 1; + } + } + + static Sample* Create(const Settings& settings) + { + return new BenchmarkManyTumblers(settings); + } + + b2BodyId m_groundId; + + int32_t m_rowCount; + int32_t m_columnCount; + + b2BodyId* m_tumblerIds; + b2JointId* m_jointIds; + b2Vec2* m_positions; + int32_t m_tumblerCount; + + b2BodyId* m_bodyIds; + int32_t m_bodyCount; + int32_t m_bodyIndex; + int32_t m_shapeType; + + float m_motorSpeed; +}; + +static int testIndex = RegisterSample("Benchmark", "Many Tumblers", BenchmarkManyTumblers::Create); diff --git a/samples/collection/benchmark_pyramid.cpp b/samples/collection/benchmark_pyramid.cpp index 4794be73..b6c4c04a 100644 --- a/samples/collection/benchmark_pyramid.cpp +++ b/samples/collection/benchmark_pyramid.cpp @@ -8,88 +8,122 @@ #include #include +BOX2D_API int32_t b2_awakeContactCount; + +BOX2D_API int b2_collideMinRange; +BOX2D_API int b2_islandMinRange; + class BenchmarkPyramid : public Sample { public: - enum - { - e_maxBaseCount = 100, - e_maxBodyCount = e_maxBaseCount * (e_maxBaseCount + 1) / 2 - }; BenchmarkPyramid(const Settings& settings) : Sample(settings) { - float groundSize = 100.0f; - m_groundThickness = 1.0f; + m_extent = 0.5f; m_round = 0.0f; + m_baseCount = 10; + m_rowCount = g_sampleDebug ? 1 : 16; + m_columnCount = g_sampleDebug ? 4 : 16; + m_groundId = b2_nullBodyId; + m_bodyIds = nullptr; + m_bodyCount = 0; + m_bodyIndex = 0; - b2BodyDef bd = b2DefaultBodyDef(); - b2BodyId groundId = b2World_CreateBody(m_worldId, &bd); - - b2Polygon box = b2MakeBox(groundSize, m_groundThickness); - b2ShapeDef sd = b2DefaultShapeDef(); - b2Body_CreatePolygon(groundId, &sd, &box); + m_collideRange = 169; + m_islandRange = 1; - for (int32_t i = 0; i < e_maxBodyCount; ++i) - { - m_bodies[i] = b2_nullBodyId; - } + m_bestCollideRange = 1; + m_minCollide = FLT_MAX; - m_baseCount = g_sampleDebug ? 8 : 100; - m_bodyCount = 0; + m_bestIslandRange = 1; + m_minIsland = FLT_MAX; CreateScene(); } - void CreateScene() + ~BenchmarkPyramid() + { + free(m_bodyIds); + } + + void CreateStack(float centerX, float baseY) { - for (int32_t i = 0; i < e_maxBodyCount; ++i) + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + + float h = m_extent - m_round; + b2Polygon cuboid = b2MakeRoundedBox(h, h, m_round); + + for (int32_t i = 0; i < m_baseCount; ++i) { - if (B2_NON_NULL(m_bodies[i])) + float y = (2.0f * i + 1.0f) * m_extent + baseY; + + for (int32_t j = i; j < m_baseCount; ++j) { - b2World_DestroyBody(m_bodies[i]); - m_bodies[i] = b2_nullBodyId; + float x = (i + 1.0f) * m_extent + 2.0f * (j - i) * m_extent + centerX; + bodyDef.position = {x, y}; + + assert(m_bodyIndex < m_bodyCount); + m_bodyIds[m_bodyIndex] = b2World_CreateBody(m_worldId, &bodyDef); + b2Body_CreatePolygon(m_bodyIds[m_bodyIndex], &shapeDef, &cuboid); + + m_bodyIndex += 1; } } + } - int32_t count = m_baseCount; - float rad = 0.5f; - float shift = rad * 2.0f; - float centerx = shift * count / 2.0f; - float centery = shift / 2.0f + m_groundThickness; // +rad * 1.5f; + void CreateScene() + { + if (B2_NON_NULL(m_groundId)) + { + b2World_DestroyBody(m_groundId); + } - b2BodyDef bd = b2DefaultBodyDef(); - bd.type = b2_dynamicBody; + for (int32_t i = 0; i < m_bodyCount; ++i) + { + b2World_DestroyBody(m_bodyIds[i]); + } - b2ShapeDef sd = b2DefaultShapeDef(); - sd.density = 1.0f; - sd.friction = 0.5f; + free(m_bodyIds); - // b2Polygon cuboid = b2MakeBox(0.5f, 0.5f); - // b2Polygon cuboid = b2MakeRoundedBox(0.4f, 0.4f, 0.1f); - float h = 0.5f - m_round; - b2Polygon cuboid = b2MakeRoundedBox(h, h, m_round); + m_bodyCount = m_rowCount * m_columnCount * m_baseCount * (m_baseCount + 1) / 2; + m_bodyIds = (b2BodyId*)malloc(m_bodyCount * sizeof(b2BodyId)); + m_bodyIndex = 0; - int32_t index = 0; + b2BodyDef bodyDef = b2DefaultBodyDef(); + m_groundId = b2World_CreateBody(m_worldId, &bodyDef); - for (int32_t i = 0; i < count; ++i) - { - float y = i * shift + centery; + float groundDeltaY = 2.0f * m_extent * (m_baseCount + 1.0f); + float groundWidth = 2.0f * m_extent * m_columnCount * (m_baseCount + 1.0f); + b2ShapeDef shapeDef = b2DefaultShapeDef(); - for (int32_t j = i; j < count; ++j) - { - float x = 0.5f * i * shift + (j - i) * shift - centerx; - bd.position = {x, y}; + float groundY = 0.0f; - assert(index < e_maxBodyCount); - m_bodies[index] = b2World_CreateBody(m_worldId, &bd); - b2Body_CreatePolygon(m_bodies[index], &sd, &cuboid); + for (int32_t i = 0; i < m_rowCount; ++i) + { + b2Segment segment = {{-0.5f * groundWidth, groundY}, {0.5f * groundWidth, groundY}}; + b2Body_CreateSegment(m_groundId, &shapeDef, &segment); + groundY += groundDeltaY; + } - index += 1; + float baseWidth = 2.0f * m_extent * m_baseCount; + float baseY = 0.0f; + + for (int32_t i = 0; i < m_rowCount; ++i) + { + for (int32_t j = 0; j < m_columnCount; ++j) + { + float centerX = -0.5f * groundWidth + j * (baseWidth + 2.0f * m_extent) + m_extent; + CreateStack(centerX, baseY); } + + baseY += groundDeltaY; } - m_bodyCount = index; + assert(m_bodyIndex == m_bodyCount); } void UpdateUI() override @@ -99,15 +133,15 @@ class BenchmarkPyramid : public Sample ImGui::Begin("Stacks", nullptr, ImGuiWindowFlags_NoResize); bool changed = false; - changed = changed || ImGui::SliderInt("Base Count", &m_baseCount, 1, e_maxBaseCount); + changed = changed || ImGui::SliderInt("Row Count", &m_rowCount, 1, 32); + changed = changed || ImGui::SliderInt("Column Count", &m_columnCount, 1, 32); + changed = changed || ImGui::SliderInt("Base Count", &m_baseCount, 1, 30); changed = changed || ImGui::SliderFloat("Round", &m_round, 0.0f, 0.4f, "%.1f"); changed = changed || ImGui::Button("Reset Scene"); - if (ImGui::Button("Wake Top")) - { - b2Body_Wake(m_bodies[m_bodyCount - 1]); - } + ImGui::SliderInt("Collide Min", &b2_collideMinRange, 1, 200); + ImGui::SliderInt("Island Min", &b2_islandMinRange, 1, 10); if (changed) { @@ -117,16 +151,72 @@ class BenchmarkPyramid : public Sample ImGui::End(); } + void Step(Settings& settings) override + { + //b2_collideMinRange = m_collideRange; + //b2_islandMinRange = m_islandRange; + + Sample::Step(settings); + + b2Profile profile = b2World_GetProfile(m_worldId); + + if (m_stepCount > 100000000) + { + if (profile.collide < m_minCollide) + { + m_minCollide = profile.collide; + m_bestCollideRange = m_collideRange; + } + + if (profile.solveIslands < m_minIsland) + { + m_minIsland = profile.solveIslands; + m_bestIslandRange = m_islandRange; + } + + g_draw.DrawString(5, m_textLine, "collide range (best) = %d (%d)", m_collideRange, m_bestCollideRange); + m_textLine += m_textIncrement; + + g_draw.DrawString(5, m_textLine, "island range (best) = %d (%d)", m_islandRange, m_bestIslandRange); + m_textLine += m_textIncrement; + + //m_collideRange += 1; + //if (m_collideRange > 300) + //{ + // m_collideRange = 32; + //} + + //m_islandRange += 1; + //if (m_islandRange > 4) + //{ + // m_islandRange = 1; + //} + } + } + static Sample* Create(const Settings& settings) { return new BenchmarkPyramid(settings); } - b2BodyId m_bodies[e_maxBodyCount]; + b2BodyId m_groundId; + b2BodyId* m_bodyIds; int32_t m_bodyCount; + int32_t m_bodyIndex; int32_t m_baseCount; + int32_t m_rowCount; + int32_t m_columnCount; float m_round; - float m_groundThickness; + float m_extent; + + int m_collideRange; + int m_islandRange; + + int32_t m_bestCollideRange; + float m_minCollide; + + int32_t m_bestIslandRange; + float m_minIsland; }; static int sampleIndex = RegisterSample("Benchmark", "Pyramid", BenchmarkPyramid::Create); diff --git a/samples/collection/benchmark_tumbler.cpp b/samples/collection/benchmark_tumbler.cpp index 4156fcd8..cb47a91f 100644 --- a/samples/collection/benchmark_tumbler.cpp +++ b/samples/collection/benchmark_tumbler.cpp @@ -41,7 +41,8 @@ class BenchmarkTumbler : public Sample polygon = b2MakeOffsetBox(10.0f, 0.5f, {0.0f, -10.0f}, 0.0); b2Body_CreatePolygon(bodyId, &sd, &polygon); - m_motorSpeed = 9.0f; + //m_motorSpeed = 9.0f; + m_motorSpeed = 25.0f; b2RevoluteJointDef jd = b2DefaultRevoluteJointDef(); jd.bodyIdA = groundId; diff --git a/samples/collection/sample_continuous1.cpp b/samples/collection/sample_continuous1.cpp index 1a16dca1..02f1b7d0 100644 --- a/samples/collection/sample_continuous1.cpp +++ b/samples/collection/sample_continuous1.cpp @@ -23,12 +23,14 @@ class Continuous1 : public Sample b2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.friction = 0.9f; b2Body_CreateSegment(groundId, &shapeDef, &segment); + + b2Polygon box = b2MakeOffsetBox(0.1f, 1.0f, {0.0f, 1.0f}, 0.0f); + b2Body_CreatePolygon(groundId, &shapeDef, &box); } - m_linearSpeed = 0.0f; - m_angularSpeed = 0.0f; m_autoTest = false; m_bullet = false; + m_capsule = false; m_bodyId = b2_nullBodyId; m_bulletId = b2_nullBodyId; @@ -49,7 +51,7 @@ class Continuous1 : public Sample } m_angularVelocity = RandomFloat(-50.0f, 50.0f); - //m_angularVelocity = 8.50093460f; + //m_angularVelocity = -30.6695766f; b2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = b2_dynamicBody; @@ -57,7 +59,16 @@ class Continuous1 : public Sample bodyDef.angularVelocity = m_angularVelocity; bodyDef.linearVelocity = {0.0f, -100.0f}; - b2Polygon polygon = b2MakeBox(2.0f, 0.1f); + b2Polygon polygon; + + if (m_capsule) + { + polygon = b2MakeCapsule({0.0f, -1.0f}, {0.0f, 1.0f}, 0.1f); + } + else + { + polygon = b2MakeBox(2.0f, 0.05f); + } b2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.density = 1.0f; @@ -83,9 +94,8 @@ class Continuous1 : public Sample ImGui::SetNextWindowSize(ImVec2(240.0f, 230.0f)); ImGui::Begin("Continuous1", nullptr, ImGuiWindowFlags_NoResize); - ImGui::SliderFloat("Linear Speed", &m_linearSpeed, 0.0f, 200.0f); - ImGui::SliderFloat("Angular Speed", &m_angularSpeed, 0.0f, 45.0f); - + ImGui::Checkbox("Capsule", &m_capsule); + if (ImGui::Button("Launch")) { Launch(); @@ -114,8 +124,7 @@ class Continuous1 : public Sample b2BodyId m_bodyId, m_bulletId; float m_angularVelocity; float m_x; - float m_linearSpeed; - float m_angularSpeed; + bool m_capsule; bool m_autoTest; bool m_bullet; }; diff --git a/samples/collection/sample_dynamic_tree.cpp b/samples/collection/sample_dynamic_tree.cpp index f52a7954..a02a9bd5 100644 --- a/samples/collection/sample_dynamic_tree.cpp +++ b/samples/collection/sample_dynamic_tree.cpp @@ -90,7 +90,7 @@ class DynamicTree : public Sample bool isStatic = false; m_tree = b2DynamicTree_Create(); - const b2Vec2 aabbExtension = {b2_aabbExtension, b2_aabbExtension}; + const b2Vec2 aabbMargin = {b2_aabbMargin, b2_aabbMargin}; for (int i = 0; i < m_rowCount; ++i) { @@ -120,8 +120,8 @@ class DynamicTree : public Sample p->box.lowerBound = {x, y}; p->box.upperBound = {x + p->width.x, y + p->width.y}; - p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbExtension); - p->fatBox.upperBound = b2Add(p->box.upperBound, aabbExtension); + p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbMargin); + p->fatBox.upperBound = b2Add(p->box.upperBound, aabbMargin); p->proxyId = b2DynamicTree_CreateProxy(&m_tree, p->fatBox, b2_defaultCategoryBits, m_proxyCount); p->rayStamp = -1; @@ -267,7 +267,7 @@ class DynamicTree : public Sample b2Color c = {0.3f, 0.3f, 0.8f, 0.7f}; b2Color qc = {0.3, 0.8f, 0.3f, 1.0f}; - const b2Vec2 aabbExtension = {b2_aabbExtension, b2_aabbExtension}; + const b2Vec2 aabbMargin = {b2_aabbMargin, b2_aabbMargin}; for (int i = 0; i < m_proxyCount; ++i) { @@ -298,8 +298,8 @@ class DynamicTree : public Sample if (b2AABB_Contains(p->fatBox, p->box) == false) { - p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbExtension); - p->fatBox.upperBound = b2Add(p->box.lowerBound, aabbExtension); + p->fatBox.lowerBound = b2Sub(p->box.lowerBound, aabbMargin); + p->fatBox.upperBound = b2Add(p->box.lowerBound, aabbMargin); p->moved = true; } else diff --git a/samples/collection/sample_manifold.cpp b/samples/collection/sample_manifold.cpp index 091fe9cf..69c22aae 100644 --- a/samples/collection/sample_manifold.cpp +++ b/samples/collection/sample_manifold.cpp @@ -319,6 +319,33 @@ class Manifold : public Sample offset = b2Add(offset, increment); } + // box-capsule + { + b2Capsule capsule = {{-0.1f, 0.0f}, {0.1f, 0.0f}, 0.075f}; + b2Polygon box = b2MakeBox(2.0f, 0.25f); + + b2Transform xf1 = {offset, b2Rot_identity}; + b2Transform xf2 = {b2Add(m_transform.p, offset), m_transform.q}; + + b2DistanceCache cache = b2_emptyDistanceCache; + b2Manifold m = b2CollidePolygonAndCapsule(&box, xf1, &capsule, xf2, b2_speculativeDistance, &cache); + + b2Vec2 vertices[b2_maxPolygonVertices]; + for (int i = 0; i < box.count; ++i) + { + vertices[i] = b2TransformPoint(xf1, box.vertices[i]); + } + g_draw.DrawPolygon(vertices, box.count, color1); + + b2Vec2 v1 = b2TransformPoint(xf2, capsule.point1); + b2Vec2 v2 = b2TransformPoint(xf2, capsule.point2); + g_draw.DrawSolidCapsule(v1, v2, capsule.radius, color2); + + DrawManifold(&m); + + offset = b2Add(offset, increment); + } + // segment-capsule { b2Segment segment = {{-1.0f, 0.0f}, {1.0f, 0.0}}; diff --git a/samples/collection/sample_restitution.cpp b/samples/collection/sample_restitution.cpp new file mode 100644 index 00000000..36688dcc --- /dev/null +++ b/samples/collection/sample_restitution.cpp @@ -0,0 +1,130 @@ +// SPDX-FileCopyrightText: 2022 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/box2d.h" +#include "box2d/geometry.h" +#include "sample.h" + +#include +#include + +// Restitution is approximate since Box2D uses speculative collision +class Restitution : public Sample +{ +public: + + enum + { + e_count = 40 + }; + + enum ShapeType + { + e_circleShape = 0, + e_boxShape + }; + + Restitution(const Settings& settings) + : Sample(settings) + { + { + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId groundId = b2World_CreateBody(m_worldId, &bodyDef); + + float h = 1.0f * e_count; + b2Segment segment = {{-h, 0.0f}, {h, 0.0f}}; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Body_CreateSegment(groundId, &shapeDef, &segment); + } + + for (int32_t i = 0; i < e_count; ++i) + { + m_bodyIds[i] = b2_nullBodyId; + } + + m_shapeType = e_circleShape; + + CreateBodies(); + } + + void CreateBodies() + { + for (int32_t i = 0; i < e_count; ++i) + { + if (B2_NON_NULL(m_bodyIds[i])) + { + b2World_DestroyBody(m_bodyIds[i]); + m_bodyIds[i] = b2_nullBodyId; + } + } + + b2Circle circle = {0}; + circle.radius = 0.5f; + + b2Polygon box = b2MakeBox(0.5f, 0.5f); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.restitution = 0.0f; + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + + float dr = 1.0f / (e_count > 1 ? e_count - 1 : 1); + float x = -1.0f * (e_count - 1); + float dx = 2.0f; + + for (int32_t i = 0; i < e_count; ++i) + { + bodyDef.position = {x, 40.0f}; + b2BodyId bodyId = b2World_CreateBody(m_worldId, &bodyDef); + + m_bodyIds[i] = bodyId; + + if (m_shapeType == e_circleShape) + { + b2Body_CreateCircle(bodyId, &shapeDef, &circle); + } + else + { + b2Body_CreatePolygon(bodyId, &shapeDef, &box); + } + + shapeDef.restitution += dr; + x += dx; + } + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 300.0f), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(240.0f, 230.0f)); + ImGui::Begin("Restitution", nullptr, ImGuiWindowFlags_NoResize); + + bool changed = false; + const char* shapeTypes[] = { "Circle", "Box" }; + + int shapeType = int(m_shapeType); + changed = changed || ImGui::Combo("Shape", &shapeType, shapeTypes, IM_ARRAYSIZE(shapeTypes)); + m_shapeType = ShapeType(shapeType); + + changed = changed || ImGui::Button("Reset"); + + if (changed) + { + CreateBodies(); + } + + ImGui::End(); + } + + static Sample* Create(const Settings& settings) + { + return new Restitution(settings); + } + + b2BodyId m_bodyIds[e_count]; + ShapeType m_shapeType; +}; + +static int sampleIndex = RegisterSample("Stacking", "Restitution", Restitution::Create); diff --git a/samples/draw.cpp b/samples/draw.cpp index df3aea7d..42bce19a 100644 --- a/samples/draw.cpp +++ b/samples/draw.cpp @@ -835,6 +835,16 @@ void DrawSolidCircleFcn(b2Vec2 center, float radius, b2Vec2 axis, b2Color color, static_cast(context)->DrawSolidCircle(center, radius, axis, color); } +void DrawCapsuleFcn(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color, void* context) +{ + static_cast(context)->DrawCapsule(p1, p2, radius, color); +} + +void DrawSolidCapsuleFcn(b2Vec2 p1, b2Vec2 p2, float radius, b2Color color, void* context) +{ + static_cast(context)->DrawSolidCapsule(p1, p2, radius, color); +} + void DrawSegmentFcn(b2Vec2 p1, b2Vec2 p2, b2Color color, void* context) { static_cast(context)->DrawSegment(p1, p2, color); @@ -886,6 +896,8 @@ void Draw::Create() DrawRoundedPolygonFcn, DrawCircleFcn, DrawSolidCircleFcn, + DrawCapsuleFcn, + DrawSolidCapsuleFcn, DrawSegmentFcn, DrawTransformFcn, DrawPointFcn, @@ -1288,10 +1300,10 @@ void Draw::DrawPoint(b2Vec2 p, float size, b2Color color) // void Draw::DrawString(int x, int y, const char* string, ...) { - if (m_showUI == false) - { - return; - } + // if (m_showUI == false) + //{ + // return; + // } va_list arg; va_start(arg, string); diff --git a/samples/main.cpp b/samples/main.cpp index 46cf2538..819867ad 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -183,115 +183,105 @@ static void KeyCallback(GLFWwindow* window, int key, int scancode, int action, i { switch (key) { - case GLFW_KEY_ESCAPE: - // Quit - glfwSetWindowShouldClose(g_mainWindow, GL_TRUE); - break; - - case GLFW_KEY_LEFT: - // Pan left - if (mods == GLFW_MOD_CONTROL) - { - b2Vec2 newOrigin = {2.0f, 0.0f}; - s_sample->ShiftOrigin(newOrigin); - } - else - { - g_camera.m_center.x -= 0.5f; - } - break; - - case GLFW_KEY_RIGHT: - // Pan right - if (mods == GLFW_MOD_CONTROL) - { - b2Vec2 newOrigin = {-2.0f, 0.0f}; - s_sample->ShiftOrigin(newOrigin); - } - else - { - g_camera.m_center.x += 0.5f; - } - break; - - case GLFW_KEY_DOWN: - // Pan down - if (mods == GLFW_MOD_CONTROL) - { - b2Vec2 newOrigin = {0.0f, 2.0f}; - s_sample->ShiftOrigin(newOrigin); - } - else - { - g_camera.m_center.y -= 0.5f; - } - break; + case GLFW_KEY_ESCAPE: + // Quit + glfwSetWindowShouldClose(g_mainWindow, GL_TRUE); + break; + + case GLFW_KEY_LEFT: + // Pan left + if (mods == GLFW_MOD_CONTROL) + { + b2Vec2 newOrigin = {2.0f, 0.0f}; + s_sample->ShiftOrigin(newOrigin); + } + else + { + g_camera.m_center.x -= 0.5f; + } + break; - case GLFW_KEY_UP: - // Pan up - if (mods == GLFW_MOD_CONTROL) - { - b2Vec2 newOrigin = {0.0f, -2.0f}; - s_sample->ShiftOrigin(newOrigin); - } - else - { - g_camera.m_center.y += 0.5f; - } - break; + case GLFW_KEY_RIGHT: + // Pan right + if (mods == GLFW_MOD_CONTROL) + { + b2Vec2 newOrigin = {-2.0f, 0.0f}; + s_sample->ShiftOrigin(newOrigin); + } + else + { + g_camera.m_center.x += 0.5f; + } + break; - case GLFW_KEY_HOME: - g_camera.ResetView(); - break; + case GLFW_KEY_DOWN: + // Pan down + if (mods == GLFW_MOD_CONTROL) + { + b2Vec2 newOrigin = {0.0f, 2.0f}; + s_sample->ShiftOrigin(newOrigin); + } + else + { + g_camera.m_center.y -= 0.5f; + } + break; - case GLFW_KEY_Z: - // Zoom out - g_camera.m_zoom = B2_MIN(1.1f * g_camera.m_zoom, 20.0f); - break; + case GLFW_KEY_UP: + // Pan up + if (mods == GLFW_MOD_CONTROL) + { + b2Vec2 newOrigin = {0.0f, -2.0f}; + s_sample->ShiftOrigin(newOrigin); + } + else + { + g_camera.m_center.y += 0.5f; + } + break; - case GLFW_KEY_X: - // Zoom in - g_camera.m_zoom = B2_MAX(0.9f * g_camera.m_zoom, 0.02f); - break; + case GLFW_KEY_HOME: + g_camera.ResetView(); + break; - case GLFW_KEY_R: - RestartTest(); - break; + case GLFW_KEY_R: + RestartTest(); + break; - case GLFW_KEY_O: - s_settings.m_singleStep = true; - break; + case GLFW_KEY_O: + s_settings.m_singleStep = true; + break; - case GLFW_KEY_P: - s_settings.m_pause = !s_settings.m_pause; - break; + case GLFW_KEY_P: + s_settings.m_pause = !s_settings.m_pause; + break; - case GLFW_KEY_LEFT_BRACKET: - // Switch to previous test - --s_selection; - if (s_selection < 0) - { - s_selection = g_sampleCount - 1; - } - break; + case GLFW_KEY_LEFT_BRACKET: + // Switch to previous test + --s_selection; + if (s_selection < 0) + { + s_selection = g_sampleCount - 1; + } + break; - case GLFW_KEY_RIGHT_BRACKET: - // Switch to next test - ++s_selection; - if (s_selection == g_sampleCount) - { - s_selection = 0; - } - break; + case GLFW_KEY_RIGHT_BRACKET: + // Switch to next test + ++s_selection; + if (s_selection == g_sampleCount) + { + s_selection = 0; + } + break; - case GLFW_KEY_TAB: - g_draw.m_showUI = !g_draw.m_showUI; + case GLFW_KEY_TAB: + g_draw.m_showUI = !g_draw.m_showUI; - default: - if (s_sample) - { - s_sample->Keyboard(key); - } + default: + if (s_sample) + { + s_sample->Keyboard(key); + } } } else if (action == GLFW_RELEASE) @@ -575,7 +565,7 @@ int main(int, char**) // MSAA glfwWindowHint(GLFW_SAMPLES, 4); - sprintf(buffer, "Box2D Version %d.%d.%d", b2_version.major, b2_version.minor, b2_version.revision); + sprintf(buffer, "Box2D Version %d.%d.%d c", b2_version.major, b2_version.minor, b2_version.revision); if (GLFWmonitor* primaryMonitor = glfwGetPrimaryMonitor()) { @@ -647,6 +637,17 @@ int main(int, char**) { double time1 = glfwGetTime(); + if (glfwGetKey(g_mainWindow, GLFW_KEY_Z) == GLFW_PRESS) + { + // Zoom out + g_camera.m_zoom = B2_MIN(1.005f * g_camera.m_zoom, 20.0f); + } + else if (glfwGetKey(g_mainWindow, GLFW_KEY_X) == GLFW_PRESS) + { + // Zoom in + g_camera.m_zoom = B2_MAX(0.995f * g_camera.m_zoom, 0.02f); + } + glfwGetWindowSize(g_mainWindow, &g_camera.m_width, &g_camera.m_height); g_camera.m_width = int(g_camera.m_width / s_windowScale); g_camera.m_height = int(g_camera.m_height / s_windowScale); @@ -672,16 +673,16 @@ int main(int, char**) ImGui::NewFrame(); + ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f)); + ImGui::SetNextWindowSize(ImVec2(float(g_camera.m_width), float(g_camera.m_height))); + ImGui::SetNextWindowBgAlpha(0.0f); + ImGui::Begin("Overlay", nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | + ImGuiWindowFlags_NoScrollbar); + ImGui::End(); + if (g_draw.m_showUI) { - ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f)); - ImGui::SetNextWindowSize(ImVec2(float(g_camera.m_width), float(g_camera.m_height))); - ImGui::SetNextWindowBgAlpha(0.0f); - ImGui::Begin("Overlay", nullptr, - ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | - ImGuiWindowFlags_NoScrollbar); - ImGui::End(); - const SampleEntry& entry = g_sampleEntries[s_settings.m_sampleIndex]; sprintf(buffer, "%s : %s", entry.category, entry.name); s_sample->DrawTitle(buffer); @@ -695,10 +696,16 @@ int main(int, char**) // ImGui::ShowDemoWindow(); - if (g_draw.m_showUI) + // if (g_draw.m_showUI) { sprintf(buffer, "%.1f ms", 1000.0f * frameTime); - g_draw.DrawString(5, g_camera.m_height - 20, buffer); + + ImGui::Begin("Overlay", nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | + ImGuiWindowFlags_NoScrollbar); + ImGui::SetCursorPos(ImVec2(5.0f, g_camera.m_height - 20.0f)); + ImGui::TextColored(ImColor(153, 230, 153, 255), buffer); + ImGui::End(); } ImGui::Render(); diff --git a/samples/sample.cpp b/samples/sample.cpp index 6fdb6b60..5472f8d4 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -16,27 +16,13 @@ #include #include -#if 0 -void DestructionListener::SayGoodbye(b2Joint* joint) -{ - if (test->m_mouseJoint == joint) - { - test->m_mouseJoint = nullptr; - } - else - { - test->JointDestroyed(joint); - } -} -#endif - bool PreSolveFcn(b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context) { Sample* sample = static_cast(context); return sample->PreSolve(shapeIdA, shapeIdB, manifold); } -static void EnqueueTask(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext) +static void* EnqueueTask(b2TaskCallback* task, int32_t itemCount, int32_t minRange, void* taskContext, void* userContext) { Sample* sample = static_cast(userContext); if (sample->m_taskCount < maxTasks) @@ -48,14 +34,24 @@ static void EnqueueTask(b2TaskCallback* task, int32_t itemCount, int32_t minRang sampleTask.m_taskContext = taskContext; sample->m_scheduler.AddTaskSetToPipe(&sampleTask); ++sample->m_taskCount; + return &sampleTask; } else { + assert(false); task(0, itemCount, 0, taskContext); + return nullptr; } } -static void FinishTasks(void* userContext) +static void FinishTask(void* taskPtr, void* userContext) +{ + SampleTask* sampleTask = static_cast(taskPtr); + Sample* sample = static_cast(userContext); + sample->m_scheduler.WaitforTask(sampleTask); +} + +static void FinishAllTasks(void* userContext) { Sample* sample = static_cast(userContext); sample->m_scheduler.WaitforAll(); @@ -74,7 +70,8 @@ Sample::Sample(const Settings& settings) b2WorldDef worldDef = b2DefaultWorldDef(); worldDef.workerCount = maxThreads; worldDef.enqueueTask = &EnqueueTask; - worldDef.finishTasks = &FinishTasks; + worldDef.finishTask = &FinishTask; + worldDef.finishAllTasks = &FinishAllTasks; worldDef.bodyCapacity = 1024; worldDef.contactCapacity = 4 * 1024; worldDef.userTaskContext = this; @@ -91,6 +88,7 @@ Sample::Sample(const Settings& settings) // m_world->SetDestructionListener(&m_destructionListener); // m_world->SetContactListener(this); + // TODO_ERIN too expensive b2World_SetPreSolveCallback(m_worldId, PreSolveFcn, this); m_stepCount = 0; @@ -256,7 +254,7 @@ void Sample::Step(Settings& settings) s.jointCount); m_textLine += m_textIncrement; - g_draw.DrawString(5, m_textLine, "proxies/height/points = %d/%d/%d", s.proxyCount, s.treeHeight, s.contactPointCount); + g_draw.DrawString(5, m_textLine, "proxies/height = %d/%d", s.proxyCount, s.treeHeight); m_textLine += m_textIncrement; g_draw.DrawString(5, m_textLine, "stack allocator capacity/used = %d/%d", s.stackCapacity, s.stackUsed); @@ -276,6 +274,7 @@ void Sample::Step(Settings& settings) m_maxProfile.buildIslands = B2_MAX(m_maxProfile.buildIslands, p.buildIslands); m_maxProfile.solveIslands = B2_MAX(m_maxProfile.solveIslands, p.solveIslands); m_maxProfile.broadphase = B2_MAX(m_maxProfile.broadphase, p.broadphase); + m_maxProfile.continuous = B2_MAX(m_maxProfile.continuous, p.continuous); m_totalProfile.step += p.step; m_totalProfile.pairs += p.pairs; @@ -284,6 +283,7 @@ void Sample::Step(Settings& settings) m_totalProfile.buildIslands += p.buildIslands; m_totalProfile.solveIslands += p.solveIslands; m_totalProfile.broadphase += p.broadphase; + m_totalProfile.continuous += p.continuous; } if (settings.m_drawProfile) @@ -302,6 +302,7 @@ void Sample::Step(Settings& settings) aveProfile.buildIslands = scale * m_totalProfile.buildIslands; aveProfile.solveIslands = scale * m_totalProfile.solveIslands; aveProfile.broadphase = scale * m_totalProfile.broadphase; + aveProfile.continuous = scale * m_totalProfile.continuous; } g_draw.DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step); @@ -322,6 +323,9 @@ void Sample::Step(Settings& settings) g_draw.DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase); m_textLine += m_textIncrement; + g_draw.DrawString(5, m_textLine, "continuous collision [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.continuous, aveProfile.continuous, + m_maxProfile.continuous); + m_textLine += m_textIncrement; } if (settings.m_drawContactPoints) diff --git a/src/body.c b/src/body.c index 29fc20d4..25b0d10b 100644 --- a/src/body.c +++ b/src/body.c @@ -161,15 +161,12 @@ void b2World_DestroyBody(b2BodyId bodyId) } // Remove from awake contact array - // TODO_ERIN perf problem? - int32_t awakeContactCount = b2Array(world->awakeContactArray).count; - for (int32_t i = 0; i < awakeContactCount; ++i) + int32_t awakeIndex = world->contactAwakeIndexArray[contactIndex]; + if (awakeIndex != B2_NULL_INDEX) { - if (world->awakeContactArray[i] == contactIndex) - { - b2Array_RemoveSwap(world->awakeContactArray, i); - break; - } + B2_ASSERT(0 <= awakeIndex && awakeIndex < b2Array(world->awakeContactArray).count); + world->awakeContactArray[awakeIndex] = B2_NULL_INDEX; + world->contactAwakeIndexArray[contactIndex] = B2_NULL_INDEX; } // Remove pair from set @@ -362,6 +359,10 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi switch (shapeType) { + case b2_capsuleShape: + shape->capsule = *(const b2Capsule*)geometry; + break; + case b2_circleShape: shape->circle = *(const b2Circle*)geometry; break; @@ -370,6 +371,10 @@ static b2ShapeId b2CreateShape(b2BodyId bodyId, const b2ShapeDef* def, const voi shape->polygon = *(const b2Polygon*)geometry; break; + case b2_segmentShape: + shape->segment = *(const b2Segment*)geometry; + break; + default: B2_ASSERT(false); break; @@ -410,12 +415,12 @@ b2ShapeId b2Body_CreateCircle(b2BodyId bodyId, const b2ShapeDef* def, const b2Ci return b2CreateShape(bodyId, def, circle, b2_circleShape); } -b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const struct b2Polygon* polygon) +b2ShapeId b2Body_CreatePolygon(b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon) { return b2CreateShape(bodyId, def, polygon, b2_polygonShape); } -b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const struct b2Segment* segment) +b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment) { float lengthSqr = b2DistanceSquared(segment->point1, segment->point2); if (lengthSqr <= b2_linearSlop * b2_linearSlop) @@ -424,18 +429,41 @@ b2ShapeId b2Body_CreateSegment(b2BodyId bodyId, const b2ShapeDef* def, const str return b2_nullShapeId; } - b2Vec2 axis = b2Normalize(b2Sub(segment->point2, segment->point1)); + //b2Vec2 axis = b2Normalize(b2Sub(segment->point2, segment->point1)); + + //b2Polygon polygon; + //polygon.vertices[0] = segment->point1; + //polygon.vertices[1] = segment->point2; + //polygon.count = 2; + //polygon.radius = 0.0f; + // + //polygon.normals[0] = b2RightPerp(axis); + //polygon.normals[1] = b2Neg(polygon.normals[0]); + + return b2CreateShape(bodyId, def, segment, b2_segmentShape); +} + +b2ShapeId b2Body_CreateCapsule(b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule) +{ + float lengthSqr = b2DistanceSquared(capsule->point1, capsule->point2); + if (lengthSqr <= b2_linearSlop * b2_linearSlop) + { + B2_ASSERT(false); + return b2_nullShapeId; + } + + //b2Vec2 axis = b2Normalize(b2Sub(segment->point2, segment->point1)); - b2Polygon polygon; - polygon.vertices[0] = segment->point1; - polygon.vertices[1] = segment->point2; - polygon.count = 2; - polygon.radius = 0.0f; - - polygon.normals[0] = b2RightPerp(axis); - polygon.normals[1] = b2Neg(polygon.normals[0]); + //b2Polygon polygon; + //polygon.vertices[0] = segment->point1; + //polygon.vertices[1] = segment->point2; + //polygon.count = 2; + //polygon.radius = 0.0f; + // + //polygon.normals[0] = b2RightPerp(axis); + //polygon.normals[1] = b2Neg(polygon.normals[0]); - return b2CreateShape(bodyId, def, &polygon, b2_polygonShape); + return b2CreateShape(bodyId, def, capsule, b2_capsuleShape); } // Destroy a shape on a body. This doesn't need to be called when destroying a body. @@ -566,7 +594,7 @@ void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle) b2BroadPhase* broadPhase = &world->broadPhase; - const b2Vec2 aabbExtension = {b2_aabbExtension, b2_aabbExtension}; + const b2Vec2 aabbMargin = {b2_aabbMargin, b2_aabbMargin}; int32_t shapeIndex = body->shapeList; while (shapeIndex != B2_NULL_INDEX) { @@ -575,8 +603,8 @@ void b2Body_SetTransform(b2BodyId bodyId, b2Vec2 position, float angle) if (b2AABB_Contains(shape->fatAABB, shape->aabb) == false) { - shape->fatAABB.lowerBound = b2Sub(shape->aabb.lowerBound, aabbExtension); - shape->fatAABB.upperBound = b2Add(shape->aabb.upperBound, aabbExtension); + shape->fatAABB.lowerBound = b2Sub(shape->aabb.lowerBound, aabbMargin); + shape->fatAABB.upperBound = b2Add(shape->aabb.upperBound, aabbMargin); b2BroadPhase_MoveProxy(broadPhase, shape->proxyKey, shape->fatAABB); } diff --git a/src/broad_phase.c b/src/broad_phase.c index 277ea303..fbea52f9 100644 --- a/src/broad_phase.c +++ b/src/broad_phase.c @@ -245,6 +245,7 @@ static bool b2PairQueryCallback(int32_t proxyId, int32_t shapeIndex, void* conte else { pair = b2Alloc(sizeof(b2MovePair)); + pair->heap = true; } pair->shapeIndexA = shapeIndexA; @@ -337,10 +338,9 @@ void b2UpdateBroadPhasePairs(b2World* world) if (b2_parallel) { - // TODO_ERIN should be 64 - int32_t minRange = 1; - world->enqueueTask(&b2FindPairsTask, moveCount, minRange, world, world->userTaskContext); - world->finishTasks(world->userTaskContext); + int32_t minRange = 64; + void* userPairTask = world->enqueueTaskFcn(&b2FindPairsTask, moveCount, minRange, world, world->userTaskContext); + world->finishTaskFcn(userPairTask, world->userTaskContext); } else { diff --git a/src/contact.c b/src/contact.c index 934b32bf..1413d179 100644 --- a/src/contact.c +++ b/src/contact.c @@ -21,7 +21,7 @@ // Contacts and determinism // A deterministic simulation requires contacts to exist in the same order in b2Island no matter the thread count. -// The order must reproduce from run to run. This is necessary because Gauss-Seidel is order dependent. +// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. // // Creation: // - Contacts are created using results from b2UpdateBroadPhasePairs @@ -35,7 +35,8 @@ // - Awake contacts are solved in parallel and they generate contact state changes. // - These state changes may link islands together using union find. // - The state changes are ordered using a bit array that encompasses all contacts -// - As long as contacts are created in deterministic order, island link order is deterministic +// - As long as contacts are created in deterministic order, island link order is deterministic. +// - This keeps the order of contacts in islands deterministic // Friction mixing law. The idea is to allow either fixture to drive the friction to zero. // For example, anything slides on ice. @@ -70,6 +71,19 @@ static b2Manifold b2CircleManifold(const b2Shape* shapeA, b2Transform xfA, const return b2CollideCircles(&shapeA->circle, xfA, &shapeB->circle, xfB, maxDistance); } +static b2Manifold b2CapsuleAndCircleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, + b2DistanceCache* cache) +{ + B2_MAYBE_UNUSED(cache); + return b2CollideCapsuleAndCircle(&shapeA->capsule, xfA, &shapeB->circle, xfB, maxDistance); +} + +static b2Manifold b2CapsuleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, + b2DistanceCache* cache) +{ + return b2CollideCapsules(&shapeA->capsule, xfA, &shapeB->capsule, xfB, maxDistance, cache); +} + static b2Manifold b2PolygonAndCircleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, b2DistanceCache* cache) { @@ -77,12 +91,37 @@ static b2Manifold b2PolygonAndCircleManifold(const b2Shape* shapeA, b2Transform return b2CollidePolygonAndCircle(&shapeA->polygon, xfA, &shapeB->circle, xfB, maxDistance); } +static b2Manifold b2PolygonAndCapsuleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + float maxDistance, b2DistanceCache* cache) +{ + return b2CollidePolygonAndCapsule(&shapeA->polygon, xfA, &shapeB->capsule, xfB, maxDistance, cache); +} + static b2Manifold b2PolygonManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, b2DistanceCache* cache) { return b2CollidePolygons(&shapeA->polygon, xfA, &shapeB->polygon, xfB, maxDistance, cache); } +static b2Manifold b2SegmentAndCircleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, + b2DistanceCache* cache) +{ + B2_MAYBE_UNUSED(cache); + return b2CollideSegmentAndCircle(&shapeA->segment, xfA, &shapeB->circle, xfB, maxDistance); +} + +static b2Manifold b2SegmentAndCapsuleManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, + b2DistanceCache* cache) +{ + return b2CollideSegmentAndCapsule(&shapeA->segment, xfA, &shapeB->capsule, xfB, maxDistance, cache); +} + +static b2Manifold b2SegmentAndPolygonManifold(const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, float maxDistance, + b2DistanceCache* cache) +{ + return b2CollideSegmentAndPolygon(&shapeA->segment, xfA, &shapeB->polygon, xfB, maxDistance, cache); +} + static void b2AddType(b2ManifoldFcn* fcn, enum b2ShapeType type1, enum b2ShapeType type2) { B2_ASSERT(0 <= type1 && type1 < b2_shapeTypeCount); @@ -103,8 +142,14 @@ void b2InitializeContactRegisters(void) if (s_initialized == false) { b2AddType(b2CircleManifold, b2_circleShape, b2_circleShape); + b2AddType(b2CapsuleAndCircleManifold, b2_capsuleShape, b2_circleShape); + b2AddType(b2CapsuleManifold, b2_capsuleShape, b2_capsuleShape); b2AddType(b2PolygonAndCircleManifold, b2_polygonShape, b2_circleShape); + b2AddType(b2PolygonAndCapsuleManifold, b2_polygonShape, b2_capsuleShape); b2AddType(b2PolygonManifold, b2_polygonShape, b2_polygonShape); + b2AddType(b2SegmentAndCircleManifold, b2_segmentShape, b2_circleShape); + b2AddType(b2SegmentAndCapsuleManifold, b2_segmentShape, b2_capsuleShape); + b2AddType(b2SegmentAndPolygonManifold, b2_segmentShape, b2_polygonShape); s_initialized = true; } } @@ -119,6 +164,7 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) if (s_registers[type1][type2].fcn == NULL) { + // For example, no segment vs segment collision return; } @@ -132,6 +178,8 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) b2Contact* contact = (b2Contact*)b2AllocObject(&world->contactPool); world->contacts = (b2Contact*)world->contactPool.memory; + int32_t contactIndex = contact->object.index; + contact->flags = b2_contactEnabledFlag; if (shapeA->isSensor || shapeB->isSensor) @@ -159,7 +207,7 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) contact->edges[0].prevKey = B2_NULL_INDEX; contact->edges[0].nextKey = bodyA->contactList; - int32_t keyA = (contact->object.index << 1) | 0; + int32_t keyA = (contactIndex << 1) | 0; if (bodyA->contactList != B2_NULL_INDEX) { b2Contact* contactA = world->contacts + (bodyA->contactList >> 1); @@ -176,7 +224,7 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) contact->edges[1].prevKey = B2_NULL_INDEX; contact->edges[1].nextKey = bodyB->contactList; - int32_t keyB = (contact->object.index << 1) | 1; + int32_t keyB = (contactIndex << 1) | 1; if (bodyB->contactList != B2_NULL_INDEX) { b2Contact* contactB = world->contacts + (bodyB->contactList >> 1); @@ -187,10 +235,20 @@ void b2CreateContact(b2World* world, b2Shape* shapeA, b2Shape* shapeB) bodyB->contactCount += 1; } - if (b2IsBodyAwake(world, bodyA) || b2IsBodyAwake(world, bodyB)) + // A contact should only be created from an awake body + B2_ASSERT(b2IsBodyAwake(world, bodyA) || b2IsBodyAwake(world, bodyB)); + + int32_t awakeIndex = b2Array(world->awakeContactArray).count; + b2Array_Push(world->awakeContactArray, contactIndex); + + if (contactIndex == b2Array(world->contactAwakeIndexArray).count) { - // A contact does not need to be in an island to be awake. - b2Array_Push(world->awakeContactArray, contact->object.index); + b2Array_Push(world->contactAwakeIndexArray, awakeIndex); + } + else + { + B2_ASSERT(contactIndex < b2Array(world->contactAwakeIndexArray).count); + world->contactAwakeIndexArray[contactIndex] = awakeIndex; } // Add to pair set for fast lookup @@ -253,7 +311,9 @@ void b2DestroyContact(b2World* world, b2Contact* contact) nextEdge->prevKey = edgeB->prevKey; } - int32_t edgeKeyB = (contact->object.index << 1) | 1; + int32_t contactIndex = contact->object.index; + + int32_t edgeKeyB = (contactIndex << 1) | 1; if (bodyB->contactList == edgeKeyB) { bodyB->contactList = edgeB->nextKey; @@ -267,16 +327,13 @@ void b2DestroyContact(b2World* world, b2Contact* contact) } // Remove from awake contact array - // TODO_ERIN perf problem? - int32_t contactIndex = contact->object.index; - int32_t awakeContactCount = b2Array(world->awakeContactArray).count; - for (int32_t i = 0; i < awakeContactCount; ++i) + b2Array_Check(world->contactAwakeIndexArray, contactIndex); + int32_t awakeIndex = world->contactAwakeIndexArray[contactIndex]; + if (awakeIndex != B2_NULL_INDEX) { - if (world->awakeContactArray[i] == contactIndex) - { - b2Array_RemoveSwap(world->awakeContactArray, i); - break; - } + B2_ASSERT(0 <= awakeIndex && awakeIndex < b2Array(world->awakeContactArray).count); + world->awakeContactArray[awakeIndex] = B2_NULL_INDEX; + world->contactAwakeIndexArray[contactIndex] = B2_NULL_INDEX; } b2FreeObject(&world->contactPool, &contact->object); diff --git a/src/contact.h b/src/contact.h index ccd78986..0b2018d4 100644 --- a/src/contact.h +++ b/src/contact.h @@ -63,6 +63,9 @@ typedef struct b2Contact uint32_t flags; + // This is too hot and has been moved to a separate array + //int32_t awakeIndex; + b2ContactEdge edges[2]; int32_t shapeIndexA; diff --git a/src/contact_solver.c b/src/contact_solver.c index cf7f5396..f0c439c5 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -1,10 +1,11 @@ // SPDX-FileCopyrightText: 2023 Erin Catto // SPDX-License-Identifier: MIT +#include "contact_solver.h" + #include "array.h" #include "body.h" #include "contact.h" -#include "contact_solver.h" #include "core.h" #include "stack_allocator.h" #include "world.h" @@ -59,8 +60,10 @@ b2ContactSolver* b2CreateContactSolver(b2ContactSolverDef* def) solver->contactCount = def->contactCount; // These are allocated conservatively because some island contacts may not have contact points - solver->positionConstraints = b2AllocateStackItem(alloc, solver->contactCount * sizeof(b2ContactPositionConstraint), "position constraints"); - solver->velocityConstraints = b2AllocateStackItem(alloc, solver->contactCount * sizeof(b2ContactVelocityConstraint), "velocity constraints"); + solver->positionConstraints = + b2AllocateStackItem(alloc, solver->contactCount * sizeof(b2ContactPositionConstraint), "position constraints"); + solver->velocityConstraints = + b2AllocateStackItem(alloc, solver->contactCount * sizeof(b2ContactVelocityConstraint), "velocity constraints"); solver->world = def->world; solver->constraintCount = 0; @@ -121,8 +124,8 @@ void b2ContactSolver_Initialize(b2ContactSolver* solver) b2Vec2 cB = bodyB->position; // TODO_ERIN testing - //qA = b2MakeRot(bodyA->angle); - //qB = b2MakeRot(bodyB->angle); + // qA = b2MakeRot(bodyA->angle); + // qB = b2MakeRot(bodyB->angle); b2Vec2 vA = bodyA->linearVelocity; float wA = bodyA->angularVelocity; @@ -198,8 +201,8 @@ void b2ContactSolver_Initialize(b2ContactSolver* solver) if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. - vc->K.cx = (b2Vec2){ k11, k12 }; - vc->K.cy = (b2Vec2){ k12, k22 }; + vc->K.cx = (b2Vec2){k11, k12}; + vc->K.cy = (b2Vec2){k12, k22}; vc->normalMass = b2GetInverse22(vc->K); } else @@ -712,7 +715,7 @@ bool b2ContactSolver_SolvePositionConstraintsBlock(b2ContactSolver* solver) float aA = bodyA->angle; b2Vec2 cB = bodyB->position; float aB = bodyB->angle; - + b2Vec2 normal = pc->normal; if (pointCount == 2) @@ -753,7 +756,7 @@ bool b2ContactSolver_SolvePositionConstraintsBlock(b2ContactSolver* solver) b2Mat22 K, invK; // Ensure a reasonable condition number. - const float k_maxConditionNumber = 1000.0f; + const float k_maxConditionNumber = 10000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. @@ -763,8 +766,9 @@ bool b2ContactSolver_SolvePositionConstraintsBlock(b2ContactSolver* solver) } else { - // The constraints are redundant, just use one. - continue; + // The constraints are redundant, however one may be deeper than the other. + // This can happen when a capsule is deeply embedded in a box. + goto manifold_degenerate; } const float k_errorTol = 1e-3f; @@ -856,6 +860,7 @@ bool b2ContactSolver_SolvePositionConstraintsBlock(b2ContactSolver* solver) } else { + manifold_degenerate: for (int32_t j = 0; j < pointCount; ++j) { b2Rot qA = b2MakeRot(aA); diff --git a/src/distance.c b/src/distance.c index 222f9180..265e630f 100644 --- a/src/distance.c +++ b/src/distance.c @@ -12,6 +12,8 @@ #include +#define B2_RESTRICT + b2Transform b2GetSweepTransform(const b2Sweep* sweep, float time) { // https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ @@ -102,7 +104,6 @@ b2SegmentDistanceResult b2SegmentDistance(b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec } // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. -int32_t b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; b2DistanceProxy b2MakeProxy(const b2Vec2* vertices, int32_t count, float radius) { @@ -191,6 +192,7 @@ static b2Simplex b2MakeSimplexFromCache(const b2DistanceCache* cache, const b2Di // Copy data from cache. s.count = cache->count; + b2SimplexVertex* vertices[] = {&s.v1, &s.v2, &s.v3}; for (int32_t i = 0; i < s.count; ++i) { @@ -207,34 +209,6 @@ static b2Simplex b2MakeSimplexFromCache(const b2DistanceCache* cache, const b2Di v->a = -1.0f; } -// TODO_ERIN not seeing any benefit to reseting. ignore metric? -#if 0 - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (s.count == 2) - { - float metric1 = cache->metric; - float metric2 = b2Simplex_Metric(&s); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < FLT_EPSILON) - { - // Reset the simplex. - s.count = 0; - } - } - else if (s.count == 3) - { - float metric1 = cache->metric; - float metric2 = b2Simplex_Metric(&s); - float abs1 = B2_ABS(metric1); - float abs2 = B2_ABS(metric2); - if (metric1 * metric2 < 0.0f || abs2 < 0.5f * abs1 || 2.0f * abs1 < abs2) - { - // Reset the simplex. - s.count = 0; - } - } -#endif - // If the cache is empty or invalid ... if (s.count == 0) { @@ -371,7 +345,7 @@ void b2ComputeSimplexWitnessPoints(b2Vec2* a, b2Vec2* b, const b2Simplex* s) // Solution // a1 = d12_1 / d12 // a2 = d12_2 / d12 -void b2SolveSimplex2(b2Simplex* s) +void b2SolveSimplex2(b2Simplex* B2_RESTRICT s) { b2Vec2 w1 = s->v1.w; b2Vec2 w2 = s->v2.w; @@ -405,7 +379,7 @@ void b2SolveSimplex2(b2Simplex* s) s->count = 2; } -void b2SolveSimplex3(b2Simplex* s) +void b2SolveSimplex3(b2Simplex* B2_RESTRICT s) { b2Vec2 w1 = s->v1.w; b2Vec2 w2 = s->v2.w; @@ -514,9 +488,20 @@ void b2SolveSimplex3(b2Simplex* s) s->count = 3; } -b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* input) +#define B2_GJK_DEBUG 0 + +// Warning: writing to these globals significantly slows multi-threading performance +#if B2_GJK_DEBUG +int32_t b2_gjkCalls; +int32_t b2_gjkIters; +int32_t b2_gjkMaxIters; +#endif + +b2DistanceOutput b2ShapeDistance(b2DistanceCache* B2_RESTRICT cache, const b2DistanceInput* B2_RESTRICT input) { +#if B2_GJK_DEBUG ++b2_gjkCalls; +#endif b2DistanceOutput output = {0}; @@ -598,7 +583,10 @@ b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* // Iteration count is equated to the number of support point calls. ++iter; + +#if B2_GJK_DEBUG ++b2_gjkIters; +#endif // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; @@ -621,7 +609,9 @@ b2DistanceOutput b2ShapeDistance(b2DistanceCache* cache, const b2DistanceInput* ++simplex.count; } +#if B2_GJK_DEBUG b2_gjkMaxIters = B2_MAX(b2_gjkMaxIters, iter); +#endif // Prepare output b2ComputeSimplexWitnessPoints(&output.pointA, &output.pointB, &simplex); diff --git a/src/geometry.c b/src/geometry.c index a1ceeb25..d1feadcc 100644 --- a/src/geometry.c +++ b/src/geometry.c @@ -104,7 +104,7 @@ b2Polygon b2MakeCapsule(b2Vec2 p1, b2Vec2 p2, float radius) shape.vertices[1] = p2; b2Vec2 axis = b2NormalizeChecked(b2Sub(p2, p1)); - b2Vec2 normal = b2CrossVS(axis, 1.0f); + b2Vec2 normal = b2RightPerp(axis); shape.normals[0] = normal; shape.normals[1] = b2Neg(normal); diff --git a/src/island.c b/src/island.c index b774bcb2..cad6e0cd 100644 --- a/src/island.c +++ b/src/island.c @@ -676,11 +676,6 @@ void b2PrepareIsland(b2Island* island, b2StepContext* stepContext) { island->stepContext = stepContext; - // b2Array_Clear(island->awakeContactArray); - - // TODO_ERIN - // b2Array_Reserve(&island->enlargedBodyArray, island->bodyCount); - b2ContactSolverDef contactSolverDef; contactSolverDef.context = island->stepContext; contactSolverDef.world = island->world; @@ -1227,7 +1222,7 @@ void b2SolveIsland(b2Island* island, uint32_t threadIndex) else { b2Contact* contacts = world->contacts; - const b2Vec2 aabbExtension = {b2_aabbExtension, b2_aabbExtension}; + const b2Vec2 aabbMargin = {b2_aabbMargin, b2_aabbMargin}; b2BitSet* awakeContactBitSet = &world->taskContextArray[threadIndex].awakeContactBitSet; b2BitSet* shapeBitSet = &world->taskContextArray[threadIndex].shapeBitSet; @@ -1264,8 +1259,8 @@ void b2SolveIsland(b2Island* island, uint32_t threadIndex) if (b2AABB_Contains(shape->fatAABB, shape->aabb) == false) { - shape->fatAABB.lowerBound = b2Sub(shape->aabb.lowerBound, aabbExtension); - shape->fatAABB.upperBound = b2Add(shape->aabb.upperBound, aabbExtension); + shape->fatAABB.lowerBound = b2Sub(shape->aabb.lowerBound, aabbMargin); + shape->fatAABB.upperBound = b2Add(shape->aabb.upperBound, aabbMargin); // Bit-set to keep the move array sorted b2SetBit(shapeBitSet, shapeIndex); diff --git a/src/island.h b/src/island.h index d1cf569b..6cf31330 100644 --- a/src/island.h +++ b/src/island.h @@ -32,10 +32,6 @@ typedef struct b2Island struct b2World* world; - // These arrays get populated in the island job to make serial work faster - // while maintaining determinism. - //b2Contact** awakeContactArray; - int32_t headBody; int32_t tailBody; int32_t bodyCount; diff --git a/src/manifold.c b/src/manifold.c index a6ec9629..f42e7c43 100644 --- a/src/manifold.c +++ b/src/manifold.c @@ -177,7 +177,7 @@ b2Manifold b2CollidePolygonAndCircle(const b2Polygon* polygonA, b2Transform xfA, // Compute circle position in the frame of the polygon. b2Vec2 c = b2InvTransformPoint(xfA, b2TransformPoint(xfB, circleB->point)); - float radius = circleB->radius; + float radius = polygonA->radius + circleB->radius; // Find the min separating edge. int32_t normalIndex = 0; diff --git a/src/shape.c b/src/shape.c index f822cefa..f6d5e13a 100644 --- a/src/shape.c +++ b/src/shape.c @@ -11,10 +11,14 @@ b2AABB b2Shape_ComputeAABB(const b2Shape* shape, b2Transform xf) { switch (shape->type) { + case b2_capsuleShape: + return b2ComputeCapsuleAABB(&shape->capsule, xf); case b2_circleShape: return b2ComputeCircleAABB(&shape->circle, xf); case b2_polygonShape: return b2ComputePolygonAABB(&shape->polygon, xf); + case b2_segmentShape: + return b2ComputeSegmentAABB(&shape->segment, xf); default: { B2_ASSERT(false); b2AABB empty = {xf.p, xf.p}; @@ -27,6 +31,8 @@ b2MassData b2Shape_ComputeMass(const b2Shape* shape) { switch (shape->type) { + case b2_capsuleShape: + return b2ComputeCapsuleMass(&shape->capsule, shape->density); case b2_circleShape: return b2ComputeCircleMass(&shape->circle, shape->density); case b2_polygonShape: @@ -43,19 +49,15 @@ void b2Shape_CreateProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Tr { // Create proxies in the broad-phase. shape->aabb = b2Shape_ComputeAABB(shape, xf); - if (type == b2_staticBody) - { - shape->fatAABB = shape->aabb; - } - else - { - shape->fatAABB.lowerBound.x = shape->aabb.lowerBound.x - b2_aabbExtension; - shape->fatAABB.lowerBound.y = shape->aabb.lowerBound.y - b2_aabbExtension; - shape->fatAABB.upperBound.x = shape->aabb.upperBound.x + b2_aabbExtension; - shape->fatAABB.upperBound.y = shape->aabb.upperBound.y + b2_aabbExtension; - } - shape->proxyKey = b2BroadPhase_CreateProxy(bp, type, shape->aabb, shape->filter.categoryBits, shape->object.index); + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = type == b2_staticBody ? 4.0f * b2_linearSlop : b2_aabbMargin; + shape->fatAABB.lowerBound.x = shape->aabb.lowerBound.x - margin; + shape->fatAABB.lowerBound.y = shape->aabb.lowerBound.y - margin; + shape->fatAABB.upperBound.x = shape->aabb.upperBound.x + margin; + shape->fatAABB.upperBound.y = shape->aabb.upperBound.y + margin; + + shape->proxyKey = b2BroadPhase_CreateProxy(bp, type, shape->fatAABB, shape->filter.categoryBits, shape->object.index); B2_ASSERT(B2_PROXY_TYPE(shape->proxyKey) < b2_bodyTypeCount); } @@ -69,10 +71,14 @@ b2DistanceProxy b2Shape_MakeDistanceProxy(const b2Shape* shape) { switch (shape->type) { + case b2_capsuleShape: + return b2MakeProxy(&shape->capsule.point1, 2, shape->capsule.radius); case b2_circleShape: return b2MakeProxy(&shape->circle.point, 1, shape->circle.radius); case b2_polygonShape: return b2MakeProxy(shape->polygon.vertices, shape->polygon.count, shape->polygon.radius); + case b2_segmentShape: + return b2MakeProxy(&shape->segment.point1, 2, 0.0f); default: { B2_ASSERT(false); b2DistanceProxy empty = {0}; @@ -81,17 +87,6 @@ b2DistanceProxy b2Shape_MakeDistanceProxy(const b2Shape* shape) } } -float b2Shape_GetRadius(const b2Shape* shape) -{ - switch (shape->type) - { - case b2_circleShape: - return shape->circle.radius; - default: - return 0.0f; - } -} - b2BodyId b2Shape_GetBody(b2ShapeId shapeId) { b2World* world = b2GetWorldFromIndex(shapeId.world); @@ -122,6 +117,9 @@ bool b2Shape_TestPoint(b2ShapeId shapeId, b2Vec2 point) switch (shape->type) { + case b2_capsuleShape: + return b2PointInCapsule(localPoint, &shape->capsule); + case b2_circleShape: return b2PointInCircle(localPoint, &shape->circle); diff --git a/src/shape.h b/src/shape.h index 4e4e35c7..6dc4ef3c 100644 --- a/src/shape.h +++ b/src/shape.h @@ -13,8 +13,10 @@ typedef struct b2BroadPhase b2BroadPhase; typedef enum b2ShapeType { + b2_capsuleShape, b2_circleShape, b2_polygonShape, + b2_segmentShape, b2_shapeTypeCount } b2ShapeType; @@ -43,8 +45,10 @@ typedef struct b2Shape // TODO_ERIN maybe not anonymous, check asm union { + b2Capsule capsule; b2Circle circle; b2Polygon polygon; + b2Segment segment; }; } b2Shape; @@ -55,5 +59,3 @@ void b2Shape_CreateProxy(b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Tr void b2Shape_DestroyProxy(b2Shape* shape, b2BroadPhase* bp); b2DistanceProxy b2Shape_MakeDistanceProxy(const b2Shape* shape); - -float b2Shape_GetRadius(const b2Shape* shape); diff --git a/src/solver_data.h b/src/solver_data.h index 751e14d0..b09797d2 100644 --- a/src/solver_data.h +++ b/src/solver_data.h @@ -26,7 +26,7 @@ typedef struct b2StepContext float restitutionThreshold; // From b2World::bodies for convenience - b2Body* bodies; + struct b2Body* bodies; int32_t bodyCapacity; bool warmStarting; diff --git a/src/world.c b/src/world.c index 933f5def..d21451e4 100644 --- a/src/world.c +++ b/src/world.c @@ -29,6 +29,8 @@ b2World b2_worlds[b2_maxWorlds]; bool b2_parallel = true; +int b2_collideMinRange = 64; +int b2_islandMinRange = 1; b2World* b2GetWorldFromId(b2WorldId id) { @@ -46,14 +48,21 @@ b2World* b2GetWorldFromIndex(int16_t index) return world; } -static void b2DefaultAddTaskFcn(b2TaskCallback* task, int32_t count, int32_t minRange, void* taskContext, void* userContext) +static void* b2DefaultAddTaskFcn(b2TaskCallback* task, int32_t count, int32_t minRange, void* taskContext, void* userContext) { B2_MAYBE_UNUSED(minRange); B2_MAYBE_UNUSED(userContext); task(0, count, 0, taskContext); + return NULL; } -static void b2DefaultFinishTasksFcn(void* userContext) +static void b2DefaultFinishTaskFcn(void* userTask, void* userContext) +{ + B2_MAYBE_UNUSED(userTask); + B2_MAYBE_UNUSED(userContext); +} + +static void b2DefaultFinishAllTasksFcn(void* userContext) { B2_MAYBE_UNUSED(userContext); } @@ -108,6 +117,7 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) world->splitIslandArray = b2CreateArray(sizeof(int32_t), B2_MAX(def->bodyCapacity, 1)); world->awakeContactArray = b2CreateArray(sizeof(int32_t), B2_MAX(def->contactCapacity, 1)); + world->contactAwakeIndexArray = b2CreateArray(sizeof(int32_t), world->contactPool.capacity); world->splitIslandIndex = B2_NULL_INDEX; world->stepId = 0; @@ -126,18 +136,20 @@ b2WorldId b2CreateWorld(const b2WorldDef* def) id.revision = world->revision; - if (def->workerCount > 0 && def->enqueueTask != NULL && def->finishTasks != NULL) + if (def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL && def->finishAllTasks != NULL) { world->workerCount = B2_MIN(def->workerCount, b2_maxWorkers); - world->enqueueTask = def->enqueueTask; - world->finishTasks = def->finishTasks; + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->finishAllTasksFcn = def->finishAllTasks; world->userTaskContext = def->userTaskContext; } else { world->workerCount = 1; - world->enqueueTask = b2DefaultAddTaskFcn; - world->finishTasks = b2DefaultFinishTasksFcn; + world->enqueueTaskFcn = b2DefaultAddTaskFcn; + world->finishTaskFcn = b2DefaultFinishTaskFcn; + world->finishAllTasksFcn = b2DefaultFinishAllTasksFcn; world->userTaskContext = NULL; } @@ -168,6 +180,7 @@ void b2DestroyWorld(b2WorldId id) b2DestroyArray(world->awakeContactArray, sizeof(int32_t)); b2DestroyArray(world->awakeIslandArray, sizeof(int32_t)); + b2DestroyArray(world->contactAwakeIndexArray, sizeof(int32_t)); b2DestroyArray(world->splitIslandArray, sizeof(int32_t)); b2Island* islands = world->islands; @@ -207,6 +220,8 @@ static void b2CollideTask(int32_t startIndex, int32_t endIndex, uint32_t threadI b2Body* bodies = world->bodies; b2Contact* contacts = world->contacts; int32_t awakeCount = b2Array(world->awakeContactArray).count; + int32_t* awakeContactArray = world->awakeContactArray; + int32_t* contactAwakeIndexArray = world->contactAwakeIndexArray; B2_MAYBE_UNUSED(awakeCount); B2_ASSERT(startIndex < endIndex); @@ -214,17 +229,22 @@ static void b2CollideTask(int32_t startIndex, int32_t endIndex, uint32_t threadI for (int32_t awakeIndex = startIndex; awakeIndex < endIndex; ++awakeIndex) { - int32_t contactIndex = world->awakeContactArray[awakeIndex]; + int32_t contactIndex = awakeContactArray[awakeIndex]; + if (contactIndex == B2_NULL_INDEX) + { + // Contact was destroyed + continue; + } B2_ASSERT(0 <= contactIndex && contactIndex < world->contactPool.capacity); b2Contact* contact = contacts + contactIndex; - // B2_ASSERT(contact->awakeIndex == awakeIndex); + B2_ASSERT(contactAwakeIndexArray[contactIndex] == awakeIndex); B2_ASSERT(contact->object.index == contactIndex && contact->object.index == contact->object.next); // Reset contact awake index. Contacts must be added to the awake contact array // each time step in the island solver. - // contact->awakeIndex = B2_NULL_INDEX; + contactAwakeIndexArray[contactIndex] = B2_NULL_INDEX; b2Shape* shapeA = shapes + contact->shapeIndexA; b2Shape* shapeB = shapes + contact->shapeIndexB; @@ -281,7 +301,20 @@ static void b2UpdateTreesTask(int32_t startIndex, int32_t endIndex, uint32_t thr static void b2Collide(b2World* world) { - world->contactPointCount = 0; + B2_ASSERT(world->workerCount > 0); + + b2TracyCZoneNC(collide, "Collide", b2_colorDarkOrchid, true); + + // Rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good. + if (b2_parallel) + { + world->userTreeTask = world->enqueueTaskFcn(&b2UpdateTreesTask, 1, 1, world, world->userTaskContext); + } + else + { + b2UpdateTreesTask(0, 1, 0, world); + world->userTreeTask = NULL; + } int32_t awakeContactCount = b2Array(world->awakeContactArray).count; @@ -290,10 +323,6 @@ static void b2Collide(b2World* world) return; } - b2TracyCZoneNC(collide, "Collide", b2_colorDarkOrchid, true); - - world->enqueueTask(&b2UpdateTreesTask, 1, 1, world, world->userTaskContext); - for (uint32_t i = 0; i < world->workerCount; ++i) { b2SetBitCountAndClear(&world->taskContextArray[i].contactStateBitSet, awakeContactCount); @@ -302,17 +331,15 @@ static void b2Collide(b2World* world) if (b2_parallel) { // Task should take at least 40us on a 4GHz CPU (10K cycles) - int32_t minRange = 64; - world->enqueueTask(&b2CollideTask, awakeContactCount, minRange, world, world->userTaskContext); - world->finishTasks(world->userTaskContext); + int32_t minRange = b2_collideMinRange; + void* userCollideTask = world->enqueueTaskFcn(&b2CollideTask, awakeContactCount, minRange, world, world->userTaskContext); + world->finishTaskFcn(userCollideTask, world->userTaskContext); } else { b2CollideTask(0, awakeContactCount, 0, world); } - b2ValidateNoEnlarged(&world->broadPhase); - // Serially update contact state b2TracyCZoneNC(contact_state, "Contact State", b2_colorCoral, true); @@ -335,10 +362,13 @@ static void b2Collide(b2World* world) B2_ASSERT(awakeIndex < (uint32_t)awakeContactCount); int32_t contactIndex = world->awakeContactArray[awakeIndex]; + B2_ASSERT(contactIndex != B2_NULL_INDEX); + b2Contact* contact = world->contacts + contactIndex; if (contact->flags & b2_contactDisjoint) { + // Bounding boxes no longer overlap b2DestroyContact(world, contact); } else if (contact->flags & b2_contactStartedTouching) @@ -360,6 +390,8 @@ static void b2Collide(b2World* world) } } + // TODO_ERIN clear awake contact array here? + b2TracyCZoneEnd(contact_state); b2TracyCZoneEnd(collide); @@ -457,7 +489,7 @@ static void b2SolveContinuous(b2World* world, int32_t bodyIndex) b2Body* fastBody = world->bodies + bodyIndex; B2_ASSERT(b2ObjectValid(&fastBody->object)); B2_ASSERT(fastBody->type == b2_dynamicBody && fastBody->isFast); - + b2Shape* shapes = world->shapes; b2Sweep sweep = b2MakeSweep(fastBody); @@ -465,7 +497,7 @@ static void b2SolveContinuous(b2World* world, int32_t bodyIndex) b2Transform xf1; xf1.q = b2MakeRot(sweep.a1); xf1.p = b2Sub(sweep.c1, b2RotateVector(xf1.q, sweep.localCenter)); - + b2Transform xf2; xf2.q = b2MakeRot(sweep.a2); xf2.p = b2Sub(sweep.c2, b2RotateVector(xf2.q, sweep.localCenter)); @@ -495,7 +527,7 @@ static void b2SolveContinuous(b2World* world, int32_t bodyIndex) // Store this for later fastShape->aabb = box2; - + b2DynamicTree_Query(staticTree, box, b2ContinuousQueryCallback, &context); shapeIndex = fastShape->nextShapeIndex; @@ -643,22 +675,29 @@ static void b2Solve(b2World* world, b2StepContext* context) if (b2_parallel) { - int32_t minRange = 1; - world->enqueueTask(&b2IslandParallelForTask, count, minRange, world, world->userTaskContext); + int32_t minRange = b2_islandMinRange; + void* userIslandTask = world->enqueueTaskFcn(&b2IslandParallelForTask, count, minRange, world, world->userTaskContext); + world->finishTaskFcn(userIslandTask, world->userTaskContext); + + // Finish the user tree task that was queued early in the time step + if (world->userTreeTask != NULL) + { + world->finishTaskFcn(world->userTreeTask, world->userTaskContext); + } + + world->userTreeTask = NULL; } else { b2IslandParallelForTask(0, count, 0, world); } - world->finishTasks(world->userTaskContext); + b2ValidateNoEnlarged(&world->broadPhase); b2TracyCZoneEnd(island_solver); world->profile.solveIslands = b2GetMillisecondsAndReset(&timer); - // TODO_ERIN sync rebuild trees task here - b2TracyCZoneNC(broad_phase, "Broadphase", b2_colorPurple, true); b2TracyCZoneNC(enlarge_proxies, "Enlarge Proxies", b2_colorDarkTurquoise, true); @@ -720,6 +759,8 @@ static void b2Solve(b2World* world, b2StepContext* context) b2Array_Clear(world->awakeContactArray); + int32_t* contactAwakeIndexArray = world->contactAwakeIndexArray; + // Iterate the bit set // The order of the awake contact array doesn't matter, but I don't want duplicates. It is possible // that body A or body B or both bodies wake the contact. @@ -734,7 +775,12 @@ static void b2Solve(b2World* world, b2StepContext* context) uint32_t ctz = b2CTZ(word); uint32_t contactIndex = 64 * k + ctz; - // TODO_ERIN consider adding the awake index to the contact to speed up deletes + B2_ASSERT(contactAwakeIndexArray[contactIndex] == B2_NULL_INDEX); + + // This cache miss is brutal but is necessary to make contact destruction reasonably quick. + contactAwakeIndexArray[contactIndex] = b2Array(world->awakeContactArray).count; + + // This is fast b2Array_Push(world->awakeContactArray, contactIndex); // Clear the smallest set bit @@ -795,15 +841,15 @@ static void b2Solve(b2World* world, b2StepContext* context) if (b2_parallel) { int32_t minRange = 8; - world->enqueueTask(&b2ContinuousParallelForTask, world->fastBodyCount, minRange, world, world->userTaskContext); + void* userContinuousTask = + world->enqueueTaskFcn(&b2ContinuousParallelForTask, world->fastBodyCount, minRange, world, world->userTaskContext); + world->finishTaskFcn(userContinuousTask, world->userTaskContext); } else { b2ContinuousParallelForTask(0, world->fastBodyCount, 0, world); } - world->finishTasks(world->userTaskContext); - // Serially enlarge broad-phase proxies for fast shapes { b2BroadPhase* broadPhase = &world->broadPhase; @@ -855,6 +901,8 @@ static void b2Solve(b2World* world, b2StepContext* context) b2FreeStackItem(world->stackAllocator, world->fastBodies); world->fastBodies = NULL; + world->profile.continuous = b2GetMilliseconds(&timer); + b2FreeStackItem(world->stackAllocator, islands); b2TracyCZoneEnd(solve); @@ -939,6 +987,11 @@ void b2World_Step(b2WorldId worldId, float timeStep, int32_t velocityIterations, // Ensure stack is large enough b2GrowStack(world->stackAllocator); + if (b2_parallel) + { + world->finishAllTasksFcn(world->userTaskContext); + } + b2TracyCZoneEnd(world_step); } @@ -946,76 +999,60 @@ static void b2DrawShape(b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2Col { switch (shape->type) { - case b2_circleShape: - { - b2Circle* circle = &shape->circle; - - b2Vec2 center = b2TransformPoint(xf, circle->point); - float radius = circle->radius; - b2Vec2 axis = b2RotateVector(xf.q, (b2Vec2){1.0f, 0.0f}); - - draw->DrawSolidCircle(center, radius, axis, color, draw->context); - } - break; - - // case b2_segmentShape: - //{ - // b2EdgeShape* edge = (b2EdgeShape*)shape->GetShape(); - // b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); - // b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); - // m_debugDraw->DrawSegment(v1, v2, color); + case b2_capsuleShape: + { + b2Capsule* capsule = &shape->capsule; + b2Vec2 p1 = b2TransformPoint(xf, capsule->point1); + b2Vec2 p2 = b2TransformPoint(xf, capsule->point2); + draw->DrawSolidCapsule(p1, p2, capsule->radius, color, draw->context); + } + break; - // if (edge->m_oneSided == false) - //{ - // m_debugDraw->DrawPoint(v1, 4.0f, color); - // m_debugDraw->DrawPoint(v2, 4.0f, color); - // } - // } - // break; + case b2_circleShape: + { + b2Circle* circle = &shape->circle; + b2Vec2 center = b2TransformPoint(xf, circle->point); + b2Vec2 axis = b2RotateVector(xf.q, (b2Vec2){1.0f, 0.0f}); + draw->DrawSolidCircle(center, circle->radius, axis, color, draw->context); + } + break; - // case b2Shape::e_chain: - //{ - // b2ChainShape* chain = (b2ChainShape*)shape->GetShape(); - // int32 count = chain->m_count; - // const b2Vec2* vertices = chain->m_vertices; + case b2_polygonShape: + { + b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f}; - // b2Vec2 v1 = b2Mul(xf, vertices[0]); - // for (int32 i = 1; i < count; ++i) - //{ - // b2Vec2 v2 = b2Mul(xf, vertices[i]); - // m_debugDraw->DrawSegment(v1, v2, color); - // v1 = v2; - // } - // } - // break; + b2Polygon* poly = &shape->polygon; + int32_t count = poly->count; + B2_ASSERT(count <= b2_maxPolygonVertices); + b2Vec2 vertices[b2_maxPolygonVertices]; - case b2_polygonShape: - { - b2Polygon* poly = &shape->polygon; - int32_t count = poly->count; - B2_ASSERT(count <= b2_maxPolygonVertices); - b2Vec2 vertices[b2_maxPolygonVertices]; + for (int32_t i = 0; i < count; ++i) + { + vertices[i] = b2TransformPoint(xf, poly->vertices[i]); + } - for (int32_t i = 0; i < count; ++i) - { - vertices[i] = b2TransformPoint(xf, poly->vertices[i]); + if (poly->radius > 0.0f) + { + draw->DrawRoundedPolygon(vertices, count, poly->radius, fillColor, color, draw->context); + } + else + { + draw->DrawSolidPolygon(vertices, count, color, draw->context); + } } + break; - b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f}; - - if (poly->radius > 0.0f) + case b2_segmentShape: { - draw->DrawRoundedPolygon(vertices, count, poly->radius, fillColor, color, draw->context); + b2Segment* segment = &shape->segment; + b2Vec2 p1 = b2TransformPoint(xf, segment->point1); + b2Vec2 p2 = b2TransformPoint(xf, segment->point2); + draw->DrawSegment(p1, p2, color, draw->context); } - else - { - draw->DrawSolidPolygon(vertices, count, color, draw->context); - } - } - break; - - default: break; + + default: + break; } } @@ -1061,7 +1098,7 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) } else if (b->isFast) { - b2DrawShape(draw, shape, xf, (b2Color){0.3f, 0.5f, 0.9f, 1.0f}); + b2DrawShape(draw, shape, xf, b2MakeColor(b2_colorSalmon, 1.0f)); } else if (b->type == b2_staticBody) { @@ -1235,14 +1272,13 @@ b2Statistics b2World_GetStatistics(b2WorldId worldId) b2DynamicTree* tree = world->broadPhase.trees + b2_dynamicBody; s.proxyCount = tree->nodeCount; s.treeHeight = b2DynamicTree_GetHeight(tree); - s.contactPointCount = world->contactPointCount; s.stackCapacity = b2GetStackCapacity(world->stackAllocator); s.stackUsed = b2GetMaxStackAllocation(world->stackAllocator); s.byteCount = b2GetByteCount(); return s; } -#if 0 +#if 0 struct b2WorldRayCastWrapper { float RayCastCallback(const b2RayCastInput& input, int32 proxyId) diff --git a/src/world.h b/src/world.h index e5931216..09e79151 100644 --- a/src/world.h +++ b/src/world.h @@ -65,6 +65,9 @@ typedef struct b2World // but a bit set is used to prevent duplicates int32_t* awakeContactArray; + // Hot data split from b2Contact + int32_t* contactAwakeIndexArray; + // This transient array holds islands created from splitting a larger island. int32_t* splitIslandArray; @@ -89,9 +92,6 @@ typedef struct b2World b2Profile profile; - // TODO_ERIN not used - _Atomic int contactPointCount; - b2PreSolveFcn* preSolveFcn; void* preSolveContext; @@ -99,10 +99,13 @@ typedef struct b2World void* postSolveContext; uint32_t workerCount; - b2EnqueueTaskCallback* enqueueTask; - b2FinishTasksCallback* finishTasks; + b2EnqueueTaskCallback* enqueueTaskFcn; + b2FinishTaskCallback* finishTaskFcn; + b2FinishAllTasksCallback* finishAllTasksFcn; void* userTaskContext; + void* userTreeTask; + bool enableSleep; bool locked; bool warmStarting; diff --git a/test/test_determinism.c b/test/test_determinism.c index 7eda1f79..80c787e5 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -45,7 +45,7 @@ void ExecuteRangeTask(uint32_t start, uint32_t end, uint32_t threadIndex, void* data->box2dTask(start, end, threadIndex, data->box2dContext); } -static void EnqueueTask(b2TaskCallback* box2dTask, int itemCount, int minRange, void* box2dContext, void* userContext) +static void* EnqueueTask(b2TaskCallback* box2dTask, int itemCount, int minRange, void* box2dContext, void* userContext) { B2_MAYBE_UNUSED(userContext); @@ -64,15 +64,28 @@ static void EnqueueTask(b2TaskCallback* box2dTask, int itemCount, int minRange, enkiSetParamsTaskSet(task, params); enkiAddTaskSet(scheduler, task); + ++taskCount; + + return task; } else { box2dTask(0, itemCount, 0, box2dContext); + + return NULL; } } -static void FinishTasks(void* userContext) +static void FinishTask(void* userTask, void* userContext) +{ + B2_MAYBE_UNUSED(userContext); + + enkiTaskSet* task = userTask; + enkiWaitForTaskSet(scheduler, task); +} + +static void FinishAllTasks(void* userContext) { B2_MAYBE_UNUSED(userContext); @@ -99,7 +112,8 @@ void TiltedStacks(int testIndex, int workerCount) b2WorldDef worldDef = b2DefaultWorldDef(); worldDef.gravity = gravity; worldDef.enqueueTask = EnqueueTask; - worldDef.finishTasks = FinishTasks; + worldDef.finishTask = FinishTask; + worldDef.finishAllTasks = FinishAllTasks; worldDef.workerCount = workerCount; worldDef.enableSleep = false;