542 lines
22 KiB
C#
542 lines
22 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
|
|
namespace PhysicsEngine
|
|
{
|
|
public static class CollisionEngine
|
|
{
|
|
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
|
|
{
|
|
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 Vector2 LocalA;
|
|
public Vector2 LocalB;
|
|
public float NormalMass;
|
|
public float TangentMass;
|
|
public float Friction;
|
|
public float Restitution;
|
|
public float RestitutionBias;
|
|
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;
|
|
|
|
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++)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
Dictionary<ContactKey, (float Normal, float Tangent)> newCache = new Dictionary<ContactKey, (float, float)>();
|
|
|
|
// pre-step / initialization
|
|
foreach (var contact in contacts)
|
|
{
|
|
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;
|
|
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.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
|
|
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.VelocityBias;
|
|
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);
|
|
}
|
|
}
|
|
|
|
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)
|
|
{
|
|
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,
|
|
Key = new ContactKey(c1, c2, 0)
|
|
});
|
|
}
|
|
|
|
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,
|
|
Key = new ContactKey(box, circle, 0)
|
|
};
|
|
|
|
if (swapped)
|
|
{
|
|
contact.BodyA = circle;
|
|
contact.BodyB = box;
|
|
contact.Normal = -contact.Normal;
|
|
contact.Key = new ContactKey(circle, box, 0);
|
|
}
|
|
|
|
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;
|
|
|
|
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<Vector2> clipped = new List<Vector2> { incFace[0], incFace[1] };
|
|
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);
|
|
if (pen > 0)
|
|
{
|
|
contacts.Add(new Contact
|
|
{
|
|
Point = p,
|
|
Normal = normal,
|
|
Penetration = pen,
|
|
BodyA = b1,
|
|
BodyB = b2,
|
|
Key = new ContactKey(b1, b2, localIndex++)
|
|
});
|
|
}
|
|
}
|
|
|
|
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,
|
|
Key = new ContactKey(b1, b2, 0)
|
|
});
|
|
}
|
|
}
|
|
|
|
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<Vector2> ClipSegmentAgainstPlane(List<Vector2> segment, Vector2 normal, float offset)
|
|
{
|
|
List<Vector2> result = new List<Vector2>();
|
|
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);
|
|
}
|
|
}
|
|
} |