/// <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;
        }
        private void UpdateContacts(ArbiterMap selectedArbiterMap)
        {
            foreach (Arbiter arbiter in selectedArbiterMap.Arbiters)
            {
                UpdateArbiterContacts(arbiter);
            }

            while (removedArbiterStack.Count > 0)
            {
                Arbiter arbiter = removedArbiterStack.Pop();
                Arbiter.Pool.GiveBack(arbiter);
                selectedArbiterMap.Remove(arbiter);

                if (selectedArbiterMap == arbiterMap)
                {
                    removedArbiterQueue.Enqueue(arbiter);
                    events.RaiseBodiesEndCollide(arbiter.body1, arbiter.body2);

                    cacheOverPairContact.SetBodies(arbiter.body1, arbiter.body2);
                    initialCollisions.Remove(cacheOverPairContact);
                }
                else
                {
                    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);
                }
            }
        }
        private void CollisionDetected(RigidBody body1, RigidBody body2, FPVector point1, FPVector point2, FPVector 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)
            {
                FPVector.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);
            }
        }