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

Added .gitignore, and implemented stable stacking

Browse Source
This commit is contained in:
max
2026-09-03 17:50:53 +02:00
parent d8758f98dc
commit 6abf8320b0
36 changed files with 249 additions and 1047 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
PhysicsEngine/Physics/CollisionEngine.cs
View File

@@ -6,10 +6,11 @@ namespace PhysicsEngine
{
public static class CollisionEngine
{
private const float Slop = 0.01f;
private const float Slop = 0.005f;
private const float PositionCorrectionPercent = 0.2f;
private const float MaxCorrection = 0.2f;
private const int VelocityIterations = 8;
private const int PositionIterations = 3;
private class Contact
{
@@ -23,14 +24,44 @@ namespace PhysicsEngine
public Body BodyB;
public Vector2 RA;
public Vector2 RB;
public Vector2 LocalA;
public Vector2 LocalB;
public float NormalMass;
public float TangentMass;
public float Friction;
public float Restitution;
public float RestitutionBias;
public float Bias; // baumgarte position correction bias
public float VelocityBias;
public ContactKey Key;
}
private struct ContactKey : IEquatable<ContactKey>
{
public Body BodyA;
public Body BodyB;
public int Index;
public ContactKey(Body a, Body b, int index)
{
BodyA = a;
BodyB = b;
Index = index;
}
public bool Equals(ContactKey other)
{
return Equals(BodyA, other.BodyA) && Equals(BodyB, other.BodyB) && Index == other.Index;
}
public override int GetHashCode()
{
return HashCode.Combine(BodyA, BodyB, Index);
}
}
private static Dictionary<ContactKey, (float Normal, float Tangent)> WarmStartCache = new Dictionary<ContactKey, (float, float)>();
public static void ResolveCollisions(List<Body> bodies, float dt)
{
if (dt <= 0) return;
@@ -57,14 +88,22 @@ namespace PhysicsEngine
}
}
Dictionary<ContactKey, (float Normal, float Tangent)> newCache = new Dictionary<ContactKey, (float, float)>();
// 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 cosA = (float)Math.Cos(-contact.BodyA.Rotation);
float sinA = (float)Math.Sin(-contact.BodyA.Rotation);
contact.LocalA = new Vector2(contact.RA.X * cosA - contact.RA.Y * sinA, contact.RA.X * sinA + contact.RA.Y * cosA);
float cosB = (float)Math.Cos(-contact.BodyB.Rotation);
float sinB = (float)Math.Sin(-contact.BodyB.Rotation);
contact.LocalB = new Vector2(contact.RB.X * cosB - contact.RB.Y * sinB, contact.RB.X * sinB + contact.RB.Y * cosB);
float invMassA = contact.BodyA.InverseMass;
float invMassB = contact.BodyB.InverseMass;
float invIA = contact.BodyA.InverseInertia;
@@ -88,8 +127,21 @@ namespace PhysicsEngine
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);
contact.VelocityBias = (PositionCorrectionPercent / dt) * penetrationError;
if (WarmStartCache.TryGetValue(contact.Key, out var impulse))
{
contact.AccumulatedNormalImpulse = impulse.Normal;
contact.AccumulatedTangentImpulse = impulse.Tangent;
Vector2 totalImpulse = contact.Normal * contact.AccumulatedNormalImpulse + contact.Tangent * contact.AccumulatedTangentImpulse;
ApplyImpulse(contact, totalImpulse);
}
else
{
contact.AccumulatedNormalImpulse = 0;
contact.AccumulatedTangentImpulse = 0;
}
}
// velocity Impulse Solver
@@ -101,7 +153,7 @@ namespace PhysicsEngine
Vector2 rv = GetRelativeVelocity(contact);
float vn = Vector2.Dot(rv, contact.Normal);
float targetVn = contact.RestitutionBias + contact.Bias;
float targetVn = contact.RestitutionBias + contact.VelocityBias;
float lambda = -contact.NormalMass * (vn - targetVn);
float newImpulse = Math.Max(contact.AccumulatedNormalImpulse + lambda, 0.0f);
@@ -123,6 +175,53 @@ namespace PhysicsEngine
ApplyImpulse(contact, contact.Tangent * lambdaT);
}
}
foreach (var contact in contacts)
{
newCache[contact.Key] = (contact.AccumulatedNormalImpulse, contact.AccumulatedTangentImpulse);
}
WarmStartCache = newCache;
for (int iter = 0; iter < PositionIterations; iter++)
{
foreach (var contact in contacts)
{
float cosA = (float)Math.Cos(contact.BodyA.Rotation);
float sinA = (float)Math.Sin(contact.BodyA.Rotation);
Vector2 rA = new Vector2(contact.LocalA.X * cosA - contact.LocalA.Y * sinA, contact.LocalA.X * sinA + contact.LocalA.Y * cosA);
float cosB = (float)Math.Cos(contact.BodyB.Rotation);
float sinB = (float)Math.Sin(contact.BodyB.Rotation);
Vector2 rB = new Vector2(contact.LocalB.X * cosB - contact.LocalB.Y * sinB, contact.LocalB.X * sinB + contact.LocalB.Y * cosB);
Vector2 pA = contact.BodyA.Position + rA;
Vector2 pB = contact.BodyB.Position + rB;
float currentPenetration = contact.Penetration - Vector2.Dot(pB - pA, contact.Normal);
float penetrationError = Math.Max(currentPenetration - Slop, 0.0f);
if (penetrationError <= 0) continue;
float correctionAmount = Math.Min(penetrationError * PositionCorrectionPercent, MaxCorrection);
float rnA = Cross(rA, contact.Normal);
float rnB = Cross(rB, contact.Normal);
float denom = contact.BodyA.InverseMass + contact.BodyB.InverseMass + rnA * rnA * contact.BodyA.InverseInertia + rnB * rnB * contact.BodyB.InverseInertia;
float normalMass = denom > 0 ? 1.0f / denom : 0;
Vector2 pImpulse = contact.Normal * (correctionAmount * normalMass);
if (!contact.BodyA.IsStatic)
{
contact.BodyA.Position -= pImpulse * contact.BodyA.InverseMass;
contact.BodyA.Rotation -= Cross(rA, pImpulse) * contact.BodyA.InverseInertia;
}
if (!contact.BodyB.IsStatic)
{
contact.BodyB.Position += pImpulse * contact.BodyB.InverseMass;
contact.BodyB.Rotation += Cross(rB, pImpulse) * contact.BodyB.InverseInertia;
}
}
}
}
private static void ResolveCircleCircle(Circle c1, Circle c2, List<Contact> contacts)
@@ -143,7 +242,8 @@ namespace PhysicsEngine
Normal = normal,
Penetration = penetration,
BodyA = c1,
BodyB = c2
BodyB = c2,
Key = new ContactKey(c1, c2, 0)
});
}
@@ -210,7 +310,8 @@ namespace PhysicsEngine
Normal = normalWorld,
Penetration = penetration,
BodyA = box,
BodyB = circle
BodyB = circle,
Key = new ContactKey(box, circle, 0)
};
if (swapped)
@@ -218,6 +319,7 @@ namespace PhysicsEngine
contact.BodyA = circle;
contact.BodyB = box;
contact.Normal = -contact.Normal;
contact.Key = new ContactKey(circle, box, 0);
}
contacts.Add(contact);
@@ -288,6 +390,7 @@ namespace PhysicsEngine
clipped = ClipSegmentAgainstPlane(clipped, edgeDir, Vector2.Dot(edgeDir, refV1));
clipped = ClipSegmentAgainstPlane(clipped, -edgeDir, -Vector2.Dot(edgeDir, refV2));
int localIndex = 0;
foreach (var p in clipped)
{
float pen = -Vector2.Dot(p - refV1, refNormal);
@@ -299,12 +402,12 @@ namespace PhysicsEngine
Normal = normal,
Penetration = pen,
BodyA = b1,
BodyB = b2
BodyB = b2,
Key = new ContactKey(b1, b2, localIndex++)
});
}
}
// fallback for edge case precision drops
if (contacts.Count == contactsBefore)
{
Vector2 contactPoint = (incFace[0] + incFace[1]) * 0.5f;
@@ -314,7 +417,8 @@ namespace PhysicsEngine
Normal = normal,
Penetration = Math.Max(minOverlap, 1e-4f),
BodyA = b1,
BodyB = b2
BodyB = b2,
Key = new ContactKey(b1, b2, 0)
});
}
}

2
PhysicsEngine/Physics/Forces/GravityForce.cs
View File

@@ -6,7 +6,7 @@ namespace PhysicsEngine
{
public static void Apply(Body body, float dt)
{
body.ApplyForce(Vector2.UnitY * 9.81f/10 * body.Mass);
body.ApplyForce(Vector2.UnitY * 9.81f * body.Mass);
}
}
}