diff --git a/Game/Physics2D/PhysicsEngine2D.cs b/Game/Physics2D/PhysicsEngine2D.cs index 7803293..71a13a9 100644 --- a/Game/Physics2D/PhysicsEngine2D.cs +++ b/Game/Physics2D/PhysicsEngine2D.cs @@ -69,6 +69,7 @@ public class PhysicsEngine2D : IPhysicsEngine2D for (int i = colliders.Count - 1; i >= 0; i--) CheckCollisions(colliders[i], colliders, (c1, c2, v) => { + // Console.WriteLine($"c1: {c1.BehaviourController.GameObject.Name}, c2: {c2.BehaviourController.GameObject.Name}"); NewMethod(c1, c2, v, intervalDeltaTime); NewMethod(c2, c1, v, intervalDeltaTime); @@ -89,7 +90,7 @@ public class PhysicsEngine2D : IPhysicsEngine2D if (c1.RigidBody2D is not IRigidBody2D) return; - Line vertexTrajectory = new Line(vertex, vertex - c1.RigidBody2D.Velocity); + Line vertexTrajectory = new Line(vertex, vertex - c1.RigidBody2D.Velocity * intervalDeltaTime); if (vertexTrajectory.LengthSquared <= 0.001f) return; @@ -100,19 +101,22 @@ public class PhysicsEngine2D : IPhysicsEngine2D foreach (var line in lines) { - if (!vertexTrajectory.Intersects(line, out Vector2? intersectionPoint) || intersectionPoint is not null) + if (!vertexTrajectory.Intersects(line, out Vector2? intersectionPoint)) continue; - t = vertexTrajectory.GetT(vertex); + if (intersectionPoint is not Vector2 ip) + continue; + + t = vertexTrajectory.GetT(ip); Vector2 lineDirection = line.Direction; normal = new(lineDirection.Y, lineDirection.X); break; } - StepRigidBody(c1.RigidBody2D, -t); + StepRigidBody(c1.RigidBody2D, -t * intervalDeltaTime); c1.RigidBody2D.Velocity = Vector2.Reflect(c1.RigidBody2D.Velocity, normal); - StepRigidBody(c1.RigidBody2D, intervalDeltaTime - t); + StepRigidBody(c1.RigidBody2D, intervalDeltaTime - t * intervalDeltaTime); } private void CheckCollisions(ICollider2D collider2D, List collider2Ds, Action OnCollisionDetectedAction) @@ -126,11 +130,8 @@ public class PhysicsEngine2D : IPhysicsEngine2D for (int y = 0; y < collider2D.Vertices.Count; y++) { Vector2 vertex = collider2D.Vertices[y]; - if (collider2D.CheckCollision(vertex)) - { + if (collider2DItem.CheckCollision(vertex)) OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem, vertex); - y--; // Recheck current vertex - } } } } diff --git a/Game/Physics2D/PhysicsMath.cs b/Game/Physics2D/PhysicsMath.cs index 052cdb1..474ab0c 100644 --- a/Game/Physics2D/PhysicsMath.cs +++ b/Game/Physics2D/PhysicsMath.cs @@ -129,7 +129,7 @@ public static class PhysicsMath } public static bool LaysOn(this Vector2 point, Line line) - => line.Resolve(point.X) == point; + => ApproximatelyEqualEpsilon(line.Resolve(point.X), point, float.Epsilon); public static LineEquation ToLineEquation(this Line line) { @@ -179,8 +179,8 @@ public static class PhysicsMath public static float GetT(this Line line, Vector2 point) { - if (!point.LaysOn(line)) - throw new Exception("Point does not lay on Line"); + // if (!point.LaysOn(line)) + // throw new Exception("Point does not lay on Line"); float fromX = MathF.Abs(line.From.X); float toX = MathF.Abs(line.To.X); @@ -263,4 +263,24 @@ public static class PhysicsMath public static bool Inside(this Circle circle, Vector2 point) => (circle.Position - point).LengthSquared() <= circle.Radius * circle.Radius; + + public static bool ApproximatelyEqualEpsilon(float a, float b, float epsilon) + { + if (a == b) + return true; + + const float floatNormal = (1 << 23) * float.Epsilon; + float absA = Math.Abs(a); + float absB = Math.Abs(b); + float diff = Math.Abs(a - b); + + if (a == 0.0f || b == 0.0f || diff < floatNormal) + return diff < (epsilon * floatNormal); + + return diff / Math.Min((absA + absB), float.MaxValue) < epsilon; + } + public static bool ApproximatelyEqualEpsilon(Vector2 a, Vector2 b, float epsilon) + { + return ApproximatelyEqualEpsilon(a.X, b.X, epsilon) && ApproximatelyEqualEpsilon(a.Y, b.Y, epsilon); + } }