diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel.cs index 4dcdc876..e8fee0e0 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel.cs @@ -311,7 +311,7 @@ public override void UpdateGui() base.UpdateGui(); float fontSize = ImGui.GetFontSize(); - float height = 6.0f * fontSize; + float height = 10.0f * fontSize; ImGui.SetNextWindowPos(new Vector2(0.5f * fontSize, m_camera.m_height - height - 2.0f * fontSize), ImGuiCond.Once); ImGui.SetNextWindowSize(new Vector2(15.0f * fontSize, height)); diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel24.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel24.cs new file mode 100644 index 00000000..da7ec604 --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkBarrel24.cs @@ -0,0 +1,91 @@ +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; + +namespace Box2D.NET.Samples.Samples.Benchmarks; + +// This is used to compare performance with Box2D v2.4 +public class BenchmarkBarrel24 : Sample +{ + private static readonly int benchmarkBarrel24 = SampleFactory.Shared.RegisterSample("Benchmark", "Barrel 2.4", Create); + + private static Sample Create(SampleContext context) + { + return new BenchmarkBarrel24(context); + } + + public BenchmarkBarrel24(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(8.0f, 53.0f); + m_context.camera.m_zoom = 25.0f * 2.35f; + } + + float groundSize = 25.0f; + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeBox(groundSize, 1.2f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(groundId, ref shapeDef, ref box); + + bodyDef.rotation = b2MakeRot(0.5f * B2_PI); + bodyDef.position = new B2Vec2(groundSize, 2.0f * groundSize); + groundId = b2CreateBody(m_worldId, ref bodyDef); + + box = b2MakeBox(2.0f * groundSize, 1.2f); + b2CreatePolygonShape(groundId, ref shapeDef, ref box); + + bodyDef.position = new B2Vec2(-groundSize, 2.0f * groundSize); + groundId = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(groundId, ref shapeDef, ref box); + } + + int num = 26; + float rad = 0.5f; + + float shift = rad * 2.0f; + float centerx = shift * num / 2.0f; + float centery = shift / 2.0f; + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.material.friction = 0.5f; + + B2Polygon cuboid = b2MakeSquare(0.5f); + + // b2Polygon top = b2MakeOffsetBox(0.8f, 0.2f, {0.0f, 0.8f}, 0.0f); + // b2Polygon leftLeg = b2MakeOffsetBox(0.2f, 0.5f, {-0.6f, 0.5f}, 0.0f); + // b2Polygon rightLeg = b2MakeOffsetBox(0.2f, 0.5f, {0.6f, 0.5f}, 0.0f); + +#if DEBUG + int numj = 5; +#else + int numj = 5 * num; +#endif + for (int i = 0; i < num; ++i) + { + float x = i * shift - centerx; + + for (int j = 0; j < numj; ++j) + { + float y = j * shift + centery + 2.0f; + + bodyDef.position = new B2Vec2(x, y); + + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(bodyId, ref shapeDef, ref cuboid); + } + } + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCapacity.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCapacity.cs new file mode 100644 index 00000000..f0ec356c --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkCapacity.cs @@ -0,0 +1,95 @@ +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.B2Worlds; + +namespace Box2D.NET.Samples.Samples.Benchmarks; + +public class BenchmarkCapacity : Sample +{ + private static readonly int benchmarkCapacity = SampleFactory.Shared.RegisterSample("Benchmark", "Capacity", Create); + + private B2Polygon m_square; + private int m_reachCount; + private bool m_done; + + private static Sample Create(SampleContext context) + { + return new BenchmarkCapacity(context); + } + + public BenchmarkCapacity(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 150.0f); + m_context.camera.m_zoom = 200.0f; + } + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.position.Y = -5.0f; + B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeBox(800.0f, 5.0f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(groundId, ref shapeDef, ref box); + } + + m_square = b2MakeSquare(0.5f); + m_done = false; + m_reachCount = 0; + } + + public override void Step() + { + base.Step(); + + float millisecondLimit = 20.0f; + + B2Profile profile = b2World_GetProfile(m_worldId); + if (profile.step > millisecondLimit) + { + m_reachCount += 1; + if (m_reachCount > 60) + { + // Hit the millisecond limit 60 times in a row + m_done = true; + } + } + else + { + m_reachCount = 0; + } + + if (m_done == true) + { + return; + } + + if ((m_stepCount & 0x1F) != 0x1F) + { + return; + } + + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position.Y = 200.0f; + + B2ShapeDef shapeDef = b2DefaultShapeDef(); + + int count = 200; + float x = -1.0f * count; + for (int i = 0; i < count; ++i) + { + bodyDef.position.X = x; + bodyDef.position.Y += 0.5f; + + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + b2CreatePolygonShape(bodyId, ref shapeDef, ref m_square); + + x += 2.0f; + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSensor.cs b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSensor.cs index 56453f25..e6a691f8 100644 --- a/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSensor.cs +++ b/src/Box2D.NET.Samples/Samples/Benchmarks/BenchmarkSensor.cs @@ -83,13 +83,25 @@ public BenchmarkSensor(SampleContext context) : base(context) shapeDef.enableSensorEvents = true; float yStart = 10.0f; - + m_filterRow = m_rowCount >> 1; + for (int j = 0; j < m_rowCount; ++j) { m_passiveSensors[j] = new ShapeUserData(); m_passiveSensors[j].row = j; m_passiveSensors[j].active = false; shapeDef.userData = m_passiveSensors[j]; + + if ( j == m_filterRow ) + { + shapeDef.enableCustomFiltering = true; + shapeDef.material.customColor = (uint)B2HexColor.b2_colorFuchsia; + } + else + { + shapeDef.enableCustomFiltering = false; + shapeDef.material.customColor = 0; + } float y = j * shift + yStart; for (int i = 0; i < m_columnCount; ++i) @@ -104,7 +116,6 @@ public BenchmarkSensor(SampleContext context) : base(context) m_maxBeginCount = 0; m_maxEndCount = 0; m_lastStepCount = 0; - m_filterRow = m_rowCount >> 1; } void CreateRow(float y) diff --git a/src/Box2D.NET.Samples/Samples/Continuous/ChainDrop.cs b/src/Box2D.NET.Samples/Samples/Continuous/ChainDrop.cs index 7a26a031..073769ff 100644 --- a/src/Box2D.NET.Samples/Samples/Continuous/ChainDrop.cs +++ b/src/Box2D.NET.Samples/Samples/Continuous/ChainDrop.cs @@ -77,7 +77,7 @@ void Launch() B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.0f), 0.5f); m_shapeId = b2CreateCircleShape(m_bodyId, ref shapeDef, ref circle); - //b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0 }, 0.25f }; + //b2Capsule capsule = { { -0.5f, 0.0f }, { 0.5f, 0.0f }, 0.25f }; //m_shapeId = b2CreateCapsuleShape( m_bodyId, &shapeDef, &capsule ); //float h = 0.5f; diff --git a/src/Box2D.NET.Samples/Samples/Events/JointEvent.cs b/src/Box2D.NET.Samples/Samples/Events/JointEvent.cs index b9ae3345..d764c176 100644 --- a/src/Box2D.NET.Samples/Samples/Events/JointEvent.cs +++ b/src/Box2D.NET.Samples/Samples/Events/JointEvent.cs @@ -99,8 +99,8 @@ public JointEvent(SampleContext context) : base(context) jointDef.@base.bodyIdA = groundId; jointDef.@base.bodyIdB = bodyId; jointDef.@base.localFrameA.p = position; - jointDef.maxForce = 1000.0f; - jointDef.maxTorque = 20.0f; + jointDef.maxVelocityForce = 1000.0f; + jointDef.maxVelocityTorque = 20.0f; jointDef.@base.forceThreshold = forceThreshold; jointDef.@base.torqueThreshold = torqueThreshold; jointDef.@base.collideConnected = true; diff --git a/src/Box2D.NET.Samples/Samples/Joints/BreakableJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/BreakableJoint.cs index 9238e4ae..cca5e694 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/BreakableJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/BreakableJoint.cs @@ -95,8 +95,8 @@ public BreakableJoint(SampleContext context) : base(context) jointDef.@base.bodyIdA = groundId; jointDef.@base.bodyIdB = bodyId; jointDef.@base.localFrameA.p = position; - jointDef.maxForce = 1000.0f; - jointDef.maxTorque = 20.0f; + jointDef.maxVelocityForce = 1000.0f; + jointDef.maxVelocityTorque = 20.0f; jointDef.@base.collideConnected = true; m_jointIds[index] = b2CreateMotorJoint(m_worldId, ref jointDef); } diff --git a/src/Box2D.NET.Samples/Samples/Joints/DistanceJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/DistanceJoint.cs index bae56b72..180ca1f7 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/DistanceJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/DistanceJoint.cs @@ -27,6 +27,8 @@ public class DistanceJoint : Sample private float m_hertz; private float m_dampingRatio; private float m_length; + private float m_tensionForce; + private float m_compressionForce; private float m_minLength; private float m_maxLength; private bool m_enableSpring; @@ -51,11 +53,13 @@ public DistanceJoint(SampleContext context) : base(context) } m_count = 0; - m_hertz = 2.0f; + m_hertz = 5.0f; m_dampingRatio = 0.5f; m_length = 1.0f; m_minLength = m_length; m_maxLength = m_length; + m_tensionForce = 2000.0f; + m_compressionForce = 100.0f; m_enableSpring = false; m_enableLimit = false; @@ -97,6 +101,8 @@ void CreateScene(int newCount) jointDef.hertz = m_hertz; jointDef.dampingRatio = m_dampingRatio; jointDef.length = m_length; + jointDef.lowerSpringForce = -m_tensionForce; + jointDef.upperSpringForce = m_compressionForce; jointDef.minLength = m_minLength; jointDef.maxLength = m_maxLength; jointDef.enableSpring = m_enableSpring; @@ -107,7 +113,7 @@ void CreateScene(int newCount) { B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; - bodyDef.angularDamping = 0.1f; + bodyDef.angularDamping = 1.0f; bodyDef.position = new B2Vec2(m_length * (i + 1.0f), yOffset); m_bodyIds[i] = b2CreateBody(m_worldId, ref bodyDef); b2CreateCircleShape(m_bodyIds[i], ref shapeDef, ref circle); @@ -129,12 +135,12 @@ public override void UpdateGui() base.UpdateGui(); float fontSize = ImGui.GetFontSize(); - float height = 240.0f; + float height = 20.0f * fontSize; ImGui.SetNextWindowPos(new Vector2(0.5f * fontSize, m_camera.m_height - height - 2.0f * fontSize), ImGuiCond.Once); - ImGui.SetNextWindowSize(new Vector2(180.0f, height)); + ImGui.SetNextWindowSize(new Vector2(18.0f * fontSize, height)); ImGui.Begin("Distance Joint", ImGuiWindowFlags.NoResize); - ImGui.PushItemWidth(100.0f); + ImGui.PushItemWidth(10.0f * fontSize); if (ImGui.SliderFloat("Length", ref m_length, 0.1f, 4.0f, "%3.1f")) { @@ -156,6 +162,25 @@ public override void UpdateGui() if (m_enableSpring) { + + if ( ImGui.SliderFloat( "Tension", ref m_tensionForce, 0.0f, 4000.0f ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetSpringForceRange( m_jointIds[i], -m_tensionForce, m_compressionForce ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + + if ( ImGui.SliderFloat( "Compression", ref m_compressionForce, 0.0f, 200.0f ) ) + { + for ( int i = 0; i < m_count; ++i ) + { + b2DistanceJoint_SetSpringForceRange( m_jointIds[i], -m_tensionForce, m_compressionForce ); + b2Joint_WakeBodies( m_jointIds[i] ); + } + } + if (ImGui.SliderFloat("Hertz", ref m_hertz, 0.0f, 15.0f, "%3.1f")) { for (int i = 0; i < m_count; ++i) diff --git a/src/Box2D.NET.Samples/Samples/Joints/MotionLocks.cs b/src/Box2D.NET.Samples/Samples/Joints/MotionLocks.cs index 66a149da..4cb60e9f 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/MotionLocks.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/MotionLocks.cs @@ -93,8 +93,8 @@ public MotionLocks(SampleContext context) : base(context) jointDef.@base.bodyIdA = groundId; jointDef.@base.bodyIdB = m_bodyIds[index]; jointDef.@base.localFrameA.p = position; - jointDef.maxForce = 200.0f; - jointDef.maxTorque = 200.0f; + jointDef.maxVelocityForce = 200.0f; + jointDef.maxVelocityTorque = 200.0f; b2CreateMotorJoint(m_worldId, ref jointDef); } diff --git a/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs b/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs index c621d7a8..c3a2b0d2 100644 --- a/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs +++ b/src/Box2D.NET.Samples/Samples/Joints/MotorJoint.cs @@ -18,18 +18,18 @@ namespace Box2D.NET.Samples.Samples.Joints; /// This test shows how to use a motor joint. A motor joint /// can be used to animate a dynamic body. With finite motor forces /// the body can be blocked by collision with other bodies. -/// By setting the correction factor to zero, the motor joint acts -/// like top-down dry friction. public class MotorJoint : Sample { private static readonly int SampleMotorJoint = SampleFactory.Shared.RegisterSample("Joints", "Motor Joint", Create); + private B2BodyId m_targetId; private B2BodyId m_bodyId; private B2JointId m_jointId; + private B2Transform m_transform; private float m_time; + private float m_speed; private float m_maxForce; private float m_maxTorque; - private float m_correctionFactor; private bool m_go; private B2Transform _transform; @@ -56,32 +56,70 @@ public MotorJoint(SampleContext context) : base(context) b2CreateSegmentShape(groundId, ref shapeDef, ref segment); } + m_transform = new B2Transform(new B2Vec2(0.0f, 8.0f), b2Rot_identity); + + // Define a target body + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_kinematicBody; + bodyDef.position = m_transform.p; + m_targetId = b2CreateBody(m_worldId, ref bodyDef); + } + // Define motorized body { B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; - bodyDef.position = new B2Vec2(0.0f, 8.0f); + bodyDef.position = m_transform.p; m_bodyId = b2CreateBody(m_worldId, ref bodyDef); B2Polygon box = b2MakeBox(2.0f, 0.5f); B2ShapeDef shapeDef = b2DefaultShapeDef(); - shapeDef.density = 1.0f; b2CreatePolygonShape(m_bodyId, ref shapeDef, ref box); - m_maxForce = 500.0f; + m_maxForce = 5000.0f; m_maxTorque = 500.0f; - m_correctionFactor = 0.3f; B2MotorJointDef jointDef = b2DefaultMotorJointDef(); - jointDef.@base.bodyIdA = groundId; + jointDef.@base.bodyIdA = m_targetId; jointDef.@base.bodyIdB = m_bodyId; - jointDef.maxForce = m_maxForce; - jointDef.maxTorque = m_maxTorque; - jointDef.correctionFactor = m_correctionFactor; + jointDef.linearHertz = 4.0f; + jointDef.linearDampingRatio = 0.7f; + jointDef.angularHertz = 4.0f; + jointDef.angularDampingRatio = 0.7f; + jointDef.maxSpringForce = m_maxForce; + jointDef.maxSpringTorque = m_maxTorque; m_jointId = b2CreateMotorJoint(m_worldId, ref jointDef); } + // Define spring body + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.position = new B2Vec2(-2.0f, 2.0f); + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + + B2Polygon box = b2MakeSquare(0.5f); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreatePolygonShape(bodyId, ref shapeDef, ref box); + + B2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.@base.bodyIdA = groundId; + jointDef.@base.bodyIdB = bodyId; + jointDef.@base.localFrameA.p = b2Add(bodyDef.position, new B2Vec2(0.25f, 0.25f)); + jointDef.@base.localFrameB.p = new B2Vec2(0.25f, 0.25f); + jointDef.linearHertz = 7.5f; + jointDef.linearDampingRatio = 0.7f; + jointDef.angularHertz = 7.5f; + jointDef.angularDampingRatio = 0.7f; + jointDef.maxSpringForce = 500.0f; + jointDef.maxSpringTorque = 10.0f; + + b2CreateMotorJoint(m_worldId, ref jointDef); + } + + m_speed = 1.0f; m_go = true; m_time = 0.0f; } @@ -97,23 +135,18 @@ public override void UpdateGui() ImGui.Begin("Motor Joint", ImGuiWindowFlags.NoResize); - if (ImGui.Checkbox("Go", ref m_go)) + if (ImGui.SliderFloat("Speed", ref m_speed, -5.0f, 5.0f, "%.0f")) { } if (ImGui.SliderFloat("Max Force", ref m_maxForce, 0.0f, 10000.0f, "%.0f")) { - b2MotorJoint_SetMaxForce(m_jointId, m_maxForce); + b2MotorJoint_SetMaxSpringForce(m_jointId, m_maxForce); } if (ImGui.SliderFloat("Max Torque", ref m_maxTorque, 0.0f, 10000.0f, "%.0f")) { - b2MotorJoint_SetMaxTorque(m_jointId, m_maxTorque); - } - - if (ImGui.SliderFloat("Correction", ref m_correctionFactor, 0.0f, 1.0f, "%.1f")) - { - b2MotorJoint_SetCorrectionFactor(m_jointId, m_correctionFactor); + b2MotorJoint_SetMaxVelocityTorque(m_jointId, m_maxTorque); } if (ImGui.Button("Apply Impulse")) @@ -127,19 +160,31 @@ public override void UpdateGui() public override void Step() { - if (m_go && m_context.settings.hertz > 0.0f) + float timeStep = m_context.settings.hertz > 0.0f ? 1.0f / m_context.settings.hertz : 0.0f; + + if (m_context.settings.pause) { - m_time += 1.0f / m_context.settings.hertz; + if (m_context.settings.singleStep == false) + { + timeStep = 0.0f; + } } - B2Vec2 linearOffset; - linearOffset.X = 6.0f * MathF.Sin(2.0f * m_time); - linearOffset.Y = 8.0f + 4.0f * MathF.Sin(1.0f * m_time); + if (timeStep > 0.0f) + { + m_time += m_speed * timeStep; + + B2Vec2 linearOffset; + linearOffset.X = 6.0f * MathF.Sin(2.0f * m_time); + linearOffset.Y = 8.0f + 4.0f * MathF.Sin(1.0f * m_time); + + float angularOffset = 2.0f * m_time; + m_transform = new B2Transform(linearOffset, b2MakeRot(angularOffset)); - float angularOffset = 2.0f * m_time; + b2Body_SetTargetTransform(m_targetId, m_transform, timeStep); + } - _transform = new B2Transform(linearOffset, b2MakeRot(angularOffset)); - b2Joint_SetLocalFrameA(m_jointId, _transform); + m_context.draw.DrawTransform(m_transform); base.Step(); } diff --git a/src/Box2D.NET.Samples/Samples/Joints/TopDownFriction.cs b/src/Box2D.NET.Samples/Samples/Joints/TopDownFriction.cs new file mode 100644 index 00000000..0ff71da9 --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Joints/TopDownFriction.cs @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using System; +using System.Numerics; +using ImGuiNET; +using static Box2D.NET.B2Joints; +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; +using static Box2D.NET.B2Worlds; +using static Box2D.NET.Shared.RandomSupports; + +namespace Box2D.NET.Samples.Samples.Joints; + +public class TopDownFriction : Sample +{ + private static int sampleTopDownFriction = SampleFactory.Shared.RegisterSample("Joints", "Top Down Friction", Create); + + private static Sample Create(SampleContext context) + { + return new TopDownFriction(context); + } + + + private TopDownFriction(SampleContext context) : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 7.0f); + m_context.camera.m_zoom = 25.0f * 0.4f; + } + + B2BodyId groundId; + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + groundId = b2CreateBody(m_worldId, ref bodyDef); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + B2Segment segment = new B2Segment(new B2Vec2(-10.0f, 0.0f), new B2Vec2(10.0f, 0.0f)); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + + segment = new B2Segment(new B2Vec2(-10.0f, 0.0f), new B2Vec2(-10.0f, 20.0f)); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + + segment = new B2Segment(new B2Vec2(10.0f, 0.0f), new B2Vec2(10.0f, 20.0f)); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + + segment = new B2Segment(new B2Vec2(-10.0f, 20.0f), new B2Vec2(10.0f, 20.0f)); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + } + + B2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.@base.bodyIdA = groundId; + jointDef.@base.collideConnected = true; + jointDef.maxVelocityForce = 10.0f; + jointDef.maxVelocityTorque = 10.0f; + + B2Capsule capsule = new B2Capsule(new B2Vec2(-0.25f, 0.0f), new B2Vec2(0.25f, 0.0f), 0.25f); + B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.0f), 0.35f); + B2Polygon square = b2MakeSquare(0.35f); + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + bodyDef.gravityScale = 0.0f; + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.material.restitution = 0.8f; + + int n = 10; + float x = -5.0f, y = 15.0f; + for (int i = 0; i < n; ++i) + { + for (int j = 0; j < n; ++j) + { + bodyDef.position = new B2Vec2(x, y); + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + + int remainder = (n * i + j) % 4; + if (remainder == 0) + { + b2CreateCapsuleShape(bodyId, ref shapeDef, ref capsule); + } + else if (remainder == 1) + { + b2CreateCircleShape(bodyId, ref shapeDef, ref circle); + } + else if (remainder == 2) + { + b2CreatePolygonShape(bodyId, ref shapeDef, ref square); + } + else + { + B2Polygon poly = RandomPolygon(0.75f); + poly.radius = 0.1f; + b2CreatePolygonShape(bodyId, ref shapeDef, ref poly); + } + + jointDef.@base.bodyIdB = bodyId; + b2CreateMotorJoint(m_worldId, ref jointDef); + + x += 1.0f; + } + + x = -5.0f; + y -= 1.0f; + } + } + } + + public override void UpdateGui() + { + float fontSize = ImGui.GetFontSize(); + float height = 180.0f; + ImGui.SetNextWindowPos(new Vector2(0.5f * fontSize, m_camera.m_height - height - 2.0f * fontSize), ImGuiCond.Once); + ImGui.SetNextWindowSize(new Vector2(240.0f, height)); + + ImGui.Begin("Top Down Friction", ImGuiWindowFlags.NoResize); + + if (ImGui.Button("Explode")) + { + B2ExplosionDef def = b2DefaultExplosionDef(); + def.position = new B2Vec2(0.0f, 10.0f); + def.radius = 10.0f; + def.falloff = 5.0f; + def.impulsePerLength = 10.0f; + b2World_Explode(m_worldId, ref def); + + m_draw.DrawCircle(def.position, 10.0f, B2HexColor.b2_colorWhite); + } + + ImGui.End(); + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Sample.cs b/src/Box2D.NET.Samples/Samples/Sample.cs index e6473d45..ea9d72f6 100644 --- a/src/Box2D.NET.Samples/Samples/Sample.cs +++ b/src/Box2D.NET.Samples/Samples/Sample.cs @@ -16,7 +16,6 @@ using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Shapes; using static Box2D.NET.B2Worlds; -using static Box2D.NET.B2MouseJoints; using static Box2D.NET.Shared.RandomSupports; using static Box2D.NET.B2Diagnostics; @@ -43,11 +42,12 @@ public class Sample : IDisposable protected int m_taskCount; protected int m_threadCount; - protected B2BodyId m_groundBodyId; + protected B2BodyId m_mouseBodyId; public B2WorldId m_worldId; - public int m_stepCount; protected B2JointId m_mouseJointId; + protected B2Vec2 m_mousePoint; + public int m_stepCount; protected B2Profile m_maxProfile; protected B2Profile m_totalProfile; @@ -81,7 +81,8 @@ public Sample(SampleContext context) m_stepCount = 0; - m_groundBodyId = b2_nullBodyId; + m_mouseBodyId = b2_nullBodyId; + m_mousePoint = new B2Vec2(); m_maxProfile = new B2Profile(); m_totalProfile = new B2Profile(); @@ -161,7 +162,6 @@ public virtual void UpdateGui() aveProfile.pairs = scale * m_totalProfile.pairs; aveProfile.collide = scale * m_totalProfile.collide; aveProfile.solve = scale * m_totalProfile.solve; - aveProfile.mergeIslands = scale * m_totalProfile.mergeIslands; aveProfile.prepareStages = scale * m_totalProfile.prepareStages; aveProfile.solveConstraints = scale * m_totalProfile.solveConstraints; aveProfile.prepareConstraints = scale * m_totalProfile.prepareConstraints; @@ -185,7 +185,6 @@ public virtual void UpdateGui() DrawTextLine($"pairs [ave] (max) = {p.pairs,5:F2} [{aveProfile.pairs,6:F2}] ({m_maxProfile.pairs,6:F2})"); DrawTextLine($"collide [ave] (max) = {p.collide,5:F2} [{aveProfile.collide,6:F2}] ({m_maxProfile.collide,6:F2})"); DrawTextLine($"solve [ave] (max) = {p.solve,5:F2} [{aveProfile.solve,6:F2}] ({m_maxProfile.solve,6:F2})"); - DrawTextLine($"> merge islands [ave] (max) = {p.mergeIslands,5:F2} [{aveProfile.mergeIslands,6:F2}] ({m_maxProfile.mergeIslands,6:F2})"); DrawTextLine($"> prepare tasks [ave] (max) = {p.prepareStages,5:F2} [{aveProfile.prepareStages,6:F2}] ({m_maxProfile.prepareStages,6:F2})"); DrawTextLine($"> solve constraints [ave] (max) = {p.solveConstraints,5:F2} [{aveProfile.solveConstraints,6:F2}] ({m_maxProfile.solveConstraints,6:F2})"); DrawTextLine($">> prepare constraints [ave] (max) = {p.prepareConstraints,5:F2} [{aveProfile.prepareConstraints,6:F2}] ({m_maxProfile.prepareConstraints,6:F2})"); @@ -290,6 +289,8 @@ public virtual void MouseDown(B2Vec2 p, MouseButton button, KeyModifiers mod) box.lowerBound = b2Sub(p, d); box.upperBound = b2Add(p, d); + m_mousePoint = p; + // Query the world for overlapping shapes. QueryContext queryContext = new QueryContext(p, b2_nullBodyId); b2World_OverlapAABB(m_worldId, box, b2DefaultQueryFilter(), QueryCallback, queryContext); @@ -298,38 +299,43 @@ public virtual void MouseDown(B2Vec2 p, MouseButton button, KeyModifiers mod) { B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_kinematicBody; - m_groundBodyId = b2CreateBody(m_worldId, ref bodyDef); + bodyDef.position = p; + bodyDef.enableSleep = false; + m_mouseBodyId = b2CreateBody(m_worldId, ref bodyDef); - B2MouseJointDef jointDef = b2DefaultMouseJointDef(); - jointDef.@base.bodyIdA = m_groundBodyId; + B2MotorJointDef jointDef = b2DefaultMotorJointDef(); + jointDef.@base.bodyIdA = m_mouseBodyId; jointDef.@base.bodyIdB = queryContext.bodyId; - jointDef.@base.localFrameA.p = p; jointDef.@base.localFrameB.p = b2Body_GetLocalPoint(queryContext.bodyId, p); - jointDef.hertz = 7.5f; - jointDef.dampingRatio = 0.7f; - jointDef.maxForce = 100.0f * b2Body_GetMass(queryContext.bodyId) * b2Length(b2World_GetGravity(m_worldId)); - m_mouseJointId = b2CreateMouseJoint(m_worldId, ref jointDef); - - b2Body_SetAwake(queryContext.bodyId, true); + jointDef.linearHertz = 7.5f; + jointDef.linearDampingRatio = 0.7f; + + B2MassData massData = b2Body_GetMassData(queryContext.bodyId); + float g = b2Length(b2World_GetGravity(m_worldId)); + float mg = massData.mass * g; + jointDef.maxSpringForce = 100.0f * mg; + + if (massData.mass > 0.0f) + { + // This acts like angular friction + float lever = MathF.Sqrt(massData.rotationalInertia / massData.mass); + jointDef.maxVelocityTorque = 1.0f * lever * mg; + } + + m_mouseJointId = b2CreateMotorJoint(m_worldId, ref jointDef); } } } public virtual void MouseUp(B2Vec2 p, MouseButton button) { - if (b2Joint_IsValid(m_mouseJointId) == false) - { - // The world or attached body was destroyed. - m_mouseJointId = b2_nullJointId; - } - if (B2_IS_NON_NULL(m_mouseJointId) && button == (int)MouseButton.Left) { b2DestroyJoint(m_mouseJointId); m_mouseJointId = b2_nullJointId; - b2DestroyBody(m_groundBodyId); - m_groundBodyId = b2_nullBodyId; + b2DestroyBody(m_mouseBodyId); + m_mouseBodyId = b2_nullBodyId; } } @@ -341,13 +347,7 @@ public virtual void MouseMove(B2Vec2 p) m_mouseJointId = b2_nullJointId; } - if (B2_IS_NON_NULL(m_mouseJointId)) - { - B2Transform localFrameA = new B2Transform(p, b2Rot_identity); - b2Joint_SetLocalFrameA(m_mouseJointId, localFrameA); - B2BodyId bodyIdB = b2Joint_GetBodyB(m_mouseJointId); - b2Body_SetAwake(bodyIdB, true); - } + m_mousePoint = p; } public void DrawTextLine(string text) @@ -387,6 +387,23 @@ public virtual void Step() } } + if (B2_IS_NON_NULL(m_mouseJointId) && b2Joint_IsValid(m_mouseJointId) == false) + { + // The world or attached body was destroyed. + m_mouseJointId = b2_nullJointId; + + if (B2_IS_NON_NULL(m_mouseBodyId)) + { + b2DestroyBody(m_mouseBodyId); + m_mouseBodyId = b2_nullBodyId; + } + } + + if (B2_IS_NON_NULL(m_mouseBodyId) && timeStep > 0.0f) + { + b2Body_SetTargetTransform(m_mouseBodyId, new B2Transform(m_mousePoint, b2Rot_identity), timeStep); + } + b2World_EnableSleeping(m_worldId, m_context.settings.enableSleep); b2World_EnableWarmStarting(m_worldId, m_context.settings.enableWarmStarting); b2World_EnableContinuous(m_worldId, m_context.settings.enableContinuous); @@ -409,7 +426,6 @@ public virtual void Step() m_maxProfile.pairs = b2MaxFloat(m_maxProfile.pairs, p.pairs); m_maxProfile.collide = b2MaxFloat(m_maxProfile.collide, p.collide); m_maxProfile.solve = b2MaxFloat(m_maxProfile.solve, p.solve); - m_maxProfile.mergeIslands = b2MaxFloat(m_maxProfile.mergeIslands, p.mergeIslands); m_maxProfile.prepareStages = b2MaxFloat(m_maxProfile.prepareStages, p.prepareStages); m_maxProfile.solveConstraints = b2MaxFloat(m_maxProfile.solveConstraints, p.solveConstraints); m_maxProfile.prepareConstraints = b2MaxFloat(m_maxProfile.prepareConstraints, p.prepareConstraints); @@ -433,7 +449,6 @@ public virtual void Step() m_totalProfile.pairs += p.pairs; m_totalProfile.collide += p.collide; m_totalProfile.solve += p.solve; - m_totalProfile.mergeIslands += p.mergeIslands; m_totalProfile.prepareStages += p.prepareStages; m_totalProfile.solveConstraints += p.solveConstraints; m_totalProfile.prepareConstraints += p.prepareConstraints; diff --git a/src/Box2D.NET.Samples/Samples/Shapes/BoxRestitution.cs b/src/Box2D.NET.Samples/Samples/Shapes/BoxRestitution.cs new file mode 100644 index 00000000..a032932d --- /dev/null +++ b/src/Box2D.NET.Samples/Samples/Shapes/BoxRestitution.cs @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com) +// SPDX-License-Identifier: MIT + +using static Box2D.NET.B2Geometries; +using static Box2D.NET.B2Types; +using static Box2D.NET.B2Bodies; +using static Box2D.NET.B2Shapes; + +namespace Box2D.NET.Samples.Samples.Shapes; + +public class BoxRestitution : Sample +{ + private static int sampleBoxRestitution = SampleFactory.Shared.RegisterSample("Shapes", "Box Restitution", Create); + + private const int m_count = 10; + + private static Sample Create(SampleContext context) + { + return new BoxRestitution(context); + } + + public BoxRestitution(SampleContext context) + : base(context) + { + if (m_context.settings.restart == false) + { + m_context.camera.m_center = new B2Vec2(0.0f, 5.0f); + m_context.camera.m_zoom = 10.0f; + } + + { + B2BodyDef bodyDef = b2DefaultBodyDef(); + B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef); + + float h = 2.0f * m_count; + B2Segment segment = new B2Segment(new B2Vec2(-h, 0.0f), new B2Vec2(h, 0.0f)); + B2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape(groundId, ref shapeDef, ref segment); + } + + B2Polygon box = b2MakeBox(0.5f, 0.5f); + + { + B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.material.restitution = 0.0f; + + B2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = B2BodyType.b2_dynamicBody; + + float dr = 1.0f / (m_count > 1 ? m_count - 1 : 1); + float x = -1.0f * (m_count - 1); + float dx = 2.0f; + + for (int i = 0; i < m_count; ++i) + { + string buffer = $"{shapeDef.material.restitution:F2}"; + + bodyDef.position = new B2Vec2(x, 1.0f); + bodyDef.name = buffer; + B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef); + + b2CreatePolygonShape(bodyId, ref shapeDef, ref box); + + bodyDef.position = new B2Vec2(x, 4.0f); + bodyDef.name = buffer; + bodyId = b2CreateBody(m_worldId, ref bodyDef); + + b2CreatePolygonShape(bodyId, ref shapeDef, ref box); + + shapeDef.material.restitution += dr; + x += dx; + } + } + } +} \ No newline at end of file diff --git a/src/Box2D.NET.Samples/Samples/Shapes/CustomFilter.cs b/src/Box2D.NET.Samples/Samples/Shapes/CustomFilter.cs index 5d35308f..2de86549 100644 --- a/src/Box2D.NET.Samples/Samples/Shapes/CustomFilter.cs +++ b/src/Box2D.NET.Samples/Samples/Shapes/CustomFilter.cs @@ -50,6 +50,7 @@ public CustomFilter(SampleContext context) : base(context) B2BodyDef bodyDef = b2DefaultBodyDef(); bodyDef.type = B2BodyType.b2_dynamicBody; B2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.enableCustomFiltering = true; B2Polygon box = b2MakeSquare(1.0f); float x = -e_count; diff --git a/src/Box2D.NET.Samples/Samples/Shapes/Explosion.cs b/src/Box2D.NET.Samples/Samples/Shapes/Explosion.cs index ced001a6..0bf9d876 100644 --- a/src/Box2D.NET.Samples/Samples/Shapes/Explosion.cs +++ b/src/Box2D.NET.Samples/Samples/Shapes/Explosion.cs @@ -12,7 +12,6 @@ using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Shapes; using static Box2D.NET.B2Worlds; -using static Box2D.NET.B2WeldJoints; namespace Box2D.NET.Samples.Samples.Shapes; diff --git a/src/Box2D.NET.Shared/Benchmarks.cs b/src/Box2D.NET.Shared/Benchmarks.cs index c3f5e913..b26c7437 100644 --- a/src/Box2D.NET.Shared/Benchmarks.cs +++ b/src/Box2D.NET.Shared/Benchmarks.cs @@ -117,10 +117,10 @@ public static void CreateLargePyramid(B2WorldId worldId) B2ShapeDef shapeDef = b2DefaultShapeDef(); shapeDef.density = 1.0f; - float h = 0.5f; - B2Polygon box = b2MakeSquare(h); + float a = 0.5f; + B2Polygon box = b2MakeSquare(a); - float shift = 1.0f * h; + float shift = 1.0f * a; for (int i = 0; i < baseCount; ++i) { @@ -128,7 +128,7 @@ public static void CreateLargePyramid(B2WorldId worldId) for (int j = i; j < baseCount; ++j) { - float x = (i + 1.0f) * shift + 2.0f * (j - i) * shift - h * baseCount; + float x = (i + 1.0f) * shift + 2.0f * (j - i) * shift - a * baseCount; bodyDef.position = new B2Vec2(x, y); B2BodyId bodyId = b2CreateBody(worldId, ref bodyDef); diff --git a/src/Box2D.NET/B2BitSets.cs b/src/Box2D.NET/B2BitSets.cs index afbb85f7..ffcbdcfd 100644 --- a/src/Box2D.NET/B2BitSets.cs +++ b/src/Box2D.NET/B2BitSets.cs @@ -128,13 +128,13 @@ public static void b2GrowBitSet(ref B2BitSet bitSet, int blockCount) // This function is here because ctz.h is included by // this file but not in bitset.c - public static int b2CountSetBits(ref B2BitSet set) + public static int b2CountSetBits(ref B2BitSet bitSet) { int popCount = 0; - int blockCount = set.blockCount; + int blockCount = bitSet.blockCount; for (uint i = 0; i < blockCount; ++i) { - popCount += b2PopCount64(set.bits[i]); + popCount += b2PopCount64(bitSet.bits[i]); } return popCount; diff --git a/src/Box2D.NET/B2BoardPhases.cs b/src/Box2D.NET/B2BoardPhases.cs index 2a8b5346..24900e34 100644 --- a/src/Box2D.NET/B2BoardPhases.cs +++ b/src/Box2D.NET/B2BoardPhases.cs @@ -286,15 +286,18 @@ public static bool b2PairQueryCallback(int proxyId, ulong userData, ref B2QueryP } // Custom user filter - b2CustomFilterFcn customFilterFcn = queryContext.world.customFilterFcn; - if (customFilterFcn != null) + if (shapeA.enableCustomFiltering || shapeB.enableCustomFiltering) { - B2ShapeId idA = new B2ShapeId(shapeIdA + 1, world.worldId, shapeA.generation); - B2ShapeId idB = new B2ShapeId(shapeIdB + 1, world.worldId, shapeB.generation); - bool shouldCollide = customFilterFcn(idA, idB, queryContext.world.customFilterContext); - if (shouldCollide == false) + b2CustomFilterFcn customFilterFcn = queryContext.world.customFilterFcn; + if (customFilterFcn != null) { - return true; + B2ShapeId idA = new B2ShapeId(shapeIdA + 1, world.worldId, shapeA.generation); + B2ShapeId idB = new B2ShapeId(shapeIdB + 1, world.worldId, shapeB.generation); + bool shouldCollide = customFilterFcn(idA, idB, queryContext.world.customFilterContext); + if (shouldCollide == false) + { + return true; + } } } diff --git a/src/Box2D.NET/B2Bodies.cs b/src/Box2D.NET/B2Bodies.cs index dd96f587..029dd59a 100644 --- a/src/Box2D.NET/B2Bodies.cs +++ b/src/Box2D.NET/B2Bodies.cs @@ -1161,8 +1161,7 @@ public static B2BodyType b2Body_GetType(B2BodyId bodyId) // - Create proxy in new tree // Notes: // - the implementation below tries to minimize the number of predicates, so some - // operations may have no effect, such as transfering a joint to the same set -#if ENABLED + // operations may have no effect, such as transferring a joint to the same set public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) { B2World world = b2GetWorld(bodyId.world0); @@ -1317,277 +1316,15 @@ public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) continue; } - bool mergeIslands = false; - b2LinkJoint(world, joint, mergeIslands); + b2LinkJoint(world, joint); } - b2MergeAwakeIslands(world); - // Body type affects the mass b2UpdateBodyMassData(world, body); b2ValidateSolverSets(world); b2ValidateIsland(world, body.islandId); } -#else - // todo keeping this buggy old version for reference - public static void b2Body_SetType(B2BodyId bodyId, B2BodyType type) - { - B2World world = b2GetWorld(bodyId.world0); - B2Body body = b2GetBodyFullId(world, bodyId); - - B2BodyType originalType = body.type; - if (originalType == type) - { - return; - } - - if (body.setIndex == (int)B2SetType.b2_disabledSet) - { - // Disabled bodies don't change solver sets or islands when they change type. - body.type = type; - - // Body type affects the mass - b2UpdateBodyMassData(world, body); - return; - } - - // Destroy all contacts but don't wake bodies. - bool wakeBodies = false; - b2DestroyBodyContacts(world, body, wakeBodies); - - // Wake this body because we assume below that it is awake or static. - b2WakeBody(world, body); - - // Unlink all joints and wake attached bodies. - { - int jointKey = body.headJointKey; - while (jointKey != B2_NULL_INDEX) - { - int jointId = jointKey >> 1; - int edgeIndex = jointKey & 1; - - B2Joint joint = b2Array_Get(ref world.joints, jointId); - if (joint.islandId != B2_NULL_INDEX) - { - b2UnlinkJoint(world, joint); - } - - // A body going from static to dynamic or kinematic goes to the awake set - // and other attached bodies must be awake as well. For consistency, this is - // done for all cases. - B2Body bodyA = b2Array_Get(ref world.bodies, joint.edges[0].bodyId); - B2Body bodyB = b2Array_Get(ref world.bodies, joint.edges[1].bodyId); - b2WakeBody(world, bodyA); - b2WakeBody(world, bodyB); - - jointKey = joint.edges[edgeIndex].nextKey; - } - } - - body.type = type; - - if (originalType == B2BodyType.b2_staticBody) - { - // Body is going from static to dynamic or kinematic. It only makes sense to move it to the awake set. - B2_ASSERT(body.setIndex == (int)B2SetType.b2_staticSet); - - B2SolverSet staticSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_staticSet); - B2SolverSet awakeSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_awakeSet); - - // Transfer body to awake set - b2TransferBody(world, awakeSet, staticSet, body); - - // Create island for body - b2CreateIslandForBody(world, (int)B2SetType.b2_awakeSet, body); - - // Transfer static joints to awake set - int jointKey = body.headJointKey; - while (jointKey != B2_NULL_INDEX) - { - int jointId = jointKey >> 1; - int edgeIndex = jointKey & 1; - - B2Joint joint = b2Array_Get(ref world.joints, jointId); - - // Transfer the joint if it is in the static set - if (joint.setIndex == (int)B2SetType.b2_staticSet) - { - b2TransferJoint(world, awakeSet, staticSet, joint); - } - else if (joint.setIndex == (int)B2SetType.b2_awakeSet) - { - // In this case the joint must be re-inserted into the constraint graph to ensure the correct - // graph color. - - // BUG BUG BUG - // This has a subtle bug where the joint transfer below can clear a color occupied by another - // joint attached to this body. - // This can happen when a body was previously static. - // For this reason, all joints must first be moved to the static set before being added to - // the constraint graph - - // First transfer to the static set. - b2TransferJoint(world, staticSet, awakeSet, joint); - - // Now transfer it back to the awake set and into the graph coloring. - b2TransferJoint(world, awakeSet, staticSet, joint); - } - else - { - // Otherwise the joint must be disabled. - B2_ASSERT(joint.setIndex == (int)B2SetType.b2_disabledSet); - } - - jointKey = joint.edges[edgeIndex].nextKey; - } - - // Recreate shape proxies in movable tree. - B2Transform transform = b2GetBodyTransformQuick(world, body); - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - shapeId = shape.nextShapeId; - b2DestroyShapeProxy(shape, world.broadPhase); - bool forcePairCreation = true; - B2BodyType proxyType = type; - b2CreateShapeProxy(shape, world.broadPhase, proxyType, transform, forcePairCreation); - } - } - else if (type == B2BodyType.b2_staticBody) - { - // The body is going from dynamic/kinematic to static. It should be awake. - B2_ASSERT(body.setIndex == (int)B2SetType.b2_awakeSet); - - B2SolverSet staticSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_staticSet); - B2SolverSet awakeSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_awakeSet); - - // Transfer body to static set - b2TransferBody(world, staticSet, awakeSet, body); - - // Remove body from island. - b2RemoveBodyFromIsland(world, body); - - B2BodySim bodySim = b2Array_Get(ref staticSet.bodySims, body.localIndex); - bodySim.flags &= ~(uint)B2BodyFlags.b2_isFast; - - // Maybe transfer joints to static set. - int jointKey = body.headJointKey; - while (jointKey != B2_NULL_INDEX) - { - int jointId = jointKey >> 1; - int edgeIndex = jointKey & 1; - - B2Joint joint = b2Array_Get(ref world.joints, jointId); - jointKey = joint.edges[edgeIndex].nextKey; - - int otherEdgeIndex = edgeIndex ^ 1; - B2Body otherBody = b2Array_Get(ref world.bodies, joint.edges[otherEdgeIndex].bodyId); - - // Skip disabled joint - if (joint.setIndex == (int)B2SetType.b2_disabledSet) - { - // Joint is disable, should be connected to a disabled body - B2_ASSERT(otherBody.setIndex == (int)B2SetType.b2_disabledSet); - continue; - } - - // Since the body was not static, the joint must be awake. - B2_ASSERT(joint.setIndex == (int)B2SetType.b2_awakeSet); - - // Only transfer joint to static set if both bodies are static. - if (otherBody.setIndex == (int)B2SetType.b2_staticSet) - { - b2TransferJoint(world, staticSet, awakeSet, joint); - } - else - { - // The other body must be awake. - B2_ASSERT(otherBody.setIndex == (int)B2SetType.b2_awakeSet); - - // The joint must live in a graph color. - B2_ASSERT(0 <= joint.colorIndex && joint.colorIndex < B2_GRAPH_COLOR_COUNT); - - // In this case the joint must be re-inserted into the constraint graph to ensure the correct - // graph color. - - // First transfer to the static set. - b2TransferJoint(world, staticSet, awakeSet, joint); - - // Now transfer it back to the awake set and into the graph coloring. - b2TransferJoint(world, awakeSet, staticSet, joint); - } - } - - // Recreate shape proxies in static tree. - B2Transform transform = b2GetBodyTransformQuick(world, body); - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - shapeId = shape.nextShapeId; - b2DestroyShapeProxy(shape, world.broadPhase); - bool forcePairCreation = true; - b2CreateShapeProxy(shape, world.broadPhase, B2BodyType.b2_staticBody, transform, forcePairCreation); - } - } - else - { - B2_ASSERT(originalType == B2BodyType.b2_dynamicBody || originalType == B2BodyType.b2_kinematicBody); - B2_ASSERT(type == B2BodyType.b2_dynamicBody || type == B2BodyType.b2_kinematicBody); - - // Recreate shape proxies in static tree. - B2Transform transform = b2GetBodyTransformQuick(world, body); - int shapeId = body.headShapeId; - while (shapeId != B2_NULL_INDEX) - { - B2Shape shape = b2Array_Get(ref world.shapes, shapeId); - shapeId = shape.nextShapeId; - b2DestroyShapeProxy(shape, world.broadPhase); - B2BodyType proxyType = type; - bool forcePairCreation = true; - b2CreateShapeProxy(shape, world.broadPhase, proxyType, transform, forcePairCreation); - } - } - - // Relink all joints - { - int jointKey = body.headJointKey; - while (jointKey != B2_NULL_INDEX) - { - int jointId = jointKey >> 1; - int edgeIndex = jointKey & 1; - - B2Joint joint = b2Array_Get(ref world.joints, jointId); - jointKey = joint.edges[edgeIndex].nextKey; - - int otherEdgeIndex = edgeIndex ^ 1; - int otherBodyId = joint.edges[otherEdgeIndex].bodyId; - B2Body otherBody = b2Array_Get(ref world.bodies, otherBodyId); - - if (otherBody.setIndex == (int)B2SetType.b2_disabledSet) - { - continue; - } - - if (body.type == B2BodyType.b2_staticBody && otherBody.type == B2BodyType.b2_staticBody) - { - continue; - } - - b2LinkJoint(world, joint, false); - } - - b2MergeAwakeIslands(world); - } - - // Body type affects the mass - b2UpdateBodyMassData(world, body); - - b2ValidateSolverSets(world); - } -#endif /// Set the body name. Up to 31 characters excluding 0 termination. public static void b2Body_SetName(B2BodyId bodyId, string name) @@ -1976,7 +1713,6 @@ public static void b2Body_Enable(B2BodyId bodyId) // Transfer joints. If the other body is disabled, don't transfer. // If the other body is sleeping, wake it. - bool mergeIslands = false; int jointKey = body.headJointKey; while (jointKey != B2_NULL_INDEX) { @@ -2019,13 +1755,10 @@ public static void b2Body_Enable(B2BodyId bodyId) // Now that the joint is in the correct set, I can link the joint in the island. if (jointSetId != (int)B2SetType.b2_staticSet) { - b2LinkJoint(world, joint, mergeIslands); + b2LinkJoint(world, joint); } } - // Now merge islands - b2MergeAwakeIslands(world); - b2ValidateSolverSets(world); } diff --git a/src/Box2D.NET/B2Constants.cs b/src/Box2D.NET/B2Constants.cs index a735b160..b942346b 100644 --- a/src/Box2D.NET/B2Constants.cs +++ b/src/Box2D.NET/B2Constants.cs @@ -17,6 +17,7 @@ public static class B2Constants // Maximum number of colors in the constraint graph. Constraints that cannot // find a color are added to the overflow set which are solved single-threaded. + // The compound barrel benchmark has minor overflow with 24 colors public const int B2_GRAPH_COLOR_COUNT = 24; // A small length used as a collision and constraint tolerance. Usually it is diff --git a/src/Box2D.NET/B2ConstraintGraphs.cs b/src/Box2D.NET/B2ConstraintGraphs.cs index 01c91c0d..b53ca927 100644 --- a/src/Box2D.NET/B2ConstraintGraphs.cs +++ b/src/Box2D.NET/B2ConstraintGraphs.cs @@ -10,7 +10,7 @@ // body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body // scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will // cause horrible cache stalls. To make this feasible I would need a way to block these writes. - +// todo should be possible to branch on the scatters to avoid writing to kinematic bodies // TODO: @ikpil, check // This is used for debugging by making all constraints be assigned to overflow. @@ -38,7 +38,6 @@ public static class B2ConstraintGraphs public static void b2CreateGraph(ref B2ConstraintGraph graph, int bodyCapacity) { - //B2_ASSERT(B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12"); B2_ASSERT(B2_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors"); B2_ASSERT(B2_OVERFLOW_INDEX == B2_GRAPH_COLOR_COUNT - 1, "bad over flow index"); B2_ASSERT(B2_DYNAMIC_COLOR_COUNT >= 2, "need more dynamic colors"); diff --git a/src/Box2D.NET/B2ContactSolvers.cs b/src/Box2D.NET/B2ContactSolvers.cs index fe236b15..e6109255 100644 --- a/src/Box2D.NET/B2ContactSolvers.cs +++ b/src/Box2D.NET/B2ContactSolvers.cs @@ -833,6 +833,8 @@ static void b2ScatterBodies( b2BodyState* states, int* indices, const b2BodyStat // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the // associated cache flushing. + // todo could add a check for kinematic bodies here + if ( indices[0] != B2_NULL_INDEX ) _mm256_store_ps( (float*)( states + indices[0] ), _mm256_permute2f128_ps( tt0, tt4, 0x20 ) ); if ( indices[1] != B2_NULL_INDEX ) diff --git a/src/Box2D.NET/B2Delegates.cs b/src/Box2D.NET/B2Delegates.cs index 3d58e2a5..150e2794 100644 --- a/src/Box2D.NET/B2Delegates.cs +++ b/src/Box2D.NET/B2Delegates.cs @@ -72,7 +72,7 @@ namespace Box2D.NET /// Notes: /// - this function must be thread-safe /// - this is only called if one of the two shapes has enabled custom filtering - /// - this is called only for awake dynamic bodies + /// - this may be called for awake dynamic bodies and sensors /// Return false if you want to disable the collision /// @see b2ShapeDef /// @warning Do not attempt to modify the world inside this callback diff --git a/src/Box2D.NET/B2DistanceJoint.cs b/src/Box2D.NET/B2DistanceJoint.cs index 6fb68380..3bceb2a1 100644 --- a/src/Box2D.NET/B2DistanceJoint.cs +++ b/src/Box2D.NET/B2DistanceJoint.cs @@ -9,6 +9,8 @@ public struct B2DistanceJoint public float length; public float hertz; public float dampingRatio; + public float lowerSpringForce; + public float upperSpringForce; public float minLength; public float maxLength; diff --git a/src/Box2D.NET/B2DistanceJointDef.cs b/src/Box2D.NET/B2DistanceJointDef.cs index 4f19d715..30c94671 100644 --- a/src/Box2D.NET/B2DistanceJointDef.cs +++ b/src/Box2D.NET/B2DistanceJointDef.cs @@ -19,6 +19,12 @@ public struct B2DistanceJointDef /// Enable the distance constraint to behave like a spring. If false /// then the distance joint will be rigid, overriding the limit and motor. public bool enableSpring; + + /// The lower spring force controls how much tension it can sustain + public float lowerSpringForce; + + /// The upper spring force controls how much compression it an sustain + public float upperSpringForce; /// The spring linear stiffness Hertz, cycles per second public float hertz; diff --git a/src/Box2D.NET/B2DistanceJoints.cs b/src/Box2D.NET/B2DistanceJoints.cs index f1c9d956..538be8ae 100644 --- a/src/Box2D.NET/B2DistanceJoints.cs +++ b/src/Box2D.NET/B2DistanceJoints.cs @@ -97,18 +97,38 @@ public static float b2DistanceJoint_GetCurrentLength(B2JointId jointId) return length; } + /// Enable/disable the distance joint spring. When disabled the distance joint is rigid. public static void b2DistanceJoint_EnableSpring(B2JointId jointId, bool enableSpring) { B2JointSim @base = b2GetJointSimCheckType(jointId, B2JointType.b2_distanceJoint); @base.uj.distanceJoint.enableSpring = enableSpring; } + /// Is the distance joint spring enabled? public static bool b2DistanceJoint_IsSpringEnabled(B2JointId jointId) { B2JointSim @base = b2GetJointSimCheckType(jointId, B2JointType.b2_distanceJoint); return @base.uj.distanceJoint.enableSpring; } + /// Set the force range for the spring. + public static void b2DistanceJoint_SetSpringForceRange(B2JointId jointId, float lowerForce, float upperForce) + { + B2_ASSERT(lowerForce <= upperForce); + B2JointSim @base = b2GetJointSimCheckType(jointId, B2JointType.b2_distanceJoint); + @base.uj.distanceJoint.lowerSpringForce = lowerForce; + @base.uj.distanceJoint.upperSpringForce = upperForce; + } + + /// Get the force range for the spring. + public static void b2DistanceJoint_GetSpringForceRange(B2JointId jointId, out float lowerForce, out float upperForce) + { + B2JointSim @base = b2GetJointSimCheckType(jointId, B2JointType.b2_distanceJoint); + lowerForce = @base.uj.distanceJoint.lowerSpringForce; + upperForce = @base.uj.distanceJoint.upperSpringForce; + } + + /// Set the spring stiffness in Hertz public static void b2DistanceJoint_SetSpringHertz(B2JointId jointId, float hertz) { B2JointSim @base = b2GetJointSimCheckType(jointId, B2JointType.b2_distanceJoint); @@ -356,8 +376,12 @@ public static void b2SolveDistanceJoint(B2JointSim @base, B2StepContext context, float bias = joint.distanceSoftness.biasRate * C; float m = joint.distanceSoftness.massScale * joint.axialMass; - float impulse = -m * (Cdot + bias) - joint.distanceSoftness.impulseScale * joint.impulse; - joint.impulse += impulse; + float oldImpulse = joint.impulse; + float impulse = -m * (Cdot + bias) - joint.distanceSoftness.impulseScale * oldImpulse; + + float h = context.h; + joint.impulse = b2ClampFloat(joint.impulse + impulse, joint.lowerSpringForce * h, joint.upperSpringForce * h); + impulse = joint.impulse - oldImpulse; B2Vec2 P = b2MulSV(impulse, axis); vA = b2MulSub(vA, mA, P); diff --git a/src/Box2D.NET/B2DynamicTrees.cs b/src/Box2D.NET/B2DynamicTrees.cs index 481e608b..ef12ed07 100644 --- a/src/Box2D.NET/B2DynamicTrees.cs +++ b/src/Box2D.NET/B2DynamicTrees.cs @@ -465,15 +465,15 @@ public static void b2RotateNodes(B2DynamicTree tree, int iA) int iF = C.children.child1; int iG = C.children.child2; - ref B2TreeNode D = ref nodes[iD]; - ref B2TreeNode E = ref nodes[iE]; - ref B2TreeNode F = ref nodes[iF]; - ref B2TreeNode G = ref nodes[iG]; - B2_ASSERT(0 <= iD && iD < tree.nodeCapacity); B2_ASSERT(0 <= iE && iE < tree.nodeCapacity); B2_ASSERT(0 <= iF && iF < tree.nodeCapacity); B2_ASSERT(0 <= iG && iG < tree.nodeCapacity); + + ref B2TreeNode D = ref nodes[iD]; + ref B2TreeNode E = ref nodes[iE]; + ref B2TreeNode F = ref nodes[iF]; + ref B2TreeNode G = ref nodes[iG]; // Base cost float areaB = b2Perimeter(B.aabb); diff --git a/src/Box2D.NET/B2Island.cs b/src/Box2D.NET/B2Island.cs index 7f605741..ce0031d8 100644 --- a/src/Box2D.NET/B2Island.cs +++ b/src/Box2D.NET/B2Island.cs @@ -32,10 +32,6 @@ public class B2Island public int tailJoint; public int jointCount; - // Union find - // todo this could go away if islands are merged immediately with b2LinkJoint and b2LinkContact - public int parentIsland; - // Keeps track of how many contacts have been removed from this island. // This is used to determine if an island is a candidate for splitting. public int constraintRemoveCount; diff --git a/src/Box2D.NET/B2Islands.cs b/src/Box2D.NET/B2Islands.cs index 1ec070b1..c6bb8f25 100644 --- a/src/Box2D.NET/B2Islands.cs +++ b/src/Box2D.NET/B2Islands.cs @@ -57,7 +57,6 @@ public static B2Island b2CreateIsland(B2World world, int setIndex) island.headJoint = B2_NULL_INDEX; island.tailJoint = B2_NULL_INDEX; island.jointCount = 0; - island.parentIsland = B2_NULL_INDEX; island.constraintRemoveCount = 0; ref B2IslandSim islandSim = ref b2Array_Add(ref set.islandSims); @@ -94,6 +93,158 @@ public static void b2DestroyIsland(B2World world, int islandId) b2FreeId(world.islandIdPool, islandId); } + + public static int b2MergeIslands(B2World world, int islandIdA, int islandIdB) + { + if (islandIdA == islandIdB) + { + return islandIdA; + } + + if (islandIdA == B2_NULL_INDEX) + { + B2_ASSERT(islandIdB != B2_NULL_INDEX); + return islandIdB; + } + + if (islandIdB == B2_NULL_INDEX) + { + B2_ASSERT(islandIdA != B2_NULL_INDEX); + return islandIdA; + } + + B2Island islandA = b2Array_Get(ref world.islands, islandIdA); + B2Island islandB = b2Array_Get(ref world.islands, islandIdB); + + // Keep the biggest island to reduce cache misses + B2Island big; + B2Island small; + if (islandA.bodyCount >= islandB.bodyCount) + { + big = islandA; + small = islandB; + } + else + { + big = islandB; + small = islandA; + } + + int bigId = big.islandId; + + // remap island indices (cache misses) + int bodyId = small.headBody; + while (bodyId != B2_NULL_INDEX) + { + B2Body body = b2Array_Get(ref world.bodies, bodyId); + body.islandId = bigId; + bodyId = body.islandNext; + } + + int contactId = small.headContact; + while (contactId != B2_NULL_INDEX) + { + B2Contact contact = b2Array_Get(ref world.contacts, contactId); + contact.islandId = bigId; + contactId = contact.islandNext; + } + + int jointId = small.headJoint; + while (jointId != B2_NULL_INDEX) + { + B2Joint joint = b2Array_Get(ref world.joints, jointId); + joint.islandId = bigId; + jointId = joint.islandNext; + } + + // connect body lists + B2_ASSERT(big.tailBody != B2_NULL_INDEX); + B2Body tailBody = b2Array_Get(ref world.bodies, big.tailBody); + B2_ASSERT(tailBody.islandNext == B2_NULL_INDEX); + tailBody.islandNext = small.headBody; + + B2_ASSERT(small.headBody != B2_NULL_INDEX); + B2Body headBody = b2Array_Get(ref world.bodies, small.headBody); + B2_ASSERT(headBody.islandPrev == B2_NULL_INDEX); + headBody.islandPrev = big.tailBody; + + big.tailBody = small.tailBody; + big.bodyCount += small.bodyCount; + + // connect contact lists + if (big.headContact == B2_NULL_INDEX) + { + // Big island has no contacts + B2_ASSERT(big.tailContact == B2_NULL_INDEX && big.contactCount == 0); + big.headContact = small.headContact; + big.tailContact = small.tailContact; + big.contactCount = small.contactCount; + } + else if (small.headContact != B2_NULL_INDEX) + { + // Both islands have contacts + B2_ASSERT(small.tailContact != B2_NULL_INDEX && small.contactCount > 0); + B2_ASSERT(big.tailContact != B2_NULL_INDEX && big.contactCount > 0); + + B2Contact tailContact = b2Array_Get(ref world.contacts, big.tailContact); + B2_ASSERT(tailContact.islandNext == B2_NULL_INDEX); + tailContact.islandNext = small.headContact; + + B2Contact headContact = b2Array_Get(ref world.contacts, small.headContact); + B2_ASSERT(headContact.islandPrev == B2_NULL_INDEX); + headContact.islandPrev = big.tailContact; + + big.tailContact = small.tailContact; + big.contactCount += small.contactCount; + } + + if (big.headJoint == B2_NULL_INDEX) + { + // Root island has no joints + B2_ASSERT(big.tailJoint == B2_NULL_INDEX && big.jointCount == 0); + big.headJoint = small.headJoint; + big.tailJoint = small.tailJoint; + big.jointCount = small.jointCount; + } + else if (small.headJoint != B2_NULL_INDEX) + { + // Both islands have joints + B2_ASSERT(small.tailJoint != B2_NULL_INDEX && small.jointCount > 0); + B2_ASSERT(big.tailJoint != B2_NULL_INDEX && big.jointCount > 0); + + B2Joint tailJoint = b2Array_Get(ref world.joints, big.tailJoint); + B2_ASSERT(tailJoint.islandNext == B2_NULL_INDEX); + tailJoint.islandNext = small.headJoint; + + B2Joint headJoint = b2Array_Get(ref world.joints, small.headJoint); + B2_ASSERT(headJoint.islandPrev == B2_NULL_INDEX); + headJoint.islandPrev = big.tailJoint; + + big.tailJoint = small.tailJoint; + big.jointCount += small.jointCount; + } + + // Track removed constraints + big.constraintRemoveCount += small.constraintRemoveCount; + + small.bodyCount = 0; + small.contactCount = 0; + small.jointCount = 0; + small.headBody = B2_NULL_INDEX; + small.headContact = B2_NULL_INDEX; + small.headJoint = B2_NULL_INDEX; + small.tailBody = B2_NULL_INDEX; + small.tailContact = B2_NULL_INDEX; + small.tailJoint = B2_NULL_INDEX; + small.constraintRemoveCount = 0; + + b2DestroyIsland(world, small.islandId); + + b2ValidateIsland(world, bigId); + + return bigId; + } + public static void b2AddContactToIsland(B2World world, int islandId, B2Contact contact) { B2_ASSERT(contact.islandId == B2_NULL_INDEX); @@ -121,10 +272,7 @@ public static void b2AddContactToIsland(B2World world, int islandId, B2Contact c b2ValidateIsland(world, islandId); } - // Link contacts into the island graph when it starts having contact points // Link a contact into an island. - // This performs union-find and path compression to join islands. - // https://en.wikipedia.org/wiki/Disjoint-set_data_structure public static void b2LinkContact(B2World world, B2Contact contact) { B2_ASSERT((contact.flags & (uint)B2ContactFlags.b2_contactTouchingFlag) != 0); @@ -158,75 +306,11 @@ public static void b2LinkContact(B2World world, B2Contact contact) B2_ASSERT(bodyB.setIndex != (int)B2SetType.b2_staticSet || islandIdB == B2_NULL_INDEX); B2_ASSERT(islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX); - if (islandIdA == islandIdB) - { - // Contact in same island - b2AddContactToIsland(world, islandIdA, contact); - return; - } - - // Union-find root of islandA - B2Island islandA = null; - if (islandIdA != B2_NULL_INDEX) - { - islandA = b2Array_Get(ref world.islands, islandIdA); - int parentId = islandA.parentIsland; - while (parentId != B2_NULL_INDEX) - { - B2Island parent = b2Array_Get(ref world.islands, parentId); - if (parent.parentIsland != B2_NULL_INDEX) - { - // path compression - islandA.parentIsland = parent.parentIsland; - } - - islandA = parent; - islandIdA = parentId; - parentId = islandA.parentIsland; - } - } - - // Union-find root of islandB - B2Island islandB = null; - if (islandIdB != B2_NULL_INDEX) - { - islandB = b2Array_Get(ref world.islands, islandIdB); - int parentId = islandB.parentIsland; - while (islandB.parentIsland != B2_NULL_INDEX) - { - B2Island parent = b2Array_Get(ref world.islands, parentId); - if (parent.parentIsland != B2_NULL_INDEX) - { - // path compression - islandB.parentIsland = parent.parentIsland; - } - - islandB = parent; - islandIdB = parentId; - parentId = islandB.parentIsland; - } - } - - B2_ASSERT(islandA != null || islandB != null); - - // Union-Find link island roots - if (islandA != islandB && islandA != null && islandB != null) - { - B2_ASSERT(islandA != islandB); - B2_ASSERT(islandB.parentIsland == B2_NULL_INDEX); - islandB.parentIsland = islandIdA; - } - - if (islandA != null) - { - b2AddContactToIsland(world, islandIdA, contact); - } - else - { - b2AddContactToIsland(world, islandIdB, contact); - } + // Merge islands. This will destroy one of the islands. + int finalIslandId = b2MergeIslands(world, islandIdA, islandIdB); - // todo why not merge the islands right here? + // Add contact to the island that survived + b2AddContactToIsland(world, finalIslandId, contact); } // Unlink contact from the island graph when it stops having contact points @@ -302,7 +386,7 @@ public static void b2AddJointToIsland(B2World world, int islandId, B2Joint joint } // Link a joint into the island graph when it is created - public static void b2LinkJoint(B2World world, B2Joint joint, bool mergeIslands) + public static void b2LinkJoint(B2World world, B2Joint joint) { B2Body bodyA = b2Array_Get(ref world.bodies, joint.edges[0].bodyId); B2Body bodyB = b2Array_Get(ref world.bodies, joint.edges[1].bodyId); @@ -321,78 +405,11 @@ public static void b2LinkJoint(B2World world, B2Joint joint, bool mergeIslands) B2_ASSERT(islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX); - if (islandIdA == islandIdB) - { - // Joint in same island - b2AddJointToIsland(world, islandIdA, joint); - return; - } - - // Union-find root of islandA - B2Island islandA = null; - if (islandIdA != B2_NULL_INDEX) - { - islandA = b2Array_Get(ref world.islands, islandIdA); - while (islandA.parentIsland != B2_NULL_INDEX) - { - B2Island parent = b2Array_Get(ref world.islands, islandA.parentIsland); - if (parent.parentIsland != B2_NULL_INDEX) - { - // path compression - islandA.parentIsland = parent.parentIsland; - } - - islandIdA = islandA.parentIsland; - islandA = parent; - } - } - - // Union-find root of islandB - B2Island islandB = null; - if (islandIdB != B2_NULL_INDEX) - { - islandB = b2Array_Get(ref world.islands, islandIdB); - while (islandB.parentIsland != B2_NULL_INDEX) - { - B2Island parent = b2Array_Get(ref world.islands, islandB.parentIsland); - if (parent.parentIsland != B2_NULL_INDEX) - { - // path compression - islandB.parentIsland = parent.parentIsland; - } - - islandIdB = islandB.parentIsland; - islandB = parent; - } - } - - B2_ASSERT(islandA != null || islandB != null); + // Merge islands. This will destroy one of the islands. + int finalIslandId = b2MergeIslands(world, islandIdA, islandIdB); - // Union-Find link island roots - if (islandA != islandB && islandA != null && islandB != null) - { - B2_ASSERT(islandA != islandB); - B2_ASSERT(islandB.parentIsland == B2_NULL_INDEX); - islandB.parentIsland = islandIdA; - } - - if (islandA != null) - { - b2AddJointToIsland(world, islandIdA, joint); - } - else - { - b2AddJointToIsland(world, islandIdB, joint); - } - - // Joints need to have islands merged immediately when they are created - // to keep the island graph valid. - // However, when a body type is being changed the merge can be deferred until - // all joints are linked. - if (mergeIslands) - { - b2MergeAwakeIslands(world); - } + // Add joint the island that survived + b2AddJointToIsland(world, finalIslandId, joint); } // Unlink a joint from the island graph when it is destroyed @@ -442,180 +459,6 @@ public static void b2UnlinkJoint(B2World world, B2Joint joint) b2ValidateIsland(world, islandId); } -// Merge an island into its root island. -// todo we can assume all islands are awake here - public static void b2MergeIsland(B2World world, B2Island island) - { - B2_ASSERT(island.parentIsland != B2_NULL_INDEX); - - int rootId = island.parentIsland; - B2Island rootIsland = b2Array_Get(ref world.islands, rootId); - B2_ASSERT(rootIsland.parentIsland == B2_NULL_INDEX); - - // remap island indices - int bodyId = island.headBody; - while (bodyId != B2_NULL_INDEX) - { - B2Body body = b2Array_Get(ref world.bodies, bodyId); - body.islandId = rootId; - bodyId = body.islandNext; - } - - int contactId = island.headContact; - while (contactId != B2_NULL_INDEX) - { - B2Contact contact = b2Array_Get(ref world.contacts, contactId); - contact.islandId = rootId; - contactId = contact.islandNext; - } - - int jointId = island.headJoint; - while (jointId != B2_NULL_INDEX) - { - B2Joint joint = b2Array_Get(ref world.joints, jointId); - joint.islandId = rootId; - jointId = joint.islandNext; - } - - // connect body lists - B2_ASSERT(rootIsland.tailBody != B2_NULL_INDEX); - B2Body tailBody = b2Array_Get(ref world.bodies, rootIsland.tailBody); - B2_ASSERT(tailBody.islandNext == B2_NULL_INDEX); - tailBody.islandNext = island.headBody; - - B2_ASSERT(island.headBody != B2_NULL_INDEX); - B2Body headBody = b2Array_Get(ref world.bodies, island.headBody); - B2_ASSERT(headBody.islandPrev == B2_NULL_INDEX); - headBody.islandPrev = rootIsland.tailBody; - - rootIsland.tailBody = island.tailBody; - rootIsland.bodyCount += island.bodyCount; - - // connect contact lists - if (rootIsland.headContact == B2_NULL_INDEX) - { - // Root island has no contacts - B2_ASSERT(rootIsland.tailContact == B2_NULL_INDEX && rootIsland.contactCount == 0); - rootIsland.headContact = island.headContact; - rootIsland.tailContact = island.tailContact; - rootIsland.contactCount = island.contactCount; - } - else if (island.headContact != B2_NULL_INDEX) - { - // Both islands have contacts - B2_ASSERT(island.tailContact != B2_NULL_INDEX && island.contactCount > 0); - B2_ASSERT(rootIsland.tailContact != B2_NULL_INDEX && rootIsland.contactCount > 0); - - B2Contact tailContact = b2Array_Get(ref world.contacts, rootIsland.tailContact); - B2_ASSERT(tailContact.islandNext == B2_NULL_INDEX); - tailContact.islandNext = island.headContact; - - B2Contact headContact = b2Array_Get(ref world.contacts, island.headContact); - B2_ASSERT(headContact.islandPrev == B2_NULL_INDEX); - headContact.islandPrev = rootIsland.tailContact; - - rootIsland.tailContact = island.tailContact; - rootIsland.contactCount += island.contactCount; - } - - if (rootIsland.headJoint == B2_NULL_INDEX) - { - // Root island has no joints - B2_ASSERT(rootIsland.tailJoint == B2_NULL_INDEX && rootIsland.jointCount == 0); - rootIsland.headJoint = island.headJoint; - rootIsland.tailJoint = island.tailJoint; - rootIsland.jointCount = island.jointCount; - } - else if (island.headJoint != B2_NULL_INDEX) - { - // Both islands have joints - B2_ASSERT(island.tailJoint != B2_NULL_INDEX && island.jointCount > 0); - B2_ASSERT(rootIsland.tailJoint != B2_NULL_INDEX && rootIsland.jointCount > 0); - - B2Joint tailJoint = b2Array_Get(ref world.joints, rootIsland.tailJoint); - B2_ASSERT(tailJoint.islandNext == B2_NULL_INDEX); - tailJoint.islandNext = island.headJoint; - - B2Joint headJoint = b2Array_Get(ref world.joints, island.headJoint); - B2_ASSERT(headJoint.islandPrev == B2_NULL_INDEX); - headJoint.islandPrev = rootIsland.tailJoint; - - rootIsland.tailJoint = island.tailJoint; - rootIsland.jointCount += island.jointCount; - } - - // Track removed constraints - rootIsland.constraintRemoveCount += island.constraintRemoveCount; - - b2ValidateIsland(world, rootId); - } - -// Iterate over all awake islands and merge any that need merging -// Islands that get merged into a root island will be removed from the awake island array -// and returned to the pool. -// todo this might be faster if b2IslandSim held the connectivity data - public static void b2MergeAwakeIslands(B2World world) - { - b2TracyCZoneNC(B2TracyCZone.merge_islands, "Merge Islands", B2HexColor.b2_colorMediumTurquoise, true); - - B2SolverSet awakeSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_awakeSet); - B2IslandSim[] islandSims = awakeSet.islandSims.data; - int awakeIslandCount = awakeSet.islandSims.count; - - // Step 1: Ensure every child island points to its root island. This avoids merging a child island with - // a parent island that has already been merged with a grand-parent island. - for (int i = 0; i < awakeIslandCount; ++i) - { - int islandId = islandSims[i].islandId; - - B2Island island = b2Array_Get(ref world.islands, islandId); - - // find the root island - int rootId = islandId; - B2Island rootIsland = island; - while (rootIsland.parentIsland != B2_NULL_INDEX) - { - B2Island parent = b2Array_Get(ref world.islands, rootIsland.parentIsland); - if (parent.parentIsland != B2_NULL_INDEX) - { - // path compression - rootIsland.parentIsland = parent.parentIsland; - } - - rootId = rootIsland.parentIsland; - rootIsland = parent; - } - - if (rootIsland != island) - { - island.parentIsland = rootId; - } - } - - // Step 2: merge every awake island into its parent (which must be a root island) - // Reverse to support removal from awake array. - for (int i = awakeIslandCount - 1; i >= 0; --i) - { - int islandId = islandSims[i].islandId; - B2Island island = b2Array_Get(ref world.islands, islandId); - - if (island.parentIsland == B2_NULL_INDEX) - { - continue; - } - - b2MergeIsland(world, island); - - // this call does a remove swap from the end of the island sim array - b2DestroyIsland(world, islandId); - } - - b2ValidateConnectivity(world); - - b2TracyCZoneEnd(B2TracyCZone.merge_islands); - } - - public static void b2SplitIsland(B2World world, int baseId) { B2Island baseIsland = b2Array_Get(ref world.islands, baseId); diff --git a/src/Box2D.NET/B2Joints.cs b/src/Box2D.NET/B2Joints.cs index c4e1a350..0379ac20 100644 --- a/src/Box2D.NET/B2Joints.cs +++ b/src/Box2D.NET/B2Joints.cs @@ -50,6 +50,8 @@ public static B2DistanceJointDef b2DefaultDistanceJointDef() { B2DistanceJointDef def = new B2DistanceJointDef(); def.@base = b2DefaultJointDef(); + def.lowerSpringForce = -float.MaxValue; + def.upperSpringForce = float.MaxValue; def.length = 1.0f; def.maxLength = B2_HUGE; def.internalValue = B2_SECRET_COOKIE; @@ -60,9 +62,7 @@ public static B2MotorJointDef b2DefaultMotorJointDef() { B2MotorJointDef def = new B2MotorJointDef(); def.@base = b2DefaultJointDef(); - def.maxForce = 1.0f; - def.maxTorque = 1.0f; - def.correctionFactor = 0.3f; + def.relativeTransform.q = b2Rot_identity; def.internalValue = B2_SECRET_COOKIE; return def; } @@ -390,8 +390,7 @@ public static B2JointPair b2CreateJoint(B2World world, ref B2JointDef def, B2Joi if (joint.setIndex > (int)B2SetType.b2_disabledSet) { // Add edge to island graph - bool mergeIslands = true; - b2LinkJoint(world, joint, mergeIslands); + b2LinkJoint(world, joint); } // If the joint prevents collisions, then destroy all contacts between attached bodies @@ -418,6 +417,7 @@ public static B2JointId b2CreateDistanceJoint(B2WorldId worldId, ref B2DistanceJ } B2_ASSERT(b2IsValidFloat(def.length) && def.length > 0.0f); + B2_ASSERT(def.lowerSpringForce <= def.upperSpringForce); B2JointPair pair = b2CreateJoint(world, ref def.@base, B2JointType.b2_distanceJoint); @@ -433,6 +433,8 @@ public static B2JointId b2CreateDistanceJoint(B2WorldId worldId, ref B2DistanceJ joint.uj.distanceJoint.maxMotorForce = def.maxMotorForce; joint.uj.distanceJoint.motorSpeed = def.motorSpeed; joint.uj.distanceJoint.enableSpring = def.enableSpring; + joint.uj.distanceJoint.lowerSpringForce = def.lowerSpringForce; + joint.uj.distanceJoint.upperSpringForce = def.upperSpringForce; joint.uj.distanceJoint.enableLimit = def.enableLimit; joint.uj.distanceJoint.enableMotor = def.enableMotor; joint.uj.distanceJoint.impulse = 0.0f; @@ -462,14 +464,22 @@ public static B2JointId b2CreateMotorJoint(B2WorldId worldId, ref B2MotorJointDe B2JointSim joint = pair.jointSim; joint.uj.motorJoint = new B2MotorJoint(); - joint.uj.motorJoint.maxForce = def.maxForce; - joint.uj.motorJoint.maxTorque = def.maxTorque; - joint.uj.motorJoint.correctionFactor = b2ClampFloat(def.correctionFactor, 0.0f, 1.0f); + joint.uj.motorJoint.linearVelocity = def.linearVelocity; + joint.uj.motorJoint.maxVelocityForce = def.maxVelocityForce; + joint.uj.motorJoint.angularVelocity = def.angularVelocity; + joint.uj.motorJoint.maxVelocityTorque = def.maxVelocityTorque; + joint.uj.motorJoint.linearHertz = def.linearHertz; + joint.uj.motorJoint.linearDampingRatio = def.linearDampingRatio; + joint.uj.motorJoint.maxSpringForce = def.maxSpringForce; + joint.uj.motorJoint.angularHertz = def.angularHertz; + joint.uj.motorJoint.angularDampingRatio = def.angularDampingRatio; + joint.uj.motorJoint.maxSpringTorque = def.maxSpringTorque; B2JointId jointId = new B2JointId(joint.jointId + 1, world.worldId, pair.joint.generation); return jointId; } + public static B2JointId b2CreateMouseJoint(B2WorldId worldId, ref B2MouseJointDef def) { B2_CHECK_DEF(ref def); @@ -955,8 +965,8 @@ public static void b2GetJointReaction(B2JointSim sim, float invTimeStep, out flo case B2JointType.b2_motorJoint: { ref B2MotorJoint joint = ref sim.uj.motorJoint; - linearImpulse = b2Length(joint.linearImpulse); - angularImpulse = b2AbsFloat(joint.angularImpulse); + linearImpulse = b2Length(b2Add(joint.linearVelocityImpulse, joint.linearSpringImpulse)); + angularImpulse = b2AbsFloat(joint.angularVelocityImpulse + joint.angularSpringImpulse); } break; @@ -1570,6 +1580,11 @@ public static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) draw.DrawSegmentFcn(pA, pB, B2HexColor.b2_colorGold, draw.context); break; + case B2JointType.b2_motorJoint: + draw.DrawPointFcn(pA, 8.0f, B2HexColor.b2_colorYellowGreen, draw.context); + draw.DrawPointFcn(pB, 8.0f, B2HexColor.b2_colorPlum, draw.context); + break; + case B2JointType.b2_prismaticJoint: b2DrawPrismaticJoint(draw, jointSim, transformA, transformB, joint.drawScale); break; @@ -1595,32 +1610,33 @@ public static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) if (draw.drawGraphColors) { - Span graphColors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] { + Span graphColors = stackalloc B2HexColor[B2_GRAPH_COLOR_COUNT] + { B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorGreen, - + B2HexColor.b2_colorCyan, B2HexColor.b2_colorBlue, B2HexColor.b2_colorViolet, B2HexColor.b2_colorPink, - + B2HexColor.b2_colorChocolate, B2HexColor.b2_colorGoldenRod, B2HexColor.b2_colorCoral, B2HexColor.b2_colorRosyBrown, - + B2HexColor.b2_colorAqua, B2HexColor.b2_colorPeru, B2HexColor.b2_colorLime, B2HexColor.b2_colorGold, - + B2HexColor.b2_colorPlum, B2HexColor.b2_colorSnow, B2HexColor.b2_colorTeal, B2HexColor.b2_colorKhaki, - + B2HexColor.b2_colorSalmon, B2HexColor.b2_colorPeachPuff, B2HexColor.b2_colorHoneyDew, @@ -1632,7 +1648,7 @@ public static void b2DrawJoint(B2DebugDraw draw, B2World world, B2Joint joint) if (colorIndex != B2_NULL_INDEX) { B2Vec2 p = b2Lerp(pA, pB, 0.5f); - draw.DrawPointFcn( p, 5.0f, graphColors[colorIndex], draw.context ); + draw.DrawPointFcn(p, 5.0f, graphColors[colorIndex], draw.context); } } diff --git a/src/Box2D.NET/B2MathFunction.cs b/src/Box2D.NET/B2MathFunction.cs index 02c6adcd..a0666664 100644 --- a/src/Box2D.NET/B2MathFunction.cs +++ b/src/Box2D.NET/B2MathFunction.cs @@ -287,7 +287,7 @@ public static B2Vec2 b2GetLengthAndNormalize(ref float length, B2Vec2 v) public static B2Rot b2NormalizeRot(B2Rot q) { float mag = MathF.Sqrt(q.s * q.s + q.c * q.c); - float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; B2Rot qn = new B2Rot(q.c * invMag, q.s * invMag); return qn; } @@ -304,7 +304,7 @@ public static B2Rot b2IntegrateRotation(B2Rot q1, float deltaAngle) // s2 = s1 + omega * h * c1 B2Rot q2 = new B2Rot(q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c); float mag = MathF.Sqrt(q2.s * q2.s + q2.c * q2.c); - float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; B2Rot qn = new B2Rot(q2.c * invMag, q2.s * invMag); return qn; } @@ -363,7 +363,7 @@ public static B2Rot b2NLerp(B2Rot q1, B2Rot q2, float t) }; float mag = MathF.Sqrt(q.s * q.s + q.c * q.c); - float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; B2Rot qn = new B2Rot(q.c * invMag, q.s * invMag); return qn; } @@ -823,7 +823,7 @@ public static B2CosSin b2ComputeCosSin(float radians) } float mag = MathF.Sqrt(s * s + c * c); - float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + float invMag = mag > 0.0f ? 1.0f / mag : 0.0f; B2CosSin cs = new B2CosSin { cosine = c * invMag, sine = s * invMag }; return cs; } diff --git a/src/Box2D.NET/B2MotorJoint.cs b/src/Box2D.NET/B2MotorJoint.cs index 97931383..cafea639 100644 --- a/src/Box2D.NET/B2MotorJoint.cs +++ b/src/Box2D.NET/B2MotorJoint.cs @@ -6,13 +6,24 @@ namespace Box2D.NET { public struct B2MotorJoint { - public B2Vec2 linearOffset; - public float angularOffset; - public B2Vec2 linearImpulse; - public float angularImpulse; - public float maxForce; - public float maxTorque; - public float correctionFactor; + public B2Vec2 linearVelocity; + public float maxVelocityForce; + public float angularVelocity; + public float maxVelocityTorque; + public float linearHertz; + public float linearDampingRatio; + public float maxSpringForce; + public float angularHertz; + public float angularDampingRatio; + public float maxSpringTorque; + + public B2Vec2 linearVelocityImpulse; + public float angularVelocityImpulse; + public B2Vec2 linearSpringImpulse; + public float angularSpringImpulse; + + public B2Softness linearSpring; + public B2Softness angularSpring; public int indexA; public int indexB; diff --git a/src/Box2D.NET/B2MotorJointDef.cs b/src/Box2D.NET/B2MotorJointDef.cs index b5d35883..d63a2c27 100644 --- a/src/Box2D.NET/B2MotorJointDef.cs +++ b/src/Box2D.NET/B2MotorJointDef.cs @@ -4,23 +4,46 @@ namespace Box2D.NET { - /// A motor joint is used to control the relative motion between two bodies - /// You may move local frame A to change the target transform. - /// A typical usage is to control the movement of a dynamic body with respect to the ground. + /// A motor joint is used to control the relative velocity and or transform between two bodies. + /// With a velocity of zero this acts like top-down friction. /// @ingroup motor_joint public struct B2MotorJointDef { /// Base joint definition public B2JointDef @base; + /// The desired linear velocity + public B2Vec2 linearVelocity; + /// The maximum motor force in newtons - public float maxForce; + public float maxVelocityForce; + + /// The desired angular velocity + public float angularVelocity; /// The maximum motor torque in newton-meters - public float maxTorque; + public float maxVelocityTorque; + + /// Linear spring hertz for position control + public float linearHertz; + + /// Linear spring damping ratio + public float linearDampingRatio; + + /// Maximum spring force in newtons + public float maxSpringForce; + + /// Angular spring hertz for position control + public float angularHertz; + + /// Angular spring damping ratio + public float angularDampingRatio; + + /// Maximum spring torque in newton-meters + public float maxSpringTorque; - /// Position correction factor in the range [0,1] - public float correctionFactor; + /// The desired relative transform. Body B relative to bodyA. + public B2Transform relativeTransform; /// Used internally to detect a valid definition. DO NOT SET. public int internalValue; diff --git a/src/Box2D.NET/B2MotorJoints.cs b/src/Box2D.NET/B2MotorJoints.cs index eb7e0e92..627b3304 100644 --- a/src/Box2D.NET/B2MotorJoints.cs +++ b/src/Box2D.NET/B2MotorJoints.cs @@ -8,7 +8,7 @@ using static Box2D.NET.B2MathFunction; using static Box2D.NET.B2Bodies; using static Box2D.NET.B2Joints; - +using static Box2D.NET.B2Solvers; namespace Box2D.NET { @@ -22,51 +22,156 @@ public static class B2MotorJoints * is set by updating the local frames using b2Joint_SetLocalFrameA or b2Joint_SetLocalFrameB. * @{ */ - public static void b2MotorJoint_SetMaxForce(B2JointId jointId, float maxForce) + /// Set the desired relative linear velocity in meters per second + public static void b2MotorJoint_SetLinearVelocity(B2JointId jointId, B2Vec2 velocity) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.linearVelocity = velocity; + } + + /// Get the desired relative linear velocity in meters per second + public static B2Vec2 b2MotorJoint_GetLinearVelocity(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.linearVelocity; + } + + /// Set the desired relative angular velocity in radians per second + public static void b2MotorJoint_SetAngularVelocity(B2JointId jointId, float velocity) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.angularVelocity = velocity; + } + + /// Get the desired relative angular velocity in radians per second + public static float b2MotorJoint_GetAngularVelocity(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.angularVelocity; + } + + /// Set the motor joint maximum force, usually in newtons + public static void b2MotorJoint_SetMaxVelocityForce(B2JointId jointId, float maxForce) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.maxVelocityForce = maxForce; + } + + /// Get the motor joint maximum force, usually in newtons + public static float b2MotorJoint_GetMaxVelocityForce(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.maxVelocityForce; + } + + /// Set the motor joint maximum torque, usually in newton-meters + public static void b2MotorJoint_SetMaxVelocityTorque(B2JointId jointId, float maxTorque) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.maxVelocityTorque = maxTorque; + } + + /// Get the motor joint maximum torque, usually in newton-meters + public static float b2MotorJoint_GetMaxVelocityTorque(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.maxVelocityTorque; + } + + + /// Set the spring linear hertz stiffness + public static void b2MotorJoint_SetLinearHertz(B2JointId jointId, float hertz) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.linearHertz = hertz; + } + + /// Get the spring linear hertz stiffness + public static float b2MotorJoint_GetLinearHertz(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.linearHertz; + } + + /// Set the spring linear damping ratio. Use 1.0 for critical damping. + public static void b2MotorJoint_SetLinearDampingRatio(B2JointId jointId, float damping) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.linearDampingRatio = damping; + } + + /// Get the spring linear damping ratio. + public static float b2MotorJoint_GetLinearDampingRatio(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.linearDampingRatio; + } + + /// Set the spring angular hertz stiffness + public static void b2MotorJoint_SetAngularHertz(B2JointId jointId, float hertz) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + joint.uj.motorJoint.angularHertz = hertz; + } + + /// Get the spring angular hertz stiffness + public static float b2MotorJoint_GetAngularHertz(B2JointId jointId) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - joint.uj.motorJoint.maxForce = b2MaxFloat(0.0f, maxForce); + return joint.uj.motorJoint.angularHertz; } - public static float b2MotorJoint_GetMaxForce(B2JointId jointId) + /// Set the spring angular damping ratio. Use 1.0 for critical damping. + public static void b2MotorJoint_SetAngularDampingRatio(B2JointId jointId, float damping) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - return joint.uj.motorJoint.maxForce; + joint.uj.motorJoint.angularDampingRatio = damping; } - public static void b2MotorJoint_SetMaxTorque(B2JointId jointId, float maxTorque) + /// Get the spring angular damping ratio. + public static float b2MotorJoint_GetAngularDampingRatio(B2JointId jointId) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - joint.uj.motorJoint.maxTorque = b2MaxFloat(0.0f, maxTorque); + return joint.uj.motorJoint.angularDampingRatio; } - public static float b2MotorJoint_GetMaxTorque(B2JointId jointId) + /// Set the maximum spring force in newtons. + public static void b2MotorJoint_SetMaxSpringForce(B2JointId jointId, float maxForce) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - return joint.uj.motorJoint.maxTorque; + joint.uj.motorJoint.maxSpringForce = b2MaxFloat(0.0f, maxForce); } - public static void b2MotorJoint_SetCorrectionFactor(B2JointId jointId, float correctionFactor) + /// Get the maximum spring force in newtons. + public static float b2MotorJoint_GetMaxSpringForce(B2JointId jointId) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - joint.uj.motorJoint.correctionFactor = b2ClampFloat(correctionFactor, 0.0f, 1.0f); + return joint.uj.motorJoint.maxSpringForce; } - public static float b2MotorJoint_GetCorrectionFactor(B2JointId jointId) + /// Set the maximum spring torque in newtons * meters + public static void b2MotorJoint_SetMaxSpringTorque(B2JointId jointId, float maxTorque) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); - return joint.uj.motorJoint.correctionFactor; + joint.uj.motorJoint.maxSpringTorque = b2MaxFloat(0.0f, maxTorque); + } + + /// Get the maximum spring torque in newtons * meters + public static float b2MotorJoint_GetMaxSpringTorque(B2JointId jointId) + { + B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_motorJoint); + return joint.uj.motorJoint.maxSpringTorque; } public static B2Vec2 b2GetMotorJointForce(B2World world, B2JointSim @base) { - B2Vec2 force = b2MulSV(world.inv_h, @base.uj.motorJoint.linearImpulse); + B2Vec2 force = b2MulSV(world.inv_h, b2Add(@base.uj.motorJoint.linearVelocityImpulse, @base.uj.motorJoint.linearSpringImpulse)); return force; } public static float b2GetMotorJointTorque(B2World world, B2JointSim @base) { - return world.inv_h * @base.uj.motorJoint.angularImpulse; + return world.inv_h * (@base.uj.motorJoint.angularVelocityImpulse + @base.uj.motorJoint.angularSpringImpulse); } // Point-to-point constraint @@ -133,20 +238,25 @@ public static void b2PrepareMotorJoint(B2JointSim @base, B2StepContext context) B2Vec2 rA = joint.frameA.p; B2Vec2 rB = joint.frameB.p; - B2Mat22 K; - K.cx.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; - K.cx.Y = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; - K.cy.X = K.cx.Y; - K.cy.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; - joint.linearMass = b2GetInverse22(K); + joint.linearSpring = b2MakeSoft(joint.linearHertz, joint.linearDampingRatio, context.h); + joint.angularSpring = b2MakeSoft(joint.angularHertz, joint.angularDampingRatio, context.h); + + B2Mat22 kl; + kl.cx.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + kl.cx.Y = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + kl.cy.X = kl.cx.Y; + kl.cy.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + joint.linearMass = b2GetInverse22(kl); float ka = iA + iB; joint.angularMass = ka > 0.0f ? 1.0f / ka : 0.0f; if (context.enableWarmStarting == false) { - joint.linearImpulse = b2Vec2_zero; - joint.angularImpulse = 0.0f; + joint.linearVelocityImpulse = b2Vec2_zero; + joint.angularVelocityImpulse = 0.0f; + joint.linearSpringImpulse = b2Vec2_zero; + joint.angularSpringImpulse = 0.0f; } } @@ -170,10 +280,13 @@ public static void b2WarmStartMotorJoint(B2JointSim @base, B2StepContext context B2Vec2 rA = b2RotateVector(stateA.deltaRotation, joint.frameA.p); B2Vec2 rB = b2RotateVector(stateB.deltaRotation, joint.frameB.p); - stateA.linearVelocity = b2MulSub(stateA.linearVelocity, mA, joint.linearImpulse); - stateA.angularVelocity -= iA * (b2Cross(rA, joint.linearImpulse) + joint.angularImpulse); - stateB.linearVelocity = b2MulAdd(stateB.linearVelocity, mB, joint.linearImpulse); - stateB.angularVelocity += iB * (b2Cross(rB, joint.linearImpulse) + joint.angularImpulse); + B2Vec2 linearImpulse = b2Add(joint.linearVelocityImpulse, joint.linearSpringImpulse); + float angularImpulse = joint.angularVelocityImpulse + joint.angularSpringImpulse; + + stateA.linearVelocity = b2MulSub(stateA.linearVelocity, mA, linearImpulse); + stateA.angularVelocity -= iA * (b2Cross(rA, linearImpulse) + angularImpulse); + stateB.linearVelocity = b2MulAdd(stateB.linearVelocity, mB, linearImpulse); + stateB.angularVelocity += iB * (b2Cross(rB, linearImpulse) + angularImpulse); } public static void b2SolveMotorJoint(B2JointSim @base, B2StepContext context) @@ -197,52 +310,116 @@ public static void b2SolveMotorJoint(B2JointSim @base, B2StepContext context) B2Vec2 vB = stateB.linearVelocity; float wB = stateB.angularVelocity; - // angular constraint + // angular spring + if (joint.maxSpringTorque > 0.0f && joint.angularHertz > 0.0f) { B2Rot qA = b2MulRot(stateA.deltaRotation, joint.frameA.q); B2Rot qB = b2MulRot(stateB.deltaRotation, joint.frameB.q); B2Rot relQ = b2InvMulRot(qA, qB); - float jointAngle = b2Rot_GetAngle(relQ); - float angularBias = context.inv_h * joint.correctionFactor * jointAngle; + float c = b2Rot_GetAngle(relQ); + float bias = joint.angularSpring.biasRate * c; + float massScale = joint.angularSpring.massScale; + float impulseScale = joint.angularSpring.impulseScale; - float Cdot = wB - wA; - float impulse = -joint.angularMass * (Cdot + angularBias); + float cdot = wB - wA; - float oldImpulse = joint.angularImpulse; - float maxImpulse = context.h * joint.maxTorque; - joint.angularImpulse = b2ClampFloat(joint.angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = joint.angularImpulse - oldImpulse; + float maxImpulse = context.h * joint.maxSpringTorque; + float oldImpulse = joint.angularSpringImpulse; + float impulse = -massScale * joint.angularMass * (cdot + bias) - impulseScale * oldImpulse; + joint.angularSpringImpulse = b2ClampFloat(oldImpulse + impulse, -maxImpulse, maxImpulse); + impulse = joint.angularSpringImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } - // linear constraint + // angular velocity + if (joint.maxVelocityTorque > 0.0) { - B2Vec2 rA = b2RotateVector(stateA.deltaRotation, joint.frameA.p); - B2Vec2 rB = b2RotateVector(stateB.deltaRotation, joint.frameB.p); + float cdot = wB - wA - joint.angularVelocity; + float impulse = -joint.angularMass * cdot; - B2Vec2 ds = b2Add(b2Sub(stateB.deltaPosition, stateA.deltaPosition), b2Sub(rB, rA)); - B2Vec2 linearSeparation = b2Add(joint.deltaCenter, ds); - B2Vec2 linearBias = b2MulSV(context.inv_h * joint.correctionFactor, linearSeparation); + float maxImpulse = context.h * joint.maxVelocityTorque; + float oldImpulse = joint.angularVelocityImpulse; + joint.angularVelocityImpulse = b2ClampFloat(oldImpulse + impulse, -maxImpulse, maxImpulse); + impulse = joint.angularVelocityImpulse - oldImpulse; - B2Vec2 Cdot = b2Sub(b2Add(vB, b2CrossSV(wB, rB)), b2Add(vA, b2CrossSV(wA, rA))); - B2Vec2 b = b2MulMV(joint.linearMass, b2Add(Cdot, linearBias)); + wA -= iA * impulse; + wB += iB * impulse; + } + + B2Vec2 rA = b2RotateVector(stateA.deltaRotation, joint.frameA.p); + B2Vec2 rB = b2RotateVector(stateB.deltaRotation, joint.frameB.p); + + // linear spring + if (joint.maxSpringForce > 0.0f && joint.linearHertz > 0.0f) + { + B2Vec2 dcA = stateA.deltaPosition; + B2Vec2 dcB = stateB.deltaPosition; + B2Vec2 c = b2Add(b2Add(b2Sub(dcB, dcA), b2Sub(rB, rA)), joint.deltaCenter); + + B2Vec2 bias = b2MulSV(joint.linearSpring.biasRate, c); + float massScale = joint.linearSpring.massScale; + float impulseScale = joint.linearSpring.impulseScale; + + B2Vec2 cdot = b2Sub(b2Add(vB, b2CrossSV(wB, rB)), b2Add(vA, b2CrossSV(wA, rA))); + cdot = b2Add(cdot, bias); + + // Updating the effective mass here may be overkill + B2Mat22 kl; + kl.cx.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; + kl.cx.Y = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; + kl.cy.X = kl.cx.Y; + kl.cy.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; + joint.linearMass = b2GetInverse22(kl); + + B2Vec2 b = b2MulMV(joint.linearMass, cdot); + + B2Vec2 oldImpulse = joint.linearSpringImpulse; + B2Vec2 impulse = new B2Vec2( + -massScale * b.X - impulseScale * oldImpulse.X, + -massScale * b.Y - impulseScale * oldImpulse.Y + ); + + float maxImpulse = context.h * joint.maxSpringForce; + joint.linearSpringImpulse = b2Add(joint.linearSpringImpulse, impulse); + + if (b2LengthSquared(joint.linearSpringImpulse) > maxImpulse * maxImpulse) + { + joint.linearSpringImpulse = b2Normalize(joint.linearSpringImpulse); + joint.linearSpringImpulse.X *= maxImpulse; + joint.linearSpringImpulse.Y *= maxImpulse; + } + + impulse = b2Sub(joint.linearSpringImpulse, oldImpulse); + + vA = b2MulSub(vA, mA, impulse); + wA -= iA * b2Cross(rA, impulse); + vB = b2MulAdd(vB, mB, impulse); + wB += iB * b2Cross(rB, impulse); + } + + // linear velocity + if (joint.maxVelocityForce > 0.0f) + { + B2Vec2 cdot = b2Sub(b2Add(vB, b2CrossSV(wB, rB)), b2Add(vA, b2CrossSV(wA, rA))); + cdot = b2Sub(cdot, joint.linearVelocity); + B2Vec2 b = b2MulMV(joint.linearMass, cdot); B2Vec2 impulse = new B2Vec2(-b.X, -b.Y); - B2Vec2 oldImpulse = joint.linearImpulse; - float maxImpulse = context.h * joint.maxForce; - joint.linearImpulse = b2Add(joint.linearImpulse, impulse); + B2Vec2 oldImpulse = joint.linearVelocityImpulse; + float maxImpulse = context.h * joint.maxVelocityForce; + joint.linearVelocityImpulse = b2Add(joint.linearVelocityImpulse, impulse); - if (b2LengthSquared(joint.linearImpulse) > maxImpulse * maxImpulse) + if (b2LengthSquared(joint.linearVelocityImpulse) > maxImpulse * maxImpulse) { - joint.linearImpulse = b2Normalize(joint.linearImpulse); - joint.linearImpulse.X *= maxImpulse; - joint.linearImpulse.Y *= maxImpulse; + joint.linearVelocityImpulse = b2Normalize(joint.linearVelocityImpulse); + joint.linearVelocityImpulse.X *= maxImpulse; + joint.linearVelocityImpulse.Y *= maxImpulse; } - impulse = b2Sub(joint.linearImpulse, oldImpulse); + impulse = b2Sub(joint.linearVelocityImpulse, oldImpulse); vA = b2MulSub(vA, mA, impulse); wA -= iA * b2Cross(rA, impulse); diff --git a/src/Box2D.NET/B2Profile.cs b/src/Box2D.NET/B2Profile.cs index 93f40695..aa45fd5f 100644 --- a/src/Box2D.NET/B2Profile.cs +++ b/src/Box2D.NET/B2Profile.cs @@ -12,7 +12,6 @@ public struct B2Profile public float pairs; public float collide; public float solve; - public float mergeIslands; public float prepareStages; public float solveConstraints; public float prepareConstraints; diff --git a/src/Box2D.NET/B2RevoluteJoints.cs b/src/Box2D.NET/B2RevoluteJoints.cs index 992dbe88..a68ded14 100644 --- a/src/Box2D.NET/B2RevoluteJoints.cs +++ b/src/Box2D.NET/B2RevoluteJoints.cs @@ -15,6 +15,20 @@ namespace Box2D.NET { public static class B2RevoluteJoints { + // Point-to-point constraint + // C = pB - pA + // Cdot = vB - vA + // = vB + cross(wB, rB) - vA - cross(wA, rA) + // J = [-E -skew(rA) E skew(rB) ] + + // Identity used: + // w k % (rx i + ry j) = w * (-ry i + rx j) + + // Motor constraint + // Cdot = wB - wA + // J = [0 0 -1 0 0 1] + // K = invIA + invIB + public static void b2RevoluteJoint_EnableSpring(B2JointId jointId, bool enableSpring) { B2JointSim joint = b2GetJointSimCheckType(jointId, B2JointType.b2_revoluteJoint); @@ -190,19 +204,6 @@ public static float b2GetRevoluteJointTorque(B2World world, B2JointSim @base) return torque; } - // 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) - - // Motor constraint - // Cdot = w2 - w1 - // J = [0 0 -1 0 0 1] - // K = invI1 + invI2 - public static void b2PrepareRevoluteJoint(B2JointSim @base, B2StepContext context) { B2_ASSERT(@base.type == B2JointType.b2_revoluteJoint); diff --git a/src/Box2D.NET/B2Sensors.cs b/src/Box2D.NET/B2Sensors.cs index 369ca6b1..188c6796 100644 --- a/src/Box2D.NET/B2Sensors.cs +++ b/src/Box2D.NET/B2Sensors.cs @@ -74,15 +74,18 @@ public static bool b2SensorQueryCallback(int proxyId, ulong userData, ref B2Sens } // Custom user filter - b2CustomFilterFcn customFilterFcn = queryContext.world.customFilterFcn; - if (customFilterFcn != null) + if (sensorShape.enableCustomFiltering || otherShape.enableCustomFiltering) { - B2ShapeId idA = new B2ShapeId(sensorShapeId + 1, world.worldId, sensorShape.generation); - B2ShapeId idB = new B2ShapeId(shapeId + 1, world.worldId, otherShape.generation); - bool shouldCollide = customFilterFcn(idA, idB, queryContext.world.customFilterContext); - if (shouldCollide == false) + b2CustomFilterFcn customFilterFcn = queryContext.world.customFilterFcn; + if (customFilterFcn != null) { - return true; + B2ShapeId idA = new B2ShapeId(sensorShapeId + 1, world.worldId, sensorShape.generation); + B2ShapeId idB = new B2ShapeId(shapeId + 1, world.worldId, otherShape.generation); + bool shouldCollide = customFilterFcn(idA, idB, queryContext.world.customFilterContext); + if (shouldCollide == false) + { + return true; + } } } diff --git a/src/Box2D.NET/B2Shape.cs b/src/Box2D.NET/B2Shape.cs index 0e53d8b2..63319117 100644 --- a/src/Box2D.NET/B2Shape.cs +++ b/src/Box2D.NET/B2Shape.cs @@ -35,6 +35,7 @@ public class B2Shape public ushort generation; public bool enableSensorEvents; public bool enableContactEvents; + public bool enableCustomFiltering; public bool enableHitEvents; public bool enablePreSolveEvents; public bool enlargedAABB; diff --git a/src/Box2D.NET/B2ShapeDef.cs b/src/Box2D.NET/B2ShapeDef.cs index e2b9144f..56cec2ea 100644 --- a/src/Box2D.NET/B2ShapeDef.cs +++ b/src/Box2D.NET/B2ShapeDef.cs @@ -24,6 +24,9 @@ public struct B2ShapeDef /// Collision filtering data. public B2Filter filter; + + /// Enable custom filtering. Only one of the two shapes needs to enable custom filtering. See b2WorldDef. + public bool enableCustomFiltering; /// A sensor shape generates overlap events but never generates a collision response. /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. diff --git a/src/Box2D.NET/B2Shapes.cs b/src/Box2D.NET/B2Shapes.cs index d829b94e..d0c6380c 100644 --- a/src/Box2D.NET/B2Shapes.cs +++ b/src/Box2D.NET/B2Shapes.cs @@ -154,6 +154,7 @@ public static B2Shape b2CreateShapeInternal(B2World world, B2Body body, B2Tra shape.enlargedAABB = false; shape.enableSensorEvents = def.enableSensorEvents; shape.enableContactEvents = def.enableContactEvents; + shape.enableCustomFiltering = def.enableCustomFiltering; shape.enableHitEvents = def.enableHitEvents; shape.enablePreSolveEvents = def.enablePreSolveEvents; shape.proxyKey = B2_NULL_INDEX; diff --git a/src/Box2D.NET/B2Solvers.cs b/src/Box2D.NET/B2Solvers.cs index 2a292348..c5de95f1 100644 --- a/src/Box2D.NET/B2Solvers.cs +++ b/src/Box2D.NET/B2Solvers.cs @@ -358,15 +358,18 @@ public static bool b2ContinuousQueryCallback(int proxyId, ulong userData, ref B2 } // Custom user filtering - b2CustomFilterFcn customFilterFcn = world.customFilterFcn; - if (customFilterFcn != null) + if (shape.enableCustomFiltering || fastShape.enableCustomFiltering) { - B2ShapeId idA = new B2ShapeId(shape.id + 1, world.worldId, shape.generation); - B2ShapeId idB = new B2ShapeId(fastShape.id + 1, world.worldId, fastShape.generation); - canCollide = customFilterFcn(idA, idB, world.customFilterContext); - if (canCollide == false) + b2CustomFilterFcn customFilterFcn = world.customFilterFcn; + if (customFilterFcn != null) { - return true; + B2ShapeId idA = new B2ShapeId(shape.id + 1, world.worldId, shape.generation); + B2ShapeId idB = new B2ShapeId(fastShape.id + 1, world.worldId, fastShape.generation); + canCollide = customFilterFcn(idA, idB, world.customFilterContext); + if (canCollide == false) + { + return true; + } } } @@ -1325,17 +1328,6 @@ public static void b2Solve(B2World world, B2StepContext stepContext) { world.stepIndex += 1; - // Merge islands - { - b2TracyCZoneNC(B2TracyCZone.merge, "Merge", B2HexColor.b2_colorLightGoldenRodYellow, true); - ulong mergeTicks = b2GetTicks(); - - b2MergeAwakeIslands(world); - - world.profile.mergeIslands = b2GetMilliseconds(mergeTicks); - b2TracyCZoneEnd(B2TracyCZone.merge); - } - // Are there any awake bodies? This scenario should not be important for profiling. B2SolverSet awakeSet = b2Array_Get(ref world.solverSets, (int)B2SetType.b2_awakeSet); int awakeBodyCount = awakeSet.bodySims.count; diff --git a/src/Box2D.NET/B2Tables.cs b/src/Box2D.NET/B2Tables.cs index f698120e..83ac07b3 100644 --- a/src/Box2D.NET/B2Tables.cs +++ b/src/Box2D.NET/B2Tables.cs @@ -77,7 +77,8 @@ public static void b2ClearSet(ref B2HashSet set) // https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ // https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ // todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ - // todo try: https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ + // todo try: + // https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ [MethodImpl(MethodImplOptions.AggressiveInlining)] public static uint b2KeyHash(ulong key) { diff --git a/src/Box2D.NET/B2WeldJoint.cs b/src/Box2D.NET/B2WeldJoint.cs index 01783525..cc2b00da 100644 --- a/src/Box2D.NET/B2WeldJoint.cs +++ b/src/Box2D.NET/B2WeldJoint.cs @@ -11,8 +11,8 @@ public struct B2WeldJoint public float angularHertz; public float angularDampingRatio; - public B2Softness linearSoftness; - public B2Softness angularSoftness; + public B2Softness linearSpring; + public B2Softness angularSpring; public B2Vec2 linearImpulse; public float angularImpulse; diff --git a/src/Box2D.NET/B2WeldJoints.cs b/src/Box2D.NET/B2WeldJoints.cs index a246787b..028d3810 100644 --- a/src/Box2D.NET/B2WeldJoints.cs +++ b/src/Box2D.NET/B2WeldJoints.cs @@ -78,19 +78,19 @@ public static float b2GetWeldJointTorque(B2World world, B2JointSim @base) return world.inv_h * @base.uj.weldJoint.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) - -// Angle constraint -// C = angle2 - angle1 - referenceAngle -// Cdot = w2 - w1 -// J = [0 0 -1 0 0 1] -// K = invI1 + invI2 + // 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 public static void b2PrepareWeldJoint(B2JointSim @base, B2StepContext context) { @@ -143,20 +143,20 @@ public static void b2PrepareWeldJoint(B2JointSim @base, B2StepContext context) if (joint.linearHertz == 0.0f) { - joint.linearSoftness = @base.constraintSoftness; + joint.linearSpring = @base.constraintSoftness; } else { - joint.linearSoftness = b2MakeSoft(joint.linearHertz, joint.linearDampingRatio, context.h); + joint.linearSpring = b2MakeSoft(joint.linearHertz, joint.linearDampingRatio, context.h); } if (joint.angularHertz == 0.0f) { - joint.angularSoftness = @base.constraintSoftness; + joint.angularSpring = @base.constraintSoftness; } else { - joint.angularSoftness = b2MakeSoft(joint.angularHertz, joint.angularDampingRatio, context.h); + joint.angularSpring = b2MakeSoft(joint.angularHertz, joint.angularDampingRatio, context.h); } if (context.enableWarmStarting == false) @@ -226,9 +226,9 @@ public static void b2SolveWeldJoint(B2JointSim @base, B2StepContext context, boo if (useBias || joint.angularHertz > 0.0f) { float C = jointAngle; - bias = joint.angularSoftness.biasRate * C; - massScale = joint.angularSoftness.massScale; - impulseScale = joint.angularSoftness.impulseScale; + bias = joint.angularSpring.biasRate * C; + massScale = joint.angularSpring.massScale; + impulseScale = joint.angularSpring.impulseScale; } float Cdot = wB - wA; @@ -253,9 +253,9 @@ public static void b2SolveWeldJoint(B2JointSim @base, B2StepContext context, boo B2Vec2 dcB = stateB.deltaPosition; B2Vec2 C = b2Add(b2Add(b2Sub(dcB, dcA), b2Sub(rB, rA)), joint.deltaCenter); - bias = b2MulSV(joint.linearSoftness.biasRate, C); - massScale = joint.linearSoftness.massScale; - impulseScale = joint.linearSoftness.impulseScale; + bias = b2MulSV(joint.linearSpring.biasRate, C); + massScale = joint.linearSpring.massScale; + impulseScale = joint.linearSpring.impulseScale; } B2Vec2 Cdot = b2Sub(b2Add(vB, b2CrossSV(wB, rB)), b2Add(vA, b2CrossSV(wA, rA))); diff --git a/src/Box2D.NET/B2Worlds.cs b/src/Box2D.NET/B2Worlds.cs index ab58092d..35088aaa 100644 --- a/src/Box2D.NET/B2Worlds.cs +++ b/src/Box2D.NET/B2Worlds.cs @@ -1337,6 +1337,8 @@ public static bool b2World_IsValid(B2WorldId id) return id.generation == world.generation; } + /// Body identifier validation. A valid body exists in a world and is non-null. + /// This can be used to detect orphaned ids. Provides validation for up to 64K allocations. public static bool b2Body_IsValid(B2BodyId id) { if (B2_MAX_WORLDS <= id.world0) @@ -2491,6 +2493,7 @@ public static void b2World_EnableSpeculative(B2WorldId worldId, bool flag) } #if DEBUG +#if FALSE // When validating islands ids I have to compare the root island // ids because islands are not merged until the next time step. public static int b2GetRootIslandId(B2World world, int islandId) @@ -2513,6 +2516,7 @@ public static int b2GetRootIslandId(B2World world, int islandId) return rootId; } +#endif // This validates island graph connectivity for each body public static void b2ValidateConnectivity(B2World world) @@ -2534,7 +2538,8 @@ public static void b2ValidateConnectivity(B2World world) B2_ASSERT(bodyIndex == body.id); // Need to get the root island because islands are not merged until the next time step - int bodyIslandId = b2GetRootIslandId(world, body.islandId); + //int bodyIslandId = b2GetRootIslandId(world, body.islandId); + int bodyIslandId = body.islandId; int bodySetIndex = body.setIndex; int contactKey = body.headContactKey; @@ -2550,7 +2555,8 @@ public static void b2ValidateConnectivity(B2World world) { if (bodySetIndex != (int)B2SetType.b2_staticSet) { - int contactIslandId = b2GetRootIslandId(world, contact.islandId); + //int contactIslandId = b2GetRootIslandId(world, contact.islandId); + int contactIslandId = contact.islandId; B2_ASSERT(contactIslandId == bodyIslandId); } } @@ -2587,7 +2593,8 @@ public static void b2ValidateConnectivity(B2World world) } else { - int jointIslandId = b2GetRootIslandId(world, joint.islandId); + //int jointIslandId = b2GetRootIslandId(world, joint.islandId); + int jointIslandId = joint.islandId; B2_ASSERT(jointIslandId == bodyIslandId); } @@ -2814,63 +2821,62 @@ public static void b2ValidateSolverSets(B2World world) for (int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex) { ref B2GraphColor color = ref world.constraintGraph.colors[colorIndex]; - int bitCount = 0; + int bitCount = 0; - B2_ASSERT(color.contactSims.count >= 0); - totalContactCount += color.contactSims.count; - for (int i = 0; i < color.contactSims.count; ++i) - { - B2ContactSim contactSim = color.contactSims.data[i]; - 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); - B2_ASSERT(contact.setIndex == (int)B2SetType.b2_awakeSet); - B2_ASSERT(contact.colorIndex == colorIndex); - B2_ASSERT(contact.localIndex == i); + B2_ASSERT(color.contactSims.count >= 0); + totalContactCount += color.contactSims.count; + for (int i = 0; i < color.contactSims.count; ++i) + { + B2ContactSim contactSim = color.contactSims.data[i]; + 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); + B2_ASSERT(contact.setIndex == (int)B2SetType.b2_awakeSet); + B2_ASSERT(contact.colorIndex == colorIndex); + B2_ASSERT(contact.localIndex == i); - int bodyIdA = contact.edges[0].bodyId; - int bodyIdB = contact.edges[1].bodyId; + int bodyIdA = contact.edges[0].bodyId; + int bodyIdB = contact.edges[1].bodyId; - if (colorIndex < B2_OVERFLOW_INDEX) - { - B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); - B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); - B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); - B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); - - bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; - bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; - } + if (colorIndex < B2_OVERFLOW_INDEX) + { + B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); + B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); + B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); + B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); + + bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; + bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; } - + } - - B2_ASSERT(color.jointSims.count >= 0); - totalJointCount += color.jointSims.count; - for (int i = 0; i < color.jointSims.count; ++i) - { - B2JointSim jointSim = color.jointSims.data[i]; - B2Joint joint = b2Array_Get(ref world.joints, jointSim.jointId); - B2_ASSERT(joint.setIndex == (int)B2SetType.b2_awakeSet); - B2_ASSERT(joint.colorIndex == colorIndex); - B2_ASSERT(joint.localIndex == i); - int bodyIdA = joint.edges[0].bodyId; - int bodyIdB = joint.edges[1].bodyId; + B2_ASSERT(color.jointSims.count >= 0); + totalJointCount += color.jointSims.count; + for (int i = 0; i < color.jointSims.count; ++i) + { + B2JointSim jointSim = color.jointSims.data[i]; + B2Joint joint = b2Array_Get(ref world.joints, jointSim.jointId); + B2_ASSERT(joint.setIndex == (int)B2SetType.b2_awakeSet); + B2_ASSERT(joint.colorIndex == colorIndex); + B2_ASSERT(joint.localIndex == i); - if (colorIndex < B2_OVERFLOW_INDEX) - { - B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); - B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); - B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); - B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); - - bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; - bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; - } + int bodyIdA = joint.edges[0].bodyId; + int bodyIdB = joint.edges[1].bodyId; + + if (colorIndex < B2_OVERFLOW_INDEX) + { + B2Body bodyA = b2Array_Get(ref world.bodies, bodyIdA); + B2Body bodyB = b2Array_Get(ref world.bodies, bodyIdB); + B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdA) == (bodyA.type != B2BodyType.b2_staticBody)); + B2_ASSERT(b2GetBit(ref color.bodySet, bodyIdB) == (bodyB.type != B2BodyType.b2_staticBody)); + + bitCount += bodyA.type == B2BodyType.b2_staticBody ? 0 : 1; + bitCount += bodyB.type == B2BodyType.b2_staticBody ? 0 : 1; } - + } + // Validate the bit population for this graph color B2_ASSERT(bitCount == b2CountSetBits(ref color.bodySet)); }