using System; using System.Collections.Generic; using System.Numerics; namespace PhysicsEngine { public static class CollisionEngine { private const float Slop = 0.01f; private const float PositionCorrectionPercent = 0.2f; private const float MaxCorrection = 0.2f; private const int VelocityIterations = 8; private class Contact { public Vector2 Point; public Vector2 Normal; // from BodyA to BodyB public Vector2 Tangent; // perpendicular to Normal public float Penetration; public float AccumulatedNormalImpulse; public float AccumulatedTangentImpulse; public Body BodyA; public Body BodyB; public Vector2 RA; public Vector2 RB; public float NormalMass; public float TangentMass; public float Friction; public float Restitution; public float RestitutionBias; public float Bias; // baumgarte position correction bias } public static void ResolveCollisions(List bodies, float dt) { if (dt <= 0) return; List contacts = new List(); // broadphase & barrowphase collision detection for (int i = 0; i < bodies.Count; i++) { for (int j = i + 1; j < bodies.Count; j++) { Body b1 = bodies[i]; Body b2 = bodies[j]; if (b1.IsStatic && b2.IsStatic) continue; if (b1 is Circle && b2 is Circle) ResolveCircleCircle((Circle)b1, (Circle)b2, contacts); else if (b1 is Box && b2 is Circle) ResolveBoxCircle((Box)b1, (Circle)b2, contacts, false); else if (b1 is Circle && b2 is Box) ResolveBoxCircle((Box)b2, (Circle)b1, contacts, true); else if (b1 is Box && b2 is Box) ResolveBoxBox((Box)b1, (Box)b2, contacts); } } // pre-step / initialization foreach (var contact in contacts) { contact.AccumulatedNormalImpulse = 0; contact.AccumulatedTangentImpulse = 0; contact.RA = contact.Point - contact.BodyA.Position; contact.RB = contact.Point - contact.BodyB.Position; float invMassA = contact.BodyA.InverseMass; float invMassB = contact.BodyB.InverseMass; float invIA = contact.BodyA.InverseInertia; float invIB = contact.BodyB.InverseInertia; float rnA = Cross(contact.RA, contact.Normal); float rnB = Cross(contact.RB, contact.Normal); float denom = invMassA + invMassB + rnA * rnA * invIA + rnB * rnB * invIB; contact.NormalMass = denom > 0 ? 1.0f / denom : 0; contact.Tangent = new Vector2(-contact.Normal.Y, contact.Normal.X); float rtA = Cross(contact.RA, contact.Tangent); float rtB = Cross(contact.RB, contact.Tangent); denom = invMassA + invMassB + rtA * rtA * invIA + rtB * rtB * invIB; contact.TangentMass = denom > 0 ? 1.0f / denom : 0; contact.Friction = (float)Math.Sqrt(contact.BodyA.Friction * contact.BodyB.Friction); contact.Restitution = Math.Max(contact.BodyA.Restitution, contact.BodyB.Restitution); float rvNormal = Vector2.Dot(GetRelativeVelocity(contact), contact.Normal); contact.RestitutionBias = rvNormal < -0.5f ? -contact.Restitution * rvNormal : 0; float penetrationError = Math.Max(contact.Penetration - Slop, 0.0f); contact.Bias = (PositionCorrectionPercent / dt) * penetrationError; contact.Bias = Math.Min(contact.Bias, MaxCorrection / dt); } // velocity Impulse Solver for (int iter = 0; iter < VelocityIterations; iter++) { foreach (var contact in contacts) { // normal impulse Vector2 rv = GetRelativeVelocity(contact); float vn = Vector2.Dot(rv, contact.Normal); float targetVn = contact.RestitutionBias + contact.Bias; float lambda = -contact.NormalMass * (vn - targetVn); float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0.0f); lambda = newImpulse - contact.AccumulatedNormalImpulse; contact.AccumulatedNormalImpulse = newImpulse; ApplyImpulse(contact, contact.Normal * lambda); // tangent impulse rv = GetRelativeVelocity(contact); float vt = Vector2.Dot(rv, contact.Tangent); float lambdaT = -contact.TangentMass * vt; float maxFriction = contact.Friction * contact.AccumulatedNormalImpulse; float oldImpulseT = contact.AccumulatedTangentImpulse; contact.AccumulatedTangentImpulse = Math.Clamp(oldImpulseT + lambdaT, -maxFriction, maxFriction); lambdaT = contact.AccumulatedTangentImpulse - oldImpulseT; ApplyImpulse(contact, contact.Tangent * lambdaT); } } } private static void ResolveCircleCircle(Circle c1, Circle c2, List contacts) { Vector2 d = c2.Position - c1.Position; float distSq = d.LengthSquared(); float radiusSum = c1.Radius + c2.Radius; if (distSq >= radiusSum * radiusSum || distSq < 1e-12f) return; float dist = (float)Math.Sqrt(distSq); Vector2 normal = d / dist; float penetration = radiusSum - dist; Vector2 contactPoint = c1.Position + normal * (c1.Radius - penetration * 0.5f); contacts.Add(new Contact { Point = contactPoint, Normal = normal, Penetration = penetration, BodyA = c1, BodyB = c2 }); } private static void ResolveBoxCircle(Box box, Circle circle, List contacts, bool swapped) { Vector2 localCircleCenter = circle.Position - box.Position; float cos = (float)Math.Cos(box.Rotation); float sin = (float)Math.Sin(box.Rotation); Vector2 local = new Vector2( localCircleCenter.X * cos + localCircleCenter.Y * sin, -localCircleCenter.X * sin + localCircleCenter.Y * cos); float hx = box.Size.X * 0.5f; float hy = box.Size.Y * 0.5f; Vector2 closestLocal = new Vector2(Math.Clamp(local.X, -hx, hx), Math.Clamp(local.Y, -hy, hy)); Vector2 normalWorld; Vector2 contactPointWorld; float penetration; bool inside = (closestLocal == local); if (inside) { float distLeft = local.X + hx; float distRight = hx - local.X; float distBottom = local.Y + hy; float distTop = hy - local.Y; float minDist = Math.Min(Math.Min(distLeft, distRight), Math.Min(distBottom, distTop)); Vector2 normalLocal; if (minDist == distLeft) normalLocal = new Vector2(-1, 0); else if (minDist == distRight) normalLocal = new Vector2(1, 0); else if (minDist == distBottom) normalLocal = new Vector2(0, -1); else normalLocal = new Vector2(0, 1); normalWorld = new Vector2(normalLocal.X * cos - normalLocal.Y * sin, normalLocal.X * sin + normalLocal.Y * cos); penetration = circle.Radius + minDist; Vector2 faceLocal = local; if (normalLocal.X != 0) faceLocal.X = normalLocal.X > 0 ? hx : -hx; else faceLocal.Y = normalLocal.Y > 0 ? hy : -hy; contactPointWorld = box.Position + new Vector2(faceLocal.X * cos - faceLocal.Y * sin, faceLocal.X * sin + faceLocal.Y * cos); } else { Vector2 closestWorld = box.Position + new Vector2(closestLocal.X * cos - closestLocal.Y * sin, closestLocal.X * sin + closestLocal.Y * cos); Vector2 delta = circle.Position - closestWorld; float distSq = delta.LengthSquared(); float radiusSq = circle.Radius * circle.Radius; if (distSq >= radiusSq) return; float dist = (float)Math.Sqrt(distSq); normalWorld = dist > 1e-6f ? delta / dist : new Vector2(1, 0); penetration = circle.Radius - dist; contactPointWorld = closestWorld; } Contact contact = new Contact { Point = contactPointWorld, Normal = normalWorld, Penetration = penetration, BodyA = box, BodyB = circle }; if (swapped) { contact.BodyA = circle; contact.BodyB = box; contact.Normal = -contact.Normal; } contacts.Add(contact); } private static void ResolveBoxBox(Box b1, Box b2, List contacts) { float h1x = b1.Size.X * 0.5f; float h1y = b1.Size.Y * 0.5f; float h2x = b2.Size.X * 0.5f; float h2y = b2.Size.Y * 0.5f; float cos1 = (float)Math.Cos(b1.Rotation); float sin1 = (float)Math.Sin(b1.Rotation); float cos2 = (float)Math.Cos(b2.Rotation); float sin2 = (float)Math.Sin(b2.Rotation); Vector2 axisX1 = new Vector2(cos1, sin1); Vector2 axisY1 = new Vector2(-sin1, cos1); Vector2 axisX2 = new Vector2(cos2, sin2); Vector2 axisY2 = new Vector2(-sin2, cos2); Vector2 d = b2.Position - b1.Position; float minOverlap = float.MaxValue; Vector2 bestAxis = Vector2.Zero; bool axisFromB1 = false; TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); if (minOverlap <= 0) return; TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1); if (minOverlap <= 0) return; TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); if (minOverlap <= 0) return; TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1); if (minOverlap <= 0 || minOverlap == float.MaxValue) return; Vector2 normal = Vector2.Dot(d, bestAxis) > 0 ? bestAxis : -bestAxis; Box refBox, incBox; Vector2 refNormal, incNormal; if (axisFromB1) { refBox = b1; incBox = b2; refNormal = normal; incNormal = -normal; } else { refBox = b2; incBox = b1; refNormal = -normal; incNormal = normal; } Vector2[] refFace = GetFaceVertices(refBox, refNormal); Vector2[] incFace = GetFaceVertices(incBox, incNormal); Vector2 refV1 = refFace[0]; Vector2 refV2 = refFace[1]; Vector2 edgeDir = refV2 - refV1; if (edgeDir.LengthSquared() < 1e-8f) return; edgeDir = Vector2.Normalize(edgeDir); int contactsBefore = contacts.Count; List clipped = new List { incFace[0], incFace[1] }; clipped = ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1)); clipped = ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2)); foreach (var p in clipped) { float pen = -Vector2.Dot(p - refV1, refNormal); if (pen > 0) { contacts.Add(new Contact { Point = p, Normal = normal, Penetration = pen, BodyA = b1, BodyB = b2 }); } } // fallback for edge case precision drops if (contacts.Count == contactsBefore) { Vector2 contactPoint = (incFace[0] + incFace[1]) * 0.5f; contacts.Add(new Contact { Point = contactPoint, Normal = normal, Penetration = Math.Max(minOverlap, 1e-4f), BodyA = b1, BodyB = b2 }); } } private static void TestAxis(Vector2 axis, Box b1, Box b2, float h1x, float h1y, float h2x, float h2y, Vector2 axisX1, Vector2 axisY1, Vector2 axisX2, Vector2 axisY2, Vector2 d, ref float minOverlap, ref Vector2 bestAxis, bool fromB1, ref bool axisFromB1) { float r1 = h1x * Math.Abs(Vector2.Dot(axisX1, axis)) + h1y * Math.Abs(Vector2.Dot(axisY1, axis)); float r2 = h2x * Math.Abs(Vector2.Dot(axisX2, axis)) + h2y * Math.Abs(Vector2.Dot(axisY2, axis)); float distance = Math.Abs(Vector2.Dot(d, axis)); float overlap = r1 + r2 - distance; if (overlap <= 0) { minOverlap = overlap; return; } if (overlap < minOverlap) { minOverlap = overlap; bestAxis = axis; axisFromB1 = fromB1; } } private static Vector2[] GetBoxVertices(Box box) { float hx = box.Size.X * 0.5f; float hy = box.Size.Y * 0.5f; float cos = (float)Math.Cos(box.Rotation); float sin = (float)Math.Sin(box.Rotation); Vector2 axisX = new Vector2(cos, sin); Vector2 axisY = new Vector2(-sin, cos); return new Vector2[] { box.Position - axisX * hx - axisY * hy, box.Position + axisX * hx - axisY * hy, box.Position + axisX * hx + axisY * hy, box.Position - axisX * hx + axisY * hy }; } private static Vector2[] GetFaceVertices(Box box, Vector2 faceNormalWorld) { Vector2[] vertices = GetBoxVertices(box); float maxDot = -float.MaxValue; int bestIndex = 0; for (int i = 0; i < 4; i++) { Vector2 v1 = vertices[i]; Vector2 v2 = vertices[(i + 1) % 4]; Vector2 edge = v2 - v1; Vector2 outwardNormal = Vector2.Normalize(new Vector2(edge.Y, -edge.X)); float dot = Vector2.Dot(outwardNormal, faceNormalWorld); if (dot > maxDot) { maxDot = dot; bestIndex = i; } } return new Vector2[] { vertices[bestIndex], vertices[(bestIndex + 1) % 4] }; } private static List ClipSegmentAgainstPlane(List segment, Vector2 normal, float offset) { List result = new List(); if (segment.Count < 2) return result; Vector2 v0 = segment[0]; Vector2 v1 = segment[1]; float d0 = Vector2.Dot(v0, normal) - offset; float d1 = Vector2.Dot(v1, normal) - offset; if (d0 >= 0) result.Add(v0); if (d0 * d1 < 0) { float t = d0 / (d0 - d1); Vector2 intersection = v0 + t * (v1 - v0); result.Add(intersection); } if (d1 >= 0) result.Add(v1); return result; } private static Vector2 GetRelativeVelocity(Contact contact) { Body a = contact.BodyA; Body b = contact.BodyB; Vector2 va = a.Velocity + Cross(a.AngularVelocity, contact.RA); Vector2 vb = b.Velocity + Cross(b.AngularVelocity, contact.RB); return vb - va; } private static void ApplyImpulse(Contact contact, Vector2 impulse) { if (!contact.BodyA.IsStatic) contact.BodyA.ApplyImpulseAtWorldPosition(-impulse, contact.Point); if (!contact.BodyB.IsStatic) contact.BodyB.ApplyImpulseAtWorldPosition(impulse, contact.Point); } private static float Cross(Vector2 a, Vector2 b) { return a.X * b.Y - a.Y * b.X; } private static Vector2 Cross(float s, Vector2 a) { return new Vector2(-s * a.Y, s * a.X); } } }