///<summary>
        /// Removes a contact from the constraint.
        ///</summary>
        ///<param name="contact">Contact to remove.</param>
        public override void RemoveContact(Contact contact)
        {
            ContactPenetrationConstraint penetrationConstraint = null;

            for (int i = 0; i < penetrationConstraints.Count; i++)
            {
                if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact)
                {
                    penetrationConstraint.CleanUp();
                    penetrationConstraints.RemoveAt(i);
                    penetrationConstraintPool.Push(penetrationConstraint);
                    break;
                }
            }

            for (int i = frictionConstraints.Count - 1; i >= 0; i--)
            {
                ContactFrictionConstraint frictionConstraint = frictionConstraints[i];
                if (frictionConstraint.PenetrationConstraint == penetrationConstraint)
                {
                    frictionConstraint.CleanUp();
                    frictionConstraints.RemoveAt(i);
                    frictionConstraintPool.Push(frictionConstraint);
                    break;
                }
            }
        }
        //TODO: PROBLEM IS that the add contact/remove contact, when they go from 0 -> !0 or !0 -> 0, the whole constraint is added/removed from the solver.
        //The Added/Removed contact methods here will run ambiguously before or after they are removed from the solver.
        //That ambiguous order doesn't really matter though, since everything that these add/remove methods do is local to this solver object and its children.
        //It doesn't go out and modify any external values on referenced entities.  That only happens when it's added or removed from the solver by whatever owns this object!

        //To avoid ANY ambiguity, some third party is now responsible for adding and removing contacts from this.

        ///<summary>
        /// Adds a contact to be managed by the constraint.
        ///</summary>
        ///<param name="contact">Contact to add.</param>
        public override void AddContact(Contact contact)
        {
            contact.Validate();
            ContactPenetrationConstraint penetrationConstraint = penetrationConstraintPool.Pop();

            penetrationConstraint.Setup(this, contact);
            penetrationConstraints.Add(penetrationConstraint);

            ContactFrictionConstraint frictionConstraint = frictionConstraintPool.Pop();

            frictionConstraint.Setup(this, penetrationConstraint);
            frictionConstraints.Add(frictionConstraint);
        }
        ///<summary>
        /// Configures the friction constraint for a new contact.
        ///</summary>
        ///<param name="contactManifoldConstraint">Manifold to which the constraint belongs.</param>
        ///<param name="penetrationConstraint">Penetration constraint associated with this friction constraint.</param>
        public void Setup(ContactManifoldConstraint contactManifoldConstraint,
                          ContactPenetrationConstraint penetrationConstraint)
        {
            ContactManifoldConstraint = contactManifoldConstraint;
            PenetrationConstraint     = penetrationConstraint;
            IsActive = true;
            linearAX = 0;
            linearAY = 0;
            linearAZ = 0;

            entityA = contactManifoldConstraint.EntityA;
            entityB = contactManifoldConstraint.EntityB;
        }
        ///<summary>
        /// Adds a contact to be managed by the constraint.
        ///</summary>
        ///<param name="contact">Contact to add.</param>
        public override void AddContact(Contact contact)
        {
            contact.Validate();
            ContactPenetrationConstraint penetrationConstraint = penetrationConstraintPool.Pop();

            penetrationConstraint.Setup(this, contact);
            penetrationConstraints.Add(penetrationConstraint);
            if (!twistFriction.isActive)
            {
                //This is the first real contact.  All constraints need to become active.
                twistFriction.Setup(this);
                slidingFriction.Setup(this);
            }
        }
        ///<summary>
        /// Cleans up the constraint.
        ///</summary>
        public override void CleanUp()
        {
            base.CleanUp();
            //Deactivate any remaining constraints.
            for (int i = penetrationConstraints.Count - 1; i >= 0; i--)
            {
                ContactPenetrationConstraint penetrationConstraint = penetrationConstraints.Elements[i];
                penetrationConstraint.CleanUp();
                penetrationConstraints.RemoveAt(i);
                penetrationConstraintPool.Push(penetrationConstraint);
            }

            if (twistFriction.isActive)
            {
                twistFriction.CleanUp();
                slidingFriction.CleanUp();
            }
        }
        ///<summary>
        /// Cleans up the constraint.
        ///</summary>
        public override void CleanUp()
        {
            base.CleanUp();
            //Deactivate any remaining constraints.
            for (int i = penetrationConstraints.Count - 1; i >= 0; i--)
            {
                ContactPenetrationConstraint penetrationConstraint = penetrationConstraints.Elements[i];
                penetrationConstraint.CleanUp();
                penetrationConstraints.RemoveAt(i);
                penetrationConstraintPool.Push(penetrationConstraint);
            }

            for (int i = frictionConstraints.Count - 1; i >= 0; i--)
            {
                ContactFrictionConstraint frictionConstraint = frictionConstraints.Elements[i];
                frictionConstraint.CleanUp();
                frictionConstraints.RemoveAt(i);
                frictionConstraintPool.Push(frictionConstraint);
            }
        }
        ///<summary>
        /// Constructs a new nonconvex manifold constraint.
        ///</summary>
        public NonConvexContactManifoldConstraint(CollidablePairHandler pairHandler)
            : base(pairHandler)
        {
            //All of the constraints are always in the solver group.  Some of them are just deactivated sometimes.
            //This reduces some bookkeeping complications.

            penetrationConstraints = new RawList <ContactPenetrationConstraint>(4);
            frictionConstraints    = new RawList <ContactFrictionConstraint>(4);

            for (int i = 0; i < 4; i++)
            {
                ContactPenetrationConstraint penetrationConstraint = new ContactPenetrationConstraint();
                penetrationConstraintPool.Push(penetrationConstraint);
                Add(penetrationConstraint);

                ContactFrictionConstraint frictionConstraint = new ContactFrictionConstraint();
                frictionConstraintPool.Push(frictionConstraint);
                Add(frictionConstraint);
            }
        }
        ///<summary>
        /// Constructs a new convex contact manifold constraint.
        ///</summary>
        public ConvexContactManifoldConstraint(CollidablePairHandler pairHandler)
            : base(pairHandler)
        {
            //All of the constraints are always in the solver group.  Some of them are just deactivated sometimes.
            //This reduces some bookkeeping complications.


            penetrationConstraints = new RawList <ContactPenetrationConstraint>(4);


            //Order matters in this adding process.  Sliding friction computes some information used by the twist friction, and both use penetration impulses.
            for (int i = 0; i < 4; i++)
            {
                ContactPenetrationConstraint penetrationConstraint = new ContactPenetrationConstraint();
                Add(penetrationConstraint);
                penetrationConstraintPool.Push(penetrationConstraint);
            }

            slidingFriction = new SlidingFrictionTwoAxis();
            Add(slidingFriction);
            twistFriction = new TwistFrictionConstraint();
            Add(twistFriction);
        }