C# RawList.Add方法代码示例

        /// <summary>
        /// Adds entities associated with the solver item to the involved entities list.
        /// Ensure that sortInvolvedEntities() is called at the end of the function.
        /// This allows the non-batched multithreading system to lock properly.
        /// </summary>
        protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
        {
            if (connectionA != null && connectionA != WorldEntity)
                outputInvolvedEntities.Add(connectionA);

            if (connectionB != null && connectionB != WorldEntity)
                outputInvolvedEntities.Add(connectionB);
        }

 protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
 {
     //The default implementation for solver groups looks at every single subconstraint.
     //That's not necessary for these special constraints.
     if (entityA != null)
         outputInvolvedEntities.Add(entityA);
     if (entityB != null)
         outputInvolvedEntities.Add(entityB);
 }

        /// <summary>
        /// Refreshes the contact manifold, removing any out of date contacts
        /// and updating others.
        /// </summary>
        public static void ContactRefresh(RawList<Contact> contacts, RawValueList<ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList<int> toRemove)
        {
            //TODO: Could also refresh normals with some trickery.
            //Would also need to refresh depth using new normals, and would require some extra information.

            for (int k = 0; k < contacts.Count; k++)
            {
                contacts.Elements[k].Validate();
                ContactSupplementData data = supplementData.Elements[k];
                System.Numerics.Vector3 newPosA, newPosB;
                RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA);
                RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB);

                //ab - (ab*n)*n
                //Compute the horizontal offset.
                System.Numerics.Vector3 ab;
                Vector3Ex.Subtract(ref newPosB, ref newPosA, out ab);
                float dot;
                Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
                System.Numerics.Vector3 temp;
                Vector3Ex.Multiply(ref contacts.Elements[k].Normal, dot, out temp);
                Vector3Ex.Subtract(ref ab, ref temp, out temp);
                dot = temp.LengthSquared();
                if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared)
                {
                    toRemove.Add(k);
                }
                else
                {
                    //Depth refresh:
                    //Find deviation ((Ra-Rb)*N) and add to base depth.
                    Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
                    contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot;
                    if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance)
                        toRemove.Add(k);
                    else
                    {
                        //Refresh position and ra/rb.
                        System.Numerics.Vector3 newPos;
                        Vector3Ex.Add(ref newPosB, ref newPosA, out newPos);
                        Vector3Ex.Multiply(ref newPos, .5f, out newPos);
                        contacts.Elements[k].Position = newPos;
                        //This is an interesting idea, but has very little effect one way or the other.
                        //data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth;
                        //RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA);
                        //RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB);
                    }
                    contacts.Elements[k].Validate();
                }

            }
        }

        public static void GetShapeMeshData(EntityCollidable collidable, List<VertexPositionNormalTexture> vertices, List<ushort> indices)
        {
            var shape = collidable.Shape as ConvexShape;
            if (shape == null)
                throw new ArgumentException("Wrong shape type for this helper.");
            var vertexPositions = new BEPUutilities.Vector3[SampleDirections.Length];

            for (int i = 0; i < SampleDirections.Length; ++i)
            {
                shape.GetLocalExtremePoint(SampleDirections[i], out vertexPositions[i]);
            }

            var hullIndices = new RawList<int>();
            ConvexHullHelper.GetConvexHull(vertexPositions, hullIndices);

            var hullTriangleVertices = new RawList<BEPUutilities.Vector3>();
            foreach (int i in hullIndices)
            {
                hullTriangleVertices.Add(vertexPositions[i]);
            }

            for (ushort i = 0; i < hullTriangleVertices.Count; i += 3)
            {
                Vector3 normal = MathConverter.Convert(BEPUutilities.Vector3.Normalize(BEPUutilities.Vector3.Cross(hullTriangleVertices[i + 2] - hullTriangleVertices[i], hullTriangleVertices[i + 1] - hullTriangleVertices[i])));
                vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i]), normal, new Vector2(0, 0)));
                vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 1]), normal, new Vector2(1, 0)));
                vertices.Add(new VertexPositionNormalTexture(MathConverter.Convert(hullTriangleVertices[i + 2]), normal, new Vector2(0, 1)));
                indices.Add(i);
                indices.Add((ushort)(i + 1));
                indices.Add((ushort)(i + 2));
            }
        }

		[Test] public void Basics()
		{
			RawList<int> intList = new RawList<int>();
			intList.Add(10);
			intList.AddRange(new int[] { 17, 42, 94 });

			Assert.AreEqual(4, intList.Count);
			Assert.IsTrue(intList.Contains(42));
			Assert.AreEqual(2, intList.IndexOf(42));
			CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList);
			CollectionAssert.AreEqual(new int[] { 10, 17, 42, 94 }, intList.Data.Take(4));

			intList.ShrinkToFit();
			Assert.AreEqual(intList.Count, intList.Capacity);

			intList.Remove(42);
			Assert.AreEqual(3, intList.Count);
			Assert.IsTrue(!intList.Contains(42));
			Assert.AreEqual(-1, intList.IndexOf(42));
			CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList);
			CollectionAssert.AreEqual(new int[] { 10, 17, 94 }, intList.Data.Take(3));

			intList.Insert(1, 100);
			CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList);
			CollectionAssert.AreEqual(new int[] { 10, 100, 17, 94 }, intList.Data.Take(4));

			intList.InsertRange(2, new int[] { 150, 200, 250, 300 });
			CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList);
			CollectionAssert.AreEqual(new int[] { 10, 100, 150, 200, 250, 300, 17, 94 }, intList.Data.Take(8));

			intList.Clear();
			Assert.AreEqual(0, intList.Count);
			Assert.IsTrue(!intList.Contains(94));
		}

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public BroadPhaseRemovalTestDemo(DemosGame game)
            : base(game)
        {
            Entity toAdd;
            //BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
            BoundingBox box = new BoundingBox(new Vector3(-500, -500, -500), new Vector3(500, 500, 500));

            DynamicHierarchy dh = new DynamicHierarchy();

            Random rand = new Random(0);

            RawList<Entity> entities = new RawList<Entity>();
            for (int k = 0; k < 1000; k++)
            {
                Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X),
                                               (float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y),
                                               (float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z));
                toAdd = new Box(MathConverter.Convert(position), 1, 1, 1, 1);

                entities.Add(toAdd);

            }

            testResults = new double[2];
            int runCount = 10;
            for (int k = 0; k < runCount; k++)
            {

                for (int i = 0; i < entities.Count; i++)
                {
                    dh.Add(entities[i].CollisionInformation);
                }

                long start = Stopwatch.GetTimestamp();
                for (int i = 0; i < entities.Count; i++)
                {
                    //dh.RemoveFast(entities[i].CollisionInformation);
                }
                long end = Stopwatch.GetTimestamp();
                testResults[0] += (end - start) / (double)Stopwatch.Frequency;

                for (int i = 0; i < entities.Count; i++)
                {
                    dh.Add(entities[i].CollisionInformation);
                }

                start = Stopwatch.GetTimestamp();
                for (int i = 0; i < entities.Count; i++)
                {
                    //dh.RemoveBrute(entities[i].CollisionInformation);
                }
                end = Stopwatch.GetTimestamp();
                testResults[1] += (end - start) / (double)Stopwatch.Frequency;

            }
            testResults[0] /= runCount;
            testResults[1] /= runCount;
        }

 /// <summary>
 /// Collects the entities which are affected by the solver group and updates the internal listing.
 /// </summary>
 protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
 {
     foreach (EntitySolverUpdateable item in solverUpdateables)
     {
         for (int i = 0; i < item.involvedEntities.count; i++)
         {
             if (!outputInvolvedEntities.Contains(item.involvedEntities.Elements[i]))
             {
                 outputInvolvedEntities.Add(item.involvedEntities.Elements[i]);
             }
         }
     }
 }

 public static void GetClosestPointOnTetrahedronToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, ref Vector3 p, RawList<Vector3> subsimplex, out Vector3 closestPoint)
 {
     // Start out assuming point inside all halfspaces, so closest to itself
     subsimplex.Clear();
     subsimplex.Add(a); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself.
     subsimplex.Add(b);
     subsimplex.Add(c);
     subsimplex.Add(d);
     closestPoint = p;
     Vector3 pq;
     Vector3 q;
     float bestSqDist = float.MaxValue;
     // If point outside face abc then compute closest point on abc
     if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c))
     {
         GetClosestPointOnTriangleToPoint(ref a, ref b, ref c, ref p, subsimplex, out q);
         Vector3.Subtract(ref q, ref p, out pq);
         float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
         // Update best closest point if (squared) distance is less than current best
         if (sqDist < bestSqDist)
         {
             bestSqDist = sqDist;
             closestPoint = q;
         }
     }
     // Repeat test for face acd
     if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d))
     {
         GetClosestPointOnTriangleToPoint(ref a, ref c, ref d, ref p, subsimplex, out q);
         Vector3.Subtract(ref q, ref p, out pq);
         float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
         if (sqDist < bestSqDist)
         {
             bestSqDist = sqDist;
             closestPoint = q;
         }
     }
     // Repeat test for face adb
     if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b))
     {
         GetClosestPointOnTriangleToPoint(ref a, ref d, ref b, ref p, subsimplex, out q);
         Vector3.Subtract(ref q, ref p, out pq);
         float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
         if (sqDist < bestSqDist)
         {
             bestSqDist = sqDist;
             closestPoint = q;
         }
     }
     // Repeat test for face bdc
     if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c))
     {
         GetClosestPointOnTriangleToPoint(ref b, ref d, ref c, ref p, subsimplex, out q);
         Vector3.Subtract(ref q, ref p, out pq);
         float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z;
         if (sqDist < bestSqDist)
         {
             closestPoint = q;
         }
     }
 }

 internal bool IsHitUnique(RawList<RayHit> hits, ref RayHit hit)
 {
     for (int i = 0; i < hits.count; i++)
     {
         if (Math.Abs(hits.Elements[i].T - hit.T) < MeshHitUniquenessThreshold)
             return false;
     }
     hits.Add(hit);
     return true;
 }

        protected RawList<ImportInputAssignment> SelectImporter(AssetImportEnvironment env)
        {
            if (!env.IsPrepareStep) throw new ArgumentException(
                "The specified import environment must be configured as a preparation environment.",
                "env");

            // Find an importer to handle some or all of the unhandled input files
            RawList<ImportInputAssignment> candidateMapping = new RawList<ImportInputAssignment>();
            foreach (IAssetImporter importer in AssetManager.Importers)
            {
                env.ResetAcquiredData();

                try
                {
                    importer.PrepareImport(env);
                }
                catch (Exception ex)
                {
                    Log.Editor.WriteError("An error occurred in the preparation step of '{1}': {0}",
                        Log.Exception(ex),
                        Log.Type(importer.GetType()));
                    continue;
                }

                if (env.HandledInput.Any())
                {
                    candidateMapping.Add(new ImportInputAssignment
                    {
                        Importer = importer,
                        HandledInput = env.HandledInput.ToArray(),
                        ExpectedOutput = env.Output.ToArray()
                    });
                }
            }

            // Sort candidate mapping from most files to least files, so we can solve the biggest conflicts first
            candidateMapping.Sort((a, b) => b.HandledInput.Length - a.HandledInput.Length);

            // Determine if multiple importers intend to handle the same files and resolve conflicts
            List<int> conflictingIndices = new List<int>();
            List<string> conflictingFiles = new List<string>();
            for (int mainIndex = 0; mainIndex < candidateMapping.Count; mainIndex++)
            {
                ImportInputAssignment assignment = candidateMapping[mainIndex];

                // Find all conflicts related to this assignment
                conflictingIndices.Clear();
                conflictingFiles.Clear();
                for (int secondIndex = 0; secondIndex < candidateMapping.Count; secondIndex++)
                {
                    if (secondIndex == mainIndex) continue;

                    ImportInputAssignment conflictAssignment = candidateMapping[secondIndex];
                    IEnumerable<string> mainFiles = assignment.HandledInput.Select(item => item.Path);
                    IEnumerable<string> secondFiles = conflictAssignment.HandledInput.Select(item => item.Path);
                    string[] conflicts = mainFiles.Intersect(secondFiles).ToArray();
                    if (conflicts.Length > 0)
                    {
                        if (conflictingIndices.Count == 0) conflictingIndices.Add(mainIndex);
                        conflictingIndices.Add(secondIndex);
                        conflictingFiles.AddRange(conflicts);
                    }
                }

                // Resolve conflicts with this assignment
                if (conflictingIndices.Count > 0)
                {
                    // Determine which importer to prefer for this conflict
                    ImportInputAssignment[] conflictingAssignments = conflictingIndices.Select(i => candidateMapping[i]).ToArray();
                    int keepIndex = this.ResolveMappingConflict(conflictingAssignments);

                    // If we somehow decided that none of the options is viable, abort the operation
                    if (keepIndex == -1)
                    {
                        candidateMapping.Clear();
                        return candidateMapping;
                    }

                    // Sort indices to remove in declining order and remove their mappings
                    conflictingIndices.Remove(keepIndex);
                    conflictingIndices.Sort((a, b) => b - a);
                    foreach (int index in conflictingIndices)
                    {
                        candidateMapping.RemoveAt(index);
                    }

                    // Start over with the conflict search
                    mainIndex = -1;
                    continue;
                }
            }

            return candidateMapping;
        }

        ///<summary>
        /// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape.
        ///</summary>
        ///<param name="localBoundingBox">Bounding box in the local space of the terrain shape.</param>
        ///<param name="overlappedElements">Indices of elements whose bounding boxes overlap the input bounding box.</param>
        public bool GetOverlaps(BoundingBox localBoundingBox, RawList<int> overlappedElements)
        {
            int width = heights.GetLength(0);
            int minX = Math.Max((int)localBoundingBox.Min.X, 0);
            int minY = Math.Max((int)localBoundingBox.Min.Z, 0);
            int maxX = Math.Min((int)localBoundingBox.Max.X, width - 2);
            int maxY = Math.Min((int)localBoundingBox.Max.Z, heights.GetLength(1) - 2);
            for (int i = minX; i <= maxX; i++)
            {
                for (int j = minY; j <= maxY; j++)
                {
                    //Before adding a triangle to the list, make sure the object isn't too high or low from the quad.
                    float highest, lowest;
                    float y1 = heights[i, j];
                    float y2 = heights[i + 1, j];
                    float y3 = heights[i, j + 1];
                    float y4 = heights[i + 1, j + 1];

                    highest = y1;
                    lowest = y1;
                    if (y2 > highest)
                        highest = y2;
                    else if (y2 < lowest)
                        lowest = y2;
                    if (y3 > highest)
                        highest = y3;
                    else if (y3 < lowest)
                        lowest = y3;
                    if (y4 > highest)
                        highest = y4;
                    else if (y4 < lowest)
                        lowest = y4;

                    if (localBoundingBox.Max.Y < lowest ||
                        localBoundingBox.Min.Y > highest)
                        continue;

                    //Now the local bounding box is very likely intersecting those of the triangles.
                    //Add the triangles to the list.
                    int quadIndex = (i + j * width) * 2;
                    overlappedElements.Add(quadIndex);
                    overlappedElements.Add(quadIndex + 1);

                }
            }
            return overlappedElements.Count > 0;
        }

 protected override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
 {
     var entityCollidable = supportData.SupportObject as EntityCollidable;
     if (entityCollidable != null)
         outputInvolvedEntities.Add(entityCollidable.Entity);
     outputInvolvedEntities.Add(character.Body);
 }

 /// <summary>
 /// Adds entities associated with the solver item to the involved entities list.
 /// Ensure that sortInvolvedEntities() is called at the end of the function.
 /// This allows the non-batched multithreading system to lock properly.
 /// </summary>
 protected internal override void CollectInvolvedEntities(RawList<Entity> outputInvolvedEntities)
 {
     if (entity != null) //sometimes, the entity is set to null to 'deactivate' it.  Don't add null to the involved entities list.
         outputInvolvedEntities.Add(entity);
 }

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public MutableStaticGroupTestDemo(DemosGame game)
            : base(game)
        {


            //Creating a bunch of separate StaticMeshes or kinematic Entity objects for an environment can pollute the broad phase.
            //This is because the broad phase implementation doesn't have guarantees about what elements can collide, so it has to
            //traverse the acceleration structure all the way down to pairs to figure it out.  That can get expensive!

            //Individual objects, like StaticMeshes, can have very complicated geometry without hurting the broad phase because the broad phase
            //has no knowledge of the thousands of triangles in the mesh.  The StaticMesh itself knows that the triangles within the mesh
            //never need to collide, so it never needs to test them against each other.

            //Similarly, the StaticGroup can be given a bunch of separate collidables.  The broad phase doesn't directly know about these child collidables-
            //it only sees the StaticGroup.  The StaticGroup knows that the things inside it can't ever collide with each other, so no tests are needed.
            //This avoids the performance problem!

            //To demonstrate, we'll be creating a set of static objects and giving them to a group to manage.
            var collidables = new List<Collidable>();

            //Start with a whole bunch of boxes.  These are entity collidables, but without entities!
            float xSpacing = 6;
            float ySpacing = 6;
            float zSpacing = 6;


            //NOTE: You might notice this demo takes a while to start, especially on the Xbox360.  Do not fear!  That's due to the creation of the graphics data, not the physics.
            //The physics can handle over 100,000 static objects pretty easily.  The graphics, not so much :)
            //Try disabling the game.ModelDrawer.Add() lines and increasing the number of static objects.  
            int xCount = 15;
            int yCount = 7;
            int zCount = 15;


            var random = new Random(5);
            for (int i = 0; i < xCount; i++)
            {
                for (int j = 0; j < yCount; j++)
                {
                    for (int k = 0; k < zCount; k++)
                    {
                        //Create a transform and the instance of the mesh.
                        var collidable = new ConvexCollidable<BoxShape>(new BoxShape((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f));

                        //This EntityCollidable isn't associated with an entity, so we must manually tell it where to sit by setting the WorldTransform.
                        //This also updates its bounding box.
                        collidable.WorldTransform = new RigidTransform(
                            new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 3, k * zSpacing - zCount * zSpacing * .5f),
                            Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100));

                        collidables.Add(collidable);
                    }
                }
            }


            //Now create a bunch of instanced meshes too.
            xSpacing = 6;
            ySpacing = 6;
            zSpacing = 6;

            xCount = 10;
            yCount = 2;
            zCount = 10;

            Vector3[] vertices;
            int[] indices;
            ModelDataExtractor.GetVerticesAndIndicesFromModel(game.Content.Load<Model>("fish"), out vertices, out indices);
            var meshShape = new InstancedMeshShape(vertices, indices);

            for (int i = 0; i < xCount; i++)
            {
                for (int j = 0; j < yCount; j++)
                {
                    for (int k = 0; k < zCount; k++)
                    {
                        //Create a transform and the instance of the mesh.
                        var transform = new AffineTransform(
                            new Vector3((float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f, (float)random.NextDouble() * 6 + .5f),
                             Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), (float)random.NextDouble() * 100),
                            new Vector3(i * xSpacing - xCount * xSpacing * .5f, j * ySpacing + 50, k * zSpacing - zCount * zSpacing * .5f));
                        var mesh = new InstancedMesh(meshShape, transform);
                        //Making the triangles one-sided makes collision detection a bit more robust, since the backsides of triangles won't try to collide with things
                        //and 'pull' them back into the mesh.
                        mesh.Sidedness = TriangleSidedness.Counterclockwise;
                        collidables.Add(mesh);
                    }
                }
            }

            var ground = new ConvexCollidable<BoxShape>(new BoxShape(200, 1, 200));
            ground.WorldTransform = new RigidTransform(new Vector3(0, -3, 0), Quaternion.Identity);
            collidables.Add(ground);

            var group = new StaticGroup(collidables);
            var removed = new RawList<Collidable>();
