From b7722b089da7dc9ade7fc19107d8623ed5431dd4 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Thu, 25 Apr 2024 23:41:33 -0700 Subject: [PATCH] Fixes and improvements (#151) fix #150 fix #147 fix #138 --- .github/workflows/build.yml | 11 +++++++-- CMakeLists.txt | 1 + benchmark/joint_grid.c | 3 +++ benchmark/main.c | 7 +++--- include/box2d/box2d.h | 3 +++ include/box2d/debug_draw.h | 6 ++--- include/box2d/math_functions.h | 15 ++++++++++++ samples/draw.cpp | 28 +++++++++++---------- samples/draw.h | 2 +- samples/main.cpp | 1 + samples/sample.cpp | 1 + samples/sample_benchmark.cpp | 19 ++++++++++++++ samples/sample_collision.cpp | 26 ++++++++++---------- samples/sample_joints.cpp | 11 ++++++--- samples/settings.h | 1 + src/joint.c | 4 ++- src/revolute_joint.c | 45 ++++++++++++++++++++++++++++------ src/solver.c | 2 +- src/world.c | 6 ++--- 19 files changed, 140 insertions(+), 52 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 5b400a3a..df7fd9d2 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -52,7 +52,9 @@ jobs: - uses: actions/checkout@v4 - name: Configure CMake - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBOX2D_SANITIZE=ON -DBUILD_SHARED_LIBS=OFF + # some problem with simde + # run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBOX2D_SANITIZE=ON -DBUILD_SHARED_LIBS=OFF + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF - name: Build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} @@ -64,10 +66,15 @@ jobs: build-windows: name: windows runs-on: windows-latest - steps: + - uses: actions/checkout@v4 + - name: Setup MSVC dev command prompt + uses: TheMrMilchmann/setup-msvc-dev@v3 + with: + arch: x64 + - name: Configure CMake # enkiTS is failing ASAN on windows # run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBOX2D_SANITIZE=ON -DBUILD_SHARED_LIBS=OFF diff --git a/CMakeLists.txt b/CMakeLists.txt index e06acb17..fd20b68a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ if (MSVC OR APPLE) if(MSVC) # address sanitizer only in the debug build add_compile_options("$<$:/fsanitize=address>") + add_link_options("$<$:/INCREMENTAL:NO>") elseif(APPLE) # more sanitizers on Apple clang # add_compile_options(-fsanitize=thread -fno-omit-frame-pointer) diff --git a/benchmark/joint_grid.c b/benchmark/joint_grid.c index 512b2aac..638a0d2d 100644 --- a/benchmark/joint_grid.c +++ b/benchmark/joint_grid.c @@ -9,6 +9,9 @@ b2WorldId JointGrid(b2WorldDef* worldDef) { + // Turning gravity off to isolate joint performance. + worldDef->gravity = b2Vec2_zero; + b2WorldId worldId = b2CreateWorld(worldDef); #ifdef NDEBUG diff --git a/benchmark/main.c b/benchmark/main.c index 60cb0ec6..b4b2f838 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -1,6 +1,8 @@ // SPDX-FileCopyrightText: 2024 Erin Catto // SPDX-License-Identifier: MIT +#define _CRT_SECURE_NO_WARNINGS + #include "TaskScheduler_c.h" #include "box2d/box2d.h" @@ -194,9 +196,8 @@ int main(int argc, char** argv) char fileName[64] = {0}; snprintf(fileName, 64, "%s.csv", benchmarks[benchmarkIndex].name); - FILE* file = NULL; - errno_t status = fopen_s(&file, fileName, "w"); - if (status != 0 || file == NULL) + FILE* file = fopen(fileName, "w"); + if (file == NULL) { continue; } diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h index c6adde19..f737897b 100644 --- a/include/box2d/box2d.h +++ b/include/box2d/box2d.h @@ -715,6 +715,9 @@ B2_API float b2PrismaticJoint_GetConstraintTorque(b2JointId jointId); /// Revolute Joint +/// Get the current joint angle in radians relative to the reference angle +B2_API float b2RevoluteJoint_GetAngle(b2JointId jointId); + /// Enable/disable a revolute joint limit. B2_API void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit); diff --git a/include/box2d/debug_draw.h b/include/box2d/debug_draw.h index 56a10b40..4ed380b2 100644 --- a/include/box2d/debug_draw.h +++ b/include/box2d/debug_draw.h @@ -15,7 +15,7 @@ typedef struct b2DebugDraw void (*DrawSolidPolygon)(const b2Vec2* vertices, int vertexCount, b2Color color, void* context); /// Draw a rounded polygon provided in CCW order. - void (*DrawRoundedPolygon)(const b2Vec2* vertices, int vertexCount, float radius, b2Color lineColor, b2Color fillColor, void* context); + void (*DrawRoundedPolygon)(const b2Vec2* vertices, int vertexCount, float radius, b2Color color, void* context); /// Draw a circle. void (*DrawCircle)(b2Vec2 center, float radius, b2Color color, void* context); @@ -33,8 +33,7 @@ typedef struct b2DebugDraw void (*DrawSegment)(b2Vec2 p1, b2Vec2 p2, b2Color color, void* context); /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - void (*DrawTransform)(b2Transform xf, void* context); + void (*DrawTransform)(b2Transform transform, void* context); /// Draw a point. void (*DrawPoint)(b2Vec2 p, float size, b2Color color, void* context); @@ -44,6 +43,7 @@ typedef struct b2DebugDraw bool drawShapes; bool drawJoints; + bool drawJointExtras; bool drawAABBs; bool drawMass; bool drawContacts; diff --git a/include/box2d/math_functions.h b/include/box2d/math_functions.h index 9ff4f9a6..e34dd083 100644 --- a/include/box2d/math_functions.h +++ b/include/box2d/math_functions.h @@ -4,6 +4,7 @@ #pragma once #include "api.h" +#include "constants.h" #include "math_types.h" #include @@ -307,6 +308,20 @@ B2_INLINE float b2RelativeAngle(b2Rot b, b2Rot a) return atan2f(s, c); } +B2_INLINE float b2UnwindAngle(float angle) +{ + if (angle < -b2_pi) + { + return angle + 2.0f * b2_pi; + } + else if (angle > b2_pi) + { + return angle - 2.0f * b2_pi; + } + + return angle; +} + /// Rotate a vector B2_INLINE b2Vec2 b2RotateVector(b2Rot q, b2Vec2 v) { diff --git a/samples/draw.cpp b/samples/draw.cpp index 604d70c8..3270e226 100644 --- a/samples/draw.cpp +++ b/samples/draw.cpp @@ -824,10 +824,10 @@ void DrawSolidPolygonFcn(const b2Vec2* vertices, int vertexCount, b2Color color, static_cast(context)->DrawSolidPolygon(vertices, vertexCount, color); } -void DrawRoundedPolygonFcn(const b2Vec2* vertices, int32_t vertexCount, float radius, b2Color fillColor, b2Color lineColor, +void DrawRoundedPolygonFcn(const b2Vec2* vertices, int32_t vertexCount, float radius, b2Color color, void* context) { - static_cast(context)->DrawRoundedPolygon(vertices, vertexCount, radius, fillColor, lineColor); + static_cast(context)->DrawRoundedPolygon(vertices, vertexCount, radius, color); } void DrawCircleFcn(b2Vec2 center, float radius, b2Color color, void* context) @@ -911,6 +911,7 @@ void Draw::Create() DrawStringFcn, true, // shapes true, // joints + false, // joint extras false, // aabbs false, // mass false, // contacts @@ -978,17 +979,18 @@ void Draw::DrawSolidPolygon(const b2Vec2* vertices, int32_t vertexCount, b2Color // Outline needs 4*count triangles. #define MAX_POLY_INDEXES (3 * (5 * MAX_POLY_VERTEXES - 2)) -void Draw::DrawRoundedPolygon(const b2Vec2* vertices, int32_t count, float radius, b2Color fillColor, b2Color outlineColor) +void Draw::DrawRoundedPolygon(const b2Vec2* vertices, int32_t vertexCount, float radius, b2Color color) { - assert(count <= MAX_POLY_VERTEXES); + assert(vertexCount <= MAX_POLY_VERTEXES); + b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f}; RGBA8 fill = MakeRGBA8(fillColor); - RGBA8 outline = MakeRGBA8(outlineColor); + RGBA8 outline = MakeRGBA8(color); uint16_t indices[MAX_POLY_INDEXES]; // Polygon fill triangles. - for (int i = 0; i < count - 2; ++i) + for (int i = 0; i < vertexCount - 2; ++i) { indices[3 * i + 0] = 0; indices[3 * i + 1] = 4 * (i + 1); @@ -996,10 +998,10 @@ void Draw::DrawRoundedPolygon(const b2Vec2* vertices, int32_t count, float radiu } // Polygon outline triangles. - uint16_t* outlineIndices = indices + 3 * (count - 2); - for (int i0 = 0; i0 < count; ++i0) + uint16_t* outlineIndices = indices + 3 * (vertexCount - 2); + for (int i0 = 0; i0 < vertexCount; ++i0) { - int i1 = (i0 + 1) % count; + int i1 = (i0 + 1) % vertexCount; outlineIndices[12 * i0 + 0] = 4 * i0 + 0; outlineIndices[12 * i0 + 1] = 4 * i0 + 1; outlineIndices[12 * i0 + 2] = 4 * i0 + 2; @@ -1029,12 +1031,12 @@ void Draw::DrawRoundedPolygon(const b2Vec2* vertices, int32_t count, float radiu // float outset = radius + lineScale; // float r = outset - inset; - Vertex* vertexes = m_roundedTriangles->AllocVertices(4 * count, indices, 3 * (5 * count - 2)); - for (int i = 0; i < count; ++i) + Vertex* vertexes = m_roundedTriangles->AllocVertices(4 * vertexCount, indices, 3 * (5 * vertexCount - 2)); + for (int i = 0; i < vertexCount; ++i) { - b2Vec2 v_prev = vertices[(i + (count - 1)) % count]; + b2Vec2 v_prev = vertices[(i + (vertexCount - 1)) % vertexCount]; b2Vec2 v0 = vertices[i]; - b2Vec2 v_next = vertices[(i + (count + 1)) % count]; + b2Vec2 v_next = vertices[(i + (vertexCount + 1)) % vertexCount]; b2Vec2 n1 = b2Normalize(b2CrossVS(b2Sub(v0, v_prev), 1.0f)); b2Vec2 n2 = b2Normalize(b2CrossVS(b2Sub(v_next, v0), 1.0f)); diff --git a/samples/draw.h b/samples/draw.h index e3f45ee4..86df2686 100644 --- a/samples/draw.h +++ b/samples/draw.h @@ -36,7 +36,7 @@ class Draw void DrawSolidPolygon(const b2Vec2* vertices, int32_t vertexCount, b2Color color); - void DrawRoundedPolygon(const b2Vec2* vertices, int32_t vertexCount, float radius, b2Color fillColor, b2Color outlineColor); + void DrawRoundedPolygon(const b2Vec2* vertices, int32_t vertexCount, float radius, b2Color color); void DrawCircle(b2Vec2 center, float radius, b2Color color); diff --git a/samples/main.cpp b/samples/main.cpp index 75623060..bb046e8e 100644 --- a/samples/main.cpp +++ b/samples/main.cpp @@ -433,6 +433,7 @@ static void UpdateUI() ImGui::Checkbox("Shapes", &s_settings.drawShapes); ImGui::Checkbox("Joints", &s_settings.drawJoints); + ImGui::Checkbox("Joint Extras", &s_settings.drawJointExtras); ImGui::Checkbox("AABBs", &s_settings.drawAABBs); ImGui::Checkbox("Contact Points", &s_settings.drawContactPoints); ImGui::Checkbox("Contact Normals", &s_settings.drawContactNormals); diff --git a/samples/sample.cpp b/samples/sample.cpp index 3a74d78f..0063a1b0 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -249,6 +249,7 @@ void Sample::Step(Settings& settings) g_draw.m_debugDraw.drawShapes = settings.drawShapes; g_draw.m_debugDraw.drawJoints = settings.drawJoints; + g_draw.m_debugDraw.drawJointExtras = settings.drawJointExtras; g_draw.m_debugDraw.drawAABBs = settings.drawAABBs; g_draw.m_debugDraw.drawMass = settings.drawMass; g_draw.m_debugDraw.drawContacts = settings.drawContactPoints; diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp index 3605d266..08ba0beb 100644 --- a/samples/sample_benchmark.cpp +++ b/samples/sample_benchmark.cpp @@ -1142,12 +1142,31 @@ class BenchmarkJointGrid : public Sample } free(bodies); + + m_gravity = 10.0f; + } + + void UpdateUI() override + { + ImGui::SetNextWindowPos(ImVec2(10.0f, 500.0f), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(240.0f, 100.0f)); + ImGui::Begin("Joint Grid", nullptr, ImGuiWindowFlags_NoResize); + + if (ImGui::SliderFloat("gravity", &m_gravity, 0.0f, 20.0f, "%.1f")) + { + b2World_SetGravity(m_worldId, {0.0f, -m_gravity}); + } + + ImGui::End(); } + static Sample* Create(Settings& settings) { return new BenchmarkJointGrid(settings); } + + float m_gravity; }; static int benchmarkJointGridIndex = RegisterSample("Benchmark", "Joint Grid", BenchmarkJointGrid::Create); diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp index d288ac32..e13ea20d 100644 --- a/samples/sample_collision.cpp +++ b/samples/sample_collision.cpp @@ -1807,7 +1807,7 @@ class RayCastWorld : public Sample } else { - g_draw.DrawRoundedPolygon(points, box.count, box.radius, yellow, yellow); + g_draw.DrawRoundedPolygon(points, box.count, box.radius, yellow); } } } @@ -1840,7 +1840,7 @@ class RayCastWorld : public Sample } else { - g_draw.DrawRoundedPolygon(points, box.count, box.radius, yellow, yellow); + g_draw.DrawRoundedPolygon(points, box.count, box.radius, yellow); } } } @@ -2434,8 +2434,8 @@ class Manifold : public Sample b2Color color1 = {0.3f, 0.8f, 0.6f, 1.0f}; b2Color color2 = {0.8f, 0.6f, 0.3f, 1.0f}; - b2Color fillColor1 = {0.5f * color1.r, 0.5f * color1.g, 0.5f * color1.b, 0.5f}; - b2Color fillColor2 = {0.5f * color2.r, 0.5f * color2.g, 0.5f * color2.b, 0.5f}; + //b2Color fillColor1 = {0.5f * color1.r, 0.5f * color1.g, 0.5f * color1.b, 0.5f}; + //b2Color fillColor2 = {0.5f * color2.r, 0.5f * color2.g, 0.5f * color2.b, 0.5f}; b2Color dim1 = {0.5f * color1.r, 0.5f * color1.g, 0.5f * color1.b, 1.0f}; @@ -2546,7 +2546,7 @@ class Manifold : public Sample { vertices[i] = b2TransformPoint(xf1, box.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, box.count, m_round, fillColor1, color1); + g_draw.DrawRoundedPolygon(vertices, box.count, m_round, color1); b2Vec2 c2 = b2TransformPoint(xf2, circle.center); b2Vec2 axis2 = b2RotateVector(xf2.q, {1.0f, 0.0f}); @@ -2687,7 +2687,7 @@ class Manifold : public Sample { vertices[i] = b2TransformPoint(xf2, rox.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color2); DrawManifold(&m); @@ -2711,13 +2711,13 @@ class Manifold : public Sample { vertices[i] = b2TransformPoint(xf1, rox.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor1, color1); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color1); for (int i = 0; i < rox.count; ++i) { vertices[i] = b2TransformPoint(xf2, rox.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color2); DrawManifold(&m); @@ -2748,7 +2748,7 @@ class Manifold : public Sample if (m_round > 0.0f) { - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color2); } else { @@ -2775,13 +2775,13 @@ class Manifold : public Sample { vertices[i] = b2TransformPoint(xf1, wox.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, wox.count, wox.radius, fillColor1, color1); + g_draw.DrawRoundedPolygon(vertices, wox.count, wox.radius, color1); for (int i = 0; i < wox.count; ++i) { vertices[i] = b2TransformPoint(xf2, wox.vertices[i]); } - g_draw.DrawRoundedPolygon(vertices, wox.count, wox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, wox.count, wox.radius, color2); DrawManifold(&m); @@ -2866,7 +2866,7 @@ class Manifold : public Sample if (m_round > 0.0f) { - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color2); g_draw.DrawPolygon(vertices, rox.count, color2); } else @@ -3218,7 +3218,7 @@ class SmoothManifold : public Sample if (m_round > 0.0f) { - g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, fillColor2, color2); + g_draw.DrawRoundedPolygon(vertices, rox.count, rox.radius, color2); } else { diff --git a/samples/sample_joints.cpp b/samples/sample_joints.cpp index 9ea207df..a37e07f0 100644 --- a/samples/sample_joints.cpp +++ b/samples/sample_joints.cpp @@ -339,7 +339,7 @@ static int sampleMotorJoint = RegisterSample("Joints", "Motor Joint", MotorJoint class RevoluteJoint : public Sample { public: - RevoluteJoint(Settings& settings) + explicit RevoluteJoint(Settings& settings) : Sample(settings) { if (settings.restart == false) @@ -386,8 +386,9 @@ class RevoluteJoint : public Sample jointDef.motorSpeed = m_motorSpeed; jointDef.maxMotorTorque = m_motorTorque; jointDef.enableMotor = m_enableMotor; - jointDef.lowerAngle = -0.25f * b2_pi; - jointDef.upperAngle = 0.5f * b2_pi; + jointDef.referenceAngle = 0.5f * b2_pi; + jointDef.lowerAngle = -0.5f * b2_pi; + jointDef.upperAngle = 0.75f * b2_pi; jointDef.enableLimit = m_enableLimit; m_jointId1 = b2CreateRevoluteJoint(m_worldId, &jointDef); @@ -471,6 +472,10 @@ class RevoluteJoint : public Sample { Sample::Step(settings); + float angle1 = b2RevoluteJoint_GetAngle(m_jointId1); + g_draw.DrawString(5, m_textLine, "Angle (Deg) 1 = %2.1f", angle1); + m_textLine += m_textIncrement; + float torque1 = b2RevoluteJoint_GetMotorTorque(m_jointId1); g_draw.DrawString(5, m_textLine, "Motor Torque 1 = %4.1f", torque1); m_textLine += m_textIncrement; diff --git a/samples/settings.h b/samples/settings.h index 67b7a9b3..923d5d46 100644 --- a/samples/settings.h +++ b/samples/settings.h @@ -17,6 +17,7 @@ struct Settings int workerCount = 1; bool drawShapes = true; bool drawJoints = true; + bool drawJointExtras = false; bool drawAABBs = false; bool drawContactPoints = false; bool drawContactNormals = false; diff --git a/src/joint.c b/src/joint.c index 55394133..4e8fc9fc 100644 --- a/src/joint.c +++ b/src/joint.c @@ -461,7 +461,7 @@ b2JointId b2CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def b2RevoluteJoint empty = {0}; joint->revoluteJoint = empty; - joint->revoluteJoint.referenceAngle = def->referenceAngle; + joint->revoluteJoint.referenceAngle = B2_CLAMP(def->referenceAngle, -b2_pi, b2_pi); joint->revoluteJoint.linearImpulse = b2Vec2_zero; joint->revoluteJoint.axialMass = 0.0f; joint->revoluteJoint.motorImpulse = 0.0f; @@ -469,6 +469,8 @@ b2JointId b2CreateRevoluteJoint(b2WorldId worldId, const b2RevoluteJointDef* def joint->revoluteJoint.upperImpulse = 0.0f; joint->revoluteJoint.lowerAngle = B2_MIN(def->lowerAngle, def->upperAngle); joint->revoluteJoint.upperAngle = B2_MAX(def->lowerAngle, def->upperAngle); + joint->revoluteJoint.lowerAngle = B2_CLAMP(joint->revoluteJoint.lowerAngle, -b2_pi, b2_pi); + joint->revoluteJoint.upperAngle = B2_CLAMP(joint->revoluteJoint.upperAngle, -b2_pi, b2_pi); joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; joint->revoluteJoint.motorSpeed = def->motorSpeed; joint->revoluteJoint.enableLimit = def->enableLimit; diff --git a/src/revolute_joint.c b/src/revolute_joint.c index 3811d9f6..be6e07ac 100644 --- a/src/revolute_joint.c +++ b/src/revolute_joint.c @@ -16,6 +16,18 @@ #include +float b2RevoluteJoint_GetAngle(b2JointId jointId) +{ + b2World* world = b2GetWorld(jointId.world0); + b2JointSim* jointSim = b2GetJointSimCheckType(jointId, b2_revoluteJoint); + b2Transform transformA = b2GetBodyTransform(world, jointSim->bodyIdA); + b2Transform transformB = b2GetBodyTransform(world, jointSim->bodyIdB); + + float angle = b2RelativeAngle(transformB.q, transformA.q) - jointSim->revoluteJoint.referenceAngle; + angle = b2UnwindAngle(angle); + return angle; +} + void b2RevoluteJoint_EnableLimit(b2JointId jointId, bool enableLimit) { b2JointSim* joint = b2GetJointSimCheckType(jointId, b2_revoluteJoint); @@ -203,6 +215,7 @@ void b2PrepareRevoluteJoint(b2JointSim* base, b2StepContext* context) joint->anchorB = b2RotateVector(bodySimB->transform.q, b2Sub(base->localOriginAnchorB, bodySimB->localCenter)); joint->deltaCenter = b2Sub(bodySimB->center, bodySimA->center); joint->deltaAngle = b2RelativeAngle(bodySimB->transform.q, bodySimA->transform.q) - joint->referenceAngle; + joint->deltaAngle = b2UnwindAngle(joint->deltaAngle); float k = iA + iB; joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; @@ -286,6 +299,7 @@ void b2SolveRevoluteJoint(b2JointSim* base, b2StepContext* context, bool useBias if (joint->enableLimit && fixedRotation == false) { float jointAngle = b2RelativeAngle(stateB->deltaRotation, stateA->deltaRotation) + joint->deltaAngle; + jointAngle = b2UnwindAngle(jointAngle); // Lower limit { @@ -435,26 +449,41 @@ void b2DrawRevoluteJoint(b2DebugDraw* draw, b2JointSim* base, b2Transform transf b2Color c1 = {0.7f, 0.7f, 0.7f, 1.0f}; b2Color c2 = {0.3f, 0.9f, 0.3f, 1.0f}; b2Color c3 = {0.9f, 0.3f, 0.3f, 1.0f}; - b2Color c4 = {0.3f, 0.3f, 0.9f, 1.0f}; - b2Color c5 = {0.4f, 0.4f, 0.4f, 1.0f}; + b2Color c4 = {0.4f, 0.4f, 0.4f, 1.0f}; + b2Color c5 = {0.3f, 0.3f, 0.9f, 1.0f}; + const float L = drawSize; draw->DrawPoint(pA, 5.0f, c4, draw->context); draw->DrawPoint(pB, 5.0f, c5, draw->context); + draw->DrawCircle(pB, L, c1, draw->context); - float angle = b2RelativeAngle(transformB.q, transformA.q) - joint->referenceAngle; + float angle = b2RelativeAngle(transformB.q, transformA.q); - const float L = drawSize; b2Vec2 r = {L * cosf(angle), L * sinf(angle)}; - draw->DrawSegment(pB, b2Add(pB, r), c1, draw->context); - draw->DrawCircle(pB, L, c1, draw->context); + b2Vec2 pC = b2Add(pB, r); + draw->DrawSegment(pB, pC, c1, draw->context); + + if (draw->drawJointExtras) + { + float jointAngle = b2UnwindAngle(angle - joint->referenceAngle); + char buffer[32]; + snprintf(buffer, 32, " %.1f deg", 180.0f * jointAngle / b2_pi); + draw->DrawString(pC, buffer, draw->context); + } + + float lowerAngle = joint->lowerAngle + joint->referenceAngle; + float upperAngle = joint->upperAngle + joint->referenceAngle; if (joint->enableLimit) { - b2Vec2 rlo = {L * cosf(joint->lowerAngle), L * sinf(joint->lowerAngle)}; - b2Vec2 rhi = {L * cosf(joint->upperAngle), L * sinf(joint->upperAngle)}; + b2Vec2 rlo = {L * cosf(lowerAngle), L * sinf(lowerAngle)}; + b2Vec2 rhi = {L * cosf(upperAngle), L * sinf(upperAngle)}; draw->DrawSegment(pB, b2Add(pB, rlo), c2, draw->context); draw->DrawSegment(pB, b2Add(pB, rhi), c3, draw->context); + + b2Vec2 ref = (b2Vec2){L * cosf(joint->referenceAngle), L * sinf(joint->referenceAngle)}; + draw->DrawSegment(pB, b2Add(pB, ref), c5, draw->context); } b2Color color = {0.5f, 0.8f, 0.8f, 1.0f}; diff --git a/src/solver.c b/src/solver.c index bb4d016f..a2fbeb2e 100644 --- a/src/solver.c +++ b/src/solver.c @@ -1854,7 +1854,7 @@ void b2Solve(b2World* world, b2StepContext* stepContext) // Collect split island candidate for the next time step. No need to split if sleeping is disabled. B2_ASSERT(world->splitIslandId == B2_NULL_INDEX); float splitSleepTimer = 0.0f; - for (int i = 1; i < world->workerCount; ++i) + for (int i = 0; i < world->workerCount; ++i) { b2TaskContext* taskContext = world->taskContextArray + i; if (taskContext->splitIslandId != B2_NULL_INDEX && taskContext->splitSleepTime > splitSleepTimer) diff --git a/src/world.c b/src/world.c index 3aa81a1b..44413266 100644 --- a/src/world.c +++ b/src/world.c @@ -779,8 +779,6 @@ static void b2DrawShape(b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2Col case b2_polygonShape: { - b2Color fillColor = {0.5f * color.r, 0.5f * color.g, 0.5f * color.b, 0.5f}; - b2Polygon* poly = &shape->polygon; int count = poly->count; B2_ASSERT(count <= b2_maxPolygonVertices); @@ -793,7 +791,7 @@ static void b2DrawShape(b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2Col if (poly->radius > 0.0f) { - draw->DrawRoundedPolygon(vertices, count, poly->radius, fillColor, color, draw->context); + draw->DrawRoundedPolygon(vertices, count, poly->radius, color, draw->context); } else { @@ -885,7 +883,7 @@ void b2World_Draw(b2WorldId worldId, b2DebugDraw* draw) } else if (body->type == b2_kinematicBody) { - color = (b2Color){0.5f, 0.5f, 0.9f, 1.0f}; + color = b2MakeColor(b2_colorRoyalBlue); } else if (isAwake) {