[Test] public void Move()
		{
			int[] testArray = Enumerable.Range(0, 10).ToArray();
			RawList<int> intList = new RawList<int>();

			intList.AddRange(testArray);
			intList.Move(0, 3, 1);
			CollectionAssert.AreEqual(new int[] { 0, 0, 1, 2, 4, 5, 6, 7, 8, 9 }, intList);
			intList.Clear();

			intList.AddRange(testArray);
			intList.Move(0, 3, 3);
			CollectionAssert.AreEqual(new int[] { 0, 0, 0, 0, 1, 2, 6, 7, 8, 9 }, intList);
			intList.Clear();

			intList.AddRange(testArray);
			intList.Move(0, 3, 5);
			CollectionAssert.AreEqual(new int[] { 0, 0, 0, 3, 4, 0, 1, 2, 8, 9 }, intList);
			intList.Clear();

			intList.AddRange(testArray);
			intList.Move(7, 3, -1);
			CollectionAssert.AreEqual(new int[] { 0, 1, 2, 3, 4, 5, 7, 8, 9, 0 }, intList);
			intList.Clear();

			intList.AddRange(testArray);
			intList.Move(7, 3, -3);
			CollectionAssert.AreEqual(new int[] { 0, 1, 2, 3, 7, 8, 9, 0, 0, 0 }, intList);
			intList.Clear();

			intList.AddRange(testArray);
			intList.Move(7, 3, -5);
			CollectionAssert.AreEqual(new int[] { 0, 1, 7, 8, 9, 5, 6, 0, 0, 0 }, intList);
			intList.Clear();
		}

        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));
            }
        }

 ///<summary>
 /// Constructs a new convex-convex manifold.
 ///</summary>
 public GeneralConvexContactManifold()
 {
     contacts = new RawList<Contact>(4);
     unusedContacts = new UnsafeResourcePool<Contact>(4);
     contactIndicesToRemove = new RawList<int>(4);
     pairTester = new GeneralConvexPairTester();
 }

 /// <summary>
 /// Returns a resource to the pool.
 /// </summary>
 /// <param name="list">List to return.</param>
 public static void GiveBack(RawList<RayCastResult> list)
 {
     if (SubPoolRayCastResultList == null)
         SubPoolRayCastResultList = new UnsafeResourcePool<RawList<RayCastResult>>();
     list.Clear();
     SubPoolRayCastResultList.GiveBack(list);
 }

		[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));
		}

		public void QueryVisibleRenderers(IDrawDevice device, RawList<ICmpRenderer> targetList)
		{
			// Empty the cached list of visible renderers
			targetList.Count = 0;
			targetList.Reserve(this.totalRendererCount);

			// Copy references to all renderers that are visible to the target device
			int visibleCount = 0;
			ICmpRenderer[] targetData = targetList.Data;
			foreach (var pair in this.renderersByType)
			{
				ICmpRenderer[] data = pair.Value.Data;
				for (int i = 0; i < data.Length; i++)
				{
					if (i >= pair.Value.Count) break;

					if ((data[i] as Component).Active && data[i].IsVisible(device))
					{
						targetData[visibleCount] = data[i];
						visibleCount++;
					}
				}
			}
			targetList.Count = visibleCount;
		}

        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public ConvexHullTestDemo(DemosGame game)
            : base(game)
        {
            var random = new Random(5);

            for (int i = 0; i < 500000; ++i)
            {
                List<Vector3> points = new List<Vector3>();
                for (int k = 0; k < random.Next(8, 60); k++)
                {
                    points.Add(new Vector3(-100 + 30 * (float)random.NextDouble(), 100 + 500 * (float)random.NextDouble(), 100 + 30 * (float)random.NextDouble()));
                }
                var convexHull = new ConvexHull(new Vector3(0, 7, 0), points, 10);
                Console.WriteLine(convexHull.CollisionInformation.Shape.Vertices.Count);
            }

            var vertices = new[]
            {
                new Vector3(0, -1.750886E-9f, -1.5f),
                new Vector3(1, 1, 0.5f),
                new Vector3(1, -1, 0.5f),
                new Vector3(-1, 1, 0.5f),
                new Vector3(-1, -1, 0.5f),
            };

            var hullVertices = new RawList<Vector3>();
            ConvexHullHelper.GetConvexHull(vertices, hullVertices);

            ConvexHull hull = new ConvexHull(vertices, 5);
            Space.Add(hull);

            Box ground = new Box(new Vector3(0, -.5f, 0), 50, 1, 50);
            Space.Add(ground);
            game.Camera.Position = new Vector3(0, 6, 15);
        }

        /// <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);
        }

		/// <summary>
		/// Adds the specified vertex array to the Canvas' buffering mechanism. As long as buffers are available, the
		/// Canvas will prefer re-using one of them over creating a new vertex array. Every vertex array will only be used once.
		/// </summary>
		/// <param name="array"></param>
		public void AddVertexArray(RawList<VertexC1P3T2> array)
		{
			if (array == null) throw new ArgumentNullException("buffer");
			if (this.dummy) return;
			if (this.vertexArraysFree.Contains(array)) return;
			if (this.vertexArraysUsed.Contains(array)) return;
			this.vertexArraysFree.Add(array);
		}

        /// <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>
 /// Averages together all the points in the point list.
 ///</summary>
 ///<param name="pointContributions">Point list to average.</param>
 ///<returns>Averaged point.</returns>
 public static Vector3 AveragePoints(RawList<Vector3> pointContributions)
 {
     var center = new Vector3();
     for (int i = 0; i < pointContributions.Count; i++)
     {
         center += pointContributions[i]; //Every point has equal weight.
     }
     return center / pointContributions.Count;
 }

 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);
 }

        public EditTilesetTileInputAction(Tileset tileset, RawList<TileInput> tileInput, RawList<bool> tileInputMask)
        {
            if (tileset == null) throw new ArgumentNullException("tileset");
            if (tileInput == null) throw new ArgumentNullException("tileInput");
            if (tileInputMask == null) throw new ArgumentNullException("tileInputMask");
            if (tileInputMask.Count != tileInput.Count) throw new ArgumentException("Input Mask needs to be the same size as input.", "tileInputMask");

            this.tileset = tileset;
            this.tileInput = tileInput;
            this.tileInputMask = tileInputMask;
        }

        ///<summary>
        /// Constructs a new convex hull shape.
        /// The point set will be recentered on the local origin.
        ///</summary>
        ///<param name="vertices">Point set to use to construct the convex hull.</param>
        /// <param name="center">Computed center of the convex hull shape prior to recentering.</param>
        /// <param name="outputHullTriangleIndices">Triangle indices computed on the surface of the point set.</param>
        /// <param name="outputUniqueSurfaceVertices">Unique vertices on the surface of the convex hull.</param>
        ///<exception cref="ArgumentException">Thrown when the point set is empty.</exception>
        public ConvexHullShape(IList<Vector3> vertices, out Vector3 center, IList<int> outputHullTriangleIndices, IList<Vector3> outputUniqueSurfaceVertices)
        {
            if (vertices.Count == 0)
                throw new ArgumentException("Vertices list used to create a ConvexHullShape cannot be empty.");


            //Ensure that the convex hull is centered on its local origin.
            center = ComputeCenter(vertices, outputHullTriangleIndices, outputUniqueSurfaceVertices);
            this.vertices = new RawList<Vector3>(outputUniqueSurfaceVertices);

            OnShapeChanged();
        }

		public void RemoveRenderer(ICmpRenderer renderer)
		{
			Type type = renderer.GetType();
			RawList<ICmpRenderer> list;
			if (!this.renderersByType.TryGetValue(type, out list))
			{
				list = new RawList<ICmpRenderer>();
				this.renderersByType.Add(type, list);
			}
			list.Remove(renderer);
			this.totalRendererCount--;
		}

        public TilesetViewPaintTilesEventArgs(
			Graphics graphics, 
			Rectangle clipRect,
			Tileset tileset,
			Bitmap sourceImage, 
			RawList<TilesetViewPaintTileData> paintedTiles)
            : base(graphics, clipRect)
        {
            this.tileset = tileset;
            this.sourceImage = sourceImage;
            this.paintedTiles = paintedTiles;
        }

        /// <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();
                }

            }
        }

 /// <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 EditTilesetAutoTileItemAction(Tileset tileset, TilesetAutoTileInput autoTile, int tileIndex, TilesetAutoTileItem tileInput)
        {
            if (tileset == null) throw new ArgumentNullException("tileset");
            if (autoTile == null) throw new ArgumentNullException("autoTile");

            this.tileset = tileset;
            this.autoTile = autoTile;

            this.tileInput = new RawList<TilesetAutoTileItem>(tileIndex + 1);
            this.tileInput.Count = tileIndex + 1;
            this.tileInput.Data[tileIndex] = tileInput;

            this.tileInputMask = new RawList<bool>(tileIndex + 1);
            this.tileInputMask.Count = tileIndex + 1;
            this.tileInputMask.Data[tileIndex] = true;
        }

        ///<summary>
        /// Constructs a new convex contact manifold constraint.
        ///</summary>
        public ConvexContactManifoldConstraint()
        {
            //All of the constraints are always in the solver group.  Some of them are just deactivated sometimes.
            //This reduces some bookkeeping complications.

            penetrationConstraints = new RawList<ContactPenetrationConstraint>(4);

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