internal void ResolveAABBAABBStatic(PhysicsBody2D BodyS, PhysicsBody2D BodyD)
{
Shapes.AABB BA = BodyS.shape as Shapes.AABB;
Shapes.AABB BB = BodyD.shape as Shapes.AABB;
Vector3 delta = BB.transform.Position - BA.transform.Position;
Vector3 tD = (BA.Dimensions + BB.Dimensions) / 2;
if (Math.Abs(delta.X) > Math.Abs(delta.Z))
{
if (delta.X < 0)
{// Left
BodyD.transform.parent.Position = BodyS.transform.Position - new Vector3(tD.X, 0, 0);
}
else
{// Right
BodyD.transform.parent.Position = BodyS.transform.Position + new Vector3(tD.X, 0, 0);
}
}
else
{
if (delta.Z < 0)
{// Top
BodyD.transform.parent.Position = BodyS.transform.Position - new Vector3(0, 0, tD.Z);
}
else
{// Bottom
BodyD.transform.parent.Position = BodyS.transform.Position + new Vector3(0, 0, tD.Z);
}
}
}
internal List <int> CalculateBoundsIndices(PhysicsBody2D body)
{
List <int> indices = new List <int>();
AABB boundingBox = body.shape.GetBoundingBox();
int dimension = (int)Math.Max(boundingBox.Dimensions.X, boundingBox.Dimensions.Z) / 4;
int x = (int)Math.Floor(body.transform.Position.X / PhysicsSettings.BOUNDINGBOX_LARGEST);
int z = (int)Math.Floor(body.transform.Position.Z / PhysicsSettings.BOUNDINGBOX_LARGEST);
indices.Add(x + (z * (int)Math.Sqrt(int.MaxValue / 2)));
for (int i_x = 0; i_x < dimension; ++i_x)
{
for (int i_z = 0; i_z < dimension; ++i_z)
{
if (i_x == 0 || i_x == dimension - 1 || i_z == 0 || i_z == dimension - 1)
{
Transform newTransform = new Transform();
newTransform.Transformation = Matrix.Identity;
newTransform.Position = new Vector3((i_x - (int)Math.Floor(dimension / 2.0f)) * PhysicsSettings.BOUNDINGBOX_LARGEST + x * PhysicsSettings.BOUNDINGBOX_LARGEST, 0, (i_z - (int)Math.Floor(dimension / 2.0f)) * PhysicsSettings.BOUNDINGBOX_LARGEST + z * PhysicsSettings.BOUNDINGBOX_LARGEST) - bounds_transform.Position;
int newIndex = CalculateBoundsIndex(newTransform);
if (!indices.Contains(newIndex))
{
indices.Add(newIndex);
}
}
}
}
return(indices);
}
/// <summary>
/// Adds a PhysicsBody to the Physics engine, bodies add themselves when made, this rarely needs to be called outside of the PhysicsBody constructor
/// </summary>
/// <param name="body">The body to be added</param>
internal void AddBody(PhysicsBody2D body)
{
if (!bodies_All.Contains(body))
{
bodies_All.Add(body);
}
else
{
AddChunk(body);
}
//if (body.flagBodyType.HasFlag(BodyType.STATIC))
//{
if (bounds == null)
{
bounds = new List <PhysicsBoundingChunk2D>();
bounds_hashtable = new Dictionary <int, PhysicsBoundingChunk2D>();
AddChunk(body);
}
else
{
AddChunk(body);
}
if (!body.flagBodyType.HasFlag(BodyType.STATIC))
{
if (!bodies_Active.Contains(body))
{
bodies_Active.Add(body);
}
}
}
/// <summary>
/// Removes a collision delegate from the physics body
/// </summary>
/// <param name="callback">The delegate to be removed</param>
/// <param name="body">The body to remove the delegate from</param>
internal void UnregisterCollisionCallback(Collision2D.OnCollision callback, PhysicsBody2D body)
{
// First check to see if the registry contains this key
if (registery_CollisionCallbacks.ContainsKey(body))
{
body.collisionCallbacks.Remove(callback);
}
}
/// <summary>
/// Adds a collision delegate to the physics bodies list of callbacks
/// This enables something using the delegate to be alerted when the body in question collides with something else
/// </summary>
/// <param name="callback">The delegate to be added</param>
/// <param name="body">The body to add the delegate to</param>
internal void RegisterCollisionCallback(Collision2D.OnCollision callback, PhysicsBody2D body)
{
// First check to see if the registry contains this key
if (!registery_CollisionCallbacks.ContainsKey(body))
{
// If the registry does not contain the body, add the body, and its list of callbacks
registery_CollisionCallbacks.Add(body, body.collisionCallbacks);
}
// Add the new callback to the list
body.collisionCallbacks.Add(callback);
}
private static Collision2D Helper_EvaluateCollisions(PhysicsBody2D bodyA, PhysicsBody2D bodyB)
{
foreach (Collision2D collision in bodyA.collisions)
{
if ((collision.BodyB == bodyB && collision.BodyA == bodyA) || (collision.BodyA == bodyB && collision.BodyB == bodyA))
{
return(collision);
}
}
return(new Collision2D(bodyA, bodyB));
}
/// <summary>
/// Attempts to add a body to the bounding chunk, the criteria for failure are:
/// The body is not static
/// The body is not overlapping the bounds
/// </summary>
/// <param name="body">The body to add</param>
/// <returns>True on success, False on failure</returns>
public bool AddBody(PhysicsBody2D body)
{
bool added = false;
//for (int i = 0; i < PhysicsEngine.PhysicsSettings.BOUNDINGBOX_ORDERS; ++i)
//{
// foreach (AABB bounds in orderToIndex[i])
// {
// if (bounds.OverlapTest(body.shape))
// {
// if (body.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC))
// {
// statics[bounds].Add(body);
// }
// else
// {
// dynamics[bounds].Add(body);
// }
// body.chunks.Add(this);
// added = true;
// }
// }
//}
for (int i = ShapeToOrder(body.shape); i < PhysicsEngine.PhysicsSettings.BOUNDINGBOX_ORDERS; ++i)
{
foreach (AABB bounds in orderToIndex[i])
{
if (bounds.OverlapTest(body.shape, bounds.transform, body.transform))
{
if (body.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC))
{
if (!statics[bounds].Contains(body))
{
statics[bounds].Add(body);
}
}
else
{
if (!dynamics[bounds].Contains(body))
{
dynamics[bounds].Add(body);
}
}
body.chunks.Add(this);
added = true;
}
}
}
return(added);
}
/// <summary>
/// Given a body get all its collision callbacks
/// </summary>
/// <param name="body">The body to get callbacks from</param>
/// <returns></returns>
internal List <Collision2D.OnCollision> GetCollisionCallbacks(PhysicsBody2D body)
{
// First check to see if the registry contains this key
if (registery_CollisionCallbacks.ContainsKey(body))
{
// If it has this body, get its list
return(registery_CollisionCallbacks[body]);
}
else
{
return(null);
}
}
// TODO fix Circle vs Circle Static collision resolution
internal void ResolveCircleCircleStatic(PhysicsBody2D BodyS, PhysicsBody2D BodyD)
{
Shapes.Circle BA = BodyS.shape as Shapes.Circle;
Shapes.Circle BB = BodyD.shape as Shapes.Circle;
Vector3 dN = Vector3.Normalize(BA.lastOverlap_delta);
float dR = BA.lastOverlap_radius;
BodyD.transform.parent.Position = BodyS.transform.Position + dN * dR;
if (BodyD.Velocity.LengthSquared() > 0)
{
float projection = 2 * -Vector3.Dot(BodyD.Velocity * Math.Min(BodyD.Restitution, BodyS.Restitution), dN);
BodyD.Velocity = BodyD.Velocity + projection * dN;
}
}
/// <summary>
/// Given a body this method returns all bodies around that body in this bounding chunk
/// </summary>
/// <param name="body">The body to get neighbours of</param>
/// <returns>Either a list of bodies, if the body is within this bounding chunk, or an empty list, if it is not</returns>
public List <PhysicsBody2D> GetNearbyBodies(PhysicsBody2D body)
{
List <PhysicsBody2D> bodies = new List <PhysicsBody2D>();
// Since every AABB in this chunk has a list that contains all the objects in that AABB and all the children AABB I only need to check against the AABBs at the same 'order' as the bodies shape
for (int i = ShapeToOrder(body.shape); i < PhysicsEngine.PhysicsSettings.BOUNDINGBOX_ORDERS; ++i)
{
foreach (AABB bounds in orderToIndex[i])
{
if (bounds.OverlapTest(body.shape, bounds.transform, body.transform))
{
bodies.AddRange(statics[bounds]);
bodies.AddRange(dynamics[bounds]);
}
}
}
return(bodies);
}
// TODO RemoveBody from PhysicsBoundingChunk2D can probably be way better
public void RemoveBody(PhysicsBody2D body)
{
foreach (AABB bounds in bounds)
{
if (body.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC))
{
if (statics[bounds].Contains(body))
{
statics[bounds].Remove(body);
}
}
else
{
if (dynamics[bounds].Contains(body))
{
dynamics[bounds].Remove(body);
}
}
}
}
/// <summary>
/// Call this method to test collisions
/// This method wants to be called as the last piece of a collision pass, after all definite non-collisions have been ruled out
/// </summary>
/// <param name="BodyA">The first body</param>
/// <param name="BodyB">The second body</param>
/// <returns>An instance of Collision</returns>
public static Collision2D Evaluate(PhysicsBody2D bodyA, PhysicsBody2D bodyB)
{
// No need to check collisions on a body with itself
if (bodyA != bodyB)
{
// This needs to evaluate whether these 2 bodies already have a Collision between them, if so, evaluate it, and return it
// If they don't it needs to make one, if they are colliding
// If they aren't colliding return a new Collision with CollisionType none
Collision2D collision;
// Neither body has any collisions
if (bodyA.collisions.Count == bodyB.collisions.Count && bodyA.collisions.Count == 0)
{
// A new collision should be made
collision = new Collision2D(bodyA, bodyB);
}
else
{
// An optimisation here is to check the smaller list of collisions
// Since it is an iteration
if (bodyA.collisions.Count <= bodyB.collisions.Count)
{
collision = Helper_EvaluateCollisions(bodyA, bodyB);
}
else
{
collision = Helper_EvaluateCollisions(bodyB, bodyA);
}
}
// Evaluate the collision
//collision.Evaluate();
// Return it
return(collision);
}
else
{
return(null);
}
}
public List <PhysicsBody2D> GetCollisionPass(PhysicsBody2D body)
{
// TODO broad phase collision pass on non-static bodies
List <PhysicsBody2D> bodies = new List <PhysicsBody2D>();
List <int> indices = CalculateBoundsIndices(body);
foreach (int index in indices)
{
if (bounds_hashtable.ContainsKey(index))
{
bodies.AddRange(bounds_hashtable[index].GetNearbyBodies(body));
}
else
{
// This means there is no chunk there, and it must be made
AddChunk(body);
}
}
return(bodies);
}
internal void ResolveCircleAABBStatic(PhysicsBody2D BodyS, PhysicsBody2D BodyD)
{
// TODO Circle AABB
Shapes.Circle BA = (BodyS.shape is Shapes.Circle) ? BodyS.shape as Shapes.Circle : BodyD.shape as Shapes.Circle;
Shapes.AABB BB = (BodyS.shape is Shapes.Circle) ? BodyD.shape as Shapes.AABB : BodyS.shape as Shapes.AABB;
// Might as well normalize the delta while we are at it
Vector3 dN = Vector3.Normalize(BA.lastOverlap_delta);
float theta = (float)Math.Atan(dN.Z / dN.X);
float length = BB.LengthAtAngle(theta);
// Time to move the circle and the aabb away from each other along the normal
// Calculate a new center of mass for the system, and offset them both their radii away from this center of mass, along the normal, based on their mass percentage of the system
float dR = length + BA.Radius;
BodyD.transform.parent.Position = BodyS.transform.Position + (dN * dR);
if (BodyB.Velocity.LengthSquared() > 0 && Vector3.Dot(Vector3.Normalize(BodyB.Velocity), dN) > 0)
{
float projection = 2 * -Vector3.Dot(BodyB.Velocity * MathHelper.Min(BodyB.Restitution, BodyA.Restitution), dN);
if (BodyA.shape is Shapes.AABB)
{
if (Math.Abs(dN.X) > Math.Abs(dN.Y))
{
BodyB.Velocity = new Vector3(-BodyB.Velocity.X, BodyB.Velocity.Y, BodyB.Velocity.Z);
}
else
{
BodyB.Velocity = new Vector3(BodyB.Velocity.X, BodyB.Velocity.Y, -BodyB.Velocity.Z);
}
}
else
{
BodyB.Velocity = BodyB.Velocity + projection * BodyB.Mass * dN;
}
}
}
private Collision2D(PhysicsBody2D A, PhysicsBody2D B)
{
// For ease of computing around static dynamic collisions I have decided the static body should always be BodyA
if (A.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC) || A.flagBodyType.HasFlag(PhysicsEngine.BodyType.KINEMATIC) || B.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC) || B.flagBodyType.HasFlag(PhysicsEngine.BodyType.KINEMATIC))
{
if (A.flagBodyType.HasFlag(PhysicsEngine.BodyType.STATIC) || A.flagBodyType.HasFlag(PhysicsEngine.BodyType.KINEMATIC))
{
BodyA = A;
BodyB = B;
}
else
{
BodyA = B;
BodyB = A;
}
}
else
{
BodyA = A;
BodyB = B;
}
type = PhysicsEngine.CollisionType.NONE;
}
public bool BoundsTest(PhysicsBody2D body)
{
return(bounds[0].OverlapTest(body.shape, bounds[0].transform, body.transform));
}
internal void RemoveBody(PhysicsBody2D body)
{
bodies_Dead.Add(body);
}
internal void AddChunk(PhysicsBody2D body)
{
int index = CalculateBoundsIndex(body.transform);
float x = (int)Math.Floor(body.transform.Position.X / PhysicsSettings.BOUNDINGBOX_LARGEST) * PhysicsSettings.BOUNDINGBOX_LARGEST;
float z = (int)Math.Floor(body.transform.Position.Z / PhysicsSettings.BOUNDINGBOX_LARGEST) * PhysicsSettings.BOUNDINGBOX_LARGEST;
if (bounds_hashtable.ContainsKey(index))
{
bounds_hashtable[index].AddBody(body);
}
else
{
bounds_transform = new Transform();
bounds_transform.Position = new Vector3(x, 0, z);
bounds.Add(new PhysicsBoundingChunk2D(bounds_transform));
bounds_hashtable.Add(index, bounds[bounds.Count - 1]);
bounds[bounds.Count - 1].AddBody(body);
}
// If the body is so large that it takes up more than 1 chunk I need to know that, and build all the chunks it is in around the one at its center
int dimension = 3;
int count = 0;
bool tobig = false;
Transform newTransform = new Transform();
while (!tobig)
{
for (int i_x = 0; i_x < dimension; ++i_x)
{
for (int i_z = 0; i_z < dimension; ++i_z)
{
if (i_x == 0 || i_x == dimension - 1 || i_z == 0 || i_z == dimension - 1)
{
newTransform.Transformation = Matrix.Identity;
newTransform.Position = new Vector3((i_x - (int)Math.Floor(dimension / 2.0f)) * PhysicsSettings.BOUNDINGBOX_LARGEST + x * PhysicsSettings.BOUNDINGBOX_LARGEST, 0, (i_z - (int)Math.Floor(dimension / 2.0f)) * PhysicsSettings.BOUNDINGBOX_LARGEST + z * PhysicsSettings.BOUNDINGBOX_LARGEST) - bounds_transform.Position;
int newIndex = CalculateBoundsIndex(newTransform);
if (bounds_hashtable.ContainsKey(newIndex))
{
if (bounds_hashtable[newIndex].BoundsTest(body))
{
bounds_hashtable[newIndex].AddBody(body);
count++;
}
}
else
{
PhysicsBoundingChunk2D newChunk = new PhysicsBoundingChunk2D(newTransform);
if (newChunk.BoundsTest(body))
{
bounds.Add(newChunk);
bounds_hashtable.Add(newIndex, newChunk);
newChunk.AddBody(body);
count++;
}
}
}
}
}
tobig = (count == 0) ? true : false;
count = 0;
dimension += 2;
}
}