/// <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, FPVector rayOrigin, FPVector rayDirection, out FPVector normal, out FP fraction)
{
fraction = FP.MaxValue; normal = FPVector.zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
FPVector tempNormal; FP tempFraction;
bool multiShapeCollides = false;
FPVector transformedOrigin; FPVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
FPVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
FPVector transformedDirection; FPVector.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);
FPVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
FPVector.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);
FPVector 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);
FPVector 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)
{
FPVector 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))
{
FPVector 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))
{
FPVector 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)
{
FPVector 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))
{
FPVector 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))
{
FPVector 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);
FPVector.Transform(ref normal, ref b1.orientation, out normal);
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out normal);
FPVector.Transform(ref normal, ref b1.orientation, out normal);
}
RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
}
else if (speculative)
{
FPVector 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))
{
FPVector 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();
}
}