/// <summary>
/// Compares bounding boxes using Seperating Axis Thereom.
/// </summary>
public static bool AABBvsAABB(AABB a, AABB b, ref Manifold manifold)
{
manifold.Normal = a.Position - b.Position;
//Calculate half widths
float aExtent = a.Width / 2f;
float bExtent = b.Width / 2f;
//Calculate the overlap.
float xExtent = aExtent + bExtent - Math.Abs(manifold.Normal.X);
//If the overlap is greater than 0
if (xExtent > 0)
{
//Calculate half widths
aExtent = a.Height/2f;
bExtent = b.Height/2f;
//Calculate overlap
float yExtent = aExtent + bExtent - Math.Abs(manifold.Normal.Y);
if (yExtent > 0)
{
//Variable to multiply the normal by to make the collision resolve
Vector2 faceNormal;
//Check to see which axis has the biggest "penetration" ;D
//Collision is happening on Y axis
if (xExtent > yExtent)
{
if (manifold.Normal.X < 0)
faceNormal = -Vector2.UnitX;
else
faceNormal = Vector2.UnitX;
manifold.PenetrationDepth = xExtent;
manifold.Normal = Physics.GetNormal(a.Position, b.Position) * faceNormal.X;
manifold.AreColliding = true;
//TODO: Finish collision code
/// Need to find the axis of deepest penetration and only display flags from that side
/// UNLESS the penetration depth on the other sides are more than half the width.
//A First
if (a.Top > b.Top && a.Top < b.Bottom && manifold.A.AllowCollisionDirection.HasMatchingBit(UP))
manifold.A.CollisionDirection.CombineMask(UP);
if (a.Bottom < b.Bottom && a.Bottom > b.Top && manifold.A.AllowCollisionDirection.HasMatchingBit(DOWN))
manifold.A.CollisionDirection.CombineMask(DOWN);
if (a.Left > b.Left && a.Left < b.Right && manifold.A.AllowCollisionDirection.HasMatchingBit(LEFT))
manifold.A.CollisionDirection.CombineMask(LEFT);
if (a.Right < b.Right && a.Right > b.Left && manifold.A.AllowCollisionDirection.HasMatchingBit(RIGHT))
manifold.A.CollisionDirection.CombineMask(RIGHT);
//B next
if (b.Top > a.Top && b.Top < a.Bottom && manifold.B.AllowCollisionDirection.HasMatchingBit(UP))
manifold.B.CollisionDirection.CombineMask(UP);
if (b.Bottom < a.Bottom && b.Bottom > a.Top && manifold.B.AllowCollisionDirection.HasMatchingBit(DOWN))
manifold.B.CollisionDirection.CombineMask(DOWN);
if (b.Left > a.Left && b.Left < a.Right && manifold.B.AllowCollisionDirection.HasMatchingBit(LEFT))
manifold.B.CollisionDirection.CombineMask(LEFT);
if (b.Right < a.Right && b.Right > a.Left && manifold.B.AllowCollisionDirection.HasMatchingBit(RIGHT))
manifold.B.CollisionDirection.CombineMask(RIGHT);
return true;
}
//Collision happening on X axis
else
{
if (manifold.Normal.Y < 0)
faceNormal = -Vector2.UnitY;
else
faceNormal = Vector2.UnitY;
manifold.Normal = Physics.GetNormal(a.Position, b.Position) * faceNormal.Y;
manifold.PenetrationDepth = yExtent;
manifold.AreColliding = true;
//A First
if (a.Top > b.Top && a.Top < b.Bottom && manifold.A.AllowCollisionDirection.HasMatchingBit(UP))
manifold.A.CollisionDirection.CombineMask(UP);
if (a.Bottom < b.Bottom && a.Bottom > b.Top && manifold.A.AllowCollisionDirection.HasMatchingBit(DOWN))
manifold.A.CollisionDirection.CombineMask(DOWN);
if (a.Left > b.Left && a.Left < b.Right && manifold.A.AllowCollisionDirection.HasMatchingBit(LEFT))
manifold.A.CollisionDirection.CombineMask(LEFT);
if (a.Right < b.Right && a.Right > b.Left && manifold.A.AllowCollisionDirection.HasMatchingBit(RIGHT))
manifold.A.CollisionDirection.CombineMask(RIGHT);
//B First
if (b.Top > a.Top && b.Top < a.Bottom && manifold.B.AllowCollisionDirection.HasMatchingBit(UP))
manifold.B.CollisionDirection.CombineMask(UP);
if (b.Bottom < a.Bottom && b.Bottom > a.Top && manifold.B.AllowCollisionDirection.HasMatchingBit(DOWN))
manifold.B.CollisionDirection.CombineMask(DOWN);
if (b.Left > a.Left && b.Left < a.Right && manifold.B.AllowCollisionDirection.HasMatchingBit(LEFT))
manifold.B.CollisionDirection.CombineMask(LEFT);
if (b.Right < a.Right && b.Right > a.Left && manifold.B.AllowCollisionDirection.HasMatchingBit(RIGHT))
manifold.B.CollisionDirection.CombineMask(RIGHT);
return true;
}
}
return false;
}
return false;
}
public static void ResolveCollision(Manifold m)
{
Vector2 relVelocity = m.B.Velocity - m.A.Velocity;
//Finds out if the objects are moving towards each other.
//We only need to resolve collisions that are moving towards, not away.
float velAlongNormal = relVelocity.X * m.Normal.X + relVelocity.Y * m.Normal.Y;
if (velAlongNormal > 0)
return;
float e = Math.Min(m.A.Restitution, m.B.Restitution);
float j = -(1 + e) * velAlongNormal;
j /= m.A.InvertedMass + m.B.InvertedMass;
Vector2 impulse = j * m.Normal;
if (CanObjectsResolve(m.A, m.B))
m.A.Velocity -= m.A.InvertedMass * impulse;
if (CanObjectsResolve(m.B, m.A))
m.B.Velocity += m.B.InvertedMass * impulse;
}
public static bool CircleVSCircle(Circle a, Circle b, ref Manifold manifold)
{
manifold.Normal = b.Position - a.Position;
float r = a.Radius + b.Radius;
r *= r;
float l = manifold.Normal.LengthSquared();
if (l > r)
{
//Set manifold for failure
manifold.AreColliding = false;
return false;
}
float d = manifold.Normal.Length();
manifold.Normal.Normalize();
if (Math.Abs(d) > float.Epsilon)
{
manifold.PenetrationDepth = a.Radius + b.Radius - d;
manifold.AreColliding = true;
return true;
}
else
{
//find which one is bigger
float maxRadius = Math.Max(a.Radius, b.Radius);
manifold.PenetrationDepth = maxRadius;
manifold.AreColliding = true;
return true;
}
}
public static void PositionalCorrection(Manifold m)
{
Vector2 correction = Math.Max(m.PenetrationDepth - SLOP, 0.0f) / (m.A.InvertedMass + m.B.InvertedMass) * PERCENT * m.Normal;
if (CanObjectsResolve(m.A, m.B))
m.A.Position -= m.A.InvertedMass * correction;
if (CanObjectsResolve(m.B, m.A))
m.B.Position += m.B.InvertedMass * correction;
}
//Collision resolver methods
/// <summary>
/// Uses a table to test collision between various shapes
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static Manifold CheckCollision(Collision a, Collision b)
{
Shape aShape, bShape;
aShape = a.GetDependency<Shape>(Collision.DEPENDENCY_SHAPE);
bShape = b.GetDependency<Shape>(Collision.DEPENDENCY_SHAPE);
Manifold manifold = new Manifold(a,b);
if (aShape is AABB && bShape is AABB)
AABBvsAABB((AABB)aShape, (AABB)bShape, ref manifold);
else if (aShape is Circle && bShape is Circle)
CircleVSCircle((Circle)aShape, (Circle)bShape, ref manifold);
else
throw new Exception("No existing methods for this kind of collision!");
return manifold;
}
