private void DetectSoftSoft(SoftBody body1, SoftBody body2)
{
List <int> my = potentialTriangleLists.GetNew();
List <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]);
FPVector point, normal;
FP penetration;
bool result;
result = XenoCollide.Detect(myTriangle, otherTriangle, ref FPMatrix.InternalIdentity, ref FPMatrix.InternalIdentity,
ref FPVector.InternalZero, ref FPVector.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 static int FindNearestTrianglePoint(SoftBody sb, int id, ref FPVector point)
{
SoftBody.Triangle triangle = sb.dynamicTree.GetUserData(id);
FPVector p;
p = sb.VertexBodies[triangle.indices.I0].position;
FPVector.Subtract(ref p, ref point, out p);
FP length0 = p.sqrMagnitude;
p = sb.VertexBodies[triangle.indices.I1].position;
FPVector.Subtract(ref p, ref point, out p);
FP length1 = p.sqrMagnitude;
p = sb.VertexBodies[triangle.indices.I2].position;
FPVector.Subtract(ref p, ref point, out p);
FP length2 = p.sqrMagnitude;
if (length0 < length1)
{
if (length0 < length2)
{
return(triangle.indices.I0);
}
else
{
return(triangle.indices.I2);
}
}
else
{
if (length1 < length2)
{
return(triangle.indices.I1);
}
else
{
return(triangle.indices.I2);
}
}
}
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody)
{
if (rigidBody.Shape is Multishape)
{
Multishape ms = (rigidBody.Shape as Multishape);
ms = ms.RequestWorkingClone();
FPBBox transformedBoundingBox = softBody.BoundingBox;
transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);
int msLength = ms.Prepare(ref transformedBoundingBox);
List <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
FPVector 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 FPMatrix.InternalIdentity,
ref rigidBody.position, ref FPVector.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
{
List <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
FPVector point, normal;
FP penetration;
bool result;
result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref FPMatrix.InternalIdentity,
ref rigidBody.position, ref FPVector.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);
}
}