///<summary>
        /// Updates the manifold.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //First, refresh all existing contacts.  This is an incremental manifold.
            var transform = MeshTransform;

            ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);

            RemoveQueuedContacts();


            CleanUpOverlappingTriangles();
            //Get all the overlapped triangle indices.
            int triangleCount = FindOverlappingTriangles(dt);

            Matrix3x3 orientation;

            Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
            var guaranteedContacts = 0;

            for (int i = 0; i < triangleCount; i++)
            {
                //Initialize the local triangle.
                TriangleIndices indices;
                if (ConfigureTriangle(i, out indices))
                {
                    //Find a pairtester for the triangle.
                    TrianglePairTester pairTester;
                    if (!activePairTesters.TryGetValue(indices, out pairTester))
                    {
                        pairTester = GetTester();
                        pairTester.Initialize(convex.Shape, localTriangleShape);
                        activePairTesters.Add(indices, pairTester);
                    }
                    pairTester.Updated = true;


                    //Put the triangle into the local space of the convex.
                    Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
                    Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
                    Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
                    Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);

                    //Now, generate a contact between the two shapes.
                    ContactData contact;
                    TinyStructList <ContactData> contactList;
                    if (pairTester.GenerateContactCandidate(out contactList))
                    {
                        for (int j = 0; j < contactList.Count; j++)
                        {
                            contactList.Get(j, out contact);


                            if (UseImprovedBoundaryHandling)
                            {
                                if (AnalyzeCandidate(ref indices, pairTester, ref contact))
                                {
                                    //This is let through if there's a face contact. Face contacts cannot be blocked.
                                    guaranteedContacts++;
                                    AddLocalContact(ref contact, ref orientation);
                                }
                            }
                            else
                            {
                                AddLocalContact(ref contact, ref orientation);
                            }
                        }
                    }

                    //Get the voronoi region from the contact candidate generation.  Possibly just recalculate, since most of the systems don't calculate it.
                    //Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region.  For face contacts, don't bother- just add it if unique.
                    //For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
                    //For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
                    //If the edge/vertex is already present in the set, then DO NOT add the contact.
                    //When adding a contact, add ALL other voronoi regions to the blocked sets.
                }
            }



            if (UseImprovedBoundaryHandling)
            {
                //If there were no face contacts that absolutely must be included, we may get into a very rare situation
                //where absolutely no contacts get created.  For example, a sphere falling directly on top of a vertex in a flat terrain.
                //It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices).
                //In some cases, the contacts will be ignored instead of corrected (e.g. spheres).
                //To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires.
                //Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts.
                //TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge,
                //and an edge only restricts the far vertex and other two edges.  This introduces an occasional bump though...

                //It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles.
                //For this state to continue beyond a brief instant generally requires the object be orientation locked and slender.
                //However, some characters fit this description, so it can't be ignored!

                //Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only).
                //However, an object stuck between multiple triangles is not in a stable state.  In the edge case, the object gets shoved to one side
                //as one contact 'wins' the solver war.  That's not enough to escape, unfortunately.
                //The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability.
                //So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :)

                //This isn't a completely free operation, but it's guarded behind pretty rare conditions.
                //Essentially, we will check to see if there's just edge contacts fighting against each other.
                //If they are, then we will correct any stuck-contributing normals to the triangle normal.
                if (vertexContacts.Count == 0 && guaranteedContacts == 0 && edgeContacts.Count > 1)
                {
                    //There are only edge contacts, check to see if:
                    //all normals are coplanar, and
                    //at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner).

                    bool allNormalsInSamePlane   = true;
                    bool atLeastOneNormalAgainst = false;

                    var firstNormal = edgeContacts.Elements[0].ContactData.Normal;
                    edgeContacts.Elements[0].CorrectedNormal.Normalize();
                    float dot;
                    Vector3.Dot(ref firstNormal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);
                    if (Math.Abs(dot) > .01f)
                    {
                        //Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity.
                        allNormalsInSamePlane = false;
                    }
                    else
                    {
                        //TODO: Note that we're only checking the new edge contacts, not the existing contacts.
                        //It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity
                        //we'll ignore that possibility for now.
                        for (int i = 1; i < edgeContacts.Count; i++)
                        {
                            Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot);
                            if (dot < 0)
                            {
                                atLeastOneNormalAgainst = true;
                            }
                            //Check to see if the normal is outside the plane.
                            Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref edgeContacts.Elements[0].CorrectedNormal, out dot);

                            if (Math.Abs(dot) > .01f)
                            {
                                //We are not stuck!
                                allNormalsInSamePlane = false;
                                break;
                            }
                        }
                    }

                    if (allNormalsInSamePlane && atLeastOneNormalAgainst)
                    {
                        //Uh oh! all the normals are parallel... The object is probably in a weird situation.
                        //Let's correct the normals!

                        //Already normalized the first contact above.
                        //We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already.
                        edgeContacts.Elements[0].ContactData.Normal = edgeContacts.Elements[0].CorrectedNormal;
                        edgeContacts.Elements[0].ShouldCorrect      = true;

                        for (int i = 1; i < edgeContacts.Count; i++)
                        {
                            //Must normalize the corrected normal before using it.
                            edgeContacts.Elements[i].CorrectedNormal.Normalize();
                            Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
                            if (dot < .01)
                            {
                                //Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness!
                                //If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal.
                                //Make them the same thing :)
                                edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal;
                                edgeContacts.Elements[i].ShouldCorrect      = true;
                                //Note that the penetration depth is NOT corrected.  The contact's depth no longer represents the true depth.
                                //However, we only need to have some penetration depth to get the object to escape the rut.
                                //Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction.
                                //If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system
                                //would have picked a different depth, as it finds a reasonable approximation of the minimum penetration!
                                //As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it).
                            }
                        }
                    }
                }



                for (int i = 0; i < edgeContacts.Count; i++)
                {
                    //Only correct if it's allowed AND it's blocked.
                    //If it's not blocked, the contact being created is necessary!
                    //The normal generated by the triangle-convex tester is already known not to
                    //violate the triangle sidedness.
                    if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
                    }
                    else if (edgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        edgeContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
                        edgeContacts.Elements[i].ContactData.Normal            = edgeContacts.Elements[i].CorrectedNormal;
                        edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.
                }



                for (int i = 0; i < vertexContacts.Count; i++)
                {
                    if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    else if (vertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        vertexContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot);
                        vertexContacts.Elements[i].ContactData.Normal            = vertexContacts.Elements[i].CorrectedNormal;
                        vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.
                }



                blockedEdgeRegions.Clear();
                blockedVertexRegions.Clear();
                vertexContacts.Clear();
                edgeContacts.Clear();
            }



            //Remove stale pair testers.
            //This will only remove 8 stale ones per frame, but it doesn't really matter.
            //VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame.
            var toRemove = new TinyList <TriangleIndices>();

            foreach (KeyValuePair <TriangleIndices, TrianglePairTester> pair in activePairTesters)
            {
                if (!pair.Value.Updated)
                {
                    if (!toRemove.Add(pair.Key))
                    {
                        break;
                    }
                }
                else
                {
                    pair.Value.Updated = false;
                }
            }



            for (int i = toRemove.Count - 1; i >= 0; i--)
            {
                var pairTester = activePairTesters[toRemove[i]];
                pairTester.CleanUp();
                GiveBackTester(pairTester);
                activePairTesters.Remove(toRemove[i]);
            }


            //Some child types will want to do some extra post processing on the manifold.
            ProcessCandidates(candidatesToAdd);


            //Check if adding the new contacts would overflow the manifold.
            if (contacts.Count + candidatesToAdd.Count > 4)
            {
                //Adding all the contacts would overflow the manifold.  Reduce to the best subset.
                ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates);
                RemoveQueuedContacts();
                for (int i = reducedCandidates.Count - 1; i >= 0; i--)
                {
                    Add(ref reducedCandidates.Elements[i]);
                    reducedCandidates.RemoveAt(i);
                }
            }
            else if (candidatesToAdd.Count > 0)
            {
                //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
                for (int i = 0; i < candidatesToAdd.Count; i++)
                {
                    Add(ref candidatesToAdd.Elements[i]);
                }
            }



            candidatesToAdd.Clear();
        }
        ///<summary>
        /// Updates the manifold.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //Now, generate a contact between the two shapes.
            float               distance;
            Vector3             axis;
            BoxContactDataCache manifold;

            if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
            {
                unsafe
                {
                    BoxContactData *manifoldPointer = &manifold.D1;
                    Vector3.Negate(ref axis, out axis);
                    var toRemove = new TinyList <int>();
                    for (int i = 0; i < contacts.Count; i++)
                    {
                        bool found = false;
                        for (int j = manifold.Count - 1; j >= 0; j--)
                        {
                            if (contacts.Elements[i].Id == manifoldPointer[j].Id)
                            {
                                found = true;
                                contacts.Elements[i].Validate();
                                //Update contact...
                                contacts.Elements[i].Position         = manifoldPointer[j].Position;
                                contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
                                contacts.Elements[i].Normal           = axis;
                                //Remove manifold entry
                                contacts.Elements[i].Validate();
                                manifold.RemoveAt(j);
                                break;
                            }
                        }
                        if (!found)
                        {//No match found
                            toRemove.Add(i);
                        }
                    }


                    //toRemove is sorted by increasing index.  Go backwards along it so that the indices are valid all the way through.
                    for (int i = toRemove.Count - 1; i >= 0; i--)
                    {
                        Remove(toRemove[i]);
                    }

                    //Add new contacts.
                    for (int i = 0; i < manifold.Count; i++)
                    {
                        var newContact = new ContactData
                        {
                            Position         = manifoldPointer[i].Position,
                            PenetrationDepth = -manifoldPointer[i].Depth,
                            Normal           = axis,
                            Id = manifoldPointer[i].Id
                        };

                        Add(ref newContact);
                    }
                }
            }
            else
            {
                //Not colliding, so get rid of it.
                for (int i = contacts.Count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
            }
        }
        public override void Update(float dt)
        {
            //Now, generate a contact between the two shapes.
            float   distance;
            Vector3 axis;
            var     manifold = new TinyStructList <BoxContactData>();

            if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
            {
                Vector3.Negate(ref axis, out axis);
                TinyList <int> toRemove = new TinyList <int>();
                BoxContactData data;
                for (int i = 0; i < contacts.Count; i++)
                {
                    bool found = false;
                    for (int j = manifold.Count - 1; j >= 0; j--)
                    {
                        manifold.Get(j, out data);
                        if (contacts.Elements[i].Id == data.Id)
                        {
                            found = true;
                            //Update contact...
                            contacts.Elements[i].Position         = data.Position;
                            contacts.Elements[i].PenetrationDepth = -data.Depth;
                            contacts.Elements[i].Normal           = axis;
                            contacts.Elements[i].Validate();
                            //Remove manifold entry
                            manifold.RemoveAt(j);
                            break;
                        }
                    }
                    if (!found)
                    {//No match found
                        toRemove.Add(i);
                    }
                }

                ////Go through the indices to remove.
                ////For each one, replace the removal index with a contact in the new manifold.
                //int removalIndex;
                //for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--)
                //{
                //    int indexToReplace = toRemove[removalIndex];
                //    toRemove.RemoveAt(removalIndex);
                //    manifold.Get(manifold.count - 1, out data);
                //    //Update contact...
                //    contacts.Elements[indexToReplace].Position = data.Position;
                //    contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth;
                //    contacts.Elements[indexToReplace].Normal = axis;
                //    contacts.Elements[indexToReplace].Id = data.Id;
                //    //Remove manifold entry
                //    manifold.RemoveAt(manifold.count - 1);

                //}

                //Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now).  Just remove the remainder.
                //toRemove is sorted by increasing index.  Go backwards along it so that the indices are valid all the way through.
                for (int i = toRemove.Count - 1; i >= 0; i--)
                {
                    Remove(toRemove[i]);
                }

                //Add new contacts.
                for (int i = 0; i < manifold.Count; i++)
                {
                    manifold.Get(i, out data);
                    ContactData newContact = new ContactData();
                    newContact.Position         = data.Position;
                    newContact.PenetrationDepth = -data.Depth;
                    newContact.Normal           = axis;
                    newContact.Id = data.Id;

                    Add(ref newContact);
                }
            }
            else
            {
                //Not colliding, so get rid of it.
                for (int i = contacts.Count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
            }
        }
        ///<summary>
        /// Updates the manifold.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {
            //First, refresh all existing contacts.  This is an incremental manifold.
            var transform = MeshTransform;

            ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);

            RemoveQueuedContacts();


            CleanUpOverlappingTriangles();
            //Get all the overlapped triangle indices.
            int triangleCount = FindOverlappingTriangles(dt);

            Matrix3X3 orientation;

            Matrix3X3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
            for (int i = 0; i < triangleCount; i++)
            {
                //Initialize the local triangle.
                TriangleIndices indices;
                if (ConfigureTriangle(i, out indices))
                {
                    //Find a pairtester for the triangle.
                    TrianglePairTester pairTester;
                    if (!activePairTesters.TryGetValue(indices, out pairTester))
                    {
                        pairTester = GetTester();
                        pairTester.Initialize(convex.Shape, localTriangleShape);
                        activePairTesters.Add(indices, pairTester);
                    }
                    pairTester.Updated = true;


                    //Put the triangle into the local space of the convex.
                    Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
                    Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
                    Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
                    Matrix3X3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
                    Matrix3X3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
                    Matrix3X3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);

                    //Now, generate a contact between the two shapes.
                    ContactData contact;
                    TinyStructList <ContactData> contactList;
                    if (pairTester.GenerateContactCandidate(out contactList))
                    {
                        for (int j = 0; j < contactList.count; j++)
                        {
                            contactList.Get(j, out contact);


                            if (UseImprovedBoundaryHandling)
                            {
                                if (AnalyzeCandidate(ref indices, pairTester, ref contact))
                                {
                                    AddLocalContact(ref contact, ref orientation);
                                }
                            }
                            else
                            {
                                AddLocalContact(ref contact, ref orientation);
                            }
                        }
                    }

                    //Get the voronoi region from the contact candidate generation.  Possibly just recalculate, since most of the systems don't calculate it.
                    //Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region.  For face contacts, don't bother- just add it if unique.
                    //For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
                    //For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
                    //If the edge/vertex is already present in the set, then DO NOT add the contact.
                    //When adding a contact, add ALL other voronoi regions to the blocked sets.
                }
            }



            if (UseImprovedBoundaryHandling)
            {
                for (int i = 0; i < edgeContacts.count; i++)
                {
                    //Only correct if it's allowed AND it's blocked.
                    //If it's not blocked, the contact being created is necessary!
                    //The normal generated by the triangle-convex tester is already known not to
                    //violate the triangle sidedness.
                    if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
                    }
                    else if (edgeContacts.Elements[i].ShouldCorrect)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        edgeContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot);
                        edgeContacts.Elements[i].ContactData.Normal            = edgeContacts.Elements[i].CorrectedNormal;
                        edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.
                }
                for (int i = 0; i < vertexContacts.count; i++)
                {
                    if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex))
                    {
                        //If it's not blocked, use the contact as-is without correcting it.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    else if (vertexContacts.Elements[i].ShouldCorrect)
                    {
                        //If it is blocked, we can still make use of the contact.  But first, we need to change the contact normal to ensure that
                        //it will not interfere (and cause a bump or something).
                        float dot;
                        vertexContacts.Elements[i].CorrectedNormal.Normalize();
                        Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot);
                        vertexContacts.Elements[i].ContactData.Normal            = vertexContacts.Elements[i].CorrectedNormal;
                        vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
                        AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation);
                    }
                    //If it's blocked AND it doesn't allow correction, ignore its existence.
                }
                blockedEdgeRegions.Clear();
                blockedVertexRegions.Clear();
                vertexContacts.Clear();
                edgeContacts.Clear();
            }



            //Remove stale pair testers.
            //This will only remove 8 stale ones per frame, but it doesn't really matter.
            //VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame.
            var toRemove = new TinyList <TriangleIndices>();

            foreach (KeyValuePair <TriangleIndices, TrianglePairTester> pair in activePairTesters)
            {
                if (!pair.Value.Updated)
                {
                    if (!toRemove.Add(pair.Key))
                    {
                        break;
                    }
                }
                else
                {
                    pair.Value.Updated = false;
                }
            }



            for (int i = toRemove.count - 1; i >= 0; i--)
            {
                var pairTester = activePairTesters[toRemove[i]];
                pairTester.CleanUp();
                GiveBackTester(pairTester);
                activePairTesters.Remove(toRemove[i]);
            }


            //Some child types will want to do some extra post processing on the manifold.
            ProcessCandidates(candidatesToAdd);


            //Check if adding the new contacts would overflow the manifold.
            if (contacts.count + candidatesToAdd.count > 4)
            {
                //Adding all the contacts would overflow the manifold.  Reduce to the best subset.
                ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates);
                RemoveQueuedContacts();
                for (int i = reducedCandidates.count - 1; i >= 0; i--)
                {
                    Add(ref reducedCandidates.Elements[i]);
                    reducedCandidates.RemoveAt(i);
                }
            }
            else if (candidatesToAdd.count > 0)
            {
                //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
                for (int i = 0; i < candidatesToAdd.count; i++)
                {
                    Add(ref candidatesToAdd.Elements[i]);
                }
            }



            candidatesToAdd.Clear();
        }
