/// <summary>
/// Raycasts a single body. NOTE: For performance reasons terrain and trianglemeshshape aren't checked
/// against rays (rays are of infinite length). They are checked against segments
/// which start at rayOrigin and end in rayOrigin + rayDirection.
/// </summary>
#region public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out FP fraction)
public override bool Raycast(RigidBody body, TSVector rayOrigin, TSVector rayDirection, out TSVector normal, out FP fraction)
{
fraction = FP.MaxValue; normal = TSVector.zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
TSVector tempNormal; FP tempFraction;
bool multiShapeCollides = false;
TSVector transformedOrigin; TSVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
TSVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
TSVector transformedDirection; TSVector.Transform(ref rayDirection, ref body.invOrientation, out transformedDirection);
int msLength = ms.Prepare(ref transformedOrigin, ref transformedDirection);
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (GJKCollide.Raycast(ms, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out tempFraction, out tempNormal))
{
if (tempFraction < fraction)
{
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out tempNormal);
TSVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
TSVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
}
ms.ReturnWorkingClone();
return(multiShapeCollides);
}
else
{
return(GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out fraction, out normal));
}
}
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();
}
}
