///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {

            mobileMesh = entryA as MobileMeshCollidable;
            convex = entryB as ConvexCollidable;

            if (mobileMesh == null || convex == null)
            {
                mobileMesh = entryB as MobileMeshCollidable;
                convex = entryA as ConvexCollidable;

                if (mobileMesh == null || convex == null)
                    throw new ArgumentException("Inappropriate types used to initialize pair.");
            }


            //Contact normal goes from A to B.
            broadPhaseOverlap.entryA = convex;
            broadPhaseOverlap.entryB = mobileMesh;

            //It's possible that the convex does not have an entity if it is a proxy for a non-entity collidable.
            //Similarly, the mesh could be a query object.
            UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, mobileMesh.entity != null ? mobileMesh.entity.material : null);


            base.Initialize(entryA, entryB);

        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {


            box = entryA as ConvexCollidable<BoxShape>;
            sphere = entryB as ConvexCollidable<SphereShape>;

            if (box == null || sphere == null)
            {
                box = entryB as ConvexCollidable<BoxShape>;
                sphere = entryA as ConvexCollidable<SphereShape>;
                if (box == null || sphere == null)
                {
                    throw new Exception("Inappropriate types used to initialize pair.");
                }
            }

            //Reorder the entries so that the guarantee that the normal points from A to B is satisfied.
            broadPhaseOverlap.entryA = box;
            broadPhaseOverlap.entryB = sphere;
            
            base.Initialize(entryA, entryB);


        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {

            terrain = entryA as Terrain;
            convex = entryB as ConvexCollidable;

            if (terrain == null || convex == null)
            {
                terrain = entryB as Terrain;
                convex = entryA as ConvexCollidable;

                if (terrain == null || convex == null)
                    throw new ArgumentException("Inappropriate types used to initialize pair.");
            }

            //Contact normal goes from A to B.
            broadPhaseOverlap.entryA = convex;
            broadPhaseOverlap.entryB = terrain;

            UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, terrain.material);

            base.Initialize(entryA, entryB);




        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {

            instancedMesh = entryA as InstancedMesh;
            convex = entryB as ConvexCollidable;

            if (instancedMesh == null || convex == null)
            {
                instancedMesh = entryB as InstancedMesh;
                convex = entryA as ConvexCollidable;

                if (instancedMesh == null || convex == null)
                    throw new Exception("Inappropriate types used to initialize pair.");
            }            
            
            //Contact normal goes from A to B.
            broadPhaseOverlap.entryA = convex;
            broadPhaseOverlap.entryB = instancedMesh;

            UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, instancedMesh.material);


            base.Initialize(entryA, entryB);



  

        }

        /// <summary>
        /// Constructs the query manager for a character.
        /// </summary>
        /// <param name="character">Character to manage queries for.</param>
        public QueryManager(SphereCharacterController character)
        {
            this.character = character;
            //We can share the real shape with the 'current' query object.
            queryObject = new ConvexCollidable<SphereShape>(character.Body.CollisionInformation.Shape);
            //Share the collision rules between the main body and its query objects.  That way, the character's queries return valid results.
            queryObject.CollisionRules = character.Body.CollisionInformation.CollisionRules;

            SupportRayFilter = SupportRayFilterFunction;
        }

        float upStepMargin = .1f; //There's a little extra space above the maximum step height to start the obstruction and downcast test rays.  Helps when a step is very close to the max step height.

        #endregion Fields

        #region Constructors

        /// <summary>
        /// Constructs a new step manager for a character.
        /// </summary>
        /// <param name="characterBody">The character's body.</param>
        /// <param name="contactCategorizer">Contact categorizer used by the character.</param>
        /// <param name="supportFinder">Support finder used by the character.</param>
        /// <param name="queryManager">Query provider to use in checking for obstructions.</param>
        /// <param name="horizontalMotionConstraint">Horizontal motion constraint used by the character. Source of 3d movement direction.</param>
        public StepManager(Cylinder characterBody, CharacterContactCategorizer contactCategorizer, SupportFinder supportFinder, QueryManager queryManager, HorizontalMotionConstraint horizontalMotionConstraint)
        {
            this.characterBody = characterBody;
            currentQueryObject = new ConvexCollidable<CylinderShape>(characterBody.CollisionInformation.Shape);
            ContactCategorizer = contactCategorizer;
            SupportFinder = supportFinder;
            QueryManager = queryManager;
            HorizontalMotionConstraint = horizontalMotionConstraint;
            //The minimum step height is just barely above where the character would generally find the ground.
            //This helps avoid excess tests.
            minimumUpStepHeight = CollisionDetectionSettings.AllowedPenetration * 1.1f;// Math.Max(0, -.01f + character.Body.CollisionInformation.Shape.CollisionMargin * (1 - character.SupportFinder.sinMaximumSlope));
        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {
            convexA = entryA as ConvexCollidable;
            convexB = entryB as ConvexCollidable;

            if (convexA == null || convexB == null)
            {
                throw new ArgumentException("Inappropriate types used to initialize pair.");
            }

            base.Initialize(entryA, entryB);
        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {
            boxA = entryA as ConvexCollidable<BoxShape>;
            boxB = entryB as ConvexCollidable<BoxShape>;

            if (boxA == null || boxB == null)
            {
                throw new Exception("Inappropriate types used to initialize pair.");
            }

            base.Initialize(entryA, entryB);

        }

 public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
 {
     base.Initialize(entryA, entryB);
     convex = entryA as ConvexCollidable;
     if (convex == null)
     {
         convex = entryB as ConvexCollidable;
         if (convex == null)
         {
             throw new ArgumentException("Incorrect types passed to pair handler.");
         }
     }
 }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {


            sphereA = entryA as ConvexCollidable<SphereShape>;
            sphereB = entryB as ConvexCollidable<SphereShape>;

            if (sphereA == null || sphereB == null)
            {
                throw new ArgumentException("Inappropriate types used to initialize pair.");
            }

            base.Initialize(entryA, entryB);

        }

        ///<summary>
        /// Initializes the pair handler.
        ///</summary>
        ///<param name="entryA">First entry in the pair.</param>
        ///<param name="entryB">Second entry in the pair.</param>
        public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
        {
            triangle = entryA as ConvexCollidable<TriangleShape>;
            convex = entryB as ConvexCollidable;

            if (triangle == null || convex == null)
            {
                triangle = entryB as ConvexCollidable<TriangleShape>;
                convex = entryA as ConvexCollidable;

                if (triangle == null || convex == null)
                    throw new ArgumentException("Inappropriate types used to initialize pair.");
            }

            //Contact normal goes from A to B.
            broadPhaseOverlap.entryA = convex;
            broadPhaseOverlap.entryB = triangle;

            base.Initialize(entryA, entryB);
        }

        /// <summary>
        /// Constructs a stance manager for a character.
        /// </summary>
        /// <param name="characterBody">The character's body entity.</param>
        /// <param name="crouchingHeight">Crouching height of the character.</param>
        /// <param name="queryManager">Provider of queries used by the stance manager to test if it is okay to change stances.</param>
        /// <param name="supportFinder">Support finder used by the character.</param>
        public StanceManager(Cylinder characterBody, float crouchingHeight, QueryManager queryManager, SupportFinder supportFinder)
        {
            this.QueryManager = queryManager;
            this.SupportFinder = supportFinder;
            this.characterBody = characterBody;
            standingHeight = characterBody.Height;
            if (crouchingHeight < standingHeight)
                this.crouchingHeight = StandingHeight * .7f;
            else
                throw new ArgumentException("Crouching height must be less than standing height.");

            //We can share the real shape with the query objects.
            currentQueryObject = new ConvexCollidable<CylinderShape>(characterBody.CollisionInformation.Shape);
            standingQueryObject = new ConvexCollidable<CylinderShape>(new CylinderShape(StandingHeight, characterBody.Radius));
            crouchingQueryObject = new ConvexCollidable<CylinderShape>(new CylinderShape(CrouchingHeight, characterBody.Radius));
            //Share the collision rules between the main body and its query objects.  That way, the character's queries return valid results.
            currentQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
            standingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
            crouchingQueryObject.CollisionRules = characterBody.CollisionInformation.CollisionRules;
        }

 public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
 {
     convex = newCollidableA as ConvexCollidable;
     mesh = newCollidableB as FullChunkObject;
     if (convex == null || mesh == null)
     {
         convex = newCollidableB as ConvexCollidable;
         mesh = newCollidableA as FullChunkObject;
         if (convex == null || mesh == null)
         {
             throw new ArgumentException("Inappropriate types used to initialize contact manifold.");
         }
     }
     ActivePairs = new QuickDictionary<Vector3i, GeneralConvexPairTester>(BufferPools<Vector3i>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3);
     activePairsBackBuffer = new QuickDictionary<Vector3i, GeneralConvexPairTester>(BufferPools<Vector3i>.Locking, BufferPools<GeneralConvexPairTester>.Locking, BufferPools<int>.Locking, 3);
 }

        /// <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>();
//.........这里部分代码省略.........

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public CharacterStressierTestDemo(DemosGame game)
            : base(game)
        {
            //Load in mesh data and create the group.
            Vector3[] staticTriangleVertices;
            int[] staticTriangleIndices;

            var playgroundModel = game.Content.Load<Model>("playground");
            //This is a little convenience method used to extract vertices and indices from a model.
            //It doesn't do anything special; any approach that gets valid vertices and indices will work.
            ModelDataExtractor.GetVerticesAndIndicesFromModel(playgroundModel, out staticTriangleVertices, out staticTriangleIndices);
            var meshShape = new InstancedMeshShape(staticTriangleVertices, staticTriangleIndices);
            var meshes = new List<Collidable>();

            var xSpacing = 400;
            var ySpacing = 400;
            var xCount = 11;
            var yCount = 11;
            for (int i = 0; i < xCount; i++)
            {
                for (int j = 0; j < yCount; j++)
                {
                    var staticMesh = new InstancedMesh(meshShape, new AffineTransform(Matrix3x3.Identity, new Vector3(-xSpacing * (xCount - 1) / 2 + i * xSpacing, 0, -ySpacing * (yCount - 1) / 2 + j * ySpacing)));
                    staticMesh.Sidedness = TriangleSidedness.Counterclockwise;
                    Space.Add(staticMesh);
                    //meshes.Add(staticMesh);
                    game.ModelDrawer.Add(staticMesh);
                }
            }
            //var group = new StaticGroup(meshes);
            //Space.Add(group);

            //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!
            xSpacing = 25;
            ySpacing = 16;
            float zSpacing = 25;

            xCount = 25;
            yCount = 7;
            int zCount = 25;

            var random = new Random();
            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() * 25 + 5.5f, (float)random.NextDouble() * 25 + 5.5f, (float)random.NextDouble() * 25 + 5.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 + -50, 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);
                        game.ModelDrawer.Add(collidable);
                    }
                }
            }
            var group = new StaticGroup(collidables);
            Space.Add(group);

            //Now drop the characters on it!
            var numColumns = 16;
            var numRows = 16;
            var numHigh = 16;
            float separation = 24;

            for (int i = 0; i < numRows; i++)
                for (int j = 0; j < numColumns; j++)
                    for (int k = 0; k < numHigh; k++)
                    {
                        var character = new CharacterController();
                        character.Body.Position =
                            new Vector3(
                            separation * i - numRows * separation / 2,
                            50f + k * separation,
                            separation * j - numColumns * separation / 2);

                        characters.Add(character);

                        Space.Add(character);
                    }

            game.Camera.Position = new Vector3(0, 10, 40);

            //Dump some boxes on top of the characters for fun.
            numColumns = 16;
            numRows = 16;
            numHigh = 8;
