public static CollisionManifold GetSphereToSphereCollisionManifold(SphereBehaviour sphereA, SphereBehaviour sphereB)
{
var manifold = new CollisionManifold();
var direction = sphereA.transform.position - sphereB.transform.position;
manifold.PenetrationDistance = (direction.magnitude - (sphereA.Radius + sphereB.Radius)) / 2;
if (manifold.PenetrationDistance <= 0.001f && manifold.PenetrationDistance > 0)
{
manifold.CollisionType = CollisionType.Colliding;
}
else if (manifold.PenetrationDistance < 0)
{
manifold.CollisionType = CollisionType.Penetrating;
}
else
{
manifold.CollisionType = CollisionType.None;
return(manifold);
}
manifold.Objects.Add(sphereA.gameObject);
manifold.Objects.Add(sphereB.gameObject);
manifold.Normal = direction.normalized;
return(manifold);
}
/// <summary>
/// If collision happened project the ParticleObject back to prevCenterMass.
/// </summary>
/// <param name="b1"></param>
/// <param name="b2"></param>
public void CollisionResolutionOBB(CollisionManifold cm, ColliderBox b1, ColliderBox b2)
{
Vector3 currPoint = cm.avg_depth;
Vector3 projPoint = cm.avg_contact;
float dist = Vector3.Magnitude(currPoint - projPoint);
if (Util.CMP(dist, 0.0f) || float.IsNaN(dist))
{
return;
}
if (!b1.IsStatic() && !b2.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint, b2.GetParticleObject());
}
else
{
if (!b1.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint);
}
else if (!b2.IsStatic())
{
this.SeparateParticleObjects(b2.GetParticleObject(), projPoint, currPoint);
}
}
}
public static CollisionManifold getSphereToAABBCollisionManifold(SphereBehaviour sphere, CubeBehaviour box)
{
var manifold = new CollisionManifold();
var closestPoint = GetAABBClosestPoint(sphere, box);
var direction = sphere.transform.position - closestPoint;
manifold.PenetrationDistance = (direction.magnitude - sphere.Radius) / 2;
if (manifold.PenetrationDistance <= 0.001f && manifold.PenetrationDistance > 0)
{
manifold.CollisionType = CollisionType.Colliding;
}
else if (manifold.PenetrationDistance < 0)
{
manifold.CollisionType = CollisionType.Penetrating;
}
else
{
manifold.CollisionType = CollisionType.None;
return(manifold);
}
// manifold.Objects.Add(sphere.gameObject);
// manifold.Objects.Add(box.gameObject);
manifold.Normal = direction.normalized;
return(manifold);
}
// Broadphase
private void GeneratePairs()
{
// Clear previous contacts and collision pairs.
Contacts.Clear();
for (var i = 0; i < Colliders.Count; ++i)
{
for (var j = i + 1; j < Colliders.Count; ++j)
{
// Prevent collider self check.
if (Colliders[i] == Colliders[j])
{
continue;
}
// Prevent colliders on same object creating collisions.
if (Colliders[i].RigidBody.GameObject == Colliders[j].RigidBody.GameObject)
{
continue;
}
if (Colliders[i].RigidBody.InvMass == 0.0f && Colliders[j].RigidBody.InvMass == 0.0f)
{
continue;
}
// Check distance between 2 colliders.
var dist = Vector2.Distance(Colliders[i].Position, Colliders[j].Position);
// Check collisions between each collider.
var colliderPair = new CollisionManifold(Colliders[i], Colliders[j]);
// Limit the distance to check for collisions. Hacky but for now works
// until I get around to TODO: spatial partitioning.
if (dist <= 5.0f)
{
colliderPair.Solve();
if (colliderPair.ContactDetected)
{
Contacts.Add(colliderPair);
continue;
}
}
// if the two colliders are further than 5 units away OR no contacts were detected this step
// check if any exit callbacks need to be called.
Colliders[i].CollisionListener.HandleExit(Colliders[j]);
Colliders[j].CollisionListener.HandleExit(Colliders[i]);
}
}
}
public void Execute(int index)
{
Entity collAEntity = CollisionPairsArray[index].ColliderEntityA;
Entity collBEntity = CollisionPairsArray[index].ColliderEntityB;
Collider collA = ColliderFromEntity[collAEntity];
Collider collB = ColliderFromEntity[collBEntity];
Entity rigidBodyAEntity = collA.RigidBodyEntity;
Entity rigidBodyBEntity = collB.RigidBodyEntity;
float3 aPos = ColliderPositionFromEntity[collAEntity].Value;
float3 bPos = ColliderPositionFromEntity[collBEntity].Value;
float3 fromAToBVector = bPos - aPos;
if (math.lengthSquared(fromAToBVector) == 0f)
{
fromAToBVector = new float3(0f, 1f, 0f);
}
float overlapDistance = -math.length(fromAToBVector) + SphereColliderFromEntity[collAEntity].Radius + SphereColliderFromEntity[collBEntity].Radius;
if (overlapDistance > 0f)
{
float3 relativeVelocity = VelocityFromEntity[collB.RigidBodyEntity].Value - VelocityFromEntity[collA.RigidBodyEntity].Value;
float3 collisionNormalAToB = math.normalize(fromAToBVector);
bool aIsKinematic = RigidBodyFromEntity[collA.RigidBodyEntity].IsKinematic > 0;
bool bIsKinematic = RigidBodyFromEntity[collB.RigidBodyEntity].IsKinematic > 0;
if (!(aIsKinematic && bIsKinematic))
{
CollisionManifold manifold = new CollisionManifold()
{
ColliderEntityA = collAEntity,
ColliderEntityB = collBEntity,
RigidBodyEntityA = rigidBodyAEntity,
RigidBodyEntityB = rigidBodyBEntity,
ContactPointA = aPos + (collisionNormalAToB * SphereColliderFromEntity[collAEntity].Radius),
ContactPointB = bPos + (-collisionNormalAToB * SphereColliderFromEntity[collBEntity].Radius),
CollisionNormalAToB = collisionNormalAToB,
OverlapDistance = overlapDistance,
};
CollisionManifoldsQueue.Enqueue(manifold);
}
}
}
private bool IsTouchingCircleCircle(IGameObject parent, IGameObject s, out CollisionManifold manifold)
{
manifold = new CollisionManifold();
float radiusA = parent.Rectangle.Width < parent.Rectangle.Height ? parent.Rectangle.Width / 2.0f : parent.Rectangle.Height / 2.0f;
float radiusB = s.Rectangle.Width < s.Rectangle.Height ? s.Rectangle.Width / 2.0f : s.Rectangle.Height / 2.0f;
//find the centre of each sprite's hit area (using .Center() will give a loss of precision)
Vector2 centreA = new Vector2(parent.transform.position.X + parent.Rectangle.X + parent.Rectangle.Width / 2, parent.transform.position.Y + parent.Rectangle.Y + parent.Rectangle.Height / 2);
Vector2 centreB = new Vector2(s.transform.position.X + s.Rectangle.X + s.Rectangle.Width / 2, s.transform.position.Y + s.Rectangle.Y + s.Rectangle.Height / 2);
float r = radiusA + radiusB;
float r2 = r * r;
float distanceSquared = Vector2.DistanceSquared(centreA, centreB);
if (r2 < distanceSquared)
{
//Not collided
return(false);
}
else
{
// Circles have collided, now compute manifold
manifold.Collidee = s;
// Calculate distance
float distance = (float)Math.Sqrt(distanceSquared);
if (distance != 0)
{
// If distance between circles is not zero
// Normal vector is points from A to B, and is a unit vector
manifold.Normal = (centreB - centreA) / distance;
// Distance is difference between radius and distance
manifold.Penetration = r - distance;
}
else
{
// If distance between circles is zero
manifold.Normal = Vector2.UnitX;
// Distance is difference between radius and distance
manifold.Penetration = (float)Math.Min(parent.Rectangle.Width / 2.0, s.Rectangle.Width / 2.0);
}
return(true);
}
}
public static void ResolveCollision(PhysicsBehaviour a, PhysicsBehaviour b)
{
CollisionManifold manifold = new CollisionManifold(a, b); // The normal will point from a to b
if (manifold.mPenetration > 0.0f)
{
// Linear displacement
if (a.mobile)
{
a.transform.position -= manifold.mNormal.normalized * manifold.mPenetration;
}
if (b.mobile)
{
b.transform.position += manifold.mNormal.normalized * manifold.mPenetration;
}
// Linear Impulse
Vector3 vr = manifold.mVelocity;
Vector3 n = manifold.mNormal;
float vrn = Vector3.Dot(vr, n);
float e = Mathf.Min(a.bounciness, b.bounciness);
float InvMasses = (1.0f / a.mass) + (1.0f / b.mass);
float j = (-(1.0f + e) * vrn) / InvMasses;
// Friction
Vector3 t = vr - (vrn * n);
float jt = (-(1.0f + e) * (Vector3.Dot(vr, t))) / InvMasses;
float friction = Mathf.Sqrt(a.friction * b.friction);
jt = Mathf.Max(jt, -j * friction);
jt = Mathf.Min(jt, j * friction);
// Adjust velocities
a.velocity = a.velocity - ((jt / a.mass) * n);
b.velocity = b.velocity + ((jt / b.mass) * n);
}
b.contacts.Remove(a);
}
/// <summary>
/// Checks and resolves the collision between particle object colliders.
/// </summary>
private void CollidersCollisions()
{
int count = Colliders.Count;
for (int i = 0; i < count; i++)
{
for (int j = i + 1; j < count; j++)
{
// OBB vs OBB
if (Colliders[i] as ColliderBox != null && Colliders[j] as ColliderBox != null)
{
CollisionManifold features = Geometry.FindCollisionFeatures((ColliderBox)Colliders[i], (ColliderBox)Colliders[j]);
if (!features.colliding)
{
//Debug.Log("No collision happened.");
}
else
{
this.CollisionResolutionOBB(features, (ColliderBox)Colliders[i], (ColliderBox)Colliders[j]);
//Debug.Log("COLLISION: collision happened.");
}
}
// SPHERE vs SPHERE
else if (Colliders[i] as SphereCollider != null && Colliders[j] as SphereCollider != null)
{
if (AreSpheresColliding((SphereCollider)Colliders[i], (SphereCollider)Colliders[j]))
{
Logger.Instance.DebugInfo("Collision happened [Sphere vs Sphere]: " +
Colliders[i].Id + " - " +
Colliders[j].Id + " !",
"COLLISION_MANAGER");
}
}
// OBB vs SPHERE
else
{
ColliderBox b = Colliders[i] as ColliderBox;
SphereCollider s = Colliders[j] as SphereCollider;
if (b == null)
{
b = Colliders[j] as ColliderBox;
s = Colliders[i] as SphereCollider;
}
if (b != null && s != null)
{
if (AreSphereOBBColliding(b, s))
{
Logger.Instance.DebugInfo("Collision happened [OBB vs Sphere]: " +
Colliders[i].Id + " - " +
Colliders[j].Id + " !",
"COLLISION_MANAGER");
}
}
}
}
}
}
public static CollisionManifold FindCollisionFeatures(ColliderBox obb1, ColliderBox obb2)
{
CollisionManifold result = new CollisionManifold();
//CollisionManifold.Reset(result);
result.Reset();
// First, make a quick collision test on spheres within the OBBs, if they don't collide => OBBs dont collide
Sphere s1 = new Sphere(new Point(obb1._center), Vector3.Magnitude(obb1._xyzLength / 2) + 0.1f);
Sphere s2 = new Sphere(new Point(obb2._center), Vector3.Magnitude(obb2._xyzLength / 2) + 0.1f);
if (!SphereSphere(s1, s2))
{
return(result);
}
Cube c1 = obb1.cube;
Cube c2 = obb2.cube;
List <Vector3> axis = new List <Vector3>();
// Get axis from first cube
Vector3 c1axis1 = (c1.vertices[(int)CubeIdx.B] - c1.vertices[(int)CubeIdx.A]).normalized;
Vector3 c1axis2 = (c1.vertices[(int)CubeIdx.D] - c1.vertices[(int)CubeIdx.A]).normalized;
Vector3 c1axis3 = (c1.vertices[(int)CubeIdx.E] - c1.vertices[(int)CubeIdx.A]).normalized;
axis.Add(c1axis1);
axis.Add(c1axis2);
axis.Add(c1axis3);
// Get axis from second cube
Vector3 c2axis1 = (c2.vertices[(int)CubeIdx.B] - c2.vertices[(int)CubeIdx.A]).normalized;
Vector3 c2axis2 = (c2.vertices[(int)CubeIdx.D] - c2.vertices[(int)CubeIdx.A]).normalized;
Vector3 c2axis3 = (c2.vertices[(int)CubeIdx.E] - c2.vertices[(int)CubeIdx.A]).normalized;
axis.Add(c2axis1);
axis.Add(c2axis2);
axis.Add(c2axis3);
// Check 9 axis given by cross product between 2 cubes
for (int i = 0; i < 3; ++i)
{
axis.Add(Vector3.Cross(axis[i], axis[3]));
axis.Add(Vector3.Cross(axis[i], axis[4]));
axis.Add(Vector3.Cross(axis[i], axis[5]));
}
Vector3[] test = axis.ToArray();
Vector3 hitNormal = new Vector3(0.0f, 0.0f, 0.0f);
bool shouldFlip = false;
for (int i = 0; i < test.Length; ++i) // axis.Count = 15
{
if (axis[i].x < 0.000001f)
{
test[i].x = 0.0f;
}
if (test[i].y < 0.000001f)
{
test[i].y = 0.0f;
}
if (test[i].z < 0.000001f)
{
test[i].z = 0.0f;
}
if (Vector3.Magnitude(test[i]) * Vector3.Magnitude(test[i]) < 0.001f)
{
continue;
}
Tuple <float, bool> pntrtion_info = PenetrationDepth(obb1, obb2, test[i], shouldFlip);
float depth = pntrtion_info.Item1;
shouldFlip = pntrtion_info.Item2;
//Debug.Log("shouldFlip" + shouldFlip);
//if (depth < 0.0f) return result; // if penetration depth < 0.0f
if (depth <= 0.001f)
{
Debug.Log("DEPTH: " + depth);
return(result);
}
else if (depth < result.depth)
{
if (shouldFlip)
{
test[i] = test[i] * -1.0f;
}
result.depth = depth;
hitNormal = test[i];
//Debug.Log("hitNormal" + hitNormal);
}
}
//Debug.Log("PenetrationDepth: " + result.depth);
//Debug.Log("shouldFlip: " + shouldFlip);
//Debug.Log("COLLIDING2: " + result.colliding);
//if (Util.CMP(hitNormal.magnitude, 0.0f))// return result; //if (hitNormal.magnitude <= 0.1f)
//{
// Debug.Log("RETURN hitNormal.magnitude: " + hitNormal.magnitude);
// return result;
//}
Vector3 axs = hitNormal.normalized;
List <Point> list1 = ClipEdgesToOBB(GetEdges(obb2), obb1);
List <Point> list2 = ClipEdgesToOBB(GetEdges(obb1), obb2);
result.contacts = new List <Vector3>();
result.depths = new List <Vector3>();
foreach (Point point in list1)
{
result.contacts.Add(point.p);
result.depths.Add(point.p);
}
foreach (Point point in list2)
{
result.contacts.Add(point.p);
result.depths.Add(point.p);
}
Interval interval = GetInterval(obb1, axs);
float depth_distance_points = (interval.max - interval.min) * 0.5f; // depth points after intersection inside obb
float contact_points = (interval.max - interval.min) * 0.5f - result.depth; // points of intersection at the surface
Vector3 pointOnPlane1 = obb1._center + axs * depth_distance_points;
Vector3 pointOnPlane2 = obb1._center + axs * contact_points;
for (int i = result.contacts.Count - 1; i >= 0; --i)
{
Vector3 contact = result.contacts[i];
result.contacts[i] = contact + (axs * Vector3.Dot(axs, pointOnPlane2 - contact));
result.depths[i] = contact + (axs * Vector3.Dot(axs, pointOnPlane1 - contact));
//result.depths.Add(contact + (axs * Vector3.Dot(axs, pointOnPlane1 - contact)));
// This bit is in the "There is more" section of the book
for (int j = result.contacts.Count - 1; j > i; --j)
{
float magnitude = Vector3.Magnitude(result.contacts[j] - result.contacts[i]);
if (magnitude * magnitude < 0.0001f)
{
result.contacts.RemoveAt(j);
result.depths.RemoveAt(j);
break;
}
}
}
foreach (Vector3 cp in result.contacts)
{
result.avg_contact += cp;
}
result.avg_contact /= result.contacts.Count;
//result.avg_depth = result.avg_contact + result.depth * result.normal;
foreach (Vector3 dp in result.depths)
{
result.avg_depth += dp;
}
result.avg_depth /= result.depths.Count;
result.colliding = true;
result.normal = axs;
return(result);
}
private bool IsTouchingAABBAABB(IGameObject parent, IGameObject s, out CollisionManifold manifold)
{
manifold = new CollisionManifold();
Vector2 halfA = new Vector2(parent.Rectangle.Width / 2.0f, parent.Rectangle.Height / 2.0f);
Vector2 halfB = new Vector2(s.Rectangle.Width / 2.0f, s.Rectangle.Height / 2.0f);
//find the centre of each sprite's hit area (using .Center() will give a loss of precision)
Vector2 centreA = new Vector2(parent.transform.position.X + parent.Rectangle.X + halfA.X, parent.transform.position.Y + parent.Rectangle.Y + halfA.Y);
Vector2 centreB = new Vector2(s.transform.position.X + s.Rectangle.X + halfB.X, s.transform.position.Y + s.Rectangle.Y + halfB.Y);
//Not collided
if (parent.Rectangle.Right < s.Rectangle.Left || parent.Rectangle.Left > s.Rectangle.Right)
{
return(false);
}
if (parent.Rectangle.Bottom < s.Rectangle.Top || parent.Rectangle.Top > s.Rectangle.Bottom)
{
return(false);
}
// AABBs have collided, now compute manifold
manifold.Collidee = s;
float distance = Vector2.Distance(centreA, centreB);
if (distance != 0)
{
// If distance between AABBs is not zero
// Distance is difference between radius and distance
float xOverlapAB = (halfA.X + halfB.X) - (centreA.X - centreB.X);
float xOverlapBA = (halfA.X + halfB.X) - (centreB.X - centreA.X);
float yOverlapAB = (halfA.Y + halfB.Y) - (centreA.Y - centreB.Y);
float yOverlapBA = (halfA.Y + halfB.Y) - (centreB.Y - centreA.Y);
manifold.Penetration = Math.Min(xOverlapAB, Math.Min(xOverlapBA, Math.Min(yOverlapAB, yOverlapBA)));
//calculate normal vector
if (manifold.Penetration == xOverlapAB)
{
//Left of A
manifold.Normal = new Vector2(-1, 0);
}
else if (manifold.Penetration == xOverlapBA)
{
//Right of A
manifold.Normal = new Vector2(1, 0);
}
else if (manifold.Penetration == yOverlapAB)
{
//Top of A
manifold.Normal = new Vector2(0, -1);
}
else if (manifold.Penetration == yOverlapBA)
{
//Bottom of A
manifold.Normal = new Vector2(0, 1);
}
}
else
{
// If distance between circles is zero
manifold.Normal = Vector2.UnitX;
// Distance is difference between radius and distance
manifold.Penetration = (float)Math.Min(parent.Rectangle.Width / 2.0, s.Rectangle.Width / 2.0);
}
return(true);
}
private bool IsTouchingCircleAABB(IGameObject parent, IGameObject s, out CollisionManifold manifold)
{
manifold = new CollisionManifold();
Vector2 halfA = new Vector2(parent.Rectangle.Width / 2.0f, parent.Rectangle.Height / 2.0f);
Vector2 halfB = new Vector2(s.Rectangle.Width / 2.0f, s.Rectangle.Height / 2.0f);
//find the centre of each sprite's hit area (using .Center() will give a loss of precision)
Vector2 centreA = new Vector2(parent.transform.position.X + parent.Rectangle.X + parent.Rectangle.Width / 2, parent.transform.position.Y + parent.Rectangle.Y + parent.Rectangle.Height / 2);
Vector2 centreB = new Vector2(s.transform.position.X + s.Rectangle.X + s.Rectangle.Width / 2, s.transform.position.Y + s.Rectangle.Y + s.Rectangle.Height / 2);
// Vector from B to A
Vector2 n = centreA - centreB;
// Closest point on B to center of A
Vector2 closest = n;
// Clamp point to edges of the AABB
closest.X = MathHelper.Clamp(closest.X, -halfB.X, halfB.X);
closest.Y = MathHelper.Clamp(closest.Y, -halfB.Y, halfB.Y);
bool inside = false;
// Circle is inside the AABB, so we need to clamp the circle's center
// to the closest edge
if (n == closest)
{
inside = true;
// Find closest axis
if (Math.Abs(n.X) > Math.Abs(n.Y))
{
// Clamp to closest extent
closest.X = closest.X > 0 ? halfB.X : -halfB.X;
}
// y axis is shorter
else
{
// Clamp to closest extent
closest.Y = closest.Y > 0 ? halfB.Y : -halfB.Y;
}
}
Vector2 normal = n - closest;
float d = normal.LengthSquared();
float r = parent.Rectangle.Width < parent.Rectangle.Height ? parent.Rectangle.Width / 2.0f : parent.Rectangle.Height / 2.0f;
// Early out of the radius is shorter than distance to closest point and
// Circle not inside the AABB
if (d > r * r && !inside)
{
return(false);
}
// AABBs have collided, now compute manifold
manifold.Collidee = s;
// Avoided sqrt until we needed
d = (float)Math.Sqrt(d);
// Collision normal needs to be flipped to point outside if circle was
// inside the AABB
if (inside)
{
manifold.Normal = Vector2.Normalize(normal);
manifold.Penetration = r - d;
}
else
{
manifold.Normal = -Vector2.Normalize(normal);
manifold.Penetration = r - d;
}
return(true);
}