Exemplo n.º 5
0
        ///<summary>
        /// Updates the manifold.
        ///</summary>
        ///<param name="dt">Timestep duration.</param>
        public override void Update(float dt)
        {

            //Now, generate a contact between the two shapes.
            float distance;
            Vector3 axis;
            BoxContactDataCache manifold;
            if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
            {
                unsafe
                {
                    BoxContactData* manifoldPointer = &manifold.D1;
                    Vector3.Negate(ref axis, out axis);
                    var toRemove = new TinyList<int>();
                    for (int i = 0; i < contacts.count; i++)
                    {
                        bool found = false;
                        for (int j = manifold.Count - 1; j >= 0; j--)
                        {
                            if (contacts.Elements[i].Id == manifoldPointer[j].Id)
                            {
                                found = true;
                                //Update contact...
                                contacts.Elements[i].Position = manifoldPointer[j].Position;
                                contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth;
                                contacts.Elements[i].Normal = axis;
                                //Remove manifold entry
                                manifold.RemoveAt(j);
                                break;
                            }
                        }
                        if (!found)
                        {//No match found
                            toRemove.Add(i);
                        }
                    }


                    //toRemove is sorted by increasing index.  Go backwards along it so that the indices are valid all the way through.
                    for (int i = toRemove.Count - 1; i >= 0; i--)
                        Remove(toRemove[i]);

                    //Add new contacts.
                    for (int i = 0; i < manifold.Count; i++)
                    {
                        var newContact = new ContactData();
                        newContact.Position = manifoldPointer[i].Position;
                        newContact.PenetrationDepth = -manifoldPointer[i].Depth;
                        newContact.Normal = axis;
                        newContact.Id = manifoldPointer[i].Id;

                        Add(ref newContact);
                    }
                }
            }
            else
            {
                //Not colliding, so get rid of it.
                for (int i = contacts.count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
            }
        }
Exemplo n.º 6
0
        public override void Update(float dt)
        {

            //Now, generate a contact between the two shapes.
            float distance;
            Vector3 axis;
            var manifold = new TinyStructList<BoxContactData>();
            if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold))
            {
                Vector3.Negate(ref axis, out axis);
                TinyList<int> toRemove = new TinyList<int>();
                BoxContactData data;
                for (int i = 0; i < contacts.count; i++)
                {
                    bool found = false;
                    for (int j = manifold.Count - 1; j >= 0; j--)
                    {
                        manifold.Get(j, out data);
                        if (contacts.Elements[i].Id == data.Id)
                        {
                            found = true;
                            //Update contact...
                            contacts.Elements[i].Position = data.Position;
                            contacts.Elements[i].PenetrationDepth = -data.Depth;
                            contacts.Elements[i].Normal = axis;
                            //Remove manifold entry
                            manifold.RemoveAt(j);
                            break;
                        }
                    }
                    if (!found)
                    {//No match found
                        toRemove.Add(i);
                    }
                }

                ////Go through the indices to remove.
                ////For each one, replace the removal index with a contact in the new manifold.
                //int removalIndex;
                //for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--)
                //{
                //    int indexToReplace = toRemove[removalIndex];
                //    toRemove.RemoveAt(removalIndex);
                //    manifold.Get(manifold.count - 1, out data);
                //    //Update contact...
                //    contacts.Elements[indexToReplace].Position = data.Position;
                //    contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth;
                //    contacts.Elements[indexToReplace].Normal = axis;
                //    contacts.Elements[indexToReplace].Id = data.Id;
                //    //Remove manifold entry
                //    manifold.RemoveAt(manifold.count - 1);

                //}

                //Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now).  Just remove the remainder.
                //toRemove is sorted by increasing index.  Go backwards along it so that the indices are valid all the way through.
                for (int i = toRemove.Count - 1; i >= 0; i--)
                    Remove(toRemove[i]);

                //Add new contacts.
                for (int i = 0; i < manifold.Count; i++)
                {
                    manifold.Get(i, out data);
                    ContactData newContact = new ContactData();
                    newContact.Position = data.Position;
                    newContact.PenetrationDepth = -data.Depth;
                    newContact.Normal = axis;
                    newContact.Id = data.Id;

                    Add(ref newContact);
                }
            }
            else
            {
                //Not colliding, so get rid of it.
                for (int i = contacts.count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
            }
        }