/*
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody)
{
if (rigidBody.Shape is Multishape)
{
Multishape ms = (rigidBody.Shape as Multishape);
ms = ms.RequestWorkingClone();
JBBox 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);
JVector point, normal;
float penetration;
bool result;
for (int e = 0; e < msLength; e++)
{
ms.SetCurrentShape(e);
result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity,
ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
if (this.RaisePassedNarrowphase(rigidBody, softBody.points[minIndex],
ref point, ref normal, penetration))
{
RaiseCollisionDetected(rigidBody,
softBody.points[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);
JVector point, normal;
float penetration;
bool result;
result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity,
ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
if (this.RaisePassedNarrowphase(rigidBody, softBody.points[minIndex],
ref point, ref normal, penetration))
{
RaiseCollisionDetected(rigidBody,
softBody.points[minIndex], ref point, ref point, ref normal, penetration);
}
}
}
detected.Clear();
potentialTriangleLists.GiveBack(detected);
}
}
public static int FindNearestTrianglePoint(SoftBody sb, int id, ref JVector point)
{
SoftBody.Triangle triangle = sb.dynamicTree.GetUserData(id);
JVector p;
p = sb.points[triangle.indices.I0].position;
JVector.Subtract(ref p, ref point, out p);
float length0 = p.LengthSquared();
p = sb.points[triangle.indices.I1].position;
JVector.Subtract(ref p, ref point, out p);
float length1 = p.LengthSquared();
p = sb.points[triangle.indices.I2].position;
JVector.Subtract(ref p, ref point, out p);
float length2 = p.LengthSquared();
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;
}
}
* */
public static void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref JVector point, ref JVector normal,
out JVector point1, out JVector point2)
{
JVector mn; JVector.Negate(ref normal, out mn);
JVector sA; SupportMapping(body1, shape1, ref mn, out sA);
JVector sB; SupportMapping(body2, shape2, ref normal, out sB);
JVector.Subtract(ref sA, ref point, out sA);
JVector.Subtract(ref sB, ref point, out sB);
float dot1 = JVector.Dot(ref sA, ref normal);
float dot2 = JVector.Dot(ref sB, ref normal);
JVector.Multiply(ref normal, dot1, out sA);
JVector.Multiply(ref normal, dot2, out sB);
JVector.Add(ref point, ref sA, out point1);
JVector.Add(ref point, ref sB, out point2);
}
private static void SupportMapping(RigidBody body, Shape workingShape, ref JVector direction, out JVector result)
{
JMatrix xform = JMatrix.CreateRotationZ(-body.orientation);
JVector.Transform(ref direction, ref xform, out result);
workingShape.SupportMapping(ref result, out result);
xform = JMatrix.CreateRotationZ(body.orientation);
JVector.Transform(ref result, ref xform, out result);
JVector.Add(ref result, ref body.position, out result);
}
public static float CalculateMassInertia(Shape shape, out JVector centerOfMass,
out JMatrix inertia)
{
throw new NotImplementedException();
}
