grillkol/Physics-Engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

stable collisions

Browse Source
This commit is contained in:
max
2026-09-03 17:27:52 +02:00
parent 815bd2d260
commit d8758f98dc
8 changed files with 151 additions and 452 deletions
Download Patch File Download Diff File Expand all files Collapse all files

215
PhysicsEngine/Physics/CollisionEngine.cs
View File

@@ -7,14 +7,15 @@ namespace PhysicsEngine
public static class CollisionEngine
{
private const float Slop = 0.01f;
private const float PositionCorrectionPercent = 0.1f;
private const float PositionCorrectionPercent = 0.2f;
private const float MaxCorrection = 0.2f;
private const int VelocityIterations = 5;
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;
@@ -26,12 +27,17 @@ namespace PhysicsEngine
public float TangentMass;
public float Friction;
public float Restitution;
public float RestitutionBias;
public float Bias; // baumgarte position correction bias
}
public static void ResolveCollisions(List<Body> bodies, float dt)
{
if (dt <= 0) return;
List<Contact> contacts = new List<Contact>();
// broadphase & barrowphase collision detection
for (int i = 0; i < bodies.Count; i++)
{
for (int j = i + 1; j < bodies.Count; j++)
@@ -51,6 +57,7 @@ namespace PhysicsEngine
}
}
// pre-step / initialization
foreach (var contact in contacts)
{
contact.AccumulatedNormalImpulse = 0;
@@ -68,73 +75,54 @@ namespace PhysicsEngine
float denom = invMassA + invMassB + rnA * rnA * invIA + rnB * rnB * invIB;
contact.NormalMass = denom > 0 ? 1.0f / denom : 0;
Vector2 tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
float rtA = Cross(contact.RA, tangent);
float rtB = Cross(contact.RB, tangent);
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 lambda = -contact.NormalMass * vn;
float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0);
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);
Vector2 tangent = rv - contact.Normal * Vector2.Dot(rv, contact.Normal);
if (tangent.LengthSquared() > 1e-6f)
tangent = Vector2.Normalize(tangent);
else
tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
float vt = Vector2.Dot(rv, tangent);
float vt = Vector2.Dot(rv, contact.Tangent);
float lambdaT = -contact.TangentMass * vt;
float maxFriction = contact.Friction * contact.AccumulatedNormalImpulse;
float newImpulseT = Math.Clamp(contact.AccumulatedTangentImpulse + lambdaT, -maxFriction, maxFriction);
lambdaT = newImpulseT - contact.AccumulatedTangentImpulse;
contact.AccumulatedTangentImpulse = newImpulseT;
float oldImpulseT = contact.AccumulatedTangentImpulse;
contact.AccumulatedTangentImpulse = Math.Clamp(oldImpulseT + lambdaT, -maxFriction, maxFriction);
lambdaT = contact.AccumulatedTangentImpulse - oldImpulseT;
ApplyImpulse(contact, tangent * lambdaT);
ApplyImpulse(contact, contact.Tangent * lambdaT);
}
}
var pairMap = new Dictionary<(Body, Body), Contact>();
foreach (var contact in contacts)
{
var key = (contact.BodyA, contact.BodyB);
if (!pairMap.ContainsKey(key) || pairMap[key].Penetration < contact.Penetration)
pairMap[key] = contact;
}
foreach (var kvp in pairMap)
{
var contact = kvp.Value;
float correctionMagnitude = Math.Max(contact.Penetration - Slop, 0) * PositionCorrectionPercent;
correctionMagnitude = Math.Min(correctionMagnitude, MaxCorrection);
if (correctionMagnitude <= 0) continue;
Vector2 correction = contact.Normal * correctionMagnitude;
float totalInvMass = contact.BodyA.InverseMass + contact.BodyB.InverseMass;
if (totalInvMass <= 0) continue;
float ratioA = contact.BodyA.InverseMass / totalInvMass;
float ratioB = contact.BodyB.InverseMass / totalInvMass;
contact.BodyA.Position -= correction * ratioA;
contact.BodyB.Position += correction * ratioB;
}
}
private static void ResolveCircleCircle(Circle c1, Circle c2, List<Contact> contacts)
@@ -142,15 +130,10 @@ namespace PhysicsEngine
Vector2 d = c2.Position - c1.Position;
float distSq = d.LengthSquared();
float radiusSum = c1.Radius + c2.Radius;
if (distSq >= radiusSum * radiusSum) return;
if (distSq >= radiusSum * radiusSum || distSq < 1e-12f) return;
float dist = (float)Math.Sqrt(distSq);
Vector2 normal;
if (dist < 1e-6f)
normal = new Vector2(1, 0);
else
normal = d / dist;
Vector2 normal = d / dist;
float penetration = radiusSum - dist;
Vector2 contactPoint = c1.Position + normal * (c1.Radius - penetration * 0.5f);
@@ -262,25 +245,13 @@ namespace PhysicsEngine
Vector2 bestAxis = Vector2.Zero;
bool axisFromB1 = false;
// Test axes. If one body is static, only test that body's axes for stability.
if (b1.IsStatic && !b2.IsStatic)
{
TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
}
else if (b2.IsStatic && !b1.IsStatic)
{
TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
}
else
{
TestAxis(axisX1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
TestAxis(axisY1, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, true, ref axisFromB1);
TestAxis(axisX2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
TestAxis(axisY2, b1, b2, h1x, h1y, h2x, h2y, axisX1, axisY1, axisX2, axisY2, d, ref minOverlap, ref bestAxis, false, ref axisFromB1);
}
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;
@@ -291,8 +262,8 @@ namespace PhysicsEngine
{
refBox = b1;
incBox = b2;
refNormal = normal; // outward normal of reference face points toward incident box
incNormal = -normal; // outward normal of incident face points toward reference box
refNormal = normal;
incNormal = -normal;
}
else
{
@@ -304,7 +275,6 @@ namespace PhysicsEngine
Vector2[] refFace = GetFaceVertices(refBox, refNormal);
Vector2[] incFace = GetFaceVertices(incBox, incNormal);
if (refFace.Length < 2 || incFace.Length < 2) return;
Vector2 refV1 = refFace[0];
Vector2 refV2 = refFace[1];
@@ -312,14 +282,12 @@ namespace PhysicsEngine
if (edgeDir.LengthSquared() < 1e-8f) return;
edgeDir = Vector2.Normalize(edgeDir);
int contactsBefore = contacts.Count; // to check if we added any new contacts
int contactsBefore = contacts.Count;
// Clip incident face against reference face side planes
List<Vector2> clipped = new List<Vector2> { incFace[0], incFace[1] };
ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1));
ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2));
clipped = ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1));
clipped = ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2));
// Add contacts from clipped incident face points
foreach (var p in clipped)
{
float pen = -Vector2.Dot(p - refV1, refNormal);
@@ -336,60 +304,15 @@ namespace PhysicsEngine
}
}
// Check reference face vertices that penetrate the incident face
foreach (var refVertex in refFace)
{
// Penetration depth along incident face normal (positive if inside)
float pen = -Vector2.Dot(refVertex - incFace[0], incNormal);
if (pen > 0)
{
// Avoid duplicate contacts at nearly the same location
bool duplicate = false;
foreach (var c in contacts)
{
if (Vector2.DistanceSquared(c.Point, refVertex) < 1e-6f)
{
duplicate = true;
break;
}
}
if (!duplicate)
{
contacts.Add(new Contact
{
Point = refVertex,
Normal = normal,
Penetration = pen,
BodyA = b1,
BodyB = b2
});
}
}
}
// Fallback: if no contacts were generated despite SAT overlap, create a single contact at the midpoint
// of the overlapping region along the normal. This prevents any missed collisions.
// fallback for edge case precision drops
if (contacts.Count == contactsBefore)
{
// Compute the projection of both boxes onto the normal
float projB1 = h1x * Math.Abs(Vector2.Dot(axisX1, normal)) + h1y * Math.Abs(Vector2.Dot(axisY1, normal));
float projB2 = h2x * Math.Abs(Vector2.Dot(axisX2, normal)) + h2y * Math.Abs(Vector2.Dot(axisY2, normal));
float dist = Vector2.Dot(d, normal); // distance from b1 to b2 along normal
float min1 = -projB1;
float max1 = projB1;
float min2 = dist - projB2;
float max2 = dist + projB2;
float overlapMin = Math.Max(min1, min2);
float overlapMax = Math.Min(max1, max2);
float overlapMid = (overlapMin + overlapMax) * 0.5f;
Vector2 contactPoint = b1.Position + normal * overlapMid; // rough position along normal
Vector2 contactPoint = (incFace[0] + incFace[1]) * 0.5f;
contacts.Add(new Contact
{
Point = contactPoint,
Normal = normal,
Penetration = minOverlap, // use SAT overlap
Penetration = Math.Max(minOverlap, 1e-4f),
BodyA = b1,
BodyB = b2
});
@@ -441,38 +364,48 @@ namespace PhysicsEngine
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 = new Vector2(edge.Y, -edge.X);
outwardNormal = Vector2.Normalize(outwardNormal);
if (Vector2.Dot(outwardNormal, faceNormalWorld) > 0.999f)
return new Vector2[] { v1, v2 };
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[0], vertices[1] };
return new Vector2[] { vertices[bestIndex], vertices[(bestIndex + 1) % 4] };
}
private static void ClipSegmentAgainstPlane(List<Vector2> segment, Vector2 normal, float offset)
private static List<Vector2> ClipSegmentAgainstPlane(List<Vector2> segment, Vector2 normal, float offset)
{
if (segment.Count == 0) return;
List<Vector2> result = new List<Vector2>();
float d0 = Vector2.Dot(segment[0], normal) - offset;
float d1 = Vector2.Dot(segment[1], normal) - offset;
if (segment.Count < 2) return result;
if (d0 >= 0) result.Add(segment[0]);
if (d1 >= 0) result.Add(segment[1]);
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 = segment[0] + t * (segment[1] - segment[0]);
Vector2 intersection = v0 + t * (v1 - v0);
result.Add(intersection);
}
segment.Clear();
segment.AddRange(result);
if (d1 >= 0) result.Add(v1);
return result;
}
private static Vector2 GetRelativeVelocity(Contact contact)