public virtual void DoSelfCollision(CollisionDetectedHandler collision)
{
if (!selfCollision)
{
return;
}
FPVector 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, ref points[i].orientation,
ref FPMatrix.InternalIdentity, ref points[i].position, ref FPVector.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);
}
}
}
}
}
/// <summary>
/// Create a new instance of the <see cref="World"/> class.
/// </summary>
/// <param name="collision">The collisionSystem which is used to detect
/// collisions. See for example: <see cref="CollisionSystemSAP"/>
/// or <see cref="CollisionSystemBrute"/>.
/// </param>
public World(CollisionSystem collision)
{
if (collision == null)
{
throw new ArgumentNullException("The CollisionSystem can't be null.", "collision");
}
RigidBody.instanceCount = 0;
Constraint.instanceCount = 0;
arbiterCallback = new Action <object>(ArbiterCallback);
integrateCallback = new Action <object>(IntegrateCallback);
// Create the readonly wrappers
this.RigidBodies = new ReadOnlyHashset <RigidBody>(rigidBodies);
this.Constraints = new ReadOnlyHashset <Constraint>(constraints);
this.SoftBodies = new ReadOnlyHashset <SoftBody>(softbodies);
this.CollisionSystem = collision;
collisionDetectionHandler = new CollisionDetectedHandler(CollisionDetected);
this.CollisionSystem.CollisionDetected += collisionDetectionHandler;
this.arbiterMap = new ArbiterMap();
this.arbiterTriggerMap = new ArbiterMap();
AllowDeactivation = false;
}
/// <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
}
private bool RemoveBody(RigidBody body, bool removeMassPoints)
{
// Its very important to clean up, after removing a body
if (!removeMassPoints && body.IsParticle)
{
return(false);
}
// remove the body from the world list
if (!rigidBodies.Remove(body))
{
return(false);
}
// Remove all connected constraints and arbiters
for (int index = 0, length = body.arbiters.Count; index < length; index++)
{
Arbiter arbiter = body.arbiters[index];
arbiterMap.Remove(arbiter);
events.RaiseBodiesEndCollide(arbiter.body1, arbiter.body2);
cacheOverPairContact.SetBodies(arbiter.body1, arbiter.body2);
initialCollisions.Remove(cacheOverPairContact);
}
for (int index = 0, length = body.arbitersTrigger.Count; index < length; index++)
{
Arbiter arbiter = body.arbitersTrigger[index];
arbiterTriggerMap.Remove(arbiter);
if (arbiter.body1.isColliderOnly)
{
events.RaiseTriggerEndCollide(arbiter.body1, arbiter.body2);
}
else
{
events.RaiseTriggerEndCollide(arbiter.body2, arbiter.body1);
}
cacheOverPairContact.SetBodies(arbiter.body1, arbiter.body2);
initialTriggers.Remove(cacheOverPairContact);
}
for (int index = 0, length = body.constraints.Count; index < length; index++)
{
Constraint constraint = body.constraints[index];
constraints.Remove(constraint);
events.RaiseRemovedConstraint(constraint);
}
// remove the body from the collision system
CollisionSystem.RemoveEntity(body);
// remove the body from the island manager
islands.RemoveBody(body);
events.RaiseRemovedRigidBody(body);
return(true);
}