public void Handle(ref Manifold manifold, Rigidbody.Rigidbody a, Rigidbody.Rigidbody b)
{
Circle A = (Circle)a.Shape;
Circle B = (Circle)b.Shape;
Vector2 normal = b.transform.position - a.transform.position;
float distance = normal.magnitude;
float radius = A.Radius + B.Radius;
if (distance >= radius)
{
manifold.ContactCount = 0;
return;
}
manifold.ContactCount = 1;
manifold.Contacts = new Vector2[manifold.ContactCount];
if (Mathf.Abs(distance) < Mathf.Epsilon)
{
manifold.Penetration = A.Radius;
manifold.Normal = new Vector2(1.0f, 0.0f);
manifold.Contacts[0] = a.transform.position;
}
else
{
manifold.Penetration = radius - distance;
manifold.Normal = normal / distance;
manifold.Contacts[0] = manifold.Normal * A.Radius + (Vector2)a.transform.position;
}
}
public static void Remove(Rigidbody.Rigidbody collider)
{
Rigidbodies.Remove(collider);
if (_isRunning && Rigidbodies.Count == 0)
{
End();
}
}
public static void Add(Rigidbody.Rigidbody collider)
{
if (!_isRunning)
{
Init();
}
Rigidbodies.Add(collider);
}
public static void Handle(ref Manifold manifold, Rigidbody.Rigidbody a, Rigidbody.Rigidbody b)
{
for (int i = 0; i < CollisionHandlers.Length; i++)
{
if (CollisionHandlers[i].CanHandle(a.Shape, b.Shape))
{
CollisionHandlers[i].Handle(ref manifold, a, b);
break;
}
}
}
private static void IntegrateForces(Rigidbody.Rigidbody rigidbody, float deltaTime)
{
if (Math.Abs(rigidbody.InverseMass) < Mathf.Epsilon)
{
return;
}
float dts = deltaTime * 0.5f;
rigidbody.SetVelocity(rigidbody.Velocity + rigidbody.Force * (dts * rigidbody.InverseMass));
rigidbody.SetVelocity(rigidbody.Velocity + rigidbody.GravityScale * Gravity * dts);
rigidbody.SetAngularVelocity(rigidbody.AngularVelocity + rigidbody.Torque * rigidbody.InverseMass * dts);
}
public Manifold(ref Rigidbody.Rigidbody a, ref Rigidbody.Rigidbody b)
{
A = a;
B = b;
AverageRestitution = Mathf.Min(A.Restitution, B.Restitution);
StaticFriction = Mathf.Sqrt(Mathf.Pow(A.StaticFriction, 2) + Mathf.Pow(B.StaticFriction, 2));
DynamicFriction = Mathf.Sqrt(Mathf.Pow(A.DynamicFriction, 2) + Mathf.Pow(B.DynamicFriction, 2));
Normal = new Vector2();
Penetration = 0f;
ContactCount = 0;
Contacts = new Vector2[ContactCount];
}
private static void IntegrateVelocity(Rigidbody.Rigidbody rigidbody, float deltaTime)
{
if (Math.Abs(rigidbody.InverseMass) < Mathf.Epsilon)
{
return;
}
rigidbody.transform.position += (Vector3)rigidbody.Velocity * deltaTime;
float orient = rigidbody.transform.eulerAngles.z * Mathf.Deg2Rad + rigidbody.AngularVelocity * deltaTime;
rigidbody.transform.eulerAngles.WithZ(orient * Mathf.Rad2Deg);
rigidbody.SetOrient(orient);
IntegrateForces(rigidbody, deltaTime);
}
public void Handle(ref Manifold manifold, Rigidbody.Rigidbody a, Rigidbody.Rigidbody b)
{
if (a.Shape.GetType() == typeof(Circle) && b.Shape.GetType() == typeof(Polygon))
{
AbsoluteHandler(ref manifold, a, b);
}
else if (a.Shape.GetType() == typeof(Polygon) && b.Shape.GetType() == typeof(Circle))
{
AbsoluteHandler(ref manifold, b, a);
if (manifold.ContactCount > 0)
{
manifold.Normal = -manifold.Normal;
}
}
}
private static void Step()
{
Contacts.Clear();
for (int i = 0; i < Rigidbodies.Count; ++i)
{
Rigidbody.Rigidbody a = Rigidbodies[i];
for (int j = i + 1; j < Rigidbodies.Count; ++j)
{
Rigidbody.Rigidbody b = Rigidbodies[j];
if (Math.Abs(a.InverseMass) < Mathf.Epsilon && Math.Abs(b.InverseMass) < Mathf.Epsilon)
{
continue;
}
Manifold manifold = new Manifold(ref a, ref b);
manifold.Solve();
if (manifold.ContactCount > 0)
{
Contacts.Add(manifold);
}
}
}
for (int i = 0; i < Rigidbodies.Count; ++i)
{
IntegrateForces(Rigidbodies[i], DeltaTime);
}
for (int i = 0; i < Contacts.Count; ++i)
{
Contacts[i].Initialize();
}
for (int j = 0; j < Iterations; ++j)
{
for (int i = 0; i < Contacts.Count; ++i)
{
Contacts[i].ApplyImpulse();
}
}
for (int i = 0; i < Rigidbodies.Count; ++i)
{
IntegrateVelocity(Rigidbodies[i], DeltaTime);
}
for (int i = 0; i < Contacts.Count; ++i)
{
Contacts[i].PositionalCorrection();
}
for (int i = 0; i < Rigidbodies.Count; ++i)
{
Rigidbody.Rigidbody rigidbody = Rigidbodies[i];
rigidbody.SetForce(Vector2.zero);
rigidbody.SetTorque(0f);
}
for (int i = 0; i < Contacts.Count; i++)
{
if (!Array.Exists(_previousFrameContacts, item => item.Equals(Contacts[i])))
{
Contacts[i].A.Shape.OnCollisionEnter?.Invoke(Contacts[i].B.Shape);
Contacts[i].B.Shape.OnCollisionEnter?.Invoke(Contacts[i].A.Shape);
}
}
_previousFrameContacts = Contacts.ToArray();
}
private void AbsoluteHandler(ref Manifold manifold, Rigidbody.Rigidbody a, Rigidbody.Rigidbody b)
{
Circle A = (Circle)a.Shape;
Polygon B = (Polygon)b.Shape;
manifold.ContactCount = 0;
Vector2 center = B.Orientation.Transpose().Multiply(a.transform.position - b.transform.position);
float separation = Mathf.NegativeInfinity;
int faceNormal = 0;
for (int i = 0; i < B.VertexCount; ++i)
{
float s = Vector2.Dot(B.Normals[i], center - B.Vertices[i]);
if (s > A.Radius)
{
return;
}
if (s > separation)
{
separation = s;
faceNormal = i;
}
}
Vector2 v1 = B.Vertices[faceNormal];
int i2 = faceNormal + 1 < B.VertexCount? faceNormal + 1 : 0;
Vector2 v2 = B.Vertices[i2];
if (separation < Mathf.Epsilon)
{
manifold.ContactCount = 1;
manifold.Normal = -B.Orientation.Multiply(B.Normals[faceNormal]);
manifold.Contacts = new Vector2[manifold.ContactCount];
manifold.Contacts[0] = manifold.Normal * A.Radius + (Vector2)a.transform.position;
manifold.Penetration = A.Radius;
return;
}
float dot1 = Vector2.Dot(center - v1, v2 - v1);
float dot2 = Vector2.Dot(center - v2, v1 - v2);
manifold.Penetration = A.Radius - separation;
if (dot1 <= 0.0f)
{
if (Vector2.Distance(center, v1) > A.Radius)
{
return;
}
manifold.ContactCount = 1;
manifold.Normal = B.Orientation.Multiply(v1 - center).normalized;
manifold.Contacts = new Vector2[manifold.ContactCount];
manifold.Contacts[0] = B.Orientation.Multiply(v1) + (Vector2)b.transform.position;
}
else if (dot2 <= 0.0f)
{
if (Vector2.Distance(center, v2) > A.Radius)
{
return;
}
manifold.ContactCount = 1;
manifold.Normal = B.Orientation.Multiply(v2 - center).normalized;
manifold.Contacts = new Vector2[manifold.ContactCount];
manifold.Contacts[0] = B.Orientation.Multiply(v2) + (Vector2)b.transform.position;
}
else
{
Vector2 n = B.Normals[faceNormal];
if (Vector2.Dot(center - v1, n) > A.Radius)
{
return;
}
manifold.ContactCount = 1;
manifold.Normal = -B.Orientation.Multiply(n);
manifold.Contacts = new Vector2[manifold.ContactCount];
manifold.Contacts[0] = manifold.Normal * A.Radius + (Vector2)a.transform.position;
}
}
