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(); } }