//.........这里部分代码省略.........

        //This works in the general case where there can be any  number of contacts and candidates.  Could specialize it as an optimization to single-contact added incremental manifolds.
        ///<summary>
        /// Reduces the contact manifold to a good subset.
        ///</summary>
        ///<param name="contacts">Contacts to reduce.</param>
        ///<param name="contactCandidates">Contact candidates to include in the reduction process.</param>
        ///<param name="contactsToRemove">Contacts that need to removed to reach the reduced state.</param>
        ///<param name="toAdd">Contact candidates that should be added to reach the reduced state.</param>
        ///<exception cref="InvalidOperationException">Thrown when the set being reduced is empty.</exception>
        public static void ReduceContacts(RawList<Contact> contacts, ref QuickList<ContactData> contactCandidates, RawList<int> contactsToRemove, ref QuickList<ContactData> toAdd)
        {
            //Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector.
            float maximumDepth = -float.MaxValue;
            int deepestIndex = -1;
            Vector3 normal = Toolbox.ZeroVector;
            for (int i = 0; i < contacts.Count; i++)
            {
                Vector3.Add(ref normal, ref contacts.Elements[i].Normal, out normal);
                if (contacts.Elements[i].PenetrationDepth > maximumDepth)
                {
                    deepestIndex = i;
                    maximumDepth = contacts.Elements[i].PenetrationDepth;
                }
            }
            for (int i = 0; i < contactCandidates.Count; i++)
            {
                Vector3.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal);
                if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth)
                {
                    deepestIndex = contacts.Count + i;
                    maximumDepth = contactCandidates.Elements[i].PenetrationDepth;
                }
            }
            //If the normals oppose each other, this can happen.  It doesn't need to be normalized, but having SOME normal is necessary.
            if (normal.LengthSquared() < Toolbox.Epsilon)
                if (contacts.Count > 0)
                    normal = contacts.Elements[0].Normal;
                else if (contactCandidates.Count > 0)
                    normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty.
                else //This method should not have been called at all if it gets here.
                    throw new ArgumentException("Cannot reduce an empty contact set.");

            //Find the contact (candidate) that is furthest away from the deepest contact (candidate).
            Vector3 deepestPosition;
            if (deepestIndex < contacts.Count)
                deepestPosition = contacts.Elements[deepestIndex].Position;
            else
                deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position;
            float distanceSquared;
            float furthestDistance = 0;
            int furthestIndex = -1;
            for (int i = 0; i < contacts.Count; i++)
            {
                Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared);
                if (distanceSquared > furthestDistance)
                {
                    furthestDistance = distanceSquared;
                    furthestIndex = i;
                }
            }
            for (int i = 0; i < contactCandidates.Count; i++)
            {
                Vector3.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared);
                if (distanceSquared > furthestDistance)
                {
                    furthestDistance = distanceSquared;
                    furthestIndex = contacts.Count + i;
                }
            }
            if (furthestIndex == -1)
            {
                //Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location.
                if (contacts.Count > 0)
                {
                    for (int i = 1; i < contacts.Count; i++)
                    {
                        contactsToRemove.Add(i);
                    }
                    return;
                }
                if (contactCandidates.Count > 0)
                {
                    toAdd.Add(ref contactCandidates.Elements[0]);
                    return;
                }
                throw new ArgumentException("Cannot reduce an empty contact set.");

            }
            Vector3 furthestPosition;
            if (furthestIndex < contacts.Count)
                furthestPosition = contacts.Elements[furthestIndex].Position;
            else
                furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position;
            Vector3 xAxis;
            Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis);

            //Create the second axis of the 2d 'coordinate system' of the manifold.
            Vector3 yAxis;
            Vector3.Cross(ref xAxis, ref normal, out yAxis);

//.........这里部分代码省略.........