private void DetectRigidRigid(RigidBody body1, RigidBody body2) { bool b1IsMulti = (body1.Shape is Multishape); bool b2IsMulti = (body2.Shape is Multishape); bool speculative = speculativeContacts || (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts); TSVector point, normal; FP penetration; if (!b1IsMulti && !b2IsMulti) { TSVector point1, point2; ShapeType type = body1.shape.shapeType | body2.shape.shapeType; switch (type) { case ShapeType.Sphere | ShapeType.Sphere: SphereSpherePair sphereSpherePair = SphereSpherePair.pool.GetNew(); sphereSpherePair.Shape1 = body1.Shape; sphereSpherePair.Shape2 = body2.Shape; if (sphereSpherePair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out point1, out point2, out normal, out penetration)) { RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } SphereSpherePair.pool.GiveBack(sphereSpherePair); break; case ShapeType.Box | ShapeType.Sphere: BoxSpherePair boxSpherePair = BoxSpherePair.pool.GetNew(); RigidBody b1, b2; if (body1.Shape is BoxShape) { b1 = body1; b2 = body2; } else { b1 = body2; b2 = body1; } boxSpherePair.Shape1 = b1.Shape; boxSpherePair.Shape2 = b2.Shape; if (boxSpherePair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out point1, out point2, out normal, out penetration)) { RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } BoxSpherePair.pool.GiveBack(boxSpherePair); break; case ShapeType.Capusle | ShapeType.Capusle: CapsuleCapsulePair capsuleCapsulePair = CapsuleCapsulePair.pool.GetNew(); capsuleCapsulePair.Shape1 = body1.Shape; capsuleCapsulePair.Shape2 = body2.Shape; if (capsuleCapsulePair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out point1, out point2, out normal, out penetration)) { RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } CapsuleCapsulePair.pool.GiveBack(capsuleCapsulePair); break; case ShapeType.Capusle | ShapeType.Sphere: CapsuleSpherePair capsuleSpherePair = CapsuleSpherePair.pool.GetNew(); if (body1.Shape is CapsuleShape) { b1 = body1; b2 = body2; } else { b1 = body2; b2 = body1; } capsuleSpherePair.Shape1 = b1.Shape; capsuleSpherePair.Shape2 = b2.Shape; if (capsuleSpherePair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out point1, out point2, out normal, out penetration)) { RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } CapsuleSpherePair.pool.GiveBack(capsuleSpherePair); break; case ShapeType.Box | ShapeType.Box: //BoxBoxPair boxBoxPair = BoxBoxPair.pool.GetNew(); //boxBoxPair.Shape1 = body1.Shape; //boxBoxPair.Shape2 = body2.Shape; //if (boxBoxPair.IsColliding(ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, // out point, out point1, out point2, out normal, out penetration)) //{ // RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); //} //BoxBoxPair.pool.GiveBack(boxBoxPair); //break; default: if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { //normal = JVector.Up; //UnityEngine.Debug.Log("FINAL --- >>> normal: " + normal); FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } break; } //UnityEngine.Debug.Log("-----------------------: " + normal); } else if (b1IsMulti && b2IsMulti) { Multishape ms1 = (body1.Shape as Multishape); Multishape ms2 = (body2.Shape as Multishape); ms1 = ms1.RequestWorkingClone(); ms2 = ms2.RequestWorkingClone(); TSBBox transformedBoundingBox = body2.boundingBox; transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation); int ms1Length = ms1.Prepare(ref transformedBoundingBox); transformedBoundingBox = body1.boundingBox; transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation); int ms2Length = ms2.Prepare(ref transformedBoundingBox); if (ms1Length == 0 || ms2Length == 0) { ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); return; } for (int i = 0; i < ms1Length; i++) { ms1.SetCurrentShape(i); for (int e = 0; e < ms2Length; e++) { ms2.SetCurrentShape(e); if (XenoCollide.Detect(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { TSVector point1, point2; FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } } ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); } else { RigidBody b1, b2; if (body2.Shape is Multishape) { b1 = body2; b2 = body1; } else { b2 = body2; b1 = body1; } Multishape ms = (b1.Shape as Multishape); ms = ms.RequestWorkingClone(); TSBBox transformedBoundingBox = b2.boundingBox; transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); if (msLength == 0) { ms.ReturnWorkingClone(); return; } ShapeType type = body1.shape.shapeType | body2.shape.shapeType; switch (type) { case ShapeType.Sphere | ShapeType.TriangleMesh: for (int i = 0; i < msLength; i++) { ms.SetCurrentShape(i); TSVector point1, point2; SphereTrianglePair pair = SphereTrianglePair.pool.GetNew(); pair.Shape1 = ms; pair.Shape2 = b2.shape; if (pair.IsColliding(ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out point1, out point2, out normal, out penetration)) { RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } SphereTrianglePair.pool.GiveBack(pair); } break; case ShapeType.Box | ShapeType.TriangleMesh: default: for (int i = 0; i < msLength; i++) { ms.SetCurrentShape(i); if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out normal, out penetration)) { TSVector point1, point2; FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2); if (useTerrainNormal && ms is TerrainShape) { (ms as TerrainShape).CollisionNormal(out normal); TSVector.Transform(ref normal, ref b1.orientation, out normal); } else if (useTriangleMeshNormal && ms is TriangleMeshShape) { (ms as TriangleMeshShape).CollisionNormal(out normal); TSVector.Transform(ref normal, ref b1.orientation, out normal); } RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration); } } } } } break; } ms.ReturnWorkingClone(); } }
private void DetectRigidRigid(RigidBody body1, RigidBody body2) { bool b1IsMulti = (body1.Shape is Multishape); bool b2IsMulti = (body2.Shape is Multishape); bool speculative = speculativeContacts || (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts); TSVector point, normal; FP penetration; if (!b1IsMulti && !b2IsMulti) { if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { //normal = JVector.Up; //UnityEngine.Debug.Log("FINAL --- >>> normal: " + normal); TSVector point1, point2; FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } //UnityEngine.Debug.Log("-----------------------: " + normal); } else if (b1IsMulti && b2IsMulti) { Multishape ms1 = (body1.Shape as Multishape); Multishape ms2 = (body2.Shape as Multishape); ms1 = ms1.RequestWorkingClone(); ms2 = ms2.RequestWorkingClone(); TSBBox transformedBoundingBox = body2.boundingBox; transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation); int ms1Length = ms1.Prepare(ref transformedBoundingBox); transformedBoundingBox = body1.boundingBox; transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation); int ms2Length = ms2.Prepare(ref transformedBoundingBox); if (ms1Length == 0 || ms2Length == 0) { ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); return; } for (int i = 0; i < ms1Length; i++) { ms1.SetCurrentShape(i); for (int e = 0; e < ms2Length; e++) { ms2.SetCurrentShape(e); if (XenoCollide.Detect(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { TSVector point1, point2; FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } } ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); } else { RigidBody b1, b2; if (body2.Shape is Multishape) { b1 = body2; b2 = body1; } else { b2 = body2; b1 = body1; } Multishape ms = (b1.Shape as Multishape); ms = ms.RequestWorkingClone(); TSBBox transformedBoundingBox = b2.boundingBox; transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); if (msLength == 0) { ms.ReturnWorkingClone(); return; } for (int i = 0; i < msLength; i++) { ms.SetCurrentShape(i); if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out normal, out penetration)) { TSVector point1, point2; FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2); if (useTerrainNormal && ms is TerrainShape) { (ms as TerrainShape).CollisionNormal(out normal); TSVector.Transform(ref normal, ref b1.orientation, out normal); } else if (useTriangleMeshNormal && ms is TriangleMeshShape) { (ms as TriangleMeshShape).CollisionNormal(out normal); TSVector.Transform(ref normal, ref b1.orientation, out normal); } RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector hit1, hit2; if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out hit1, out hit2, out normal)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration); } } } } } ms.ReturnWorkingClone(); } }