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 void Update(GameTime gameTime) { _aabb1.Update (gameTime); _aabb2.Update (gameTime); Manifold m = new Manifold (); //bgcolor = Collision.TestAABBvsAABB(_aabb1, _aabb2, ref m) ? Color.Green : Color.Brown; Collision.TestAABBvsAABB(_aabb1, _aabb2, ref m); if (m.AreColliding) { m = new Manifold(); Collision.TestAABBvsAABB (_aabb1, _aabb2, ref m); Collision.ResolveCollision (m); Collision.PositionalCorrection(m); } _aabb3.Update(gameTime); _aabb4.Update(gameTime); m = new Manifold(); Collision.TestAABBvsAABB(_aabb3, _aabb4, ref m); if (m.AreColliding) { m = new Manifold(); Collision.TestAABBvsAABB (_aabb3, _aabb4, ref m); Collision.ResolveCollision (m); Collision.PositionalCorrection(m); } _aabb5.Update(gameTime); _aabb6.Update(gameTime); m = new Manifold(); Collision.TestAABBvsAABB(_aabb5, _aabb6, ref m); if (m.AreColliding) { m = new Manifold(); Collision.TestAABBvsAABB (_aabb5, _aabb6, ref m); Collision.ResolveCollision (m); Collision.PositionalCorrection(m); } }
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; } }
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 = PhysicsMath.DotProduct(relVelocity, m.Normal); 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; m.A.Velocity -= m.A.InvertedMass*impulse; m.B.Velocity += m.B.InvertedMass*impulse; }
public static void PositionalCorrection(Manifold m) { const float percent = 0.2f; const float slop = 0.01f; Vector2 correction = Math.Max (m.PenetrationDepth - slop, 0.0f) / (m.A.InvertedMass + m.B.InvertedMass) * percent * m.Normal; m.A.Position -= m.A.InvertedMass * correction; m.B.Position += m.B.InvertedMass * correction; }
public static bool CirclevsCircle(Circle a, Circle b, ref Manifold m) { m = new Manifold(); m.A = a; m.B = b; float collisionzone = a.Radius + b.Radius; //collisionzone *= collisionzone; float distance = 0f; if (a.Position == b.Position) distance = 0; else if (a.Position.Y == b.Position.Y) distance = Math.Abs(a.Position.X - b.Position.X); else if (a.Position.X == b.Position.X) distance = Math.Abs(a.Position.Y - b.Position.Y); else { var lega = Math.Abs(a.Position.X - b.Position.X); var legb = Math.Abs(a.Position.Y - b.Position.Y); //lega *= lega; //legb *= legb; distance = lega + legb; } m.Normal = PhysicsMath.GetNormal(a.Position, b.Position); bool collided = (collisionzone > distance); if (collided) { //Perform an actual square root here m.PenetrationDepth = a.Radius + b.Radius - (float)Math.Sqrt(distance); } else m.PenetrationDepth = 0; m.AreColliding = collided; return collided; }