public static bool AABBvsAABB(AABB a, AABB b, ref Manifold m)
{
m.A = a;
m.B = b;
m.Normal = b.Position - a.Position;
//Calculate the extent on the X axis
float aExtent = (a.Right - a.Left) / 2;
float bExtent = (b.Right - b.Left) / 2;
//Find the X overlap
float xExtent = aExtent + bExtent - Math.Abs (m.Normal.X);
//SAT Test on X
if (xExtent > 0) {
//There was overlap on the X axis, now lets try to Y
aExtent = (a.Bottom - a.Top) / 2;
bExtent = (b.Bottom - b.Top) / 2;
//Calculate Y overlap
float yExtent = aExtent + bExtent - Math.Abs(m.Normal.Y);
//SAT Test on Y axis
if (yExtent > 0){
//Find which axis has the biggest penetration ;D
if (xExtent > yExtent){
// if(m.Normal.X < 0)
// m.Normal = -Vector2.UnitX;
// else
// m.Normal= Vector2.UnitX;
m.Normal = PhysicsMath.GetNormal(m.A.Position, m.B.Position);
m.PenetrationDepth = xExtent;
m.AreColliding = true;
return true;
}
else {
// if(m.Normal.Y < 0)
// m.Normal = -Vector2.UnitY;
// else
// m.Normal= Vector2.UnitY;
m.Normal = PhysicsMath.GetNormal(m.A.Position, m.B.Position);
m.PenetrationDepth = yExtent;
m.AreColliding = true;
return true;
}
}
}
return false;
}
public AABBvsAABBTest()
{
//AABB vs AABB X
_aabb1 = new AABB(new Vector2(300,20), 20, 20);
_aabb1.Color = Color.Chocolate;
_aabb1.Velocity = Vector2.UnitY * .5f;
_aabb1.Mass = 10;
_aabb1.Restitution = .5f;
_aabb2 = new AABB(new Vector2(300,140), 20, 20);
_aabb2.Color = Color.CadetBlue;
_aabb2.Velocity = -Vector2.UnitY * .7f;
_aabb2.Mass = 30;
_aabb2.Restitution = .5f;
//AABB vs AABB Y
_aabb3 = new AABB(new Vector2(50,200), 20, 20);
_aabb3.Color = Color.Chocolate;
_aabb3.Velocity = Vector2.UnitX * .5f;
_aabb3.Mass = 10;
_aabb3.Restitution = .5f;
_aabb4 = new AABB(new Vector2(150,200), 20, 20);
_aabb4.Color = Color.CadetBlue;
_aabb4.Velocity = -Vector2.UnitX * .7f;
_aabb4.Mass = 30;
_aabb4.Restitution = .5f;
//AABB Corner Test
_aabb5 = new AABB(new Vector2(110,100), 20, 20);
_aabb5.Color = Color.Chocolate;
_aabb5.Velocity = Vector2.One * .5f;
_aabb5.Mass = 10;
_aabb5.Restitution = .5f;
_aabb6 = new AABB(new Vector2(300,300), 20, 20);
_aabb6.Color = Color.CadetBlue;
_aabb6.Velocity = -Vector2.One * .7f;
_aabb6.Mass = 30;
_aabb6.Restitution = .5f;
}
public static bool AABBvsCircle(AABB a, Circle b, ref Manifold m)
{
m.A = a;
m.B = b;
Vector2 n = b.Position - a.Position;
//Closest edge
Vector2 closest = m.Normal;
//Find extents for our AABB
float xExtent = (a.Right - a.Left) / 2;
float yExtent = (a.Right - a.Left) / 2;
closest.X = MathHelper.Clamp (-xExtent, xExtent, closest.X);
closest.Y = MathHelper.Clamp (-yExtent, yExtent, closest.Y);
//whether or not the circle is inside the aabb
bool inside = false;
if (n == closest) {
inside = true;
//Find closest edge
if (Math.Abs (n.X) > Math.Abs (n.Y)) {
// Clamp to closest extent
if (closest.X > 0)
closest.X = xExtent;
else
closest.X = -xExtent;
}
// Y axis is shorter
else {
// Clamp to closest extent
if (closest.Y > 0)
closest.Y = yExtent;
else
closest.Y = -yExtent;
}
}
m.Normal = n - closest;
float d = n.LengthSquared ();
float r = b.Radius;
//Early out if the circle's radius is shorter than the distance to the closest point
//and the circle isn't in the AABB.
if (d > r * r && !inside)
return false;
d = (float)Math.Sqrt (d);
if (inside) {
m.Normal = -PhysicsMath.GetNormal(a.Position, b.Position);
m.PenetrationDepth = r + d;
m.AreColliding = true;
return true;
}
else {
m.Normal = PhysicsMath.GetNormal(a.Position, b.Position);
m.PenetrationDepth = r + d;
m.AreColliding = true;
return true;
}
}
