/// <summary>
        /// Initializes a new CollisionSystem which uses a grid to speed up collision detection.
        /// Use this system for larger scenes with many objects.
        /// </summary>
        /// <param name="nx">Number of GridEntries in X Direction.</param>
        /// <param name="ny">Number of GridEntries in Y Direction.</param>
        /// <param name="nz">Number of GridEntries in Z Direction.</param>
        /// <param name="dx">Size of a single GridEntry in X Direction.</param>
        /// <param name="dy">Size of a single GridEntry in Y Direction.</param>
        /// <param name="dz">Size of a single GridEntry in Z Direction.</param>
        public CollisionSystemGrid(int nx, int ny, int nz, float dx, float dy, float dz)
        {
            this.nx = nx; this.ny = ny; this.nz = nz;
            this.dx = dx; this.dy = dy; this.dz = dz;

            this.sizeX = nx * dx;
            this.sizeY = ny * dy;
            this.sizeZ = nz * dz;
            this.minDelta = System.Math.Min(System.Math.Min(dx, dy), dz);

            int numEntries = nx * ny * nz * 2; // we allocate twice as many as need for collision skins
            gridEntries = new List<GridEntry>(numEntries);
            gridBoxes = new List<AABox>(numEntries);

            tempGridLists = new List<GridEntry>(numEntries);

            freeGrids = new Stack<GridEntry>(numEntries);
            for (int i = 0; i < numEntries; ++i)
            {
                GridEntry entry = new GridEntry();
                entry.GridIndex = -2;
                freeGrids.Push(entry);
            }

            for (int i = 0; i < (nx * ny * nz); ++i)
            {
                GridEntry gridEntry = freeGrids.Pop();
                gridEntry.GridIndex = i;
                gridEntries.Add(gridEntry);
                gridBoxes.Add(null);
            }

            overflowEntries = freeGrids.Pop();
            overflowEntries.GridIndex = -1;

            for (int iX = 0; iX < nx; ++iX)
            {
                for (int iY = 0; iY < ny; ++iY)
                {
                    for (int iZ = 0; iZ < nz; ++iZ)
                    {
                        AABox box = new AABox();
                        box.AddPoint(new Vector3(iX * dx, iY * dy, iZ + dz));
                        box.AddPoint(new Vector3(iX * dx + dx, iY * dy + dy, iZ * dz + dz));

                        int index = CalcIndex(iX, iY, iZ);
                        gridBoxes[index] = box;
                    }
                }
            }
        }
 /// <summary>
 /// Removes the entry by updating its neighbours. Also zaps the prev/next
 /// pointers in the entry, to help debugging
 /// </summary>
 /// <param name="entry"></param>
 public static void RemoveGridEntry(GridEntry entry)
 {
     // link the previous to the next (may be 0)
     entry.Previous.Next = entry.Next;
     // link the next (if it exists) to the previous.
     if (entry.Next != null)
         entry.Next.Previous = entry.Previous;
     // tidy up this entry
     entry.Previous = entry.Next = null;
     entry.GridIndex = -2;
 }
 /// <summary>
 /// Inserts an entry after prev, updating all links
 /// </summary>
 /// <param name="entry"></param>
 /// <param name="prev"></param>
 public static void InsertGridEntryAfter(GridEntry entry, GridEntry prev)
 {
     GridEntry next = prev.Next;
     prev.Next = entry;
     entry.Previous = prev;
     entry.Next = next;
     if (next != null)
         next.Previous = entry;
     entry.GridIndex = prev.GridIndex;
 }
 public GridEntry(CollisionSkin skin)
 {
     this.Skin = skin;
     this.Previous = this.Next = null;
 }