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);
}
/// <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 void CheckDeactivation()
{
if (!AllowDeactivation)
{
return;
}
// A body deactivation DOESN'T kill the contacts - they are stored in
// the arbitermap within the arbiters. So, waking up ist STABLE - old
// contacts are reused. Also the collisionislands build every frame (based
// on the contacts) keep the same.
foreach (CollisionIsland island in islands)
{
bool deactivateIsland = true;
// global allowdeactivation
if (!this.AllowDeactivation)
{
deactivateIsland = false;
}
else
{
for (int index = 0, length = island.bodies.Count; index < length; index++)
{
RigidBody body = island.bodies[index];
// body allowdeactivation
if (body.AllowDeactivation && (body.angularVelocity.sqrMagnitude < inactiveAngularThresholdSq &&
(body.linearVelocity.sqrMagnitude < inactiveLinearThresholdSq)))
{
body.inactiveTime += timestep;
if (body.inactiveTime < deactivationTime)
{
deactivateIsland = false;
}
}
else
{
body.inactiveTime = FP.Zero;
deactivateIsland = false;
}
}
}
for (int index = 0, length = island.bodies.Count; index < length; index++)
{
RigidBody body = island.bodies[index];
if (body.isActive == deactivateIsland)
{
if (body.isActive)
{
body.IsActive = false;
events.RaiseDeactivatedBody(body);
}
else
{
body.IsActive = true;
events.RaiseActivatedBody(body);
}
}
}
}
}
/// <summary>
/// Removes a <see cref="RigidBody"/> from the world.
/// </summary>
/// <param name="body">The body which should be removed.</param>
/// <returns>Returns false if the body could not be removed from the world.</returns>
public bool RemoveBody(RigidBody body)
{
return(RemoveBody(body, false));
}
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public Spring(RigidBody body1, RigidBody body2)
: base(body1, body2)
{
distance = (body1.position - body2.position).magnitude;
}
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
{
bool anyBodyColliderOnly = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
ArbiterMap selectedArbiterMap = null;
if (anyBodyColliderOnly)
{
selectedArbiterMap = arbiterTriggerMap;
}
else
{
selectedArbiterMap = arbiterMap;
}
bool arbiterCreated = false;
lock (selectedArbiterMap) {
selectedArbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1; arbiter.body2 = body2;
selectedArbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
arbiterCreated = true;
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
TSVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (arbiterCreated)
{
if (anyBodyColliderOnly)
{
/*if (body1.isColliderOnly) {
* events.RaiseTriggerBeginCollide(body1, body2);
* } else {
* events.RaiseTriggerBeginCollide(body2, body1);
* }*/
events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialTriggers.Add(overlapContact);
}
else
{
events.RaiseBodiesBeginCollide(contact);
addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialCollisions.Add(overlapContact);
}
}
if (!anyBodyColliderOnly && contact != null)
{
events.RaiseContactCreated(contact);
}
}
/// <summary>
/// Sends a ray (definied by start and direction) through the scene (all bodies added).
/// NOTE: For performance reasons terrain and trianglemeshshape aren't checked
/// against rays (rays are of infinite length). They are checked against segments
/// which start at rayOrigin and end in rayOrigin + rayDirection.
/// </summary>
public override bool Raycast(TSVector rayOrigin, TSVector rayDirection, RaycastCallback raycast, int layerMask, out RigidBody body, out TSVector normal, out FP fraction)
{
body = null; normal = TSVector.zero; fraction = FP.MaxValue;
TSVector tempNormal; FP tempFraction;
bool result = false;
// TODO: This can be done better in CollisionSystemPersistenSAP
foreach (IBroadphaseEntity e in bodyList)
{
if (e is SoftBody)
{
SoftBody softBody = e as SoftBody;
foreach (RigidBody b in softBody.VertexBodies)
{
int bodyLayerMask = 1 << PhysicsManager.instance.GetBodyLayer(b);
if ((layerMask & bodyLayerMask) != bodyLayerMask)
{
continue;
}
if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction))
{
if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction)))
{
body = b;
normal = tempNormal;
fraction = tempFraction;
result = true;
}
}
}
}
else
{
RigidBody b = e as RigidBody;
int bodyLayerMask = 1 << PhysicsManager.instance.GetBodyLayer(b);
if ((layerMask & bodyLayerMask) != bodyLayerMask)
{
continue;
}
if (this.Raycast(b, rayOrigin, rayDirection, out tempNormal, out tempFraction))
{
if (tempFraction < fraction && (raycast == null || raycast(b, tempNormal, tempFraction)))
{
body = b;
normal = tempNormal;
fraction = tempFraction;
result = true;
}
}
}
}
return(result);
}
/// <summary> /// Raycasts a single body. NOTE: For performance reasons terrain and trianglemeshshape aren't checked /// against rays (rays are of infinite length). They are checked against segments /// which start at rayOrigin and end in rayOrigin + rayDirection. /// </summary> public abstract bool Raycast(RigidBody body, TSVector rayOrigin, TSVector rayDirection, out TSVector normal, out FP fraction);
public abstract bool Raycast(TSVector rayOrigin, TSVector rayDirection, RaycastCallback raycast, int layerMask, out RigidBody body, out TSVector normal, out FP fraction);
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);
}
}
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();
}
}
