diff --git a/src/Box2D.NET.Samples/SampleApp.cs b/src/Box2D.NET.Samples/SampleApp.cs index 5a2f4a8c..c897108b 100644 --- a/src/Box2D.NET.Samples/SampleApp.cs +++ b/src/Box2D.NET.Samples/SampleApp.cs @@ -219,18 +219,19 @@ private void OnWindowLoad() return; } - Span temp1 = stackalloc float[2]; - _ctx.gl.GetFloat( GLEnum.AliasedLineWidthRange, temp1 ); - - Span temp2 = stackalloc float[2]; - _ctx.gl.GetFloat( GLEnum.SmoothLineWidthRange, temp2 ); + { + Span temp1 = stackalloc float[2]; + _ctx.gl.GetFloat(GLEnum.AliasedLineWidthRange, temp1); - var glVersion = _ctx.gl.GetStringS(StringName.Version); - Logger.Information($"GL {glVersion}"); - Logger.Information($"OpenGL {_ctx.gl.GetStringS(GLEnum.Version)}, GLSL {_ctx.gl.GetStringS(GLEnum.ShadingLanguageVersion)}"); - Logger.Information($"OpenGL aliased line width range : [{temp1[0]}, {temp1[1]}]"); - Logger.Information($"OpenGL smooth line width range : [{temp2[0]}, {temp2[1]}]"); + Span temp2 = stackalloc float[2]; + _ctx.gl.GetFloat(GLEnum.SmoothLineWidthRange, temp2); + string glVersionString = _ctx.gl.GetStringS(GLEnum.Version); + string glslVersionString = _ctx.gl.GetStringS(GLEnum.ShadingLanguageVersion); + Logger.Information($"OpenGL {glVersionString}, GLSL {glslVersionString}"); + Logger.Information($"OpenGL aliased line width range : [{temp1[0]}, {temp1[1]}]"); + Logger.Information($"OpenGL smooth line width range : [{temp2[0]}, {temp2[1]}]"); + } unsafe { diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCast.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCast.cs index b2d37a4b..e784f798 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCast.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCast.cs @@ -15,6 +15,7 @@ using static Box2D.NET.B2Worlds; using static Box2D.NET.Shared.RandomSupports; using static Box2D.NET.B2Timers; +using static Box2D.NET.B2Distances; namespace Box2D.NET.Samples.Samples.Benchmarks; @@ -242,11 +243,12 @@ public override void Step(Settings settings) for (int i = 0; i < sampleCount; ++i) { - B2Circle circle = new B2Circle(m_origins[i], m_radius); + B2ShapeProxy proxy = b2MakeProxy( m_origins[i], 1, m_radius ); + B2Vec2 translation = m_translations[i]; CastResult result = new CastResult(); - B2TreeStats traversalResult = b2World_CastCircle(m_worldId, ref circle, translation, filter, CastCallback, result); + B2TreeStats traversalResult = b2World_CastShape(m_worldId, ref proxy, translation, filter, CastCallback, result); if (i == m_drawIndex) { diff --git a/src/Box2D.NET.Samples/Samples/Bodies/Kinematic.cs b/src/Box2D.NET.Samples/Samples/Bodies/Kinematic.cs index 9bf9d306..207519c7 100644 --- a/src/Box2D.NET.Samples/Samples/Bodies/Kinematic.cs +++ b/src/Box2D.NET.Samples/Samples/Bodies/Kinematic.cs @@ -51,16 +51,9 @@ public Kinematic(SampleAppContext ctx, Settings settings) : base(ctx, settings) public override void Step(Settings settings) { m_timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; - if (settings.pause) + if (settings.pause && settings.singleStep == false) { - if (settings.singleStep) - { - settings.singleStep = false; - } - else - { - m_timeStep = 0.0f; - } + m_timeStep = 0.0f; } @@ -84,7 +77,7 @@ public override void Draw(Settings settings) m_context.draw.DrawSegment(point - 0.5f * axis, point + 0.5f * axis, B2HexColor.b2_colorPlum); m_context.draw.DrawPoint(point, 10.0f, B2HexColor.b2_colorPlum); - b2Body_SetKinematicTarget(m_bodyId, new B2Transform(point, rotation), m_timeStep); + b2Body_SetTargetTransform(m_bodyId, new B2Transform(point, rotation), m_timeStep); } } } \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Bodies/Sleep.cs b/src/Box2D.NET.Samples/Samples/Bodies/Sleep.cs index 41825ba9..7d73df36 100644 --- a/src/Box2D.NET.Samples/Samples/Bodies/Sleep.cs +++ b/src/Box2D.NET.Samples/Samples/Bodies/Sleep.cs @@ -20,11 +20,11 @@ public class Sleep : Sample private static readonly int SampleSleep = SampleFactory.Shared.RegisterSample("Bodies", "Sleep", Create); private B2BodyId m_pendulumId; + private B2BodyId m_staticBodyId; private B2ShapeId m_groundShapeId; private B2ShapeId[] m_sensorIds = new B2ShapeId[2]; private bool[] m_sensorTouching = new bool[2]; - private static Sample Create(SampleAppContext ctx, Settings settings) { return new Sleep(ctx, settings); @@ -43,7 +43,7 @@ public Sleep(SampleAppContext ctx, Settings settings) : base(ctx, settings) B2BodyDef bodyDef = b2DefaultBodyDef(); groundId = b2CreateBody(m_worldId, ref bodyDef); - B2Segment segment = new B2Segment(new B2Vec2(-20.0f, 0.0f), new B2Vec2(20.0f, 0.0f)); + B2Segment segment = new B2Segment(new B2Vec2(-40.0f, 0.0f), new B2Vec2(40.0f, 0.0f)); B2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.enableSensorEvents = true; m_groundShapeId = b2CreateSegmentShape(groundId, ref shapeDef, ref segment); @@ -133,12 +133,48 @@ public Sleep(SampleAppContext ctx, Settings settings) : base(ctx, settings) jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); b2CreateRevoluteJoint(m_worldId, ref jointDef); } + + // A sleeping body to test waking on contact destroyed + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(-10.0f, 1.0f); + bodyDef.isAwake = false; + bodyDef.enableSleep = true; + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeSquare(1.0f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(bodyId, ref shapeDef, ref box); + } + + m_staticBodyId = b2_nullBodyId; + } + + void ToggleInvoker() + { + if (B2_IS_NULL(m_staticBodyId)) + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position = new B2Vec2(-10.5f, 3.0f); + m_staticBodyId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeOffsetBox(2.0f, 0.1f, new B2Vec2(0.0f, 0.0f), b2MakeRot(0.25f * B2_PI)); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.invokeContactCreation = true; + b2CreatePolygonShape(m_staticBodyId, ref shapeDef, ref box); + } + else + { + b2DestroyBody(m_staticBodyId); + m_staticBodyId = b2_nullBodyId; + } } public override void UpdateGui() { base.UpdateGui(); - float height = 100.0f; + float height = 160.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("Sleep", ImGuiWindowFlags.NoMove | ImGuiWindowFlags.NoResize); @@ -162,6 +198,23 @@ public override void UpdateGui() ImGui.PopItemWidth(); + ImGui.Separator(); + + if (B2_IS_NULL(m_staticBodyId)) + { + if (ImGui.Button("Create")) + { + ToggleInvoker(); + } + } + else + { + if (ImGui.Button("Destroy")) + { + ToggleInvoker(); + } + } + ImGui.End(); } @@ -208,7 +261,7 @@ public override void Step(Settings settings) public override void Draw(Settings settings) { base.Draw(settings); - + for (int i = 0; i < 2; ++i) { m_context.draw.DrawString(5, m_textLine, $"sensor touch {i} = {m_sensorTouching[i]}"); diff --git a/src/Box2D.NET.Samples/Samples/Characters/Mover.cs b/src/Box2D.NET.Samples/Samples/Characters/Mover.cs index 4c91d82f..628de6bb 100644 --- a/src/Box2D.NET.Samples/Samples/Characters/Mover.cs +++ b/src/Box2D.NET.Samples/Samples/Characters/Mover.cs @@ -2,6 +2,7 @@ // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT +using System; using System.Diagnostics; using System.Numerics; using Box2D.NET.Samples.Helpers; @@ -15,6 +16,7 @@ using static Box2D.NET.B2Movers; using static Box2D.NET.B2Geometries; using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Distances; namespace Box2D.NET.Samples.Samples.Characters; @@ -31,13 +33,14 @@ public class ShapeUserData private const ulong StaticBit = 0x0001; private const ulong MoverBit = 0x0002; private const ulong DynamicBit = 0x0004; + private const ulong DebrisBit = 0x0008; private const ulong AllBits = ~0u; public enum PogoShape { PogoPoint, PogoCircle, - PogoBox + PogoSegment } public class CastResult @@ -49,6 +52,8 @@ public class CastResult } private const int m_planeCapacity = 8; + private readonly B2Vec2 m_elevatorBase = new B2Vec2(112.0f, 10.0f); + private const float m_elevatorAmplitude = 4.0f; private float m_jumpSpeed = 10.0f; private float m_maxSpeed = 6.0f; @@ -61,17 +66,21 @@ public class CastResult private float m_pogoHertz = 5.0f; private float m_pogoDampingRatio = 0.8f; - private int m_pogoShape = (int)PogoShape.PogoBox; - + private int m_pogoShape = (int)PogoShape.PogoSegment; private B2Transform m_transform; private B2Vec2 m_velocity; private B2Capsule m_capsule; + private B2BodyId m_elevatorId; + private B2ShapeId m_ballId; private ShapeUserData m_friendlyShape = new ShapeUserData(); + private ShapeUserData m_elevatorShape = new ShapeUserData(); private B2CollisionPlane[] m_planes = new B2CollisionPlane[m_planeCapacity]; private int m_planeCount; private int m_totalIterations; private float m_pogoVelocity; + private float m_time; private bool m_onGround; + private bool m_jumpReleased; private bool m_lockCamera; private int m_deltaX; @@ -211,7 +220,7 @@ public Mover(SampleAppContext ctx, Settings settings) : base(ctx, settings) { B2BodyDef bodyDef = b2DefaultBodyDef(); - bodyDef.position = new B2Vec2(32.0f, 4.0f); + bodyDef.position = new B2Vec2(32.0f, 4.5f); B2ShapeDef shapeDef = b2DefaultShapeDef(); m_friendlyShape.maxPush = 0.025f; @@ -219,27 +228,51 @@ public Mover(SampleAppContext ctx, Settings settings) : base(ctx, settings) shapeDef.filter = new B2Filter(MoverBit, AllBits, 0); shapeDef.userData = m_friendlyShape; - B2BodyId body = b2CreateBody(m_worldId, ref bodyDef); - b2CreateCapsuleShape(body, ref shapeDef, ref m_capsule); + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + b2CreateCapsuleShape(bodyId, ref shapeDef, ref m_capsule); } { B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; bodyDef.position = new B2Vec2(7.0f, 7.0f); - B2BodyId body = b2CreateBody(m_worldId, ref bodyDef); + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = new B2Filter(DebrisBit, AllBits, 0); + shapeDef.material.restitution = 0.7f; + shapeDef.material.rollingResistance = 0.2f; + + B2Circle circle = new B2Circle(b2Vec2_zero, 0.3f); + m_ballId = b2CreateCircleShape(bodyId, ref shapeDef, ref circle); + } + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_kinematicBody; + bodyDef.position = new B2Vec2(m_elevatorBase.X, m_elevatorBase.Y - m_elevatorAmplitude); + m_elevatorId = b2CreateBody(m_worldId, ref bodyDef); + m_elevatorShape = new ShapeUserData + { + maxPush = 0.1f, + clipVelocity = true, + }; B2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.filter = new B2Filter(DynamicBit, AllBits, 0); - B2Circle circle = new B2Circle(b2Vec2_zero, 0.5f); - b2CreateCircleShape(body, ref shapeDef, ref circle); + shapeDef.userData = m_elevatorShape; + + B2Polygon box = b2MakeBox(2.0f, 0.1f); + b2CreatePolygonShape(m_elevatorId, ref shapeDef, ref box); } m_totalIterations = 0; m_pogoVelocity = 0.0f; m_onGround = false; + m_jumpReleased = true; m_lockCamera = true; m_planeCount = 0; + m_time = 0.0f; } // https://github.com/id-Software/Quake/blob/master/QW/client/pmove.c#L390 @@ -298,32 +331,44 @@ void SolveMove(float timeStep, float throttle) float rayLength = pogoRestLength + m_capsule.radius; B2Vec2 origin = b2TransformPoint(ref m_transform, m_capsule.center1); B2Circle circle = new B2Circle(origin, 0.5f * m_capsule.radius); - float boxHalfWidth = 0.75f * m_capsule.radius; - float boxHalfHeight = 0.05f * m_capsule.radius; - B2Polygon box = b2MakeOffsetBox(boxHalfWidth, boxHalfHeight, origin, b2Rot_identity); + B2Vec2 segmentOffset = new B2Vec2(0.75f * m_capsule.radius, 0.0f); + B2Segment segment = new B2Segment(origin - segmentOffset, origin + segmentOffset); + + B2ShapeProxy proxy = new B2ShapeProxy(); B2Vec2 translation; - B2QueryFilter skipTeamFilter = new B2QueryFilter(1, ~2u); - CastResult result = new CastResult(); + B2QueryFilter pogoFilter = new B2QueryFilter(MoverBit, StaticBit | DynamicBit); + CastResult castResult = new CastResult(); if (m_pogoShape == (int)PogoShape.PogoPoint) { + proxy = b2MakeProxy(origin, 1, 0.0f); translation = new B2Vec2(0.0f, -rayLength); - b2World_CastRay(m_worldId, origin, translation, skipTeamFilter, CastCallback, result); } else if (m_pogoShape == (int)PogoShape.PogoCircle) { + proxy = b2MakeProxy(origin, 1, circle.radius); translation = new B2Vec2(0.0f, -rayLength + circle.radius); - b2World_CastCircle(m_worldId, ref circle, translation, skipTeamFilter, CastCallback, result); } else { - translation = new B2Vec2(0.0f, -rayLength + boxHalfHeight); - b2World_CastPolygon(m_worldId, ref box, translation, skipTeamFilter, CastCallback, result); + proxy = b2MakeProxy(segment.point1, segment.point2, 2, 0.0f); + translation = new B2Vec2(0.0f, -rayLength); + } + + b2World_CastShape(m_worldId, ref proxy, translation, pogoFilter, CastCallback, castResult); + + // Avoid snapping to ground if still going up + if (m_onGround == false) + { + m_onGround = castResult.hit && m_velocity.Y <= 0.01f; + } + else + { + m_onGround = castResult.hit; } - if (result.hit == false) + if (castResult.hit == false) { - m_onGround = false; m_pogoVelocity = 0.0f; B2Vec2 delta = translation; @@ -339,14 +384,12 @@ void SolveMove(float timeStep, float throttle) } else { - B2Transform xf = new B2Transform(delta, b2Rot_identity); - m_context.draw.DrawSolidPolygon(ref xf, box.vertices.AsSpan(), box.count, 0.0f, B2HexColor.b2_colorGray); + m_context.draw.DrawSegment(segment.point1 + delta, segment.point2 + delta, B2HexColor.b2_colorGray); } } else { - m_onGround = true; - float pogoCurrentLength = result.fraction * rayLength; + float pogoCurrentLength = castResult.fraction * rayLength; float zeta = m_pogoDampingRatio; float hertz = m_pogoHertz; @@ -356,7 +399,7 @@ void SolveMove(float timeStep, float throttle) m_pogoVelocity = (m_pogoVelocity - omega * omegaH * (pogoCurrentLength - pogoRestLength)) / (1.0f + 2.0f * zeta * omegaH + omegaH * omegaH); - B2Vec2 delta = result.fraction * translation; + B2Vec2 delta = castResult.fraction * translation; m_context.draw.DrawSegment(origin, origin + delta, B2HexColor.b2_colorGray); if (m_pogoShape == (int)PogoShape.PogoPoint) @@ -369,17 +412,16 @@ void SolveMove(float timeStep, float throttle) } else { - B2Transform xf = new B2Transform(delta, b2Rot_identity); - m_context.draw.DrawSolidPolygon(ref xf, box.vertices.AsSpan(), box.count, 0.0f, B2HexColor.b2_colorPlum); + m_context.draw.DrawSegment(segment.point1 + delta, segment.point2 + delta, B2HexColor.b2_colorPlum); } - b2Body_ApplyForce(result.bodyId, new B2Vec2(0.0f, -50.0f), result.point, true); + b2Body_ApplyForce(castResult.bodyId, new B2Vec2(0.0f, -50.0f), castResult.point, true); } B2Vec2 target = m_transform.p + timeStep * m_velocity + timeStep * m_pogoVelocity * new B2Vec2(0.0f, 1.0f); - // Movers collide with every thing - B2QueryFilter collideFilter = new B2QueryFilter(MoverBit, AllBits); + // Mover overlap filter + B2QueryFilter collideFilter = new B2QueryFilter(MoverBit, StaticBit | DynamicBit | MoverBit); // Movers don't sweep against other movers, allows for soft collision B2QueryFilter castFilter = new B2QueryFilter(MoverBit, StaticBit | DynamicBit); @@ -447,7 +489,7 @@ public override void UpdateGui() ImGui.SameLine(); ImGui.RadioButton("Circle", ref m_pogoShape, (int)PogoShape.PogoCircle); ImGui.SameLine(); - ImGui.RadioButton("Box", ref m_pogoShape, (int)PogoShape.PogoBox); + ImGui.RadioButton("Segment", ref m_pogoShape, (int)PogoShape.PogoSegment); ImGui.Checkbox("Lock Camera", ref m_lockCamera); @@ -478,31 +520,97 @@ static bool PlaneResultFcn(B2ShapeId shapeId, ref B2PlaneResult planeResult, obj return true; } - public override void Step(Settings settings) + static bool Kick(B2ShapeId shapeId, object context) { - base.Step(settings); - - float throttle = 0.0f; + Mover self = (Mover)context; + B2BodyId bodyId = b2Shape_GetBody(shapeId); + B2BodyType type = b2Body_GetType(bodyId); - if (InputAction.Press == GetKey(Keys.A)) + if (type != B2BodyType.b2_dynamicBody) { - throttle -= 1.0f; + return true; } - if (InputAction.Press == GetKey(Keys.D)) + B2Vec2 center = b2Body_GetWorldCenterOfMass(bodyId); + B2Vec2 direction = b2Normalize(center - self.m_transform.p); + B2Vec2 impulse = new B2Vec2(2.0f * direction.X, 2.0f); + b2Body_ApplyLinearImpulseToCenter(bodyId, impulse, true); + + return true; + } + + public override void Keyboard(Keys key) + { + if (key == Keys.K) { - throttle += 1.0f; + B2Vec2 point = b2TransformPoint(ref m_transform, new B2Vec2(0.0f, m_capsule.center1.Y - 3.0f * m_capsule.radius)); + B2Circle circle = new B2Circle(point, 0.5f); + B2ShapeProxy proxy = b2MakeProxy(circle.center, 1, circle.radius); + B2QueryFilter filter = new B2QueryFilter(MoverBit, DebrisBit); + b2World_OverlapShape(m_worldId, ref proxy, filter, Kick, this); + m_context.draw.DrawCircle(circle.center, circle.radius, B2HexColor.b2_colorGoldenRod); } - if (InputAction.Press == GetKey(Keys.Space) && m_onGround == true) + base.Keyboard(key); + } + + public override void Step(Settings settings) + { + base.Step(settings); + + bool pause = false; + if ( settings.pause ) { - m_velocity.Y = m_jumpSpeed; - m_onGround = false; + pause = settings.singleStep != true; } float timeStep = settings.hertz > 0.0f ? 1.0f / settings.hertz : 0.0f; + if ( pause ) + { + timeStep = 0.0f; + } + + if ( timeStep > 0.0f ) + { + B2Vec2 point; + point.X = m_elevatorBase.X; + point.Y = m_elevatorAmplitude * MathF.Cos(1.0f * m_time + B2_PI) + m_elevatorBase.Y; + + b2Body_SetTargetTransform( m_elevatorId, new B2Transform( point, b2Rot_identity ), timeStep ); + } + + m_time += timeStep; + + if (pause == false) + { + float throttle = 0.0f; - SolveMove(timeStep, throttle); + if (InputAction.Press == GetKey(Keys.A)) + { + throttle -= 1.0f; + } + + if (InputAction.Press == GetKey(Keys.D)) + { + throttle += 1.0f; + } + + if (InputAction.Press == GetKey(Keys.Space) ) + { + if (m_onGround && m_jumpReleased) + { + m_velocity.Y = m_jumpSpeed; + m_onGround = false; + m_jumpReleased = false; + } + } + else + { + m_jumpReleased = true; + } + + SolveMove(timeStep, throttle); + } } public override void Draw(Settings settings) @@ -522,7 +630,9 @@ public override void Draw(Settings settings) { B2Vec2 p1 = b2TransformPoint(ref m_transform, m_capsule.center1); B2Vec2 p2 = b2TransformPoint(ref m_transform, m_capsule.center2); - m_context.draw.DrawSolidCapsule(p1, p2, m_capsule.radius, B2HexColor.b2_colorOrange); + + B2HexColor color = m_onGround ? B2HexColor.b2_colorOrange : B2HexColor.b2_colorAquamarine; + m_context.draw.DrawSolidCapsule(p1, p2, m_capsule.radius, color); m_context.draw.DrawSegment(m_transform.p, m_transform.p + m_velocity, B2HexColor.b2_colorPurple); } diff --git a/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs b/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs index 59ed7d2b..f3c60cba 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/CastWorld.cs @@ -16,6 +16,7 @@ using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Shapes; using static Box2D.NET.B2Worlds; +using static Box2D.NET.B2Distances; using static Box2D.NET.Shared.RandomSupports; namespace Box2D.NET.Samples.Samples.Collisions; @@ -397,8 +398,14 @@ public override void Step(Settings settings) } else { - b2CastResultFcn[] fcns = [RayCastAnyCallback, RayCastClosestCallback, RayCastMultipleCallback, RayCastSortedCallback]; - b2CastResultFcn modeFcn = fcns[m_mode]; + b2CastResultFcn[] functions = + [ + RayCastAnyCallback, + RayCastClosestCallback, + RayCastMultipleCallback, + RayCastSortedCallback + ]; + b2CastResultFcn modeFcn = functions[m_mode]; CastContext context = new CastContext(); @@ -410,25 +417,29 @@ public override void Step(Settings settings) B2Transform transform = new B2Transform(m_rayStart, b2MakeRot(m_angle)); B2Circle circle = new B2Circle(m_rayStart, m_castRadius); B2Capsule capsule = new B2Capsule(b2TransformPoint(ref transform, new B2Vec2(-0.25f, 0.0f)), b2TransformPoint(ref transform, new B2Vec2(0.25f, 0.0f)), m_castRadius); - B2Polygon box = b2MakeOffsetRoundedBox( 0.25f, 0.5f, transform.p, transform.q, m_castRadius ); + B2Polygon box = b2MakeOffsetRoundedBox(0.25f, 0.5f, transform.p, transform.q, m_castRadius); - switch (m_castType) + B2ShapeProxy proxy = new B2ShapeProxy(); + if (m_castType == CastType.e_rayCast) { - case CastType.e_rayCast: - b2World_CastRay(m_worldId, m_rayStart, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); - break; - - case CastType.e_circleCast: - b2World_CastCircle(m_worldId, ref circle, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); - break; - - case CastType.e_capsuleCast: - b2World_CastCapsule(m_worldId, ref capsule, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); - break; + b2World_CastRay(m_worldId, m_rayStart, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); + } + else + { + if (m_castType == CastType.e_circleCast) + { + proxy = b2MakeProxy(circle.center, 1, circle.radius); + } + else if (m_castType == CastType.e_capsuleCast) + { + proxy = b2MakeProxy(capsule.center1, capsule.center2, 2, capsule.radius); + } + else + { + proxy = b2MakeProxy(box.vertices.AsSpan(), box.count, box.radius); + } - case CastType.e_polygonCast: - b2World_CastPolygon(m_worldId, ref box, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); - break; + b2World_CastShape(m_worldId, ref proxy, rayTranslation, b2DefaultQueryFilter(), modeFcn, context); } if (context.count > 0) diff --git a/src/Box2D.NET.Samples/Samples/Collisions/OverlapWorld.cs b/src/Box2D.NET.Samples/Samples/Collisions/OverlapWorld.cs index 01b77abe..41805be0 100644 --- a/src/Box2D.NET.Samples/Samples/Collisions/OverlapWorld.cs +++ b/src/Box2D.NET.Samples/Samples/Collisions/OverlapWorld.cs @@ -16,8 +16,8 @@ using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Shapes; using static Box2D.NET.B2Worlds; +using static Box2D.NET.B2Distances; using static Box2D.NET.Shared.RandomSupports; -using static Box2D.NET.B2Constants; namespace Box2D.NET.Samples.Samples.Collisions; @@ -43,10 +43,6 @@ public class OverlapWorld : Sample private B2ShapeId[] m_doomIds = new B2ShapeId[e_maxDoomed]; private int m_doomCount; - private B2Circle m_queryCircle; - private B2Capsule m_queryCapsule; - private B2Polygon m_queryBox; - private int m_shapeType; private B2Transform m_transform; @@ -152,10 +148,6 @@ public OverlapWorld(SampleAppContext ctx, Settings settings) : base(ctx, setting m_shapeType = e_circleShape; - m_queryCircle = new B2Circle(new B2Vec2(0.0f, 0.0f), 1.0f); - m_queryCapsule = new B2Capsule(new B2Vec2(-1.0f, 0.0f), new B2Vec2(1.0f, 0.0f), 0.5f); - m_queryBox = b2MakeBox(2.0f, 0.5f); - m_position = new B2Vec2(.0f, 10.0f); m_angle = 0.0f; m_dragging = false; @@ -346,31 +338,36 @@ public override void Step(Settings settings) m_doomCount = 0; B2Transform transform = new B2Transform(m_position, b2MakeRot(m_angle)); + B2ShapeProxy proxy = new B2ShapeProxy(); if (m_shapeType == e_circleShape) { - b2World_OverlapCircle(m_worldId, ref m_queryCircle, transform, b2DefaultQueryFilter(), OverlapResultFcn, this); - m_context.draw.DrawSolidCircle(ref transform, b2Vec2_zero, m_queryCircle.radius, B2HexColor.b2_colorWhite); + B2Circle circle; + circle.center = transform.p; + circle.radius = 1.0f; + proxy = b2MakeProxy(circle.center, 1, circle.radius); + B2Transform identity = b2Transform_identity; + m_context.draw.DrawSolidCircle(ref identity, circle.center, circle.radius, B2HexColor.b2_colorWhite); } else if (m_shapeType == e_capsuleShape) { - b2World_OverlapCapsule(m_worldId, ref m_queryCapsule, transform, b2DefaultQueryFilter(), OverlapResultFcn, this); - B2Vec2 p1 = b2TransformPoint(ref transform, m_queryCapsule.center1); - B2Vec2 p2 = b2TransformPoint(ref transform, m_queryCapsule.center2); - m_context.draw.DrawSolidCapsule(p1, p2, m_queryCapsule.radius, B2HexColor.b2_colorWhite); + B2Capsule capsule; + capsule.center1 = b2TransformPoint(ref transform, new B2Vec2(-1.0f, 0.0f)); + capsule.center2 = b2TransformPoint(ref transform, new B2Vec2(1.0f, 0.0f)); + capsule.radius = 0.5f; + proxy = b2MakeProxy(capsule.center1, capsule.center2, 2, capsule.radius); + m_context.draw.DrawSolidCapsule(capsule.center1, capsule.center2, capsule.radius, B2HexColor.b2_colorWhite); } else if (m_shapeType == e_boxShape) { - b2World_OverlapPolygon(m_worldId, ref m_queryBox, transform, b2DefaultQueryFilter(), OverlapResultFcn, this); - B2Vec2[] points = new B2Vec2[B2_MAX_POLYGON_VERTICES]; - for (int i = 0; i < m_queryBox.count; ++i) - { - points[i] = b2TransformPoint(ref transform, m_queryBox.vertices[i]); - } - - m_context.draw.DrawPolygon(points, m_queryBox.count, B2HexColor.b2_colorWhite); + B2Polygon box = b2MakeOffsetBox(2.0f, 0.5f, transform.p, transform.q); + proxy = b2MakeProxy(box.vertices.AsSpan(), box.count, box.radius); + m_context.draw.DrawPolygon(box.vertices.AsSpan(), box.count, B2HexColor.b2_colorWhite); } + b2World_OverlapShape(m_worldId, ref proxy, b2DefaultQueryFilter(), OverlapResultFcn, this); + + for (int i = 0; i < m_doomCount; ++i) { B2ShapeId shapeId = m_doomIds[i]; @@ -392,7 +389,7 @@ public override void Step(Settings settings) public override void Draw(Settings settings) { base.Draw(settings); - + m_context.draw.DrawString(5, m_textLine, "left mouse button: drag query shape"); m_textLine += m_textIncrement; m_context.draw.DrawString(5, m_textLine, "left mouse button + shift: rotate query shape"); diff --git a/src/Box2D.NET.Samples/Samples/Joints/GearLift.cs b/src/Box2D.NET.Samples/Samples/Joints/GearLift.cs index e1249455..896803ec 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/GearLift.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/GearLift.cs @@ -323,14 +323,14 @@ public override void Step(Settings settings) { if (InputAction.Release == GetKey(Keys.A)) { - m_motorSpeed = b2MaxFloat(-0.3f, m_motorSpeed - 0.001f); + m_motorSpeed = b2MaxFloat(-0.3f, m_motorSpeed - 0.01f); b2RevoluteJoint_SetMotorSpeed(m_driverId, m_motorSpeed); b2Joint_WakeBodies(m_driverId); } if (InputAction.Release == GetKey(Keys.D)) { - m_motorSpeed = b2MinFloat(0.3f, m_motorSpeed + 0.001f); + m_motorSpeed = b2MinFloat(0.3f, m_motorSpeed + 0.01f); b2RevoluteJoint_SetMotorSpeed(m_driverId, m_motorSpeed); b2Joint_WakeBodies(m_driverId); } diff --git a/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs index a72a564d..6b40b2e9 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/RevoluteJoint.cs @@ -128,7 +128,7 @@ public RevoluteJoint(SampleAppContext ctx, Settings settings) : base(ctx, settin jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot); jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot); jointDef.lowerAngle = -0.25f * B2_PI; - jointDef.upperAngle = 0.0f * B2_PI; + jointDef.upperAngle = 0.5f * B2_PI; jointDef.enableLimit = true; jointDef.enableMotor = true; jointDef.motorSpeed = 0.0f; @@ -204,15 +204,15 @@ public override void Draw(Settings settings) base.Draw(settings); float angle1 = b2RevoluteJoint_GetAngle(m_jointId1); - m_context.draw.DrawString(5, m_textLine, $"Angle (Deg) 1 = {angle1:2,F1}"); + m_context.draw.DrawString(5, m_textLine, $"Angle (Deg) 1 = {angle1:F1}"); m_textLine += m_textIncrement; float torque1 = b2RevoluteJoint_GetMotorTorque(m_jointId1); - m_context.draw.DrawString(5, m_textLine, $"Motor Torque 1 = {torque1:4,F1}"); + m_context.draw.DrawString(5, m_textLine, $"Motor Torque 1 = {torque1:F1}"); m_textLine += m_textIncrement; float torque2 = b2RevoluteJoint_GetMotorTorque(m_jointId2); - m_context.draw.DrawString(5, m_textLine, $"Motor Torque 2 = {torque2:4,F1}"); + m_context.draw.DrawString(5, m_textLine, $"Motor Torque 2 = {torque2:F1}"); m_textLine += m_textIncrement; } } \ No newline at end of file diff --git a/src/Box2D.NET.Shared/RandomSupports.cs b/src/Box2D.NET.Shared/RandomSupports.cs index c8c2db48..c7887119 100644 --- a/src/Box2D.NET.Shared/RandomSupports.cs +++ b/src/Box2D.NET.Shared/RandomSupports.cs @@ -3,6 +3,7 @@ // SPDX-License-Identifier: MIT using System; +using System.Runtime.CompilerServices; using static Box2D.NET.B2Constants; using static Box2D.NET.B2Geometries; using static Box2D.NET.B2Hulls; @@ -18,7 +19,7 @@ public static class RandomSupports public const int RAND_LIMIT = 32767; public const int RAND_SEED = 12345; - // Simple random number generator. Using this instead of rand() for cross platform determinism. + // Simple random number generator. Using this instead of rand() for cross-platform determinism. public static int RandomInt() { // XorShift32 algorithm @@ -33,12 +34,14 @@ public static int RandomInt() } // Random integer in range [lo, hi] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static int RandomIntRange(int lo, int hi) { return lo + RandomInt() % (hi - lo + 1); } // Random number in range [-1,1] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float RandomFloat() { float r = (float)(RandomInt() & (RAND_LIMIT)); @@ -48,6 +51,7 @@ public static float RandomFloat() } // Random floating point number in range [lo, hi] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float RandomFloatRange(float lo, float hi) { float r = (float)(RandomInt() & (RAND_LIMIT)); @@ -57,6 +61,7 @@ public static float RandomFloatRange(float lo, float hi) } // Random vector with coordinates in range [lo, hi] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Vec2 RandomVec2(float lo, float hi) { B2Vec2 v; @@ -66,12 +71,14 @@ public static B2Vec2 RandomVec2(float lo, float hi) } // Random rotation with angle in range [-pi, pi] + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Rot RandomRot() { float angle = RandomFloatRange(-B2_PI, B2_PI); return b2MakeRot(angle); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Polygon RandomPolygon(float extent) { Span points = stackalloc B2Vec2[B2_MAX_POLYGON_VERTICES]; diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index fa5497f7..6cfe6a08 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/src/Box2D.NET/B2Bodies.cs @@ -329,12 +329,6 @@ public static B2BodyId b2CreateBody(B2WorldId worldId, ref B2BodyDef def) return id; } - public static bool b2IsBodyAwake(B2World world, B2Body body) - { - B2_UNUSED(world); - return body.setIndex == (int)B2SetType.b2_awakeSet; - } - // careful calling this because it can invalidate body, state, joint, and contact pointers public static bool b2WakeBody(B2World world, B2Body body) { @@ -431,8 +425,8 @@ public static void b2DestroyBody(B2BodyId bodyId) if (body.setIndex == (int)B2SetType.b2_awakeSet) { int result = b2Array_RemoveSwap(ref set.bodyStates, body.localIndex); - B2_UNUSED(result); Debug.Assert(result == movedIndex); + B2_UNUSED(result); } else if (set.setIndex >= (int)B2SetType.b2_firstSleepingSet && set.bodySims.count == 0) { @@ -825,7 +819,7 @@ public static void b2Body_SetAngularVelocity(B2BodyId bodyId, float angularVeloc /// Set the velocity to reach the given transform after a given time step. /// The result will be close but maybe not exact. This is meant for kinematic bodies. /// This will automatically wake the body if asleep. - public static void b2Body_SetKinematicTarget(B2BodyId bodyId, B2Transform target, float timeStep) + public static void b2Body_SetTargetTransform(B2BodyId bodyId, B2Transform target, float timeStep) { B2World world = b2GetWorld(bodyId.world0); B2Body body = b2GetBodyFullId(world, bodyId); @@ -854,7 +848,7 @@ public static void b2Body_SetKinematicTarget(B2BodyId bodyId, B2Transform target } // Return if velocity would be zero - if (b2LengthSquared(linearVelocity) == 0.0f || b2AbsFloat(angularVelocity) == 0.0f) + if (b2LengthSquared(linearVelocity) == 0.0f && b2AbsFloat(angularVelocity) == 0.0f) { return; } @@ -1392,6 +1386,12 @@ public static B2MassData b2Body_GetMassData(B2BodyId bodyId) return massData; } + /// This update the mass properties to the sum of the mass properties of the shapes. + /// This normally does not need to be called unless you called SetMassData to override + /// the mass and you later want to reset the mass. + /// You may also use this when automatic mass computation has been disabled. + /// You should call this regardless of body type. + /// Note that sensor shapes may have mass. public static void b2Body_ApplyMassFromShapes(B2BodyId bodyId) { B2World world = b2GetWorldLocked(bodyId.world0); diff --git a/src/Box2D.NET/B2CollisionPlane.cs b/src/Box2D.NET/B2CollisionPlane.cs index c00c0e12..a6325f53 100644 --- a/src/Box2D.NET/B2CollisionPlane.cs +++ b/src/Box2D.NET/B2CollisionPlane.cs @@ -4,11 +4,21 @@ namespace Box2D.NET { + /// These are collision planes that can be fed to b2SolvePlanes. Normally + /// this is assembled by the user from plane results in b2PlaneResult public struct B2CollisionPlane { + /// The collision plane between the mover and some shape public B2Plane plane; + + /// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can + /// make the plane collision soft. Usually in meters. public float pushLimit; + + /// The push on the mover determined by b2SolvePlanes. Usually in meters. public float push; + + /// Indicates if b2ClipVector should clip against this plane. Should be false for soft collision. public bool clipVelocity; public B2CollisionPlane(B2Plane plane, float pushLimit, float push, bool clipVelocity) diff --git a/src/Box2D.NET/B2ContactSimFlags.cs b/src/Box2D.NET/B2ContactSimFlags.cs index 68634bc1..16124537 100644 --- a/src/Box2D.NET/B2ContactSimFlags.cs +++ b/src/Box2D.NET/B2ContactSimFlags.cs @@ -7,7 +7,7 @@ namespace Box2D.NET // Shifted to be distinct from b2ContactFlags public enum B2ContactSimFlags { - // Set when the shapes are touching, including sensors + // Set when the shapes are touching b2_simTouchingFlag = 0x00010000, // This contact no longer has overlapping AABBs diff --git a/src/Box2D.NET/B2Contacts.cs b/src/Box2D.NET/B2Contacts.cs index 01a48817..596657c7 100644 --- a/src/Box2D.NET/B2Contacts.cs +++ b/src/Box2D.NET/B2Contacts.cs @@ -327,9 +327,10 @@ public static void b2DestroyContact(B2World world, B2Contact contact, bool wakeB B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); uint flags = contact.flags; + bool touching = (flags & (uint)B2ContactFlags.b2_contactTouchingFlag) != 0; // End touch event - if ((flags & (uint)B2ContactFlags.b2_contactTouchingFlag) != 0 && (flags & (uint)B2ContactFlags.b2_contactEnableContactEvents) != 0) + if (touching && (flags & (uint)B2ContactFlags.b2_contactEnableContactEvents) != 0) { ushort worldId = world.worldId; B2Shape shapeA = b2Array_Get(ref world.shapes, contact.shapeIdA); @@ -422,7 +423,7 @@ public static void b2DestroyContact(B2World world, B2Contact contact, bool wakeB b2FreeId(world.contactIdPool, contactId); - if (wakeBodies) + if (wakeBodies && touching) { b2WakeBody(world, bodyA); b2WakeBody(world, bodyB); diff --git a/src/Box2D.NET/B2Cores.cs b/src/Box2D.NET/B2Cores.cs index 9ab8ed24..f6d53b64 100644 --- a/src/Box2D.NET/B2Cores.cs +++ b/src/Box2D.NET/B2Cores.cs @@ -108,20 +108,20 @@ public static B2Version b2GetVersion() #if BOX2D_PROFILE - /// Tracy profiler instrumentation - /// https://github.com/wolfpld/tracy - public static void b2TracyCZoneC( object ctx, object color, object active ) - { - TracyCZoneC(ctx, color, active); - } - public static void b2TracyCZoneNC(object object ctx, object name, object color, object active ) - { - TracyCZoneNC(ctx, name, color, active); - } - public static void b2TracyCZoneEnd(object ctx) - { - TracyCZoneEnd(ctx); - } + /// Tracy profiler instrumentation + /// https://github.com/wolfpld/tracy + public static void b2TracyCZoneC( object ctx, object color, object active ) + { + TracyCZoneC(ctx, color, active); + } + public static void b2TracyCZoneNC(object object ctx, object name, object color, object active ) + { + TracyCZoneNC(ctx, name, color, active); + } + public static void b2TracyCZoneEnd(object ctx) + { + TracyCZoneEnd(ctx); + } #else [Conditional("DEBUG")] public static void b2TracyCZoneC(B2TracyCZone ctx, B2HexColor color, bool active) diff --git a/src/Box2D.NET/B2Delegates.cs b/src/Box2D.NET/B2Delegates.cs index f7c8622d..77a6e467 100644 --- a/src/Box2D.NET/B2Delegates.cs +++ b/src/Box2D.NET/B2Delegates.cs @@ -56,11 +56,13 @@ namespace Box2D.NET /// Optional friction mixing callback. This intentionally provides no context objects because this is called /// from a worker thread. /// @warning This function should not attempt to modify Box2D state or user application state. + /// @ingroup world public delegate float b2FrictionCallback(float frictionA, int userMaterialIdA, float frictionB, int userMaterialIdB); /// Optional restitution mixing callback. This intentionally provides no context objects because this is called /// from a worker thread. /// @warning This function should not attempt to modify Box2D state or user application state. + /// @ingroup world public delegate float b2RestitutionCallback(float restitutionA, int userMaterialIdA, float restitutionB, int userMaterialIdB); /// Prototype for a contact filter callback. diff --git a/src/Box2D.NET/B2Distances.cs b/src/Box2D.NET/B2Distances.cs index 3de4549b..d8db98b1 100644 --- a/src/Box2D.NET/B2Distances.cs +++ b/src/Box2D.NET/B2Distances.cs @@ -5,6 +5,7 @@ using System; using System.Diagnostics; using System.Runtime.CompilerServices; +using static Box2D.NET.B2Cores; using static Box2D.NET.B2MathFunction; using static Box2D.NET.B2Constants; using static Box2D.NET.B2Timers; @@ -120,16 +121,17 @@ public static B2SegmentDistanceResult b2SegmentDistance(B2Vec2 p1, B2Vec2 q1, B2 return result; } + /// Make a proxy for use in overlap, shape cast, and related functions. This is a deep copy of the points. /// Make a proxy for use in GJK and related functions. // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. // todo try not copying - public static B2ShapeProxy b2MakeProxy(ReadOnlySpan vertices, int count, float radius) + public static B2ShapeProxy b2MakeProxy(ReadOnlySpan points, int count, float radius) { count = b2MinInt(count, B2_MAX_POLYGON_VERTICES); B2ShapeProxy proxy = new B2ShapeProxy(); for (int i = 0; i < count; ++i) { - proxy.points[i] = vertices[i]; + proxy.points[i] = points[i]; } proxy.count = count; @@ -137,6 +139,24 @@ public static B2ShapeProxy b2MakeProxy(ReadOnlySpan vertices, int count, return proxy; } + /// Make a proxy with a transform. This is a deep copy of the points. + public static B2ShapeProxy b2MakeOffsetProxy(ReadOnlySpan points, int count, float radius, B2Vec2 position, B2Rot rotation) + { + count = b2MinInt(count, B2_MAX_POLYGON_VERTICES); + B2Transform transform = new B2Transform(position, rotation); + + B2ShapeProxy proxy = new B2ShapeProxy(); + for (int i = 0; i < count; ++i) + { + proxy.points[i] = b2TransformPoint(ref transform, points[i]); + } + + proxy.count = count; + proxy.radius = radius; + return proxy; + } + + // for single public static B2ShapeProxy b2MakeProxy(B2Vec2 v1, int count, float radius) { @@ -240,22 +260,6 @@ public static void b2MakeSimplexCache(ref B2SimplexCache cache, ref B2Simplex si } } - [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static B2Vec2 b2ComputeSimplexClosestPoint(ref B2Simplex s) - { - if (s.count == 1) - { - return s.v1.w; - } - - if (s.count == 2) - { - return b2Weight2(s.v1.a, s.v1.w, s.v2.a, s.v2.w); - } - - return b2Vec2_zero; - } - public static void b2ComputeSimplexWitnessPoints(ref B2Vec2 a, ref B2Vec2 b, ref B2Simplex s) { switch (s.count) @@ -467,6 +471,11 @@ public static B2Vec2 b2SolveSimplex3(ref B2Simplex s) // I spent time optimizing this and could find no further significant gains 3/30/2025 public static B2DistanceOutput b2ShapeDistance(ref B2DistanceInput input, ref B2SimplexCache cache, B2Simplex[] simplexes, int simplexCapacity) { + B2_UNUSED( simplexes, simplexCapacity ); + Debug.Assert( input.proxyA.count > 0 && input.proxyB.count > 0 ); + Debug.Assert( input.proxyA.radius >= 0.0f ); + Debug.Assert( input.proxyB.radius >= 0.0f ); + B2DistanceOutput output = new B2DistanceOutput(); ref B2ShapeProxy proxyA = ref input.proxyA; @@ -742,6 +751,21 @@ public static B2CastOutput b2ShapeCast(ref B2ShapeCastPairInput input) #if FALSE + static inline b2Vec2 b2ComputeSimplexClosestPoint( const b2Simplex* s ) + { + if ( s->count == 1 ) + { + return s->v1.w; + } + + if ( s->count == 2 ) + { + return b2Weight2( s->v1.a, s->v1.w, s->v2.a, s->v2.w ); + } + + return b2Vec2_zero; + } + public struct b2ShapeCastData { b2Simplex simplex; @@ -1197,7 +1221,7 @@ public static B2TOIOutput b2TimeOfImpact(ref B2TOIInput input) // to get a separating axis. distanceInput.transformA = xfA; distanceInput.transformB = xfB; - B2DistanceOutput distanceOutput = b2ShapeDistance(ref distanceInput, ref cache, null, 0 ); + B2DistanceOutput distanceOutput = b2ShapeDistance(ref distanceInput, ref cache, null, 0); // Progressive time of impact. This handles slender geometry well but introduces // significant time loss. @@ -1213,7 +1237,7 @@ public static B2TOIOutput b2TimeOfImpact(ref B2TOIInput input) // target = b2MaxFloat( target, 2.0f * tolerance ); // } //} - + distanceIterations += 1; #if B2_SNOOP_TOI_COUNTERS b2_toiDistanceIterations += 1; diff --git a/src/Box2D.NET/B2DynamicTrees.cs b/src/Box2D.NET/B2DynamicTrees.cs index e36411d6..55ded9ac 100644 --- a/src/Box2D.NET/B2DynamicTrees.cs +++ b/src/Box2D.NET/B2DynamicTrees.cs @@ -921,6 +921,7 @@ public static B2AABB b2DynamicTree_GetRootBounds(B2DynamicTree tree) return empty; } +#if B2_VALIDATE // Compute the height of a sub-tree. public static int b2ComputeHeight(B2DynamicTree tree, int nodeId) { @@ -937,7 +938,6 @@ public static int b2ComputeHeight(B2DynamicTree tree, int nodeId) return 1 + b2MaxInt(height1, height2); } -#if B2_VALIDATE public static void b2ValidateStructure(B2DynamicTree tree, int index) { if (index == B2_NULL_INDEX) diff --git a/src/Box2D.NET/B2Geometries.cs b/src/Box2D.NET/B2Geometries.cs index 293f4f89..43df5a3a 100644 --- a/src/Box2D.NET/B2Geometries.cs +++ b/src/Box2D.NET/B2Geometries.cs @@ -988,7 +988,7 @@ public static B2PlaneResult b2CollideMoverAndCircle(ref B2Circle shape, ref B2Ca if (distanceOutput.distance <= totalRadius) { B2Plane plane = new B2Plane(distanceOutput.normal, totalRadius - distanceOutput.distance); - return new B2PlaneResult(plane, distanceOutput.pointA, true); + return new B2PlaneResult(plane, true); } return new B2PlaneResult(); @@ -1011,7 +1011,7 @@ public static B2PlaneResult b2CollideMoverAndCapsule(ref B2Capsule shape, ref B2 if (distanceOutput.distance <= totalRadius) { B2Plane plane = new B2Plane(distanceOutput.normal, totalRadius - distanceOutput.distance); - return new B2PlaneResult(plane, distanceOutput.pointA, true); + return new B2PlaneResult(plane, true); } return new B2PlaneResult(); @@ -1034,7 +1034,7 @@ public static B2PlaneResult b2CollideMoverAndPolygon(ref B2Polygon shape, ref B2 if (distanceOutput.distance <= totalRadius) { B2Plane plane = new B2Plane(distanceOutput.normal, totalRadius - distanceOutput.distance); - return new B2PlaneResult(plane, distanceOutput.pointA, true); + return new B2PlaneResult(plane, true); } return new B2PlaneResult(); @@ -1057,7 +1057,7 @@ public static B2PlaneResult b2CollideMoverAndSegment(ref B2Segment shape, ref B2 if (distanceOutput.distance <= totalRadius) { B2Plane plane = new B2Plane(distanceOutput.normal, totalRadius - distanceOutput.distance); - return new B2PlaneResult(plane, distanceOutput.pointA, true); + return new B2PlaneResult(plane, true); } return new B2PlaneResult(); diff --git a/src/Box2D.NET/B2Joints.cs b/src/Box2D.NET/B2Joints.cs index a5aaaf08..f4adc42c 100644 --- a/src/Box2D.NET/B2Joints.cs +++ b/src/Box2D.NET/B2Joints.cs @@ -520,6 +520,10 @@ public static B2JointId b2CreateFilterJoint(B2WorldId worldId, ref b2FilterJoint public static B2JointId b2CreateRevoluteJoint(B2WorldId worldId, ref B2RevoluteJointDef def) { B2_CHECK_DEF(ref def); + Debug.Assert(def.lowerAngle <= def.upperAngle); + Debug.Assert(def.lowerAngle >= -0.95f * B2_PI); + Debug.Assert(def.upperAngle <= 0.95f * B2_PI); + B2World world = b2GetWorldFromId(worldId); Debug.Assert(world.locked == false); @@ -552,10 +556,8 @@ public static B2JointId b2CreateRevoluteJoint(B2WorldId worldId, ref B2RevoluteJ joint.uj.revoluteJoint.upperImpulse = 0.0f; joint.uj.revoluteJoint.hertz = def.hertz; joint.uj.revoluteJoint.dampingRatio = def.dampingRatio; - joint.uj.revoluteJoint.lowerAngle = b2MinFloat(def.lowerAngle, def.upperAngle); - joint.uj.revoluteJoint.upperAngle = b2MaxFloat(def.lowerAngle, def.upperAngle); - joint.uj.revoluteJoint.lowerAngle = b2ClampFloat(joint.uj.revoluteJoint.lowerAngle, -B2_PI, B2_PI); - joint.uj.revoluteJoint.upperAngle = b2ClampFloat(joint.uj.revoluteJoint.upperAngle, -B2_PI, B2_PI); + joint.uj.revoluteJoint.lowerAngle = def.lowerAngle; + joint.uj.revoluteJoint.upperAngle = def.upperAngle; joint.uj.revoluteJoint.maxMotorTorque = def.maxMotorTorque; joint.uj.revoluteJoint.motorSpeed = def.motorSpeed; joint.uj.revoluteJoint.enableSpring = def.enableSpring; diff --git a/src/Box2D.NET/B2MathFunction.cs b/src/Box2D.NET/B2MathFunction.cs index 7b497984..d195e6d1 100644 --- a/src/Box2D.NET/B2MathFunction.cs +++ b/src/Box2D.NET/B2MathFunction.cs @@ -243,13 +243,14 @@ public static float b2Distance(B2Vec2 a, B2Vec2 b) } /// Convert a vector into a unit vector if possible, otherwise returns the zero vector. + /// todo MSVC is not inlining this function in several places per warning 4710 [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Vec2 b2Normalize(B2Vec2 v) { float length = MathF.Sqrt(v.X * v.X + v.Y * v.Y); if (length < FLT_EPSILON) { - return b2Vec2_zero; + return new B2Vec2(0.0f, 0.0f); } float invLength = 1.0f / length; @@ -257,6 +258,7 @@ public static B2Vec2 b2Normalize(B2Vec2 v) return n; } + /// Determines if the provided vector is normalized (norm(a) == 1). [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool b2IsNormalized(B2Vec2 a) { @@ -269,10 +271,10 @@ public static bool b2IsNormalized(B2Vec2 a) [MethodImpl(MethodImplOptions.AggressiveInlining)] public static B2Vec2 b2GetLengthAndNormalize(ref float length, B2Vec2 v) { - length = b2Length(v); + length = MathF.Sqrt(v.X * v.X + v.Y * v.Y); if (length < FLT_EPSILON) { - return b2Vec2_zero; + return new B2Vec2(0.0f, 0.0f); } float invLength = 1.0f / length; @@ -353,7 +355,10 @@ public static B2Rot b2NLerp(B2Rot q1, B2Rot q2, float t) s = omt * q1.s + t * q2.s, }; - return b2NormalizeRot(q); + float mag = MathF.Sqrt(q.s * q.s + q.c * q.c); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + B2Rot qn = new B2Rot(q.c * invMag, q.s * invMag); + return qn; } /// Compute the angular velocity necessary to rotate between two rotations over a give time @@ -614,6 +619,24 @@ public static B2AABB b2AABB_Union(B2AABB a, B2AABB b) return c; } + /// Compute the bounding box of an array of circles + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2AABB b2MakeAABB(ReadOnlySpan points, int count, float radius) + { + Debug.Assert(count > 0); + B2AABB a = new B2AABB(points[0], points[0]); + for (int i = 1; i < count; ++i) + { + a.lowerBound = b2Min(a.lowerBound, points[i]); + a.upperBound = b2Max(a.upperBound, points[i]); + } + + B2Vec2 r = new B2Vec2(radius, radius); + a.lowerBound = b2Sub(a.lowerBound, r); + a.upperBound = b2Add(a.upperBound, r); + + return a; + } /// Signed separation of a point from a plane [MethodImpl(MethodImplOptions.AggressiveInlining)] diff --git a/src/Box2D.NET/B2Movers.cs b/src/Box2D.NET/B2Movers.cs index b64bebc8..8dc72546 100644 --- a/src/Box2D.NET/B2Movers.cs +++ b/src/Box2D.NET/B2Movers.cs @@ -10,14 +10,18 @@ namespace Box2D.NET { public static class B2Movers { - public static B2PlaneSolverResult b2SolvePlanes(B2Vec2 initialPosition, Span planes, int count) + /// Solves the position of a mover that satisfies the given collision planes. + /// @param position this must be the position used to generate the collision planes + /// @param planes the collision planes + /// @param count the number of collision planes + public static B2PlaneSolverResult b2SolvePlanes(B2Vec2 position, Span planes, int count) { for (int i = 0; i < count; ++i) { planes[i].push = 0.0f; } - B2Vec2 position = new B2Vec2(); + B2Vec2 delta = new B2Vec2(); float tolerance = B2_LINEAR_SLOP; int iteration; @@ -29,7 +33,7 @@ public static B2PlaneSolverResult b2SolvePlanes(B2Vec2 initialPosition, Span 0.0f) //{ // continue; @@ -41,7 +45,7 @@ public static B2PlaneSolverResult b2SolvePlanes(B2Vec2 initialPosition, Span planes, int count) { B2Vec2 v = vector; diff --git a/src/Box2D.NET/B2PlaneResult.cs b/src/Box2D.NET/B2PlaneResult.cs index 9a9cb6b4..6345d646 100644 --- a/src/Box2D.NET/B2PlaneResult.cs +++ b/src/Box2D.NET/B2PlaneResult.cs @@ -4,16 +4,18 @@ namespace Box2D.NET { + /// These are the collision planes returned from b2World_CollideMover public struct B2PlaneResult { + /// The collision plane between the mover and a convex shape public B2Plane plane; - public B2Vec2 point; + + /// Did the collision register a hit? If not this plane should be ignored. public bool hit; - public B2PlaneResult(B2Plane plane, B2Vec2 point, bool hit) + public B2PlaneResult(B2Plane plane, bool hit) { this.plane = plane; - this.point = point; this.hit = hit; } } diff --git a/src/Box2D.NET/B2PlaneSolverResult.cs b/src/Box2D.NET/B2PlaneSolverResult.cs index 360adaa5..0ae5044e 100644 --- a/src/Box2D.NET/B2PlaneSolverResult.cs +++ b/src/Box2D.NET/B2PlaneSolverResult.cs @@ -4,9 +4,13 @@ namespace Box2D.NET { + /// Result returned by b2SolvePlane public struct B2PlaneSolverResult { + /// The final position of the mover public B2Vec2 position; + + /// The number of iterations used by the plane solver. For diagnostics. public int iterationCount; public B2PlaneSolverResult(B2Vec2 position, int iterationCount) diff --git a/src/Box2D.NET/B2RevoluteJointDef.cs b/src/Box2D.NET/B2RevoluteJointDef.cs index 2ed799f9..d616a07c 100644 --- a/src/Box2D.NET/B2RevoluteJointDef.cs +++ b/src/Box2D.NET/B2RevoluteJointDef.cs @@ -46,10 +46,10 @@ public struct B2RevoluteJointDef /// A flag to enable joint limits public bool enableLimit; - /// The lower angle for the joint limit in radians + /// The lower angle for the joint limit in radians. Minimum of -0.95*pi radians. public float lowerAngle; - /// The upper angle for the joint limit in radians + /// The upper angle for the joint limit in radians. Maximum of 0.95*pi radians. public float upperAngle; /// A flag to enable the joint motor diff --git a/src/Box2D.NET/B2ShapeProxy.cs b/src/Box2D.NET/B2ShapeProxy.cs index 16b4b31e..be96d38b 100644 --- a/src/Box2D.NET/B2ShapeProxy.cs +++ b/src/Box2D.NET/B2ShapeProxy.cs @@ -5,15 +5,16 @@ namespace Box2D.NET { /// A distance proxy is used by the GJK algorithm. It encapsulates any shape. + /// You can provide between 1 and B2_MAX_POLYGON_VERTICES and a radius. public struct B2ShapeProxy { /// The point cloud public B2FixedArray8 points; - /// The number of points + /// The number of points. Must be greater than 0. public int count; - /// The external radius of the point cloud + /// The external radius of the point cloud. May be zero. public float radius; } } diff --git a/src/Box2D.NET/B2Shapes.cs b/src/Box2D.NET/B2Shapes.cs index f020bfb5..72a042c2 100644 --- a/src/Box2D.NET/B2Shapes.cs +++ b/src/Box2D.NET/B2Shapes.cs @@ -338,7 +338,6 @@ public static void b2DestroyShape(B2ShapeId shapeId, bool updateBodyMass) // need to wake bodies because this might be a static body bool wakeBodies = true; - B2Body body = b2Array_Get(ref world.bodies, shape.bodyId); b2DestroyShapeInternal(world, shape, body, wakeBodies); @@ -506,7 +505,6 @@ public static void b2DestroyChain(B2ChainId chainId) } B2ChainShape chain = b2GetChainShape(world, chainId); - bool wakeBodies = true; B2Body body = b2Array_Get(ref world.bodies, chain.bodyId); @@ -538,6 +536,7 @@ public static void b2DestroyChain(B2ChainId chainId) { int shapeId = chain.shapeIndices[i]; B2Shape shape = b2Array_Get(ref world.shapes, shapeId); + bool wakeBodies = true; b2DestroyShapeInternal(world, shape, body, wakeBodies); } @@ -909,7 +908,6 @@ public static B2PlaneResult b2CollideMover(B2Shape shape, B2Transform transform, } result.plane.normal = b2RotateVector(transform.q, result.plane.normal); - result.point = b2TransformPoint(ref transform, result.point); return result; } @@ -985,6 +983,9 @@ public static object b2Shape_GetUserData(B2ShapeId shapeId) return shape.userData; } + /// Returns true if the shape is a sensor. It is not possible to change a shape + /// from sensor to solid dynamically because this breaks the contract for + /// sensor events. public static bool b2Shape_IsSensor(B2ShapeId shapeId) { B2World world = b2GetWorld(shapeId.world0); diff --git a/src/Box2D.NET/B2Solvers.cs b/src/Box2D.NET/B2Solvers.cs index cb031bee..6c7f215b 100644 --- a/src/Box2D.NET/B2Solvers.cs +++ b/src/Box2D.NET/B2Solvers.cs @@ -562,7 +562,7 @@ public static void b2FinalizeBodiesTask(int startIndex, int endIndex, uint threa ushort worldId = world.worldId; - // The body move event array has should already have the correct size + // The body move event array should already have the correct size Debug.Assert(endIndex <= world.bodyMoveEvents.count); B2BodyMoveEvent[] moveEvents = world.bodyMoveEvents.data; diff --git a/src/Box2D.NET/B2Vec2.cs b/src/Box2D.NET/B2Vec2.cs index 8b4f6c15..0bb1b987 100644 --- a/src/Box2D.NET/B2Vec2.cs +++ b/src/Box2D.NET/B2Vec2.cs @@ -2,6 +2,7 @@ // SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) // SPDX-License-Identifier: MIT +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; namespace Box2D.NET @@ -28,45 +29,52 @@ public B2Vec2(float x, float y) */ /// Unary negate a vector - public static B2Vec2 operator-( B2Vec2 a ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2Vec2 operator -(B2Vec2 a) { return new B2Vec2(-a.X, -a.Y); } - + /// Binary vector addition - public static B2Vec2 operator+( B2Vec2 a, B2Vec2 b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2Vec2 operator +(B2Vec2 a, B2Vec2 b) { return new B2Vec2(a.X + b.X, a.Y + b.Y); } - + /// Binary vector subtraction - public static B2Vec2 operator-( B2Vec2 a, B2Vec2 b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2Vec2 operator -(B2Vec2 a, B2Vec2 b) { return new B2Vec2(a.X - b.X, a.Y - b.Y); } - + /// Binary scalar and vector multiplication - public static B2Vec2 operator*( float a, B2Vec2 b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2Vec2 operator *(float a, B2Vec2 b) { return new B2Vec2(a * b.X, a * b.Y); } - + /// Binary scalar and vector multiplication - public static B2Vec2 operator*( B2Vec2 a, float b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static B2Vec2 operator *(B2Vec2 a, float b) { return new B2Vec2(a.X * b, a.Y * b); } - + /// Binary vector equality - public static bool operator==( B2Vec2 a, B2Vec2 b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator ==(B2Vec2 a, B2Vec2 b) { return a.X == b.X && a.Y == b.Y; } - + /// Binary vector inequality - public static bool operator!=( B2Vec2 a, B2Vec2 b ) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool operator !=(B2Vec2 a, B2Vec2 b) { return a.X != b.X || a.Y != b.Y; } } -} +} \ No newline at end of file diff --git a/src/Box2D.NET/B2WorldOverlapContext.cs b/src/Box2D.NET/B2WorldOverlapContext.cs index 515cc60c..d8973874 100644 --- a/src/Box2D.NET/B2WorldOverlapContext.cs +++ b/src/Box2D.NET/B2WorldOverlapContext.cs @@ -10,16 +10,14 @@ public struct B2WorldOverlapContext public b2OverlapResultFcn fcn; public B2QueryFilter filter; public B2ShapeProxy proxy; - public B2Transform transform; public object userContext; - public B2WorldOverlapContext(B2World world, b2OverlapResultFcn fcn, B2QueryFilter filter, B2ShapeProxy proxy, B2Transform transform, object userContext) + public B2WorldOverlapContext(B2World world, b2OverlapResultFcn fcn, B2QueryFilter filter, B2ShapeProxy proxy, object userContext) { this.world = world; this.fcn = fcn; this.filter = filter; this.proxy = proxy; - this.transform = transform; this.userContext = userContext; } } diff --git a/src/Box2D.NET/B2Worlds.cs b/src/Box2D.NET/B2Worlds.cs index 618a627c..3bcb3f5e 100644 --- a/src/Box2D.NET/B2Worlds.cs +++ b/src/Box2D.NET/B2Worlds.cs @@ -1607,7 +1607,7 @@ public static bool b2World_IsValid(B2WorldId id) public static bool b2Body_IsValid(B2BodyId id) { - if (id.world0 < 0 || B2_MAX_WORLDS <= id.world0) + if (B2_MAX_WORLDS <= id.world0) { // invalid world return false; @@ -1678,7 +1678,7 @@ public static bool b2Shape_IsValid(B2ShapeId id) public static bool b2Chain_IsValid(B2ChainId id) { - if (id.world0 < 0 || B2_MAX_WORLDS <= id.world0) + if (B2_MAX_WORLDS <= id.world0) { return false; } @@ -1710,7 +1710,7 @@ public static bool b2Chain_IsValid(B2ChainId id) public static bool b2Joint_IsValid(B2JointId id) { - if (id.world0 < 0 || B2_MAX_WORLDS <= id.world0) + if (B2_MAX_WORLDS <= id.world0) { return false; } @@ -2161,7 +2161,7 @@ public static bool TreeOverlapCallback(int proxyId, ulong userData, ref B2WorldO input.proxyA = worldContext.proxy; input.proxyB = b2MakeShapeDistanceProxy(shape); input.transformA = b2Transform_identity; - input.transformB = b2InvMulTransforms(worldContext.transform, transform); + input.transformB = transform; input.useRadii = true; B2SimplexCache cache = new B2SimplexCache(); @@ -2178,101 +2178,27 @@ public static bool TreeOverlapCallback(int proxyId, ulong userData, ref B2WorldO return result; } - public static B2TreeStats b2World_OverlapPoint(B2WorldId worldId, B2Vec2 point, B2Transform transform, B2QueryFilter filter, - b2OverlapResultFcn fcn, object context) - { - B2Circle circle = new B2Circle(point, 0.0f); - return b2World_OverlapCircle(worldId, ref circle, transform, filter, fcn, context); - } - - public static B2TreeStats b2World_OverlapCircle(B2WorldId worldId, ref B2Circle circle, B2Transform transform, B2QueryFilter filter, - b2OverlapResultFcn fcn, object context) - { - B2TreeStats treeStats = new B2TreeStats(); - - B2World world = b2GetWorldFromId(worldId); - Debug.Assert(world.locked == false); - if (world.locked) - { - return treeStats; - } - - Debug.Assert(b2IsValidVec2(transform.p)); - Debug.Assert(b2IsValidRotation(transform.q)); - - B2AABB aabb = b2ComputeCircleAABB(ref circle, transform); - B2WorldOverlapContext worldContext = new B2WorldOverlapContext( - world, fcn, filter, b2MakeProxy(circle.center, 1, circle.radius), transform, context - ); - - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) - { - B2TreeStats treeResult = - b2DynamicTree_Query(world.broadPhase.trees[i], aabb, filter.maskBits, TreeOverlapCallback, ref worldContext); - - treeStats.nodeVisits += treeResult.nodeVisits; - treeStats.leafVisits += treeResult.leafVisits; - } - - return treeStats; - } - - public static B2TreeStats b2World_OverlapCapsule(B2WorldId worldId, ref B2Capsule capsule, B2Transform transform, B2QueryFilter filter, - b2OverlapResultFcn fcn, object context) - { - B2TreeStats treeStats = new B2TreeStats(); - - B2World world = b2GetWorldFromId(worldId); - Debug.Assert(world.locked == false); - if (world.locked) - { - return treeStats; - } - - Debug.Assert(b2IsValidVec2(transform.p)); - Debug.Assert(b2IsValidRotation(transform.q)); - - B2AABB aabb = b2ComputeCapsuleAABB(ref capsule, transform); - B2WorldOverlapContext worldContext = new B2WorldOverlapContext( - world, fcn, filter, b2MakeProxy(capsule.center1, capsule.center2, 2, capsule.radius), transform, context - ); - - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) - { - B2TreeStats treeResult = - b2DynamicTree_Query(world.broadPhase.trees[i], aabb, filter.maskBits, TreeOverlapCallback, ref worldContext); - - treeStats.nodeVisits += treeResult.nodeVisits; - treeStats.leafVisits += treeResult.leafVisits; - } - - return treeStats; - } - - public static B2TreeStats b2World_OverlapPolygon(B2WorldId worldId, ref B2Polygon polygon, B2Transform transform, B2QueryFilter filter, - b2OverlapResultFcn fcn, object context) + /// Overlap test for all shapes that overlap the provided shape proxy. + public static B2TreeStats b2World_OverlapShape( B2WorldId worldId, ref B2ShapeProxy proxy, B2QueryFilter filter, b2OverlapResultFcn fcn, object context ) { B2TreeStats treeStats = new B2TreeStats(); - B2World world = b2GetWorldFromId(worldId); - Debug.Assert(world.locked == false); - if (world.locked) + B2World world = b2GetWorldFromId( worldId ); + Debug.Assert( world.locked == false ); + if ( world.locked ) { return treeStats; } - Debug.Assert(b2IsValidVec2(transform.p)); - Debug.Assert(b2IsValidRotation(transform.q)); - - B2AABB aabb = b2ComputePolygonAABB(ref polygon, transform); + B2AABB aabb = b2MakeAABB( proxy.points.AsSpan(), proxy.count, proxy.radius ); B2WorldOverlapContext worldContext = new B2WorldOverlapContext( - world, fcn, filter, b2MakeProxy(polygon.vertices.AsSpan(), polygon.count, polygon.radius), transform, context + world, fcn, filter, proxy, context ); - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) + for ( int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i ) { B2TreeStats treeResult = - b2DynamicTree_Query(world.broadPhase.trees[i], aabb, filter.maskBits, TreeOverlapCallback, ref worldContext); + b2DynamicTree_Query( world.broadPhase.trees[i], aabb, filter.maskBits, TreeOverlapCallback, ref worldContext ); treeStats.nodeVisits += treeResult.nodeVisits; treeStats.leafVisits += treeResult.leafVisits; @@ -2443,110 +2369,22 @@ public static float ShapeCastCallback(ref B2ShapeCastInput input, int proxyId, u { B2ShapeId id = new B2ShapeId(shapeId + 1, world.worldId, shape.generation); float fraction = worldContext.fcn(id, output.point, output.normal, output.fraction, worldContext.userContext); - + // The user may return -1 to skip this shape - if ( 0.0f <= fraction && fraction <= 1.0f ) + if (0.0f <= fraction && fraction <= 1.0f) { worldContext.fraction = fraction; } - + return fraction; } return input.maxFraction; } - /// Cast a circle through the world. Similar to a cast ray except that a circle is cast instead of a point. - /// @see b2World_CastRay - public static B2TreeStats b2World_CastCircle(B2WorldId worldId, ref B2Circle circle, B2Vec2 translation, B2QueryFilter filter, - b2CastResultFcn fcn, object context) - { - B2TreeStats treeStats = new B2TreeStats(); - - B2World world = b2GetWorldFromId(worldId); - Debug.Assert(world.locked == false); - if (world.locked) - { - return treeStats; - } - - Debug.Assert(b2IsValidVec2(translation)); - - B2ShapeCastInput input = new B2ShapeCastInput(); - input.proxy.points[0] = circle.center; - input.proxy.count = 1; - input.proxy.radius = circle.radius; - input.translation = translation; - input.maxFraction = 1.0f; - - B2WorldRayCastContext worldContext = new B2WorldRayCastContext(world, fcn, filter, 1.0f, context); - - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) - { - B2TreeStats treeResult = - b2DynamicTree_ShapeCast(world.broadPhase.trees[i], ref input, filter.maskBits, ShapeCastCallback, ref worldContext); - treeStats.nodeVisits += treeResult.nodeVisits; - treeStats.leafVisits += treeResult.leafVisits; - - if (worldContext.fraction == 0.0f) - { - return treeStats; - } - - input.maxFraction = worldContext.fraction; - } - - return treeStats; - } - - /// Cast a capsule through the world. Similar to a cast ray except that a capsule is cast instead of a point. - /// @see b2World_CastRay - public static B2TreeStats b2World_CastCapsule(B2WorldId worldId, ref B2Capsule capsule, B2Vec2 translation, B2QueryFilter filter, - b2CastResultFcn fcn, object context) - { - B2TreeStats treeStats = new B2TreeStats(); - - B2World world = b2GetWorldFromId(worldId); - Debug.Assert(world.locked == false); - if (world.locked) - { - return treeStats; - } - - Debug.Assert(b2IsValidVec2(translation)); - - B2ShapeCastInput input = new B2ShapeCastInput(); - // Note: these world space points get transformed into local space in b2ShapeCastShape - input.proxy.points[0] = capsule.center1; - input.proxy.points[1] = capsule.center2; - input.proxy.count = 2; - input.proxy.radius = capsule.radius; - input.translation = translation; - input.maxFraction = 1.0f; - - B2WorldRayCastContext worldContext = new B2WorldRayCastContext(world, fcn, filter, 1.0f, context); - - for (int i = 0; i < (int)B2BodyType.b2_bodyTypeCount; ++i) - { - B2TreeStats treeResult = - b2DynamicTree_ShapeCast(world.broadPhase.trees[i], ref input, filter.maskBits, ShapeCastCallback, ref worldContext); - treeStats.nodeVisits += treeResult.nodeVisits; - treeStats.leafVisits += treeResult.leafVisits; - - if (worldContext.fraction == 0.0f) - { - return treeStats; - } - - input.maxFraction = worldContext.fraction; - } - - return treeStats; - } - - /// Cast a polygon through the world. Similar to a cast ray except that a polygon is cast instead of a point. + /// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. /// @see b2World_CastRay - public static B2TreeStats b2World_CastPolygon(B2WorldId worldId, ref B2Polygon polygon, B2Vec2 translation, B2QueryFilter filter, + public static B2TreeStats b2World_CastShape(B2WorldId worldId, ref B2ShapeProxy proxy, B2Vec2 translation, B2QueryFilter filter, b2CastResultFcn fcn, object context) { B2TreeStats treeStats = new B2TreeStats(); @@ -2561,14 +2399,7 @@ public static B2TreeStats b2World_CastPolygon(B2WorldId worldId, ref B2Polygon p Debug.Assert(b2IsValidVec2(translation)); B2ShapeCastInput input = new B2ShapeCastInput(); - // Note: these world space points get transformed into local space in b2ShapeCastShape - for (int i = 0; i < polygon.count; ++i) - { - input.proxy.points[i] = polygon.vertices[i]; - } - - input.proxy.count = polygon.count; - input.proxy.radius = polygon.radius; + input.proxy = proxy; input.translation = translation; input.maxFraction = 1.0f; diff --git a/test/Box2D.NET.Test/B2DeterminismTest.cs b/test/Box2D.NET.Test/B2DeterminismTest.cs index fbba126d..c00f9f97 100644 --- a/test/Box2D.NET.Test/B2DeterminismTest.cs +++ b/test/Box2D.NET.Test/B2DeterminismTest.cs @@ -182,7 +182,7 @@ public void MultithreadingTest() } } - // Test cross platform determinism based on the FallingHinges sample. + // Test cross-platform determinism. [Test] public void CrossPlatformTest() {