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