private void DetectSoftSoft(SoftBody body1, SoftBody body2)
{
ResourcePoolItemList <int> my = potentialTriangleLists.GetNew();
ResourcePoolItemList <int> other = potentialTriangleLists.GetNew();
body1.dynamicTree.Query(other, my, body2.dynamicTree);
for (int i = 0; i < other.Count; i++)
{
SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]);
SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]);
TSVector point, normal;
FP penetration;
bool result;
result = XenoCollide.Detect(myTriangle, otherTriangle, ref TSMatrix.InternalIdentity, ref TSMatrix.InternalIdentity,
ref TSVector.InternalZero, ref TSVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point);
int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point);
RaiseCollisionDetected(body1.VertexBodies[minIndexMy],
body2.VertexBodies[minIndexOther], ref point, ref point, ref normal, penetration);
}
}
my.Clear();
other.Clear();
potentialTriangleLists.GiveBack(my);
potentialTriangleLists.GiveBack(other);
}
public virtual void DoSelfCollision(CollisionDetectedHandler collision)
{
if (!selfCollision)
{
return;
}
TSVector point, normal;
FP penetration;
for (int i = 0; i < points.Count; i++)
{
queryList.Clear();
this.dynamicTree.Query(queryList, ref points[i].boundingBox);
for (int e = 0; e < queryList.Count; e++)
{
Triangle t = this.dynamicTree.GetUserData(queryList[e]);
if (!(t.VertexBody1 == points[i] || t.VertexBody2 == points[i] || t.VertexBody3 == points[i]))
{
if (XenoCollide.Detect(points[i].Shape, t, points[i].orientation,
TSMatrix.InternalIdentity, points[i].position, TSVector.InternalZero,
out point, out normal, out penetration))
{
int nearest = CollisionSystem.FindNearestTrianglePoint(this, queryList[e], ref point);
collision(points[i], points[nearest], point, point, normal, penetration);
}
}
}
}
}
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody)
{
if (rigidBody.Shape is Multishape)
{
Multishape ms = (rigidBody.Shape as Multishape);
ms = ms.RequestWorkingClone();
TSBBox transformedBoundingBox = softBody.BoundingBox;
transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);
int msLength = ms.Prepare(ref transformedBoundingBox);
ResourcePoolItemList <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
TSVector point, normal;
FP penetration;
bool result;
for (int e = 0; e < msLength; e++)
{
ms.SetCurrentShape(e);
result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity,
ref rigidBody.position, ref TSVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
RaiseCollisionDetected(rigidBody,
softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
}
}
}
detected.Clear(); potentialTriangleLists.GiveBack(detected);
ms.ReturnWorkingClone();
}
else
{
ResourcePoolItemList <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
TSVector point, normal;
FP penetration;
bool result;
result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity,
ref rigidBody.position, ref TSVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
RaiseCollisionDetected(rigidBody,
softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
}
}
detected.Clear();
potentialTriangleLists.GiveBack(detected);
}
}
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();
}
}