//.........这里部分代码省略.........

        CharacterContactPositionState TrySupportLocation(ConvexCollidable<CylinderShape> queryObject, ref Vector3 position, out float hintOffset,
            ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts)
        {
            hintOffset = 0;
            PrepareQueryObject(queryObject, ref position);
            QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);

            bool obstructed = IsObstructed(ref sideContacts, ref headContacts);
            if (obstructed)
            {
                return CharacterContactPositionState.Obstructed;
            }
            if (supportContacts.Count > 0)
            {
                CharacterContactPositionState supportState;
                CharacterContact supportContact;
                QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
                var down = characterBody.orientationMatrix.Down;
                //Note that traction is not tested for; it isn't important for the stance manager.
                if (supportState == CharacterContactPositionState.Accepted)
                {
                    //We're done! The guess found a good spot to stand on.
                    //We need to have fairly good contacts after this process, so only push it up a bit.
                    hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                    return CharacterContactPositionState.Accepted;
                }
                else if (supportState == CharacterContactPositionState.TooDeep)
                {
                    //Looks like we have to keep trying, but at least we found a good hint.
                    hintOffset = Math.Min(0, Vector3.Dot(supportContact.Contact.Normal, down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                    return CharacterContactPositionState.TooDeep;
                }
                else //if (supportState == SupportState.Separated)
                {
                    //It's not obstructed, but the support isn't quite right.
                    //It's got a negative penetration depth.
                    //We can use that as a hint.
                    hintOffset = -.001f - Vector3.Dot(supportContact.Contact.Normal, down) * supportContact.Contact.PenetrationDepth;
                    return CharacterContactPositionState.NoHit;
                }
            }
            else //Not obstructed, but no support.
            {
                return CharacterContactPositionState.NoHit;
            }
        }

        ///<summary>
        /// Initializes the manifold.
        ///</summary>
        ///<param name="newCollidableA">First collidable.</param>
        ///<param name="newCollidableB">Second collidable.</param>
        public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
        {
            collidableA = newCollidableA as ConvexCollidable;
            collidableB = newCollidableB as ConvexCollidable;
            pairTester.Initialize(newCollidableA, newCollidableB);


            if (collidableA == null || collidableB == null)
            {
                throw new Exception("Inappropriate types used to initialize pair tester.");
            }
        }

        ///<summary>
        /// Initializes the manifold.
        ///</summary>
        ///<param name="newCollidableA">First collidable.</param>
        ///<param name="newCollidableB">Second collidable.</param>
        ///<exception cref="Exception">Thrown when the collidables being used are not of the proper type.</exception>
        public override void Initialize(Collidable newCollidableA, Collidable newCollidableB)
        {
            box = newCollidableA as ConvexCollidable<BoxShape>;
            sphere = newCollidableB as ConvexCollidable<SphereShape>;

            if (box == null || sphere == null)
            {
                box = newCollidableB as ConvexCollidable<BoxShape>;
                sphere = newCollidableA as ConvexCollidable<SphereShape>;
                if (box == null || sphere == null)
                {
                    throw new ArgumentException("Inappropriate types used to initialize pair.");
                }
            }

        }

        ///<summary>
        /// Cleans up the pair handler.
        ///</summary>
        public override void CleanUp()
        {
            base.CleanUp();

            triangle = null;
            convex = null;
        }

        ///<summary>
        /// Cleans up the pair handler.
        ///</summary>
        public override void CleanUp()
        {
            base.CleanUp();

            terrain = null;
            convex = null;
        }