diff --git a/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs b/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs index 868a4a5..63b445b 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs @@ -21,6 +21,8 @@ namespace Box2D.NET.Samples.Samples.Collisions; +// This sample shows how to use the ray and shape cast functions on a b2World. This +// sample is configured to ignore initial overlap. public class CastWorld : Sample { private static readonly int SampleRayCastWorld = SampleFactory.Shared.RegisterSample("Collision", "Cast World", Create); @@ -383,7 +385,7 @@ public override void Step() // This version doesn't have a callback, but it doesn't skip the ignored shape B2RayResult result = b2World_CastRayClosest(m_worldId, m_rayStart, rayTranslation, b2DefaultQueryFilter()); - if (result.hit == true) + if (result.hit == true && result.fraction > 0.0f) { B2Vec2 c = b2MulAdd(m_rayStart, result.fraction, rayTranslation); m_draw.DrawPoint(result.point, 5.0f, color1); @@ -453,7 +455,7 @@ public override void Step() B2Vec2 n = context.normals[i]; m_draw.DrawPoint(p, 5.0f, colors[i]); m_draw.DrawSegment(m_rayStart, c, color2); - B2Vec2 head = b2MulAdd(p, 0.5f, n); + B2Vec2 head = b2MulAdd(p, 1.0f, n); m_draw.DrawSegment(p, head, color3); B2Vec2 t = b2MulSV(context.fractions[i], rayTranslation); @@ -505,16 +507,11 @@ public override void Draw(Settings settings) base.Draw(settings); DrawTextLine("Click left mouse button and drag to modify ray cast"); - DrawTextLine("Shape 7 is intentionally ignored by the ray"); - - - if (m_simple) { DrawTextLine("Simple closest point ray cast"); - } else { @@ -540,8 +537,6 @@ public override void Draw(Settings settings) B2_ASSERT(false); break; } - - } if (B2_IS_NON_NULL(m_bodyIds[m_ignoreIndex])) @@ -559,7 +554,9 @@ static float RayCastClosestCallback(B2ShapeId shapeId, B2Vec2 point, B2Vec2 norm CastContext rayContext = (CastContext)context; ShapeUserData userData = (ShapeUserData)b2Shape_GetUserData(shapeId); - if (userData != null && userData.ignore) + + // Ignore a specific shape. Also ignore initial overlap. + if ((userData != null && userData.ignore) || fraction == 0.0f) { // By returning -1, we instruct the calling code to ignore this shape and // continue the ray-cast to the next shape. @@ -585,7 +582,9 @@ static float RayCastAnyCallback(B2ShapeId shapeId, B2Vec2 point, B2Vec2 normal, CastContext rayContext = (CastContext)context; ShapeUserData userData = (ShapeUserData)b2Shape_GetUserData(shapeId); - if (userData != null && userData.ignore) + + // Ignore a specific shape. Also ignore initial overlap. + if ((userData != null && userData.ignore) || fraction == 0.0f) { // By returning -1, we instruct the calling code to ignore this shape and // continue the ray-cast to the next shape. @@ -613,7 +612,9 @@ static float RayCastMultipleCallback(B2ShapeId shapeId, B2Vec2 point, B2Vec2 nor CastContext rayContext = (CastContext)context; ShapeUserData userData = (ShapeUserData)b2Shape_GetUserData(shapeId); - if (userData != null && userData.ignore) + + // Ignore a specific shape. Also ignore initial overlap. + if ((userData != null && userData.ignore) || fraction == 0.0f) { // By returning -1, we instruct the calling code to ignore this shape and // continue the ray-cast to the next shape. @@ -639,13 +640,15 @@ static float RayCastMultipleCallback(B2ShapeId shapeId, B2Vec2 point, B2Vec2 nor return 1.0f; } -// This ray cast collects multiple hits along the ray and sorts them. + // This ray cast collects multiple hits along the ray and sorts them. static float RayCastSortedCallback(B2ShapeId shapeId, B2Vec2 point, B2Vec2 normal, float fraction, object context) { CastContext rayContext = (CastContext)context; ShapeUserData userData = (ShapeUserData)b2Shape_GetUserData(shapeId); - if (userData != null && userData.ignore) + + // Ignore a specific shape. Also ignore initial overlap. + if ((userData != null && userData.ignore) || fraction == 0.0f) { // By returning -1, we instruct the calling code to ignore this shape and // continue the ray-cast to the next shape. diff --git a/src/Box2D.NET.Samples/Samples/Collisions/RayCast.cs b/src/Box2D.NET.Samples/Samples/Collisions/RayCast.cs index a9f8b31..ae3aac9 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/RayCast.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/RayCast.cs @@ -8,6 +8,7 @@ using static Box2D.NET.B2Hulls; using static Box2D.NET.B2Geometries; using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Diagnostics; namespace Box2D.NET.Samples.Samples.Collisions; @@ -182,19 +183,24 @@ void DrawRay(ref B2CastOutput output) if (output.hit) { - B2Vec2 p = b2MulAdd(p1, output.fraction, d); - m_draw.DrawSegment(p1, p, B2HexColor.b2_colorWhite); - m_draw.DrawPoint(p1, 5.0f, B2HexColor.b2_colorGreen); - m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorWhite); + B2Vec2 p; - B2Vec2 n = b2MulAdd(p, 1.0f, output.normal); - m_draw.DrawSegment(p, n, B2HexColor.b2_colorViolet); + if (output.fraction == 0.0f) + { + B2_ASSERT(output.normal.X == 0.0f && output.normal.Y == 0.0f); + p = output.point; + m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorPeru); + } + else + { + p = b2MulAdd(p1, output.fraction, d); + m_draw.DrawSegment(p1, p, B2HexColor.b2_colorWhite); + m_draw.DrawPoint(p1, 5.0f, B2HexColor.b2_colorGreen); + m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorWhite); - // if (m_rayRadius > 0.0f) - //{ - // m_context.g_draw.DrawCircle(p1, m_rayRadius, b2HexColor.b2_colorGreen); - // m_context.g_draw.DrawCircle(p, m_rayRadius, b2HexColor.b2_colorRed); - // } + B2Vec2 n = b2MulAdd(p, 1.0f, output.normal); + m_draw.DrawSegment(p, n, B2HexColor.b2_colorViolet); + } if (m_showFraction) { @@ -207,12 +213,6 @@ void DrawRay(ref B2CastOutput output) m_draw.DrawSegment(p1, p2, B2HexColor.b2_colorWhite); m_draw.DrawPoint(p1, 5.0f, B2HexColor.b2_colorGreen); m_draw.DrawPoint(p2, 5.0f, B2HexColor.b2_colorRed); - - // if (m_rayRadius > 0.0f) - //{ - // m_context.g_draw.DrawCircle(p1, m_rayRadius, b2HexColor.b2_colorGreen); - // m_context.g_draw.DrawCircle(p2, m_rayRadius, b2HexColor.b2_colorRed); - // } } } diff --git a/src/Box2D.NET.Samples/Samples/Collisions/ShapeCast.cs b/src/Box2D.NET.Samples/Samples/Collisions/ShapeCast.cs index 3698f86..0053fa5 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/ShapeCast.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/ShapeCast.cs @@ -113,8 +113,8 @@ public ShapeCast(SampleContext context) : base(context) m_box = b2MakePolygon( &hull, 0.0f ); } #endif - - m_transform = new B2Transform( new B2Vec2( -0.6f, 0.0f ), b2Rot_identity ); + + m_transform = new B2Transform(new B2Vec2(-0.6f, 0.0f), b2Rot_identity); m_translation = new B2Vec2(2.0f, 0.0f); m_angle = 0.0f; m_startPoint = new B2Vec2(0.0f, 0.0f); @@ -376,7 +376,7 @@ public override void Draw(Settings settings) { base.Draw(settings); - DrawTextLine($"hit = {output.hit}, iterations = {output.iterations}, lambda = {output.fraction}, distance = {_distanceOutput.distance}"); + DrawTextLine($"hit = {output.hit}, iterations = {output.iterations}, fraction = {output.fraction}, distance = {_distanceOutput.distance}"); DrawShape(m_typeA, b2Transform_identity, m_radiusA, B2HexColor.b2_colorCyan); DrawShape(m_typeB, m_transform, m_radiusB, B2HexColor.b2_colorLightGreen); @@ -386,8 +386,17 @@ public override void Draw(Settings settings) if (output.hit) { DrawShape(m_typeB, inputTransform, m_radiusB, B2HexColor.b2_colorPlum); - m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorWhite); - m_draw.DrawSegment(output.point, output.point + 0.5f * output.normal, B2HexColor.b2_colorYellow); + + + if (output.fraction > 0.0f) + { + m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorWhite); + m_draw.DrawSegment(output.point, output.point + 0.5f * output.normal, B2HexColor.b2_colorYellow); + } + else + { + m_draw.DrawPoint(output.point, 5.0f, B2HexColor.b2_colorPeru); + } } if (m_showIndices) diff --git a/src/Box2D.NET.Samples/Samples/Collisions/SmoothManifold.cs b/src/Box2D.NET.Samples/Samples/Collisions/SmoothManifold.cs index 81b4b20..b921892 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/SmoothManifold.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/SmoothManifold.cs @@ -256,8 +256,10 @@ void DrawManifold(ref B2Manifold manifold) } } - public override void Step() + public override void Draw(Settings settings) { + base.Draw(settings); + B2HexColor color1 = B2HexColor.b2_colorYellow; B2HexColor color2 = B2HexColor.b2_colorMagenta; diff --git a/src/Box2D.NET.Samples/Samples/Joints/Door.cs b/src/Box2D.NET.Samples/Samples/Joints/Door.cs new file mode 100644 index 0000000..b07f1e6 --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Joints/Door.cs @@ -0,0 +1,129 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System.Numerics; +using ImGuiNET; +using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.B2Ids; +using static Box2D.NET.B2RevoluteJoints; + +namespace Box2D.NET.Samples.Samples.Joints; + +// A top down door +public class Door : Sample +{ + private static readonly int SampleDoor = SampleFactory.Shared.RegisterSample("Joints", "Door", Create); + + private B2BodyId m_doorId; + private B2JointId m_jointId; + private float m_impulse; + private float m_translationError; + private bool m_enableLimit; + + private static Sample Create(SampleContext context) + { + return new Door(context); + } + + public Door(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 0.0f); + m_context.camera.m_zoom = 4.0f; + } + + B2BodyId groundId = b2_nullBodyId; + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = new B2Vec2(0.0f, 0.0f); + groundId = b2CreateBody(m_worldId, ref bodyDef); + } + + m_enableLimit = true; + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(0.0f, 1.5f); + bodyDef.gravityScale = 0.0f; + + m_doorId = b2CreateBody(m_worldId, ref bodyDef); + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1000.0f; + + B2Polygon box = b2MakeBox(0.1f, 1.5f); + b2CreatePolygonShape(m_doorId, ref shapeDef, ref box); + + B2Vec2 pivot = new B2Vec2(0.0f, 0.0f); + B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_doorId; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.targetAngle = 0.0f; + jointDef.enableSpring = true; + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.5f; + jointDef.motorSpeed = 0.0f; + jointDef.maxMotorTorque = 0.0f; + jointDef.enableMotor = false; + jointDef.referenceAngle = 0.0f; + jointDef.lowerAngle = -0.5f * B2_PI; + jointDef.upperAngle = 0.5f * B2_PI; + jointDef.enableLimit = m_enableLimit; + + m_jointId = b2CreateRevoluteJoint(m_worldId, ref jointDef); + } + + m_impulse = 50000.0f; + m_translationError = 0.0f; + } + + public override void UpdateGui() + { + float height = 220.0f; + ImGui.SetNextWindowPos(new Vector2(10.0f, m_context.camera.m_height - height - 50.0f), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(240.0f, height)); + + ImGui.Begin("Door", ImGuiWindowFlags.NoResize); + + if (ImGui.Button("impulse")) + { + B2Vec2 p = b2Body_GetWorldPoint(m_doorId, new B2Vec2(0.0f, 1.5f)); + b2Body_ApplyLinearImpulse(m_doorId, new B2Vec2(m_impulse, 0.0f), p, true); + m_translationError = 0.0f; + } + + ImGui.SliderFloat("magnitude", ref m_impulse, 1000.0f, 100000.0f, "%.0f"); + + if (ImGui.Checkbox("limit", ref m_enableLimit)) + { + b2RevoluteJoint_EnableLimit(m_jointId, m_enableLimit); + } + + ImGui.End(); + } + + public override void Draw(Settings settings) + { + base.Draw(settings); + + B2Vec2 p = b2Body_GetWorldPoint(m_doorId, new B2Vec2(0.0f, 1.5f)); + m_draw.DrawPoint(p, 5.0f, B2HexColor.b2_colorDarkKhaki); + + m_draw.DrawTransform(b2Transform_identity); + + float translationError = b2Joint_GetLinearSeparation(m_jointId); + m_translationError = b2MaxFloat(m_translationError, translationError); + + DrawTextLine($"translation error = {m_translationError}"); + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Joints/JointSeparation.cs b/src/Box2D.NET.Samples/Samples/Joints/JointSeparation.cs new file mode 100644 index 0000000..520155e --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Joints/JointSeparation.cs @@ -0,0 +1,221 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System.Numerics; +using ImGuiNET; +using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.B2Diagnostics; +using static Box2D.NET.B2Worlds; +using static Box2D.NET.B2Ids; + +namespace Box2D.NET.Samples.Samples.Joints; + +// This sample shows how to measure joint separation. This is the unresolved constraint error. +public class JointSeparation : Sample +{ + private static readonly int SampleJointSeparation = SampleFactory.Shared.RegisterSample("Joints", "Separation", Create); + + private const int e_count = 5; + + private B2BodyId[] m_bodyIds = new B2BodyId[e_count]; + private B2JointId[] m_jointIds = new B2JointId[e_count]; + private float m_impulse; + + private static Sample Create(SampleContext context) + { + return new JointSeparation(context); + } + + public JointSeparation(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 8.0f); + m_context.camera.m_zoom = 25.0f; + } + + B2BodyDef bodyDef = b2DefaultBodyDef(); + B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef); + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + B2Segment segment = new B2Segment(new B2Vec2(-40.0f, 0.0f), new B2Vec2(40.0f, 0.0f)); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + + B2Vec2 position = new B2Vec2(-20.0f, 10.0f); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.enableSleep = false; + + B2Polygon box = b2MakeBox(1.0f, 1.0f); + + int index = 0; + + // distance joint + { + B2_ASSERT(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(m_bodyIds[index], ref shapeDef, ref box); + + float length = 2.0f; + B2Vec2 pivot1 = new B2Vec2(position.X, position.Y + 1.0f + length); + B2Vec2 pivot2 = new B2Vec2(position.X, position.Y + 1.0f); + B2DistanceJointDef jointDef = b2DefaultDistanceJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot1); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot2); + jointDef.length = length; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateDistanceJoint(m_worldId, ref jointDef); + } + + position.X += 10.0f; + ++index; + + // prismatic joint + { + B2_ASSERT(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(m_bodyIds[index], ref shapeDef, ref box); + + B2Vec2 pivot = new B2Vec2(position.X - 1.0f, position.Y); + B2PrismaticJointDef jointDef = b2DefaultPrismaticJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, new B2Vec2(1.0f, 0.0f)); + jointDef.collideConnected = true; + m_jointIds[index] = b2CreatePrismaticJoint(m_worldId, jointDef); + } + + position.X += 10.0f; + ++index; + + // revolute joint + { + B2_ASSERT(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(m_bodyIds[index], ref shapeDef, ref box); + + B2Vec2 pivot = new B2Vec2(position.X - 1.0f, position.Y); + B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateRevoluteJoint(m_worldId, ref jointDef); + } + + position.X += 10.0f; + ++index; + + // weld joint + { + B2_ASSERT(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(m_bodyIds[index], ref shapeDef, ref box); + + B2Vec2 pivot = new B2Vec2(position.X - 1.0f, position.Y); + B2WeldJointDef jointDef = b2DefaultWeldJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateWeldJoint(m_worldId, ref jointDef); + } + + position.X += 10.0f; + ++index; + + // wheel joint + { + B2_ASSERT(index < e_count); + + bodyDef.position = position; + m_bodyIds[index] = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(m_bodyIds[index], ref shapeDef, ref box); + + B2Vec2 pivot = new B2Vec2(position.X - 1.0f, position.Y); + B2WheelJointDef jointDef = b2DefaultWheelJointDef(); + jointDef.bodyIdA = groundId; + jointDef.bodyIdB = m_bodyIds[index]; + jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); + jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.localAxisA = b2Body_GetLocalVector(jointDef.bodyIdA, new B2Vec2(1.0f, 0.0f)); + jointDef.hertz = 1.0f; + jointDef.dampingRatio = 0.7f; + jointDef.lowerTranslation = -1.0f; + jointDef.upperTranslation = 1.0f; + jointDef.enableLimit = true; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 10.0f; + jointDef.motorSpeed = 1.0f; + jointDef.collideConnected = true; + m_jointIds[index] = b2CreateWheelJoint(m_worldId, ref jointDef); + } + + m_impulse = 500.0f; + } + + public override void UpdateGui() + { + float height = 120.0f; + ImGui.SetNextWindowPos(new Vector2(10.0f, m_context.camera.m_height - height - 50.0f), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(260.0f, height)); + + ImGui.Begin("Joint Separation", ImGuiWindowFlags.NoResize); + + B2Vec2 gravity = b2World_GetGravity(m_worldId); + if (ImGui.SliderFloat("gravity", ref gravity.Y, -500.0f, 500.0f, "%.0f")) + { + b2World_SetGravity(m_worldId, gravity); + } + + if (ImGui.Button("impulse")) + { + for (int i = 0; i < e_count; ++i) + { + B2Vec2 p = b2Body_GetWorldPoint(m_bodyIds[i], new B2Vec2(1.0f, 1.0f)); + b2Body_ApplyLinearImpulse(m_bodyIds[i], new B2Vec2(m_impulse, -m_impulse), p, true); + } + } + + ImGui.SliderFloat("magnitude", ref m_impulse, 0.0f, 1000.0f, "%.0f"); + + ImGui.End(); + } + + public override void Draw(Settings settings) + { + base.Draw(settings); + + for (int i = 0; i < e_count; ++i) + { + if (B2_IS_NULL(m_jointIds[i])) + { + continue; + } + + float linear = b2Joint_GetLinearSeparation(m_jointIds[i]); + float angular = b2Joint_GetAngularSeparation(m_jointIds[i]); + B2Vec2 point = b2Joint_GetLocalAnchorA(m_jointIds[i]); + m_draw.DrawString(point, $"{linear:F2} m, {180.0f * angular / B2_PI:F1} deg"); + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs index 270db0f..ffa51ba 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs @@ -101,12 +101,12 @@ public override void UpdateGui() { } - if (ImGui.SliderFloat("Max Force", ref m_maxForce, 0.0f, 1000.0f, "%.0f")) + if (ImGui.SliderFloat("Max Force", ref m_maxForce, 0.0f, 10000.0f, "%.0f")) { b2MotorJoint_SetMaxForce(m_jointId, m_maxForce); } - if (ImGui.SliderFloat("Max Torque", ref m_maxTorque, 0.0f, 1000.0f, "%.0f")) + if (ImGui.SliderFloat("Max Torque", ref m_maxTorque, 0.0f, 10000.0f, "%.0f")) { b2MotorJoint_SetMaxTorque(m_jointId, m_maxTorque); } @@ -136,7 +136,7 @@ public override void Step() linearOffset.X = 6.0f * MathF.Sin(2.0f * m_time); linearOffset.Y = 8.0f + 4.0f * MathF.Sin(1.0f * m_time); - float angularOffset = B2_PI * MathF.Sin(-0.5f * m_time); + float angularOffset = 2.0f * m_time; b2MotorJoint_SetLinearOffset(m_jointId, linearOffset); b2MotorJoint_SetAngularOffset(m_jointId, angularOffset); diff --git a/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs index ed57ef8..574bf8b 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs @@ -26,7 +26,7 @@ public class RevoluteJoint : Sample private float m_motorTorque; private float m_hertz; private float m_dampingRatio; - private float m_targetAngle; + private float m_targetDegrees; private bool m_enableSpring; private bool m_enableMotor; private bool m_enableLimit; @@ -59,9 +59,9 @@ public RevoluteJoint(SampleContext context) : base(context) m_enableSpring = false; m_enableLimit = true; m_enableMotor = false; - m_hertz = 1.0f; + m_hertz = 2.0f; m_dampingRatio = 0.5f; - m_targetAngle = 0.0f; + m_targetDegrees = 45.0f; m_motorSpeed = 1.0f; m_motorTorque = 1000.0f; @@ -83,6 +83,7 @@ public RevoluteJoint(SampleContext context) : base(context) jointDef.bodyIdB = bodyId; jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); + jointDef.targetAngle = B2_PI * m_targetDegrees / 180.0f; jointDef.enableSpring = m_enableSpring; jointDef.hertz = m_hertz; jointDef.dampingRatio = m_dampingRatio; @@ -196,12 +197,12 @@ public override void UpdateGui() b2RevoluteJoint_SetSpringDampingRatio(m_jointId1, m_dampingRatio); b2Joint_WakeBodies(m_jointId1); } - - - if ( ImGui.SliderFloat( "Degrees", ref m_targetAngle, -180.0f, 180.0f, "%.0f" ) ) + + + if (ImGui.SliderFloat("Degrees", ref m_targetDegrees, -180.0f, 180.0f, "%.0f")) { - b2RevoluteJoint_SetTargetAngle( m_jointId1, B2_PI * m_targetAngle / 180.0f ); - b2Joint_WakeBodies( m_jointId1 ); + b2RevoluteJoint_SetTargetAngle(m_jointId1, B2_PI * m_targetDegrees / 180.0f); + b2Joint_WakeBodies(m_jointId1); } } @@ -211,17 +212,16 @@ public override void UpdateGui() public override void Draw(Settings settings) { base.Draw(settings); - + float angle1 = b2RevoluteJoint_GetAngle(m_jointId1); DrawTextLine($"Angle (Deg) 1 = {angle1:F1}"); - + float torque1 = b2RevoluteJoint_GetMotorTorque(m_jointId1); DrawTextLine($"Motor Torque 1 = {torque1:F1}"); - + float torque2 = b2RevoluteJoint_GetMotorTorque(m_jointId2); DrawTextLine($"Motor Torque 2 = {torque2:F1}"); - } } \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Joints/ScaleRagdoll.cs b/src/Box2D.NET.Samples/Samples/Joints/ScaleRagdoll.cs new file mode 100644 index 0000000..93ad990 --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Joints/ScaleRagdoll.cs @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System.Numerics; +using Box2D.NET.Shared; +using ImGuiNET; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.Shared.Humans; + +namespace Box2D.NET.Samples.Samples.Joints; + +public class ScaleRagdoll : Sample +{ + private static readonly int SampleScaleRagdoll = SampleFactory.Shared.RegisterSample("Joints", "Scale Ragdoll", Create); + + private Human m_human; + private float m_scale; + + private static Sample Create(SampleContext context) + { + return new ScaleRagdoll(context); + } + + public ScaleRagdoll(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 4.5f); + m_context.camera.m_zoom = 6.0f; + } + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + + B2Polygon box = b2MakeOffsetBox(20.0f, 1.0f, new B2Vec2(0.0f, -1.0f), b2Rot_identity); + b2CreatePolygonShape(groundId, ref shapeDef, ref box); + } + + m_scale = 1.0f; + + m_human = new Human(); + + Spawn(); + } + + void Spawn() + { + float jointFrictionTorque = 0.03f; + float jointHertz = 1.0f; + float jointDampingRatio = 0.5f; + CreateHuman(ref m_human, m_worldId, new B2Vec2(0.0f, 5.0f), m_scale, jointFrictionTorque, jointHertz, jointDampingRatio, 1, null, false); + Human_ApplyRandomAngularImpulse(ref m_human, 10.0f); + } + + public override void UpdateGui() + { + float height = 60.0f; + ImGui.SetNextWindowPos(new Vector2(10.0f, m_context.camera.m_height - height - 50.0f), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(260.0f, height)); + + ImGui.Begin("Scale Ragdoll", ImGuiWindowFlags.NoResize); + ImGui.PushItemWidth(200.0f); + + if (ImGui.SliderFloat("Scale", ref m_scale, 0.1f, 10.0f, "%3.2f", ImGuiSliderFlags.AlwaysClamp)) + { + Human_SetScale(ref m_human, m_scale); + } + + ImGui.PopItemWidth(); + ImGui.End(); + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Shared/Bone.cs b/src/Box2D.NET.Shared/Bone.cs index b55d3ad..3a91cf5 100644 --- a/src/Box2D.NET.Shared/Bone.cs +++ b/src/Box2D.NET.Shared/Bone.cs @@ -9,6 +9,7 @@ public struct Bone public B2BodyId bodyId; public B2JointId jointId; public float frictionScale; + public float maxTorque; public int parentIndex; } } diff --git a/src/Box2D.NET.Shared/Human.cs b/src/Box2D.NET.Shared/Human.cs index d4a4bed..08e7454 100644 --- a/src/Box2D.NET.Shared/Human.cs +++ b/src/Box2D.NET.Shared/Human.cs @@ -9,6 +9,8 @@ namespace Box2D.NET.Shared public struct Human { public B2FixedArray11 bones; + public float frictionTorque; + public float originalScale; public float scale; public bool isSpawned; @@ -23,6 +25,8 @@ public void Clear() scale = 0.0f; isSpawned = false; + frictionTorque = 0.0f; + originalScale = 0.0f; } } } \ No newline at end of file diff --git a/src/Box2D.NET.Shared/Humans.cs b/src/Box2D.NET.Shared/Humans.cs index 9d9e202..0be2d87 100644 --- a/src/Box2D.NET.Shared/Humans.cs +++ b/src/Box2D.NET.Shared/Humans.cs @@ -32,7 +32,9 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi human.bones[i].parentIndex = -1; } + human.originalScale = scale; human.scale = scale; + human.frictionTorque = frictionTorque; B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; @@ -81,7 +83,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.95f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "hip"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); if (colorize) @@ -101,7 +103,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 1.2f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "torso"; - + // bodyDef.type = b2_staticBody; bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.5f; @@ -185,7 +187,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.775f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "upper_left_leg"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 1.0f; @@ -233,7 +235,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.475f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "lower_left_leg"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.5f; @@ -280,7 +282,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.775f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "upper_right_leg"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 1.0f; @@ -319,7 +321,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.475f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "lower_right_leg"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.5f; @@ -367,7 +369,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 1.225f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "upper_left_arm"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); if (colorize) @@ -405,7 +407,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.975f * s), position); bodyDef.linearDamping = 0.1f; bodyDef.name = "lower_left_arm"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.1f; @@ -445,7 +447,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 1.225f * s), position); bodyDef.linearDamping = 0.0f; bodyDef.name = "upper_right_arm"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.5f; @@ -484,7 +486,7 @@ public static void CreateHuman(ref Human human, B2WorldId worldId, B2Vec2 positi bodyDef.position = b2Add(new B2Vec2(0.0f, 0.975f * s), position); bodyDef.linearDamping = 0.1f; bodyDef.name = "lower_right_arm"; - + bone.bodyId = b2CreateBody(worldId, ref bodyDef); bone.frictionScale = 0.1f; @@ -633,5 +635,78 @@ public static void Human_EnableSensorEvents(ref Human human, bool enable) b2Shape_EnableSensorEvents(shapeId[0], enable); } } + + public static void Human_SetScale(ref Human human, float scale) + { + B2_ASSERT(human.isSpawned == true); + B2_ASSERT(0.01f < scale && scale < 100.0f); + B2_ASSERT(0.0f < human.scale); + + float ratio = scale / human.scale; + + // Torque scales by pow(length, 4) due to mass change and length change. However, gravity is also a factor + // so I'm using pow(length, 3) + float originalRatio = scale / human.originalScale; + float frictionTorque = (originalRatio * originalRatio * originalRatio) * human.frictionTorque; + + B2Vec2 origin = b2Body_GetPosition(human.bones[0].bodyId); + + for (int boneIndex = 0; boneIndex < (int)BoneId.bone_count; ++boneIndex) + { + ref Bone bone = ref human.bones[boneIndex]; + + if (boneIndex > 0) + { + B2Transform transform = b2Body_GetTransform(bone.bodyId); + transform.p = b2MulAdd(origin, ratio, b2Sub(transform.p, origin)); + b2Body_SetTransform(bone.bodyId, transform.p, transform.q); + + B2Vec2 localAnchorA = b2Joint_GetLocalAnchorA(bone.jointId); + B2Vec2 localAnchorB = b2Joint_GetLocalAnchorB(bone.jointId); + localAnchorA = b2MulSV(ratio, localAnchorA); + localAnchorB = b2MulSV(ratio, localAnchorB); + b2Joint_SetLocalAnchorA(bone.jointId, localAnchorA); + b2Joint_SetLocalAnchorB(bone.jointId, localAnchorB); + + B2JointType type = b2Joint_GetType(bone.jointId); + if (type == B2JointType.b2_revoluteJoint) + { + b2RevoluteJoint_SetMaxMotorTorque(bone.jointId, bone.frictionScale * frictionTorque); + } + } + + B2ShapeId[] shapeIds = new B2ShapeId[2]; + int shapeCount = b2Body_GetShapes(bone.bodyId, shapeIds, 2); + for (int shapeIndex = 0; shapeIndex < shapeCount; ++shapeIndex) + { + B2ShapeType type = b2Shape_GetType(shapeIds[shapeIndex]); + if (type == B2ShapeType.b2_capsuleShape) + { + B2Capsule capsule = b2Shape_GetCapsule(shapeIds[shapeIndex]); + capsule.center1 = b2MulSV(ratio, capsule.center1); + capsule.center2 = b2MulSV(ratio, capsule.center2); + capsule.radius *= ratio; + b2Shape_SetCapsule(shapeIds[shapeIndex], ref capsule); + } + else if (type == B2ShapeType.b2_polygonShape) + { + B2Polygon polygon = b2Shape_GetPolygon(shapeIds[shapeIndex]); + for (int pointIndex = 0; pointIndex < polygon.count; ++pointIndex) + { + polygon.vertices[pointIndex] = b2MulSV(ratio, polygon.vertices[pointIndex]); + } + + polygon.centroid = b2MulSV(ratio, polygon.centroid); + polygon.radius *= ratio; + + b2Shape_SetPolygon(shapeIds[shapeIndex], ref polygon); + } + } + + b2Body_ApplyMassFromShapes(bone.bodyId); + } + + human.scale = scale; + } } } \ No newline at end of file diff --git a/src/Box2D.NET/B2AABBs.cs b/src/Box2D.NET/B2AABBs.cs index 7b39064..96dcaf7 100644 --- a/src/Box2D.NET/B2AABBs.cs +++ b/src/Box2D.NET/B2AABBs.cs @@ -50,15 +50,6 @@ public static bool b2EnlargeAABB(ref B2AABB a, B2AABB b) return changed; } - /// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static bool b2IsValidAABB(B2AABB a) - { - B2Vec2 d = b2Sub(a.upperBound, a.lowerBound); - bool valid = d.X >= 0.0f && d.Y >= 0.0f; - valid = valid && b2IsValidVec2(a.lowerBound) && b2IsValidVec2(a.upperBound); - return valid; - } // Ray cast an AABB // From Real-time Collision Detection, p179. diff --git a/src/Box2D.NET/B2BoardPhases.cs b/src/Box2D.NET/B2BoardPhases.cs index 7cd6e9e..2a8b534 100644 --- a/src/Box2D.NET/B2BoardPhases.cs +++ b/src/Box2D.NET/B2BoardPhases.cs @@ -16,6 +16,7 @@ using static Box2D.NET.B2Worlds; using static Box2D.NET.B2ArenaAllocators; using static Box2D.NET.B2MathFunction; +using static Box2D.NET.B2Shapes; namespace Box2D.NET { diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index 617de2b..f73d561 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/src/Box2D.NET/B2Bodies.cs @@ -3,6 +3,7 @@ // SPDX-License-Identifier: MIT using System; +using System.Runtime.CompilerServices; using static Box2D.NET.B2Arrays; using static Box2D.NET.B2Cores; using static Box2D.NET.B2Diagnostics; @@ -44,6 +45,15 @@ public static B2Sweep b2MakeSweep(B2BodySim bodySim) return s; } + public static void b2LimitVelocity(B2BodyState state, float maxLinearSpeed) + { + float v2 = b2LengthSquared(state.linearVelocity); + if (v2 > maxLinearSpeed * maxLinearSpeed) + { + state.linearVelocity = b2MulSV(maxLinearSpeed / MathF.Sqrt(v2), state.linearVelocity); + } + } + // Get a validated body from a world using an id. public static B2Body b2GetBodyFullId(B2World world, B2BodyId bodyId) { @@ -975,6 +985,8 @@ public static void b2Body_ApplyLinearImpulse(B2BodyId bodyId, B2Vec2 impulse, B2 B2BodySim bodySim = b2Array_Get(ref set.bodySims, localIndex); state.linearVelocity = b2MulAdd(state.linearVelocity, bodySim.invMass, impulse); state.angularVelocity += bodySim.invInertia * b2Cross(b2Sub(point, bodySim.center), impulse); + + b2LimitVelocity(state, world.maxLinearSpeed); } } @@ -995,6 +1007,8 @@ public static void b2Body_ApplyLinearImpulseToCenter(B2BodyId bodyId, B2Vec2 imp B2BodyState state = b2Array_Get(ref set.bodyStates, localIndex); B2BodySim bodySim = b2Array_Get(ref set.bodySims, localIndex); state.linearVelocity = b2MulAdd(state.linearVelocity, bodySim.invMass, impulse); + + b2LimitVelocity(state, world.maxLinearSpeed); } } @@ -1864,6 +1878,7 @@ public static int b2Body_GetJoints(B2BodyId bodyId, Span jointArray, return jointCount; } + public static bool b2ShouldBodiesCollide(B2World world, B2Body bodyA, B2Body bodyB) { if (bodyA.type != B2BodyType.b2_dynamicBody && bodyB.type != B2BodyType.b2_dynamicBody) diff --git a/src/Box2D.NET/B2CastOutput.cs b/src/Box2D.NET/B2CastOutput.cs index 0e5ef10..3ddd0e8 100644 --- a/src/Box2D.NET/B2CastOutput.cs +++ b/src/Box2D.NET/B2CastOutput.cs @@ -4,7 +4,7 @@ namespace Box2D.NET { - /// Low level ray cast or shape-cast output data + /// Low level ray cast or shape-cast output data. Returns a zero fraction and normal in the case of initial overlap. public struct B2CastOutput { /// The surface normal at the hit point @@ -22,4 +22,4 @@ public struct B2CastOutput /// Did the cast hit? public bool hit; } -} +} \ No newline at end of file diff --git a/src/Box2D.NET/B2Contacts.cs b/src/Box2D.NET/B2Contacts.cs index 936927d..c902640 100644 --- a/src/Box2D.NET/B2Contacts.cs +++ b/src/Box2D.NET/B2Contacts.cs @@ -42,17 +42,6 @@ public static class B2Contacts private static readonly B2ContactRegister[,] s_registers = new B2ContactRegister[(int)B2ShapeType.b2_shapeTypeCount, (int)B2ShapeType.b2_shapeTypeCount]; private static bool s_initialized = false; - public static bool b2ShouldShapesCollide(B2Filter filterA, B2Filter filterB) - { - if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) - { - return filterA.groupIndex > 0; - } - - bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; - return collide; - } - public static B2Manifold b2CircleManifold(B2Shape shapeA, B2Transform xfA, B2Shape shapeB, B2Transform xfB, ref B2SimplexCache cache) { B2_UNUSED(cache); diff --git a/src/Box2D.NET/B2Cores.cs b/src/Box2D.NET/B2Cores.cs index dbe87d1..a99baa1 100644 --- a/src/Box2D.NET/B2Cores.cs +++ b/src/Box2D.NET/B2Cores.cs @@ -16,7 +16,7 @@ public static class B2Cores /// Get the current version of Box2D public static B2Version b2GetVersion() { - return new B2Version(3, 1, 0); + return new B2Version(3, 1, 1); } // This allows the user to change the length units at runtime diff --git a/src/Box2D.NET/B2Delegates.cs b/src/Box2D.NET/B2Delegates.cs index 77a6e46..1440495 100644 --- a/src/Box2D.NET/B2Delegates.cs +++ b/src/Box2D.NET/B2Delegates.cs @@ -101,17 +101,18 @@ namespace Box2D.NET /// @ingroup world public delegate bool b2OverlapResultFcn(B2ShapeId shapeId, object context); - /// Prototype callback for ray casts. + /// Prototype callback for ray and shape casts. /// Called for each shape found in the query. You control how the ray cast /// proceeds by returning a float: /// return -1: ignore this shape and continue /// return 0: terminate the ray cast /// return fraction: clip the ray to this point /// return 1: don't clip the ray and continue + /// A cast with initial overlap will return a zero fraction and a zero normal. /// @param shapeId the shape hit by the ray /// @param point the point of initial intersection - /// @param normal the normal vector at the point of intersection - /// @param fraction the fraction along the ray at the point of intersection + /// @param normal the normal vector at the point of intersection, zero for a shape cast with initial overlap + /// @param fraction the fraction along the ray at the point of intersection, zero for a shape cast with initial overlap /// @param context the user context /// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue /// @see b2World_CastRay diff --git a/src/Box2D.NET/B2Distances.cs b/src/Box2D.NET/B2Distances.cs index d099af8..efc5a6a 100644 --- a/src/Box2D.NET/B2Distances.cs +++ b/src/Box2D.NET/B2Distances.cs @@ -546,6 +546,10 @@ public static B2DistanceOutput b2ShapeDistance(ref B2DistanceInput input, ref B2 if (simplex.count == 3) { // Overlap + B2Vec2 localPointA = new B2Vec2(), localPointB = new B2Vec2(); + b2ComputeSimplexWitnessPoints(ref localPointA, ref localPointB, ref simplex); + output.pointA = b2TransformPoint(ref input.transformA, localPointA); + output.pointB = b2TransformPoint(ref input.transformA, localPointB); return output; } @@ -568,6 +572,10 @@ public static B2DistanceOutput b2ShapeDistance(ref B2DistanceInput input, ref B2 // or triangle. Thus the shapes are overlapped. // Must return overlap due to invalid normal. + B2Vec2 localPointA = new B2Vec2(), localPointB = new B2Vec2(); + b2ComputeSimplexWitnessPoints(ref localPointA, ref localPointB, ref simplex); + output.pointA = b2TransformPoint(ref input.transformA, localPointA); + output.pointB = b2TransformPoint(ref input.transformA, localPointB); return output; } @@ -620,15 +628,17 @@ public static B2DistanceOutput b2ShapeDistance(ref B2DistanceInput input, ref B2 B2_ASSERT(b2IsNormalized(normal)); normal = b2RotateVector(input.transformA.q, normal); - B2Vec2 localPointA = new B2Vec2(); - B2Vec2 localPointB = new B2Vec2(); - b2ComputeSimplexWitnessPoints(ref localPointA, ref localPointB, ref simplex); - output.normal = normal; - output.distance = b2Distance(localPointA, localPointB); - output.pointA = b2TransformPoint(ref input.transformA, localPointA); - output.pointB = b2TransformPoint(ref input.transformA, localPointB); - output.iterations = iteration; - output.simplexCount = simplexIndex; + { + B2Vec2 localPointA = new B2Vec2(); + B2Vec2 localPointB = new B2Vec2(); + b2ComputeSimplexWitnessPoints(ref localPointA, ref localPointB, ref simplex); + output.normal = normal; + output.distance = b2Distance(localPointA, localPointB); + output.pointA = b2TransformPoint(ref input.transformA, localPointA); + output.pointB = b2TransformPoint(ref input.transformA, localPointB); + output.iterations = iteration; + output.simplexCount = simplexIndex; + } // Cache the simplex b2MakeSimplexCache(ref cache, ref simplex); @@ -664,7 +674,7 @@ public static B2CastOutput b2ShapeCast(ref B2ShapeCastPairInput input) // Prepare input for distance query B2SimplexCache cache = new B2SimplexCache(); - float alpha = 0.0f; + float fraction = 0.0f; B2DistanceInput distanceInput = new B2DistanceInput(); distanceInput.proxyA = input.proxyA; @@ -694,19 +704,13 @@ public static B2CastOutput b2ShapeCast(ref B2ShapeCastPairInput input) } else { - if (distanceOutput.distance < float.Epsilon) - { - // Normal may be invalid - return output; - } - - // Initial overlap but distance is non-zero due to radius. - // Note: this can result in initial hits for shapes with a radius - B2_ASSERT(b2IsNormalized(distanceOutput.normal)); - output.fraction = alpha; - output.point = b2MulAdd(distanceOutput.pointA, input.proxyA.radius, distanceOutput.normal); - output.normal = distanceOutput.normal; + // Initial overlap output.hit = true; + + // Compute a common point + B2Vec2 c1 = b2MulAdd(distanceOutput.pointA, input.proxyA.radius, distanceOutput.normal); + B2Vec2 c2 = b2MulAdd(distanceOutput.pointB, -input.proxyB.radius, distanceOutput.normal); + output.point = b2Lerp(c1, c2, 0.5f); return output; } } @@ -714,7 +718,7 @@ public static B2CastOutput b2ShapeCast(ref B2ShapeCastPairInput input) { // Regular hit B2_ASSERT(distanceOutput.distance > 0.0f && b2IsNormalized(distanceOutput.normal)); - output.fraction = alpha; + output.fraction = fraction; output.point = b2MulAdd(distanceOutput.pointA, input.proxyA.radius, distanceOutput.normal); output.normal = distanceOutput.normal; output.hit = true; @@ -730,20 +734,18 @@ public static B2CastOutput b2ShapeCast(ref B2ShapeCastPairInput input) if (denominator >= 0.0f) { // Miss - output.fraction = 1.0f; return output; } // Advance sweep - alpha += (target - distanceOutput.distance) / denominator; - if (alpha >= input.maxFraction) + fraction += (target - distanceOutput.distance) / denominator; + if (fraction >= input.maxFraction) { // Miss - output.fraction = 1.0f; return output; } - distanceInput.transformB.p = b2MulAdd(input.transformB.p, alpha, delta2); + distanceInput.transformB.p = b2MulAdd(input.transformB.p, fraction, delta2); } // Failure! diff --git a/src/Box2D.NET/B2Geometries.cs b/src/Box2D.NET/B2Geometries.cs index 02c7a73..9f95c55 100644 --- a/src/Box2D.NET/B2Geometries.cs +++ b/src/Box2D.NET/B2Geometries.cs @@ -553,11 +553,23 @@ public static B2CastOutput b2RayCastCircle(ref B2RayCastInput input, ref B2Circl // Shift ray so circle center is the origin B2Vec2 s = b2Sub(input.origin, p); + + float r = shape.radius; + float rr = r * r; + float length = 0; B2Vec2 d = b2GetLengthAndNormalize(ref length, input.translation); if (length == 0.0f) { // zero length ray + + if (b2LengthSquared(s) < rr) + { + // initial overlap + output.point = input.origin; + output.hit = true; + } + return output; } @@ -570,8 +582,6 @@ public static B2CastOutput b2RayCastCircle(ref B2RayCastInput input, ref B2Circl B2Vec2 c = b2MulAdd(s, t, d); float cc = b2Dot(c, c); - float r = shape.radius; - float rr = r * r; if (cc > rr) { @@ -586,7 +596,15 @@ public static B2CastOutput b2RayCastCircle(ref B2RayCastInput input, ref B2Circl if (fraction < 0.0f || input.maxFraction * length < fraction) { - // outside the range of the ray segment + // intersection is point outside the range of the ray segment + + if (b2LengthSquared(s) < rr) + { + // initial overlap + output.point = input.origin; + output.hit = true; + } + return output; } @@ -645,7 +663,7 @@ public static B2CastOutput b2RayCastCapsule(ref B2RayCastInput input, ref B2Caps return b2RayCastCircle(ref input, ref circle); } - if (qa > 1.0f) + if (qa > capsuleLength) { // start point ahead of capsule segment B2Circle circle = new B2Circle(v2, shape.radius); @@ -653,6 +671,8 @@ public static B2CastOutput b2RayCastCapsule(ref B2RayCastInput input, ref B2Caps } // ray starts inside capsule . no hit + output.point = input.origin; + output.hit = true; return output; } @@ -826,8 +846,11 @@ public static B2CastOutput b2RayCastPolygon(ref B2RayCastInput input, ref B2Poly if (shape.radius == 0.0f) { - // Put the ray into the polygon's frame of reference. - B2Vec2 p1 = input.origin; + // Shift all math to first vertex since the polygon may be far + // from the origin. + B2Vec2 @base = shape.vertices[0]; + + B2Vec2 p1 = b2Sub(input.origin, @base); B2Vec2 d = input.translation; float lower = 0.0f, upper = input.maxFraction; @@ -841,7 +864,8 @@ public static B2CastOutput b2RayCastPolygon(ref B2RayCastInput input, ref B2Poly // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float numerator = b2Dot(shape.normals[i], b2Sub(shape.vertices[i], p1)); + B2Vec2 vertex = b2Sub(shape.vertices[i], @base); + float numerator = b2Dot(shape.normals[i], b2Sub(vertex, p1)); float denominator = b2Dot(shape.normals[i], d); if (denominator == 0.0f) @@ -872,23 +896,26 @@ public static B2CastOutput b2RayCastPolygon(ref B2RayCastInput input, ref B2Poly } } - // The use of epsilon here causes the Debug.Assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - // if (upper < lower - b2_epsilon) if (upper < lower) { + // Ray misses return output; } } B2_ASSERT(0.0f <= lower && lower <= input.maxFraction); - if (index >= 0) + if ( index >= 0 ) { output.fraction = lower; output.normal = shape.normals[index]; - output.point = b2MulAdd(p1, lower, d); + output.point = b2MulAdd( input.origin, lower, d ); + output.hit = true; + } + else + { + // initial overlap + output.point = input.origin; output.hit = true; } diff --git a/src/Box2D.NET/B2Joints.cs b/src/Box2D.NET/B2Joints.cs index 9aa8989..089765b 100644 --- a/src/Box2D.NET/B2Joints.cs +++ b/src/Box2D.NET/B2Joints.cs @@ -407,6 +407,8 @@ public static B2JointId b2CreateDistanceJoint(B2WorldId worldId, ref B2DistanceJ return jointId; } + /// Create a motor joint + /// @see b2MotorJointDef for details public static B2JointId b2CreateMotorJoint(B2WorldId worldId, ref B2MotorJointDef def) { B2_CHECK_DEF(ref def); @@ -549,6 +551,7 @@ public static B2JointId b2CreateRevoluteJoint(B2WorldId worldId, ref B2RevoluteJ joint.uj.revoluteJoint = empty; joint.uj.revoluteJoint.referenceAngle = b2ClampFloat(def.referenceAngle, -B2_PI, B2_PI); + joint.uj.revoluteJoint.targetAngle = b2ClampFloat(def.targetAngle, -B2_PI, B2_PI); joint.uj.revoluteJoint.hertz = def.hertz; joint.uj.revoluteJoint.dampingRatio = def.dampingRatio; joint.uj.revoluteJoint.lowerAngle = def.lowerAngle; @@ -598,12 +601,7 @@ public static B2JointId b2CreatePrismaticJoint(B2WorldId worldId, B2PrismaticJoi joint.uj.prismaticJoint.localAxisA = b2Normalize(def.localAxisA); joint.uj.prismaticJoint.referenceAngle = def.referenceAngle; - joint.uj.prismaticJoint.impulse = b2Vec2_zero; - joint.uj.prismaticJoint.axialMass = 0.0f; - joint.uj.prismaticJoint.springImpulse = 0.0f; - joint.uj.prismaticJoint.motorImpulse = 0.0f; - joint.uj.prismaticJoint.lowerImpulse = 0.0f; - joint.uj.prismaticJoint.upperImpulse = 0.0f; + joint.uj.prismaticJoint.targetTranslation = def.targetTranslation; joint.uj.prismaticJoint.hertz = def.hertz; joint.uj.prismaticJoint.dampingRatio = def.dampingRatio; joint.uj.prismaticJoint.lowerTranslation = def.lowerTranslation; @@ -860,12 +858,24 @@ public static B2BodyId b2Joint_GetBodyB(B2JointId jointId) return b2MakeBodyId(world, joint.edges[1].bodyId); } + /// Get the world that owns this joint public static B2WorldId b2Joint_GetWorld(B2JointId jointId) { B2World world = b2GetWorld(jointId.world0); return new B2WorldId((ushort)(jointId.world0 + 1), world.generation); } + /// Set the local anchor on bodyA + public static void b2Joint_SetLocalAnchorA(B2JointId jointId, B2Vec2 localAnchor) + { + B2_ASSERT(b2IsValidVec2(localAnchor)); + + B2World world = b2GetWorld(jointId.world0); + B2Joint joint = b2GetJointFullId(world, jointId); + B2JointSim jointSim = b2GetJointSim(world, joint); + jointSim.localOriginAnchorA = localAnchor; + } + public static B2Vec2 b2Joint_GetLocalAnchorA(B2JointId jointId) { B2World world = b2GetWorld(jointId.world0); @@ -874,6 +884,17 @@ public static B2Vec2 b2Joint_GetLocalAnchorA(B2JointId jointId) return jointSim.localOriginAnchorA; } + /// Set the local anchor on bodyB + public static void b2Joint_SetLocalAnchorB(B2JointId jointId, B2Vec2 localAnchor) + { + B2_ASSERT(b2IsValidVec2(localAnchor)); + + B2World world = b2GetWorld(jointId.world0); + B2Joint joint = b2GetJointFullId(world, jointId); + B2JointSim jointSim = b2GetJointSim(world, joint); + jointSim.localOriginAnchorB = localAnchor; + } + public static B2Vec2 b2Joint_GetLocalAnchorB(B2JointId jointId) { B2World world = b2GetWorld(jointId.world0); @@ -1002,6 +1023,7 @@ public static B2Vec2 b2Joint_GetConstraintForce(B2JointId jointId) } } + /// Get the current constraint torque for this joint. Usually in Newton * meters. public static float b2Joint_GetConstraintTorque(B2JointId jointId) { B2World world = b2GetWorld(jointId.world0); @@ -1040,6 +1062,198 @@ public static float b2Joint_GetConstraintTorque(B2JointId jointId) } } + /// Get the current linear separation error for this joint. Does not consider admissible movement. Usually in meters. + public static float b2Joint_GetLinearSeparation(B2JointId jointId) + { + B2World world = b2GetWorld(jointId.world0); + B2Joint joint = b2GetJointFullId(world, jointId); + B2JointSim @base = b2GetJointSim(world, joint); + + B2Transform xfA = b2GetBodyTransform(world, joint.edges[0].bodyId); + B2Transform xfB = b2GetBodyTransform(world, joint.edges[1].bodyId); + + B2Vec2 pA = b2TransformPoint(ref xfA, @base.localOriginAnchorA); + B2Vec2 pB = b2TransformPoint(ref xfB, @base.localOriginAnchorB); + B2Vec2 dp = b2Sub(pB, pA); + + switch (joint.type) + { + case B2JointType.b2_distanceJoint: + { + ref B2DistanceJoint distanceJoint = ref @base.uj.distanceJoint; + float length = b2Length(dp); + if (distanceJoint.enableSpring) + { + if (distanceJoint.enableLimit) + { + if (length < distanceJoint.minLength) + { + return distanceJoint.minLength - length; + } + else if (length > distanceJoint.maxLength) + { + return length - distanceJoint.maxLength; + } + + return 0.0f; + } + + return 0.0f; + } + + return b2AbsFloat(length - distanceJoint.length); + } + + case B2JointType.b2_motorJoint: + return 0.0f; + + case B2JointType.b2_mouseJoint: + return 0.0f; + + case B2JointType.b2_filterJoint: + return 0.0f; + + case B2JointType.b2_prismaticJoint: + { + ref B2PrismaticJoint prismaticJoint = ref @base.uj.prismaticJoint; + B2Vec2 axisA = b2RotateVector(xfA.q, prismaticJoint.localAxisA); + B2Vec2 perpA = b2LeftPerp(axisA); + float perpendicularSeparation = b2AbsFloat(b2Dot(perpA, dp)); + float limitSeparation = 0.0f; + + if (prismaticJoint.enableLimit) + { + float translation = b2Dot(axisA, dp); + if (translation < prismaticJoint.lowerTranslation) + { + limitSeparation = prismaticJoint.lowerTranslation - translation; + } + + if (prismaticJoint.upperTranslation < translation) + { + limitSeparation = translation - prismaticJoint.upperTranslation; + } + } + + return MathF.Sqrt(perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation); + } + + case B2JointType.b2_revoluteJoint: + return b2Length(dp); + + case B2JointType.b2_weldJoint: + { + ref B2WeldJoint weldJoint = ref @base.uj.weldJoint; + if (weldJoint.linearHertz == 0.0f) + { + return b2Length(dp); + } + + return 0.0f; + } + + case B2JointType.b2_wheelJoint: + { + ref B2WheelJoint wheelJoint = ref @base.uj.wheelJoint; + B2Vec2 axisA = b2RotateVector(xfA.q, wheelJoint.localAxisA); + B2Vec2 perpA = b2LeftPerp(axisA); + float perpendicularSeparation = b2AbsFloat(b2Dot(perpA, dp)); + float limitSeparation = 0.0f; + + if (wheelJoint.enableLimit) + { + float translation = b2Dot(axisA, dp); + if (translation < wheelJoint.lowerTranslation) + { + limitSeparation = wheelJoint.lowerTranslation - translation; + } + + if (wheelJoint.upperTranslation < translation) + { + limitSeparation = translation - wheelJoint.upperTranslation; + } + } + + return MathF.Sqrt(perpendicularSeparation * perpendicularSeparation + limitSeparation * limitSeparation); + } + + default: + B2_ASSERT(false); + return 0.0f; + } + } + + /// Get the current angular separation error for this joint. Does not consider admissible movement. Usually in meters. + public static float b2Joint_GetAngularSeparation(B2JointId jointId) + { + B2World world = b2GetWorld(jointId.world0); + B2Joint joint = b2GetJointFullId(world, jointId); + B2JointSim @base = b2GetJointSim(world, joint); + + B2Transform xfA = b2GetBodyTransform(world, joint.edges[0].bodyId); + B2Transform xfB = b2GetBodyTransform(world, joint.edges[1].bodyId); + float relativeAngle = b2RelativeAngle(xfB.q, xfA.q); + + switch (joint.type) + { + case B2JointType.b2_distanceJoint: + return 0.0f; + + case B2JointType.b2_motorJoint: + return 0.0f; + + case B2JointType.b2_mouseJoint: + return 0.0f; + + case B2JointType.b2_filterJoint: + return 0.0f; + + case B2JointType.b2_prismaticJoint: + { + ref B2PrismaticJoint prismaticJoint = ref @base.uj.prismaticJoint; + return b2UnwindAngle(relativeAngle - prismaticJoint.referenceAngle); + } + + case B2JointType.b2_revoluteJoint: + { + ref B2RevoluteJoint revoluteJoint = ref @base.uj.revoluteJoint; + if (revoluteJoint.enableLimit) + { + float angle = b2UnwindAngle(relativeAngle - revoluteJoint.referenceAngle); + if (angle < revoluteJoint.lowerAngle) + { + return revoluteJoint.lowerAngle - angle; + } + + if (revoluteJoint.upperAngle < angle) + { + return angle - revoluteJoint.upperAngle; + } + } + + return 0.0f; + } + + case B2JointType.b2_weldJoint: + { + ref B2WeldJoint weldJoint = ref @base.uj.weldJoint; + if (weldJoint.angularHertz == 0.0f) + { + return b2UnwindAngle(relativeAngle - weldJoint.referenceAngle); + } + + return 0.0f; + } + + case B2JointType.b2_wheelJoint: + return 0.0f; + + default: + B2_ASSERT(false); + return 0.0f; + } + } + public static void b2PrepareJoint(B2JointSim joint, B2StepContext context) { switch (joint.type) diff --git a/src/Box2D.NET/B2MathFunction.cs b/src/Box2D.NET/B2MathFunction.cs index 8c798cb..e0a1136 100644 --- a/src/Box2D.NET/B2MathFunction.cs +++ b/src/Box2D.NET/B2MathFunction.cs @@ -444,37 +444,12 @@ public static float b2RelativeAngle(B2Rot b, B2Rot a) return b2Atan2(s, c); } - /// Convert an angle in the range [-2*pi, 2*pi] into the range [-pi, pi] + /// Convert any angle into the range [-pi, pi] [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float b2UnwindAngle(float radians) { - if (radians < -B2_PI) - { - return radians + 2.0f * B2_PI; - } - else if (radians > B2_PI) - { - return radians - 2.0f * B2_PI; - } - - return radians; - } - - /// Convert any into the range [-pi, pi] (slow) - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static float b2UnwindLargeAngle(float radians) - { - while (radians > B2_PI) - { - radians -= 2.0f * B2_PI; - } - - while (radians < -B2_PI) - { - radians += 2.0f * B2_PI; - } - - return radians; + // Assuming this is deterministic + return (float)Math.IEEERemainder(radians, 2.0f * B2_PI); } /// Rotate a vector @@ -708,6 +683,17 @@ public static bool b2IsValidRotation(B2Rot q) return b2IsNormalizedRot(q); } + /// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool b2IsValidAABB(B2AABB a) + { + B2Vec2 d = b2Sub(a.upperBound, a.lowerBound); + bool valid = d.X >= 0.0f && d.Y >= 0.0f; + valid = valid && b2IsValidVec2(a.lowerBound) && b2IsValidVec2(a.upperBound); + return valid; + } + + /// Is this a valid plane? Normal is a unit vector. Not Nan or infinity. [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool b2IsValidPlane(B2Plane a) { @@ -769,7 +755,7 @@ public static float b2Atan2(float y, float x) // https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula public static B2CosSin b2ComputeCosSin(float radians) { - float x = b2UnwindLargeAngle(radians); + float x = b2UnwindAngle(radians); float pi2 = B2_PI * B2_PI; // cosine needs angle in [-pi/2, pi/2] diff --git a/src/Box2D.NET/B2MotorJoints.cs b/src/Box2D.NET/B2MotorJoints.cs index 179e464..675ff48 100644 --- a/src/Box2D.NET/B2MotorJoints.cs +++ b/src/Box2D.NET/B2MotorJoints.cs @@ -14,22 +14,26 @@ namespace Box2D.NET { public static class B2MotorJoints { + /// Set the motor joint linear offset target public static void b2MotorJoint_SetLinearOffset(B2JointId jointId, B2Vec2 linearOffset) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); joint.uj.motorJoint.linearOffset = linearOffset; } + /// Get the motor joint linear offset target public static B2Vec2 b2MotorJoint_GetLinearOffset(B2JointId jointId) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); return joint.uj.motorJoint.linearOffset; } + /// Set the motor joint angular offset target in radians. This angle will be unwound + /// so the motor will drive along the shortest arc. public static void b2MotorJoint_SetAngularOffset(B2JointId jointId, float angularOffset) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - joint.uj.motorJoint.angularOffset = b2ClampFloat(angularOffset, -B2_PI, B2_PI); + joint.uj.motorJoint.angularOffset = angularOffset; } public static float b2MotorJoint_GetAngularOffset(B2JointId jointId) @@ -85,19 +89,19 @@ public static float b2GetMotorJointTorque(B2World world, B2JointSim @base) return world.inv_h * @base.uj.motorJoint.angularImpulse; } -// Point-to-point constraint -// C = p2 - p1 -// Cdot = v2 - v1 -// = v2 + cross(w2, r2) - v1 - cross(w1, r1) -// J = [-I -r1_skew I r2_skew ] -// Identity used: -// w k % (rx i + ry j) = w * (-ry i + rx j) + // Point-to-point constraint + // C = p2 - p1 + // Cdot = v2 - v1 + // = v2 + cross(w2, r2) - v1 - cross(w1, r1) + // J = [-I -r1_skew I r2_skew ] + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) -// Angle constraint -// C = angle2 - angle1 - referenceAngle -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 + // Angle constraint + // C = angle2 - angle1 - referenceAngle + // Cdot = w2 - w1 + // J = [0 0 -1 0 0 1] + // K = invI1 + invI2 public static void b2PrepareMotorJoint(B2JointSim @base, B2StepContext context) { @@ -141,7 +145,6 @@ public static void b2PrepareMotorJoint(B2JointSim @base, B2StepContext context) joint.anchorB = b2RotateVector(bodySimB.transform.q, b2Sub(@base.localOriginAnchorB, bodySimB.localCenter)); joint.deltaCenter = b2Sub(b2Sub(bodySimB.center, bodySimA.center), joint.linearOffset); joint.deltaAngle = b2RelativeAngle(bodySimB.transform.q, bodySimA.transform.q) - joint.angularOffset; - joint.deltaAngle = b2UnwindAngle(joint.deltaAngle); B2Vec2 rA = joint.anchorA; B2Vec2 rB = joint.anchorB; @@ -211,10 +214,10 @@ public static void b2SolveMotorJoint(B2JointSim @base, B2StepContext context, bo // angular constraint { - float angularSeperation = b2RelativeAngle(bodyB.deltaRotation, bodyA.deltaRotation) + joint.deltaAngle; - angularSeperation = b2UnwindAngle(angularSeperation); + float angularSeparation = b2RelativeAngle(bodyB.deltaRotation, bodyA.deltaRotation) + joint.deltaAngle; + angularSeparation = b2UnwindAngle(angularSeparation); - float angularBias = context.inv_h * joint.correctionFactor * angularSeperation; + float angularBias = context.inv_h * joint.correctionFactor * angularSeparation; float Cdot = wB - wA; float impulse = -joint.angularMass * (Cdot + angularBias); diff --git a/src/Box2D.NET/B2PrismaticJointDef.cs b/src/Box2D.NET/B2PrismaticJointDef.cs index 75d9681..5b730e8 100644 --- a/src/Box2D.NET/B2PrismaticJointDef.cs +++ b/src/Box2D.NET/B2PrismaticJointDef.cs @@ -30,6 +30,10 @@ public struct B2PrismaticJointDef /// The constrained angle between the bodies: bodyB_angle - bodyA_angle public float referenceAngle; + + /// The target translation for the joint in meters. The spring-damper will drive + /// to this translation. + public float targetTranslation; /// Enable a linear spring along the prismatic joint axis public bool enableSpring; diff --git a/src/Box2D.NET/B2RayResult.cs b/src/Box2D.NET/B2RayResult.cs index 8d83ac5..d849946 100644 --- a/src/Box2D.NET/B2RayResult.cs +++ b/src/Box2D.NET/B2RayResult.cs @@ -5,6 +5,7 @@ namespace Box2D.NET { /// Result from b2World_RayCastClosest + /// If there is initial overlap the fraction and normal will be zero while the point is an arbitrary point in the overlap region. /// @ingroup world public class B2RayResult { diff --git a/src/Box2D.NET/B2RevoluteJointDef.cs b/src/Box2D.NET/B2RevoluteJointDef.cs index 8767a04..25e323b 100644 --- a/src/Box2D.NET/B2RevoluteJointDef.cs +++ b/src/Box2D.NET/B2RevoluteJointDef.cs @@ -33,6 +33,10 @@ public struct B2RevoluteJointDef /// The bodyB angle minus bodyA angle in the reference state (radians). /// This defines the zero angle for the joint limit. public float referenceAngle; + + /// The target angle for the joint in radians. The spring-damper will drive + /// to this angle. + public float targetAngle; /// Enable a rotational spring on the revolute hinge axis public bool enableSpring; diff --git a/src/Box2D.NET/B2Shapes.cs b/src/Box2D.NET/B2Shapes.cs index 3ee7b1f..a3fc8b8 100644 --- a/src/Box2D.NET/B2Shapes.cs +++ b/src/Box2D.NET/B2Shapes.cs @@ -3,6 +3,7 @@ // SPDX-License-Identifier: MIT using System; +using System.Runtime.CompilerServices; using static Box2D.NET.B2Arrays; using static Box2D.NET.B2Cores; using static Box2D.NET.B2Diagnostics; @@ -23,6 +24,7 @@ namespace Box2D.NET { public static class B2Shapes { + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float b2GetShapeRadius(B2Shape shape) { switch (shape.type) @@ -38,6 +40,24 @@ public static float b2GetShapeRadius(B2Shape shape) } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool b2ShouldShapesCollide(B2Filter filterA, B2Filter filterB) + { + if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) + { + return filterA.groupIndex > 0; + } + + return (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool b2ShouldQueryCollide(B2Filter shapeFilter, B2QueryFilter queryFilter) + { + return (shapeFilter.categoryBits & queryFilter.maskBits) != 0 && (shapeFilter.maskBits & queryFilter.categoryBits) != 0; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Shape b2GetShape(B2World world, B2ShapeId shapeId) { int id = shapeId.index1 - 1; diff --git a/src/Box2D.NET/B2Worlds.cs b/src/Box2D.NET/B2Worlds.cs index 3ecb419..b1150b2 100644 --- a/src/Box2D.NET/B2Worlds.cs +++ b/src/Box2D.NET/B2Worlds.cs @@ -650,7 +650,7 @@ public static void b2Collide(B2StepContext context) else if (0 != (simFlags & (uint)B2ContactSimFlags.b2_simStartedTouching)) { B2_ASSERT(contact.islandId == B2_NULL_INDEX); - + if (0 != (flags & (uint)B2ContactFlags.b2_contactEnableContactEvents)) { B2ContactBeginTouchEvent @event = new B2ContactBeginTouchEvent(shapeIdA, shapeIdB, ref contactSim.manifold); @@ -1344,7 +1344,7 @@ public static void b2World_Draw(B2WorldId worldId, B2DebugDraw draw) } B2BodySim bodySim = b2GetBodySim(world, body); - + B2Transform transform = new B2Transform(bodySim.center, bodySim.transform.q); B2Vec2 p = b2TransformPoint(ref transform, offset); draw.DrawStringFcn(p, body.name, B2HexColor.b2_colorBlueViolet, draw.context); @@ -2091,10 +2091,7 @@ static bool TreeQueryCallback(int proxyId, ulong userData, ref B2WorldQueryConte B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return true; } @@ -2143,10 +2140,7 @@ public static bool TreeOverlapCallback(int proxyId, ulong userData, ref B2WorldO B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return true; } @@ -2214,10 +2208,8 @@ public static float RayCastCallback(ref B2RayCastInput input, int proxyId, ulong B2World world = worldContext.world; B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return input.maxFraction; } @@ -2245,7 +2237,6 @@ public static float RayCastCallback(ref B2RayCastInput input, int proxyId, ulong /// Cast a ray into the world to collect shapes in the path of the ray. /// Your callback function controls whether you get the closest point, any point, or n-points. - /// The ray-cast ignores shapes that contain the starting point. /// @note The callback function may receive shapes in any order /// @param worldId The world to cast the ray against /// @param origin The start point of the ray @@ -2293,6 +2284,12 @@ public static B2TreeStats b2World_CastRay(B2WorldId worldId, B2Vec2 origin, B2Ve // This callback finds the closest hit. This is the most common callback used in games. public static float b2RayCastClosestFcn(B2ShapeId shapeId, B2Vec2 point, B2Vec2 normal, float fraction, object context) { + // Ignore initial overlap + if (fraction == 0.0f) + { + return -1.0f; + } + B2RayResult rayResult = context as B2RayResult; rayResult.shapeId = shapeId; rayResult.point = point; @@ -2302,7 +2299,7 @@ public static float b2RayCastClosestFcn(B2ShapeId shapeId, B2Vec2 point, B2Vec2 return fraction; } - /// Cast a ray into the world to collect the closest hit. This is a convenience function. + /// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. /// This is less general than b2World_CastRay() and does not allow for custom filtering. public static B2RayResult b2World_CastRayClosest(B2WorldId worldId, B2Vec2 origin, B2Vec2 translation, B2QueryFilter filter) { @@ -2349,10 +2346,8 @@ public static float ShapeCastCallback(ref B2ShapeCastInput input, int proxyId, u B2World world = worldContext.world; B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return input.maxFraction; } @@ -2429,10 +2424,8 @@ public static float MoverCastCallback(ref B2ShapeCastInput input, int proxyId, u B2World world = worldContext.world; B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return worldContext.fraction; } @@ -2501,14 +2494,12 @@ public static bool TreeCollideCallback(int proxyId, ulong userData, ref B2WorldM B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - B2Filter shapeFilter = shape.filter; - B2QueryFilter queryFilter = worldContext.filter; - - if ((shapeFilter.categoryBits & queryFilter.maskBits) == 0 || (shapeFilter.maskBits & queryFilter.categoryBits) == 0) + if (b2ShouldQueryCollide(shape.filter, worldContext.filter) == false) { return true; } + B2Body body = b2Array_Get(ref world.bodies, shape.bodyId); B2Transform transform = b2GetBodyTransformQuick(world, body); @@ -3041,7 +3032,7 @@ public static void b2ValidateSolverSets(B2World world) // contact should be non-touching if awake // or it could be this contact hasn't been transferred yet B2_ASSERT(contactSim.manifold.pointCount == 0 || - (contactSim.simFlags & (uint)B2ContactSimFlags.b2_simStartedTouching) != 0); + (contactSim.simFlags & (uint)B2ContactSimFlags.b2_simStartedTouching) != 0); } B2_ASSERT(contact.setIndex == setIndex); @@ -3109,7 +3100,7 @@ public static void b2ValidateSolverSets(B2World world) B2Contact contact = b2Array_Get(ref world.contacts, contactSim.contactId); // contact should be touching in the constraint graph or awaiting transfer to non-touching B2_ASSERT(contactSim.manifold.pointCount > 0 || - (contactSim.simFlags & ((uint)B2ContactSimFlags.b2_simStoppedTouching | (uint)B2ContactSimFlags.b2_simDisjoint)) != 0); + (contactSim.simFlags & ((uint)B2ContactSimFlags.b2_simStoppedTouching | (uint)B2ContactSimFlags.b2_simDisjoint)) != 0); B2_ASSERT(contact.setIndex == (int)B2SetType.b2_awakeSet); B2_ASSERT(contact.colorIndex == colorIndex); B2_ASSERT(contact.localIndex == i); diff --git a/test/Box2D.NET.Test/B2MathTest.cs b/test/Box2D.NET.Test/B2MathTest.cs index 924656b..a47b27e 100644 --- a/test/Box2D.NET.Test/B2MathTest.cs +++ b/test/Box2D.NET.Test/B2MathTest.cs @@ -28,7 +28,9 @@ public void MathTest() Assert.That(r.c - c, Is.LessThan(0.002f)); Assert.That(r.s - s, Is.LessThan(0.002f)); - float xn = b2UnwindLargeAngle(angle); + float xn = b2UnwindAngle(angle); + Assert.That(-B2_PI <= xn && xn <= B2_PI, Is.True); + float a = b2Atan2(s, c); Assert.That(b2IsValidFloat(a)); diff --git a/test/Box2D.NET.Test/B2RotTest.cs b/test/Box2D.NET.Test/B2RotTest.cs index e603fe2..1e32213 100644 --- a/test/Box2D.NET.Test/B2RotTest.cs +++ b/test/Box2D.NET.Test/B2RotTest.cs @@ -44,9 +44,9 @@ public void Test_B2Rot_Integration() var rot = b2MakeRot(0.0f); float deltaAngle = B2_PI / 2; var result = b2IntegrateRotation(rot, deltaAngle); - float expectedMag = MathF.Sqrt(1.0f + (B2_PI/2) * (B2_PI/2)); + float expectedMag = MathF.Sqrt(1.0f + (B2_PI / 2) * (B2_PI / 2)); float expectedC = 1.0f / expectedMag; - float expectedS = (B2_PI/2) / expectedMag; + float expectedS = (B2_PI / 2) / expectedMag; Assert.That(result.c, Is.EqualTo(expectedC).Within(0.0001f), "Integrated rotation cosine should match expected value"); Assert.That(result.s, Is.EqualTo(expectedS).Within(0.0001f), "Integrated rotation sine should match expected value"); Assert.That(result.c * result.c + result.s * result.s, Is.EqualTo(1.0f).Within(0.0001f), "Integrated rotation should remain normalized"); @@ -119,13 +119,8 @@ public void Test_B2Rot_UnwindAngle() Assert.That(b2UnwindAngle(-3 * B2_PI / 2), Is.EqualTo(B2_PI / 2).Within(FLT_EPSILON), "Angle less than -PI should be normalized to [-PI, PI]"); Assert.That(b2UnwindAngle(B2_PI), Is.EqualTo(B2_PI).Within(FLT_EPSILON), "PI should remain unchanged"); Assert.That(b2UnwindAngle(-B2_PI), Is.EqualTo(-B2_PI).Within(FLT_EPSILON), "-PI should remain unchanged"); - } - - [Test] - public void Test_B2Rot_UnwindLargeAngle() - { - Assert.That(b2UnwindLargeAngle(5 * B2_PI), Is.EqualTo(B2_PI).Within(0.0001f), "Large angle should be normalized to [-PI, PI]"); - Assert.That(b2UnwindLargeAngle(-5 * B2_PI), Is.EqualTo(-B2_PI).Within(0.0001f), "Large negative angle should be normalized to [-PI, PI]"); + Assert.That(b2UnwindAngle(5 * B2_PI), Is.EqualTo(-B2_PI).Within(0.0001f), "Large angle should be normalized to [-PI, PI]"); + Assert.That(b2UnwindAngle(-5 * B2_PI), Is.EqualTo(B2_PI).Within(0.0001f), "Large negative angle should be normalized to [-PI, PI]"); } [Test] @@ -137,4 +132,4 @@ public void Test_B2Rot_InvRotateVector() Assert.That(result.X, Is.EqualTo(1.0f).Within(FLT_EPSILON), "Inverse rotation should map (0,1) back to (1,0)"); Assert.That(result.Y, Is.EqualTo(0.0f).Within(FLT_EPSILON), "Inverse rotation should map (0,1) back to (1,0)"); } -} \ No newline at end of file +} \ No newline at end of file