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);
List <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
{
List <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);
}
}
/// <summary>
/// Integrates the whole world a timestep further in time.
/// </summary>
/// <param name="timestep">The timestep in seconds.
/// It should be small as possible to keep the simulation stable.
/// The physics simulation shouldn't run slower than 60fps.
/// (timestep=1/60).</param>
public void Step(FP timestep)
{
this.timestep = timestep;
// yeah! nothing to do!
if (timestep == FP.Zero)
{
return;
}
// throw exception if the timestep is smaller zero.
if (timestep < FP.Zero)
{
throw new ArgumentException("The timestep can't be negative.", "timestep");
}
// Calculate this
//currentAngularDampFactor = (FP)Math.Pow((double)(float)angularDamping, (double)(float)timestep);
//currentLinearDampFactor = (FP)Math.Pow((double)(float)linearDamping, (double)(float)timestep);
#if (WINDOWS_PHONE)
events.RaiseWorldPreStep(timestep);
foreach (RigidBody body in rigidBodies)
{
body.PreStep(timestep);
}
UpdateContacts();
while (removedArbiterQueue.Count > 0)
{
islands.ArbiterRemoved(removedArbiterQueue.Dequeue());
}
foreach (SoftBody body in softbodies)
{
body.Update(timestep);
body.DoSelfCollision(collisionDetectionHandler);
}
CollisionSystem.Detect();
while (addedArbiterQueue.Count > 0)
{
islands.ArbiterCreated(addedArbiterQueue.Dequeue());
}
CheckDeactivation();
IntegrateForces();
HandleArbiter(contactIterations);
Integrate();
foreach (RigidBody body in rigidBodies)
{
body.PostStep(timestep);
}
events.RaiseWorldPostStep(timestep);
#else
events.RaiseWorldPreStep(timestep);
UpdateContacts();
for (int index = 0, length = initialCollisions.Count; index < length; index++)
{
OverlapPairContact op = initialCollisions[index];
events.RaiseBodiesStayCollide(op.contact);
}
for (int index = 0, length = initialTriggers.Count; index < length; index++)
{
OverlapPairContact op = initialTriggers[index];
events.RaiseTriggerStayCollide(op.contact);
}
while (removedArbiterQueue.Count > 0)
{
islands.ArbiterRemoved(removedArbiterQueue.Dequeue());
}
for (int index = 0, length = softbodies.Count; index < length; index++)
{
SoftBody body = softbodies[index];
body.Update(timestep);
body.DoSelfCollision(collisionDetectionHandler);
}
CollisionSystem.Detect();
while (addedArbiterQueue.Count > 0)
{
islands.ArbiterCreated(addedArbiterQueue.Dequeue());
}
CheckDeactivation();
IntegrateForces();
HandleArbiter(contactIterations);
Integrate();
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
body.PostStep();
for (int index2 = 0, length2 = body.constraints.Count; index2 < length2; index2++)
{
body.constraints[index2].PostStep();
}
}
events.RaiseWorldPostStep(timestep);
#endif
}