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

rigidbody, forces and collision testing

Browse Source
This commit is contained in:
maxwes08
2026-09-03 16:50:36 +02:00
parent cf1f083cc5
commit 815bd2d260
28 changed files with 1381 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

505
PhysicsEngine/Physics/CollisionEngine.cs Normal file
View File

@@ -0,0 +1,505 @@
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.1f;
private const float MaxCorrection = 0.2f;
private const int VelocityIterations = 5;
private class Contact
{
public Vector2 Point;
public Vector2 Normal; // from BodyA to BodyB
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 static void ResolveCollisions(List<Body> bodies, float dt)
{
List<Contact> contacts = new List<Contact>();
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);
}
}
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;
Vector2 tangent = new Vector2(-contact.Normal.Y, contact.Normal.X);
float rtA = Cross(contact.RA, tangent);
float rtB = Cross(contact.RB, 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);
}
for (int iter = 0; iter < VelocityIterations; iter++)
{
foreach (var contact in contacts)
{
Vector2 rv = GetRelativeVelocity(contact);
float vn = Vector2.Dot(rv, contact.Normal);
float lambda = -contact.NormalMass * vn;
float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0);
lambda = newImpulse - contact.AccumulatedNormalImpulse;
contact.AccumulatedNormalImpulse = newImpulse;
ApplyImpulse(contact, contact.Normal * lambda);
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 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;
ApplyImpulse(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)
{
Vector2 d = c2.Position - c1.Position;
float distSq = d.LengthSquared();
float radiusSum = c1.Radius + c2.Radius;
if (distSq >= radiusSum * radiusSum) return;
float dist = (float)Math.Sqrt(distSq);
Vector2 normal;
if (dist < 1e-6f)
normal = new Vector2(1, 0);
else
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<Contact> 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<Contact> 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;
// 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);
}
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; // outward normal of reference face points toward incident box
incNormal = -normal; // outward normal of incident face points toward reference box
}
else
{
refBox = b2;
incBox = b1;
refNormal = -normal;
incNormal = normal;
}
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];
Vector2 edgeDir = refV2 - refV1;
if (edgeDir.LengthSquared() < 1e-8f) return;
edgeDir = Vector2.Normalize(edgeDir);
int contactsBefore = contacts.Count; // to check if we added any new contacts
// 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));
// Add contacts from clipped incident face points
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
});
}
}
// 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.
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
contacts.Add(new Contact
{
Point = contactPoint,
Normal = normal,
Penetration = minOverlap, // use SAT overlap
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);
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 };
}
return new Vector2[] { vertices[0], vertices[1] };
}
private static void 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 (d0 >= 0) result.Add(segment[0]);
if (d1 >= 0) result.Add(segment[1]);
if (d0 * d1 < 0)
{
float t = d0 / (d0 - d1);
Vector2 intersection = segment[0] + t * (segment[1] - segment[0]);
result.Add(intersection);
}
segment.Clear();
segment.AddRange(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);
}
}
}