///<summary>
 ///     Add the location to the work list if it isn't already on the list
 ///</summary>
 internal void AddToWorkList(CellLocation loc)
 {
     foreach (CellLocation c in workList) {
         if (c.xCell == loc.xCell && c.zCell == loc.zCell && c.height == loc.height)
             return;
     }
     workList.Add(loc);
 }
 internal CellLocation(int xCell, int zCell, float height, CellLocation parent)
 {
     this.xCell = xCell;
     this.zCell = zCell;
     this.height = height;
     this.parent = parent;
 }
 internal CellLocation(CellLocation other)
 {
     this.xCell = other.xCell;
     this.zCell = other.zCell;
     this.height = other.height;
     this.parent = other.parent;
 }
 // The Plane is set when the status is OverCV or OverTerrain,
 // and when all the neighbors agree on the slope
 // internal Plane plane; (unused)
 internal GridCell(CellLocation loc)
 {
     this.loc = loc;
 }
 ///<summary>
 ///    Run the grid traversal algorithm, pushing the cells
 ///    around the model
 ///</summary>
 protected void TraverseGridCells()
 {
     cellsProcessed = 0;
     cellsOnCVs = 0;
     Stopwatch collisionStopwatch = new Stopwatch();
     int collisionCount = 0;
     // Create the CollisionParms object
     CollisionParms parms = new CollisionParms();
     // Set the big step size to 5% of the box height; this
     // will make the small step size .5% of the box height.
     // For a box height of 1.8 meters, this is .009m, or 9mm
     // Since the grid resolution is .25 of model width, and
     // for the human model, that width is .5m, the cell width
     // is .125m or 125mm.  So .009 / .125 = .072, so the
     // maximum variation in slope due exclusively to the step
     // size is 7.2%
     float stepSize = boxHeight * .05f;
     // Iterate over work list items until there aren't any
     while (workList.Count > 0) {
         CellLocation loc = workList[0];
         workList.RemoveAt(0);
         GridCell cell = FindCellAtHeight(loc);
         if (cell != null)
             // Skip, because it's already been visited
             continue;
         // Position the moving object over the cell
         SetMovingBoxOverCell(loc);
         // If we're above terrain level, we need to drop the
         // cell.  If we're at or below terrain level, we just
         // mark the cell as supported by the terrain
         float distanceToTerrain = movingBox.min.y - terrainLevel;
         if (distanceToTerrain <= 0f) {
             loc.height = terrainLevel;
             if (FindCellAtHeight(loc) != null)
                 continue;
             cell = new GridCell(loc);
             cell.status = CellStatus.OverTerrain;
         }
         else {
             // Now drop it until it hits a collision object, or
             // it's at terrain level.  Note: this means that we
             // can't have "basements" until we find a way to have
             // a non-constant terrain level
             Vector3 displacement = new Vector3(0, -distanceToTerrain, 0);
             collisionCount++;
             collisionStopwatch.Start();
             bool hit = collisionAPI.TestCollision(movingObject, stepSize, ref displacement, parms);
             collisionStopwatch.Stop();
             float oldHeight = loc.height;
             loc.height = movingBox.min.y;
             if (FindCellAtHeight(loc) != null)
                 continue;
             cell = new GridCell(loc);
             if (hit) {
                 // We hit a collision object - - if it's below
                 // us, then set the height accordingly.  If
                 // it's not below us, mark the cell as inaccessible.
                 if (displacement.y != -distanceToTerrain) {
                     cell.status = CellStatus.OverCV;
                     cell.supportingShape = parms.obstacle;
                     cellsOnCVs++;
                 }
                 else {
                     loc.height = oldHeight;
                     if (FindCellAtHeight(loc) != null)
                         continue;
                     cell.loc.height = oldHeight;
                     cell.status = CellStatus.Inaccessible;
                 }
             } else {
                 loc.height = terrainLevel;
                 cell.loc.height = terrainLevel;
                 cell.status = CellStatus.OverTerrain;
                 if (FindCellAtHeight(loc) != null)
                     continue;
             }
         }
         // Add the cell to the grid, now that we know its
         // actual height
         cellsProcessed++;
         grid[loc.xCell, loc.zCell].Add(cell);
         if (cell.status == CellStatus.Inaccessible)
             continue;
         // Now add the neighbors to the work list, if they
         // haven't already been visited
         for (GridDirection dir = GridDirection.PlusX; dir <= GridDirection.MinusZ; dir++) {
             int neighborX = loc.xCell + XIncrement[(int)dir];
             int neighborZ = loc.zCell + ZIncrement[(int)dir];
             // If the neighbor is outside the grid, ignore it
             if (neighborX < 0 || neighborX >= xCount ||
                 neighborZ < 0 || neighborZ >= zCount)
                 continue;
             // Test to see if it exists; if so, it's been visited
             CellLocation neighborLoc = new CellLocation(neighborX, neighborZ, cell.loc.height, loc);
             GridCell neighborCell = FindCellAtHeight(neighborLoc);
             // If it doesn't exist, add it to the work queue
             if (neighborCell == null)
                 workList.Add(neighborLoc);
                 //AddToWorkList(neighborLoc);
         }
     }
     DumpCurrentTime(string.Format("Processing {0} box drops, {1} collision tests, took {2} ms",
             collisionCount, collisionAPI.partCalls, collisionStopwatch.ElapsedMilliseconds));
     DumpGrid("Prefiltered Grid", GridDumpKind.Cells, false);
 }
 ///<summary>
 ///    Delete arcs smaller than the minimum feature size
 ///</summary>
 protected void DeleteTinyArcs(List<PolygonArc> arcs)
 {
     // The minimum width of an aggregated arc, in units of cells
     int minWidth = 2;
     List<PolygonArc> arcsToDelete = new List<PolygonArc>();
     // Now iterate through, finding the first in the pred
     // chain, and asking if the total chain length is long enough
     List<PolygonArc> firstArcs = new List<PolygonArc>();
     foreach (PolygonArc arc in arcs) {
         PolygonArc next = arc;
         while (next.pred != null)
             next = next.pred;
         firstArcs.Add(next);
     }
     foreach (PolygonArc arc in arcs) {
         PolygonArc first = arc;
         PolygonArc next = first;
         PolygonArc last = null;
         int count = 0;
         do {
             count += next.edge.LengthInCells();
             last = next;
             next = next.succ;
         } while (next != null);
         if (count <= minWidth) {
             // Not big enough to get through - - get rid of it
             next = first;
             do {
                 arcsToDelete.Add(next);
                 next = next.succ;
             } while (next != null);
         }
         else {
             CellLocation firstLoc = new CellLocation(first.edge.start.loc);
             CellLocation lastLoc = new CellLocation(last.edge.end.loc);
             int origCount = count;
             // See if there are ones at the ends that should
             // be added to the delete list
             next = first;
             while (next.edge.LengthInCells() <= minimumFeatureSize && count - minimumFeatureSize >= minWidth) {
                 arcsToDelete.Add(next);
                 count -= next.edge.LengthInCells();
                 next = next.succ;
             }
             PolygonArc newFirst = first;
             next = last;
             while (next.edge.LengthInCells() <= minimumFeatureSize && count - minimumFeatureSize >= minWidth) {
                 arcsToDelete.Add(next);
                 count -= next.edge.LengthInCells();
                 next = next.pred;
             }
             PolygonArc newLast = last;
             // Now see if it's possible to move start and end
             // of the arcs in so that they are half-width from
             // their maximum extent
             int maxExcess = (origCount - minWidth) / 2;
             if (maxExcess <= 0)
                 continue;
             int excess = maxExcess - CellDistance(first.edge.start, newFirst.edge.start);
             next = newFirst;
             while (next.edge.LengthInCells() <= excess) {
                 arcsToDelete.Add(next);
                 excess -= next.edge.LengthInCells();
                 next = next.succ;
             }
             while (excess > 0) {
                 next.edge.start.arc = null;
                 next.edge.start = NthNeighbor(next.edge.start, next.edge.forward, 1);
                 excess--;
             }
             excess = maxExcess - CellDistance(last.edge.end, newLast.edge.end);
             next = newLast;
             while (next != null && next.edge.LengthInCells() <= excess) {
                 arcsToDelete.Add(next);
                 excess -= next.edge.LengthInCells();
                 next = next.pred;
             }
             GridDirection forward = next.edge.forward;
             GridDirection back = (forward == GridDirection.PlusX ? GridDirection.MinusX : GridDirection.MinusZ);
             while (excess > 0) {
                 next.edge.start.arc = null;
                 next.edge.end = NthNeighbor(next.edge.end, back, 1);
                 excess--;
             }
         }
     }
     foreach (PolygonArc arc in arcsToDelete)
         arcs.Remove(arc);
     // Now give each arc an index, for the dump routine.
     for (int i=0; i<arcs.Count; i++)
         arcs[i].index = i + 1;
     // Remove references to deleted arcs
     for (int i=0; i<xCount; i++) {
         for (int j=0; j<zCount; j++) {
             foreach (GridCell cell in grid[i,j]) {
                 if (cell.arc != null && cell.arc.index == 0)
                     cell.arc = null;
             }
         }
     }
 }
 ///<summary>
 ///    Move the moving box so it's centered over the given
 ///    cell, about to drop
 ///</summary>
 internal void SetMovingBoxOverCell(CellLocation loc)
 {
     Vector3 min = lowerLeftCorner;
     min.x += (float)loc.xCell * cellWidth;
     min.y = loc.height + maxClimbDistance + maxDisjointDistance;
     min.z += (float)loc.zCell * cellWidth;
     Vector3 max = min;
     max.x = min.x + cellWidth;
     max.y = min.y + boxHeight;
     max.z = min.z + cellWidth;
     Vector3 center = 0.5f * (min + max);
     movingBox.min = min;
     movingBox.center = center;
     movingBox.max = max;
 }
 ///<summary>
 ///    Return the vector location of the cell in the grid
 ///</summary>
 internal Vector3 GridLocation(CellLocation loc)
 {
     Vector3 p = lowerLeftCorner;
     p.x += ((float)loc.xCell + .5f) * cellWidth;
     p.y = loc.height;
     p.z += ((float)loc.zCell + .5f) * cellWidth;
     return p;
 }
 ///<summary>
 ///    Return the cell at the grid location at the approximate
 ///    height, or return null.
 ///</summary>
 internal GridCell FindCellAtHeight(CellLocation loc)
 {
     List<GridCell> cells = grid[loc.xCell, loc.zCell];
     if (cells.Count == 0)
         return null;
     float range = maxClimbDistance + maxDisjointDistance + 10f;
     foreach (GridCell cell in cells) {
         if (cell.loc.height >= loc.height - range &&
             cell.loc.height <= loc.height + range)
             return cell;
     }
     return null;
 }
 ///<summary>
 ///    Count the number of cells at "compatible" heights,
 ///    starting with the given cell, and moving in the given
 ///    direction given by xIncr and zIncr.  If we get
 ///    distance-1 cells away, stop.  In any case, return the
 ///    count.
 ///</summary>
 internal int FeatureSpan(GridCell cell, int distance, int xIncr, int zIncr)
 {
     int count = 0;
     CellLocation loc = new CellLocation(cell.loc.xCell, cell.loc.zCell, cell.loc.height, null);
     for (int i=0; i<distance-1; i++) {
         loc.xCell += xIncr;
         loc.zCell += zIncr;
         if (loc.xCell < 0 || loc.xCell >= xCount ||
             loc.zCell < 0 || loc.zCell >= zCount)
             return count;
         GridCell newCell = FindCellAtHeight(loc);
         if (newCell == null || newCell.status != CellStatus.OverCV)
             return count;
         loc.height = newCell.loc.height;
         count++;
     }
     return count;
 }