/// <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; }