public void SolveCollision(CollisionBody colA, CollisionBody colB, out CollisionResult result)
{
result = CollisionResult.NoCollision;
ICollisionSolver solver = FindCollisionSovler(colA, colB);
if (solver != null)
result = solver.SolveCollision(colA, colB);
}
public bool IsSolveable(CollisionBody colA, CollisionBody colB)
{
// we the assume the calling function has checked for null shapes.
if (colA.Shape is Circle && colB.Shape is Box) return true;
else if (colA.Shape is Box && colB.Shape is Circle) return true;
else return false;
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
Circle circleA = (Circle)colA.Shape;
Circle circleB = (Circle)colB.Shape;
Vector2 distance = colA.Position - colB.Position;
float abSize = circleA.Radius + circleB.Radius;
float penetration = abSize - distance.Length();
Vector2 mtv = default(Vector2);
if (penetration <= 0)
{
return CollisionResult.NoCollision;
}
mtv = distance;
Vector2Extensions.SafeNormalize(ref mtv);
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
mtv = -mtv;
return new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = penetration * mtv,
IsColliding = true
};
}
protected override void Initialize()
{
Vector2 offset = Vector2.Zero;
CollisionBody temp = new CollisionBody();
Play(AnimationName, AnimationOptions);
foreach(ColBodyAttributes ColAttribute in CollisionBodiesAttributes)
{
if (ColAttribute._bodytype == ColBodyType.Box)
{
temp = CollisionBody.Create(null, new Box(ColAttribute._dimensions.X, ColAttribute._dimensions.Y));
offset = new Vector2((temp.Shape as Box).HalfWidth, (temp.Shape as Box).HalfHeight);
}
else if(ColAttribute._bodytype == ColBodyType.Circle)
{
temp = CollisionBody.Create(null, new Circle(ColAttribute._dimensions.X));
offset = new Vector2((temp.Shape as Circle).Radius, (temp.Shape as Circle).Radius);
}
else
{
//temp._body = CollisionBody.Create(null, new Ellipse(ColAttribute._dimensions.X, ColAttribute._dimensions.Y));
}
temp.Flags = CollisionFlags.Ignore;
temp.SimulationPosition = Position - Offset + ColAttribute._offset + offset;
temp.AddToSimulation();
temp.SetGroup(ColAttribute._collisionGroup);
temp.CollidesWithGroup(ColAttribute._collideWith);
CollisionBodies.Add(temp);
}
Depth = Position.Y;
base.Initialize();
}
private bool HandleOnCollision(CollisionBody them, Vector2 normal)
{
if (Collision != null)
{
return(Collision(them as IWrappedBody, normal));
}
return(true);
}
private bool HandleBeforeCollision(CollisionBody them)
{
if (BeforeCollisionEvent != null)
{
return(BeforeCollisionEvent(them as IWrappedBody));
}
return(true);
}
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
Box boxA = colA.Shape as Box;
Box boxB = colB.Shape as Box;
Matrix3 transformA = colA.WorldTransform;
Matrix3 transformB = colB.WorldTransform;
float projectedDistance = 0;
float minPenetration = float.MaxValue;
Vector2 distance = colA.Position - colB.Position;
Vector2 mtv = default(Vector2); // the minimum translation vector
// merge normals from both polygons
// NOTE: For OBB's we only need to check their half widths. ie. 4 axis total.
// For AABB's we only need to check 2 axis since they don't rotate.
boxA.CalculateOrientation(ref transformA, out _axisToCheck[0]);
boxB.CalculateOrientation(ref transformB, out _axisToCheck[1]);
_axisToCheck[2] = Vector2Extensions.PerpendicularLeft(_axisToCheck[0]);
_axisToCheck[3] = Vector2Extensions.PerpendicularLeft(_axisToCheck[1]);
// TODO: remove parallel normals
for (int i = 0; i < _axisToCheck.Length; i++)
{
Vector2 projectionA, projectionB;
projectedDistance = Math.Abs(Vector2.Dot(distance, _axisToCheck[i]));
boxA.ProjectOnto(ref transformA, ref _axisToCheck[i], out projectionA);
boxB.ProjectOnto(ref transformB, ref _axisToCheck[i], out projectionB);
float aSize = Math.Abs(projectionA.X) + Math.Abs(projectionA.Y);
float bSize = Math.Abs(projectionB.X) + Math.Abs(projectionB.Y);
float abSize = aSize + bSize;
float penetration = abSize - projectedDistance;
// a seperating axis found; there is no collision.
if (penetration <= 0)
{
return CollisionResult.NoCollision;
}
// project the object along the axis with the smalled penetration depth.
else if (Math.Abs(penetration) < Math.Abs(minPenetration))
{
minPenetration = penetration;
mtv = _axisToCheck[i];
}
}
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
mtv = -mtv;
// seperating axis could not be found; a collision occurs.
return new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = minPenetration * mtv,
IsColliding = true
};
}
public static CollisionBody Create(TransformableEntity entity, CollisionShape shape)
{
CollisionBody collider = Pool.Acquire <CollisionBody>();
collider.Owner = entity;
collider.Shape = shape;
collider.OnCollision += collider.HandleOnCollision;
collider.BeforeCollision += collider.HandleBeforeCollision;
return(collider);
}
public void SolveCollision(CollisionBody colA, CollisionBody colB, out CollisionResult result)
{
result = CollisionResult.NoCollision;
ICollisionSolver solver = FindCollisionSovler(colA, colB);
if (solver != null)
{
result = solver.SolveCollision(colA, colB);
}
}
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
// we want boxes and circles included too..
ConvexShape polyA = (ConvexShape)colA.Shape;
ConvexShape polyB = (ConvexShape)colB.Shape;
float projectedDistance = 0;
float minPenetration = float.MaxValue;
Vector2 distance = colA.Position - colB.Position;
Vector2 mtv = default(Vector2); // the minimum translation vector
// merge normals from both polygons
Vector2[] axisToCheck = new Vector2[polyA.Normals.Length + polyB.Normals.Length];
for (int i = 0; i < polyA.Normals.Length; i++)
axisToCheck[i] = polyA.Normals[i];
for (int i = polyA.Normals.Length; i < axisToCheck.Length; i++)
axisToCheck[i] = polyB.Normals[i - polyA.Normals.Length];
// TODO: remove parallel normals
for (int i = 0; i < axisToCheck.Length; i++)
{
float minA, maxA, minB, maxB;
minA = maxA = minB = maxB = 0;
projectedDistance = Math.Abs(Vector2.Dot(distance, axisToCheck[i]));
polyA.ProjectOnto(ref axisToCheck[i], out minA, out maxA);
polyB.ProjectOnto(ref axisToCheck[i], out minB, out maxB);
float penetration = maxB - minA;
// a seperating axis has been found; there is no collision.
if (minA - maxB > 0f || minB - maxA > 0f)
{
return CollisionResult.NoCollision;
}
// project the object along the axis with the smalled penetration depth.
else if (Math.Abs(penetration) < Math.Abs(minPenetration))
{
minPenetration = penetration;
mtv = axisToCheck[i];
}
}
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
mtv = -mtv;
// seperating axis could not be found; a collision occurs.
return new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = minPenetration * mtv,
IsColliding = true
};
}
public override void RemoveCollider(CollisionBody collider)
{
Debug.Assert(collider != null);
if (_colliders.Contains(collider))
{
_broadphase.RemoveProxy(collider.BroadphaseProxy);
_colliders.FastRemove<CollisionBody>(collider);
collider.OnRemoved();
CollisionGlobals.TotalColliders--;
}
}
public override void RemoveCollider(CollisionBody collider)
{
Debug.Assert(collider != null);
if (_colliders.Contains(collider))
{
_broadphase.RemoveProxy(collider.BroadphaseProxy);
_colliders.FastRemove <CollisionBody>(collider);
collider.OnRemoved();
CollisionGlobals.TotalColliders--;
}
}
/// <summary>
/// From the available algorithms, find the algorithm that matches the shapes the closest.
/// </summary>
private ICollisionSolver FindCollisionSovler(CollisionBody colA, CollisionBody colB)
{
// factor out the null checks for speed. TODO: what about null collisionbodies?
if (colA.Shape == null || colB.Shape == null)
return null;
foreach (ICollisionSolver solver in _solvers)
{
if (solver.IsSolveable(colA, colB))
return solver;
}
return null;
}
public bool IsSolveable(CollisionBody colA, CollisionBody colB)
{
// we the assume calling function has checked for null shapes.
if (colA.Shape is Circle && colB.Shape is Circle)
{
return(true);
}
else
{
return(false);
}
}
public bool IsSolveable(CollisionBody colA, CollisionBody colB)
{
// we assume the calling function has checked for null shapes.
if (colA.Shape is ConvexShape && colB.Shape is ConvexShape)
{
return(true); // catch all.
}
else
{
return(false);
}
}
public override void AddCollider(CollisionBody collider)
{
Debug.Assert(collider != null);
if (!_colliders.Contains(collider))
{
_colliders.Add(collider);
collider.OnAdded();
// add broadphase proxy
collider.BroadphaseProxy = BroadphaseProxy.Create(collider);
UpdateAABB(collider);
CollisionGlobals.TotalColliders++;
}
}
public override void AddCollider(CollisionBody collider)
{
Debug.Assert(collider != null);
if (!_colliders.Contains(collider))
{
_colliders.Add(collider);
collider.OnAdded();
// add broadphase proxy
collider.BroadphaseProxy = BroadphaseProxy.Create(collider);
UpdateAABB(collider);
CollisionGlobals.TotalColliders++;
}
}
/// <summary>
/// From the available algorithms, find the algorithm that matches the shapes the closest.
/// </summary>
private ICollisionSolver FindCollisionSovler(CollisionBody colA, CollisionBody colB)
{
// factor out the null checks for speed. TODO: what about null collisionbodies?
if (colA.Shape == null || colB.Shape == null)
{
return(null);
}
foreach (ICollisionSolver solver in _solvers)
{
if (solver.IsSolveable(colA, colB))
{
return(solver);
}
}
return(null);
}
protected void UpdateAABBs()
{
CollisionGlobals.UpdatedAABBs = 0;
for (int i = 0; i < _colliders.Count; i++)
{
CollisionBody collider = _colliders[i];
// only update an aabb if the collider has moved
if (collider.IsAwake || _forceUpdateAABBs)
{
UpdateAABB(collider);
_colliders[i].IsAwake = false; // HACK: collider should deactivate itself.
CollisionGlobals.UpdatedAABBs++;
}
}
_forceUpdateAABBs = false;
}
public void SolveCollisions(OverlappingPairCache overlappingPairs)
{
CollisionGlobals.NarrowphaseDetections = 0;
foreach (OverlappingPair pair in overlappingPairs._pairs)
{
CollisionResult result;
CollisionBody colA = pair.ProxyA.ClientObject as CollisionBody;
CollisionBody colB = pair.ProxyB.ClientObject as CollisionBody;
// calculates the collision result if there's an algorithm that matches the pair of shapes.
SolveCollision(colA, colB, out result);
// handle collision events and resolve shape penetration.
if (result.IsColliding)
{
CollisionGlobals.Results.Push(result);
// translate the body to a safe non-penetrating position.
if ((result.Us.Flags & CollisionFlags.Response) != 0)
{
result.Us.Position += result.CollisionResponse;
}
if ((result.Them.Flags & CollisionFlags.Response) != 0)
{
result.Them.Position += -result.CollisionResponse;
}
// handle collision events
if (colA.OnCollision != null)
{
colA.OnCollision(colB, Vector2.Zero);
}
if (colB.OnCollision != null)
{
colB.OnCollision(colA, Vector2.Zero);
}
CollisionGlobals.NarrowphaseDetections++;
}
}
}
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
// normalize parameters such that colA is the circle and colB is the box.
// there are only two possibilities, therefore we only need to swap if colB is the circle.
if (colB.Shape is Circle)
{
CollisionBody tmp = colA;
colA = colB;
colB = tmp;
tmp = null;
}
Circle circleA = (Circle)colA.Shape;
Box boxB = (Box)colB.Shape;
Matrix3 transformB = colB.WorldTransform;
float projectedDistance = 0;
float minPenetration = float.MaxValue;
Vector2 distance = colA.Position - colB.Position;
Vector2 mtv = default(Vector2); // the minimum translation vector
Vector2[] axisToCheck = new Vector2[3];
boxB.CalculateOrientation(ref transformB, out axisToCheck[0]);
axisToCheck[1] = Vector2Extensions.PerpendicularLeft(axisToCheck[0]);
float minDistance = float.MaxValue;
Vector2 closestVertex = default(Vector2);
for (int i = 0; i < boxB.Vertices.Length; i++)
{
float vertexDistance = Vector2.DistanceSquared(colA.Position, boxB.Vertices[i]);
if (vertexDistance < minDistance)
{
minDistance = vertexDistance;
closestVertex = colB.Position - boxB.Vertices[i];
}
}
axisToCheck[2] = closestVertex;
Vector2Extensions.SafeNormalize(ref axisToCheck[2]);
for (int i = 0; i < axisToCheck.Length; i++)
{
Vector2 projectionA, projectionB;
projectedDistance = Math.Abs(Vector2.Dot(distance, axisToCheck[i]));
circleA.ProjectOnto(ref axisToCheck[i], out projectionA);
boxB.ProjectOnto(ref transformB, ref axisToCheck[i], out projectionB);
float aSize = projectionA.Length(); //Math.Abs(projectionA.X) + Math.Abs(projectionA.Y);
float bSize = Math.Abs(projectionB.X) + Math.Abs(projectionB.Y);
float abSize = aSize + bSize;
float penetration = abSize - projectedDistance;
// a seperating axis found; there is no collision.
if (penetration <= 0)
{
return(CollisionResult.NoCollision);
}
// project the object along the axis with the smalled penetration depth.
else if (Math.Abs(penetration) < Math.Abs(minPenetration))
{
minPenetration = penetration;
mtv = axisToCheck[i];
}
}
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
{
mtv = -mtv;
}
// seperating axis could not be found; a collision occurs.
return(new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = minPenetration * mtv,
IsColliding = true
});
}
public override void DrawDebug(ref Matrix view, ref Matrix projection)
{
G.PrimitiveBatch.Begin(ref projection, ref view);
for (int i = 0; i < _colliders.Count; i++)
{
CollisionBody col = _colliders[i];
if (col.Shape == null || !col.BelongsToGroup(CollisionGlobals.ViewGroups))
{
continue;
}
Matrix3 transform = col.WorldTransform;
if ((CollisionGlobals.ViewFlags & DebugViewFlags.Shape) != 0)
{
Vector2[] transformedVerts = col.Shape.VerticesCopy;
for (int j = 0; j < transformedVerts.Length; j++)
{
transform.TransformVector(ref transformedVerts[j]);
}
G.PrimitiveBatch.DrawPolygon(transformedVerts, transformedVerts.Length, CollisionGlobals.ShapeColor);
}
if ((CollisionGlobals.ViewFlags & DebugViewFlags.Extents) != 0)
{
if (col.Shape is Box)
{
Box box = (Box)col.Shape;
Vector2 halfwidthX, halfwidthY;
box.CalculateExtents(ref transform, out halfwidthX, out halfwidthY);
G.PrimitiveBatch.DrawSegment(col.Position, halfwidthX, CollisionGlobals.ExtentsColor);
G.PrimitiveBatch.DrawSegment(col.Position, halfwidthY, CollisionGlobals.ExtentsColor);
}
else if (col.Shape is Circle)
{
Circle circle = (Circle)col.Shape;
Vector2 extents;
circle.CalculateExtents(ref transform, out extents);
G.PrimitiveBatch.DrawSegment(col.Position, extents, CollisionGlobals.ExtentsColor);
}
}
if ((CollisionGlobals.ViewFlags & DebugViewFlags.AABB) != 0)
{
AABB aabb;
col.Shape.CalculateAABB(ref transform, out aabb);
G.PrimitiveBatch.DrawSegment(new Vector2(aabb.Min.X, aabb.Min.Y), new Vector2(aabb.Max.X, aabb.Min.Y), CollisionGlobals.BoundingColor);
G.PrimitiveBatch.DrawSegment(new Vector2(aabb.Max.X, aabb.Min.Y), new Vector2(aabb.Max.X, aabb.Max.Y), CollisionGlobals.BoundingColor);
G.PrimitiveBatch.DrawSegment(new Vector2(aabb.Min.X, aabb.Max.Y), new Vector2(aabb.Max.X, aabb.Max.Y), CollisionGlobals.BoundingColor);
G.PrimitiveBatch.DrawSegment(new Vector2(aabb.Min.X, aabb.Min.Y), new Vector2(aabb.Min.X, aabb.Max.Y), CollisionGlobals.BoundingColor);
}
}
if ((CollisionGlobals.ViewFlags & DebugViewFlags.CollisionResponse) != 0)
{
while (CollisionGlobals.Results.Count > 0)
{
CollisionResult result = CollisionGlobals.Results.Pop();
G.PrimitiveBatch.DrawSegment(result.Us.Position, result.Us.Position + 10 * result.CollisionResponse, CollisionGlobals.ResponseColor);
}
}
G.PrimitiveBatch.End();
//G.SpriteBatch.Begin();
//G.SpriteBatch.DrawString(G.Font, DebugInfo, new Vector2(0, 50), Color.White);
//G.SpriteBatch.End();
}
public bool IsSolveable(CollisionBody colA, CollisionBody colB)
{
// we assume the calling function has checked for null shapes.
if (colA.Shape is ConvexShape && colB.Shape is ConvexShape) return true; // catch all.
else return false;
}
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
// normalize parameters such that colA is the circle and colB is the box.
// there are only two possibilities, therefore we only need to swap if colB is the circle.
if (colB.Shape is Circle) {
CollisionBody tmp = colA;
colA = colB;
colB = tmp;
tmp = null;
}
Circle circleA = (Circle)colA.Shape;
Box boxB = (Box)colB.Shape;
Matrix3 transformB = colB.WorldTransform;
float projectedDistance = 0;
float minPenetration = float.MaxValue;
Vector2 distance = colA.Position - colB.Position;
Vector2 mtv = default(Vector2); // the minimum translation vector
Vector2[] axisToCheck = new Vector2[3];
boxB.CalculateOrientation(ref transformB, out axisToCheck[0]);
axisToCheck[1] = Vector2Extensions.PerpendicularLeft(axisToCheck[0]);
float minDistance = float.MaxValue;
Vector2 closestVertex = default(Vector2);
for (int i = 0; i < boxB.Vertices.Length; i++)
{
float vertexDistance = Vector2.DistanceSquared(colA.Position, boxB.Vertices[i]);
if (vertexDistance < minDistance)
{
minDistance = vertexDistance;
closestVertex = colB.Position - boxB.Vertices[i];
}
}
axisToCheck[2] = closestVertex;
Vector2Extensions.SafeNormalize(ref axisToCheck[2]);
for (int i = 0; i < axisToCheck.Length; i++)
{
Vector2 projectionA, projectionB;
projectedDistance = Math.Abs(Vector2.Dot(distance, axisToCheck[i]));
circleA.ProjectOnto(ref axisToCheck[i], out projectionA);
boxB.ProjectOnto(ref transformB, ref axisToCheck[i], out projectionB);
float aSize = projectionA.Length(); //Math.Abs(projectionA.X) + Math.Abs(projectionA.Y);
float bSize = Math.Abs(projectionB.X) + Math.Abs(projectionB.Y);
float abSize = aSize + bSize;
float penetration = abSize - projectedDistance;
// a seperating axis found; there is no collision.
if (penetration <= 0)
{
return CollisionResult.NoCollision;
}
// project the object along the axis with the smalled penetration depth.
else if (Math.Abs(penetration) < Math.Abs(minPenetration))
{
minPenetration = penetration;
mtv = axisToCheck[i];
}
}
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
mtv = -mtv;
// seperating axis could not be found; a collision occurs.
return new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = minPenetration * mtv,
IsColliding = true
};
public CollisionResult SolveCollision(CollisionBody colA, CollisionBody colB)
{
// we want boxes and circles included too..
ConvexShape polyA = (ConvexShape)colA.Shape;
ConvexShape polyB = (ConvexShape)colB.Shape;
float projectedDistance = 0;
float minPenetration = float.MaxValue;
Vector2 distance = colA.Position - colB.Position;
Vector2 mtv = default(Vector2); // the minimum translation vector
// merge normals from both polygons
Vector2[] axisToCheck = new Vector2[polyA.Normals.Length + polyB.Normals.Length];
for (int i = 0; i < polyA.Normals.Length; i++)
{
axisToCheck[i] = polyA.Normals[i];
}
for (int i = polyA.Normals.Length; i < axisToCheck.Length; i++)
{
axisToCheck[i] = polyB.Normals[i - polyA.Normals.Length];
}
// TODO: remove parallel normals
for (int i = 0; i < axisToCheck.Length; i++)
{
float minA, maxA, minB, maxB;
minA = maxA = minB = maxB = 0;
projectedDistance = Math.Abs(Vector2.Dot(distance, axisToCheck[i]));
polyA.ProjectOnto(ref axisToCheck[i], out minA, out maxA);
polyB.ProjectOnto(ref axisToCheck[i], out minB, out maxB);
float penetration = maxB - minA;
// a seperating axis has been found; there is no collision.
if (minA - maxB > 0f || minB - maxA > 0f)
{
return(CollisionResult.NoCollision);
}
// project the object along the axis with the smalled penetration depth.
else if (Math.Abs(penetration) < Math.Abs(minPenetration))
{
minPenetration = penetration;
mtv = axisToCheck[i];
}
}
// the distance vector determines the direction of movement. the distance and mtv
// should always oppose each other to repel collisions.
if (Vector2.Dot(distance, mtv) < 0f)
{
mtv = -mtv;
}
// seperating axis could not be found; a collision occurs.
return(new CollisionResult()
{
Us = colA,
Them = colB,
CollisionResponse = minPenetration * mtv,
IsColliding = true
});
}
private bool HandleBeforeCollision(CollisionBody them)
{
if (BeforeCollisionEvent != null)
return BeforeCollisionEvent(them as IWrappedBody);
return true;
}
protected void UpdateAABB(CollisionBody collider)
{
AABB aabb;
Matrix3 worldTransform = collider.WorldTransform;
collider.Shape.CalculateAABB(ref worldTransform, out aabb);
_broadphase.SetProxyAABB(collider.BroadphaseProxy, ref aabb);
}
public abstract void RemoveCollider(CollisionBody colider);
private bool HandleOnCollision(CollisionBody them, Vector2 normal)
{
if (Collision != null)
return Collision(them as IWrappedBody, normal);
return true;
}
public abstract void AddCollider(CollisionBody colider);
