///<summary>
 ///    Return the count of unused neighbors in the given
 ///    GridDirection, to a maximum of distance.  This is used
 ///    by the polygon synthesis routines.
 ///</summary>
 internal int CountUnusedNeighbors(GridCell cell, GridDirection direction, int distance)
 {
     int count = 0;
     GridCell temp = cell;
     if (temp == null)
         return 0;
     for (int i=0; i<distance; i++) {
         if (temp.neighbors == null)
             break;
         temp = temp.neighbors[(int)direction];
         if (temp == null || temp.used)
             break;
         count++;
     }
     return count;
 }
 internal Vector3 CoordinatesOfCell(GridCell cell, GridDirection outside, bool start)
 {
     Vector3 loc = GridLocation(cell.loc);
     if (start) {
         loc.x += .5f * cellWidth * fromOutsideToStart[(int)outside, 0];
         loc.z += .5f * cellWidth * fromOutsideToStart[(int)outside, 1];
     }
     else {
         loc.x += .5f * cellWidth * fromOutsideToEnd[(int)outside, 0];
         loc.z += .5f * cellWidth * fromOutsideToEnd[(int)outside, 1];
     }
     return modelTransform * loc;
 }
 internal Vector3 CoordinatesOfCell(GridCell cell, int xOffset, int zOffset)
 {
     Vector3 loc = GridLocation(cell.loc);
     loc.x += .5f * cellWidth * xOffset;
     loc.z += .5f * cellWidth * zOffset;
     return modelTransform * loc;
 }
 internal GridPolygon(CellStatus status, GridCell c1, GridCell c2, GridCell c3, GridCell c4,
                      Vector3 c1Loc, Vector3 c2Loc, Vector3 c3Loc, Vector3 c4Loc)
 {
     this.status = status;
     corners = new GridCell[] { c1, c2, c3, c4 };
     cornerLocs = new Vector3[] { c1Loc, c2Loc, c3Loc, c4Loc };
 }
 internal GridPolygon(CellStatus status, GridCell[] corners, Vector3[] cornerLocs)
 {
     this.status = status;
     this.corners = corners;
     this.cornerLocs = cornerLocs;
 }
 internal GridEdge(GridCell start, GridCell end, GridDirection forward, GridDirection outside)
 {
     this.start = start;
     this.end = end;
     this.forward = forward;
     this.outside = outside;
 }
 ///<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>
 ///    Return the nth neighbor in the given direction.
 ///</summary>
 internal GridCell NthNeighborOrNull(GridCell cell, GridDirection direction, int n)
 {
     GridCell temp = cell;
     for (int i=0; i<n; i++) {
         if (temp == null) {
             return null;
         }
         temp = temp.neighbors[(int)direction];
     }
     return temp;
 }
 ///<summary>
 ///    For cells that have either same xCell value or the same
 ///    zcell value, return the count of cells between them,
 ///    including them.
 ///</summary>
 protected int CellDistance(GridCell c1, GridCell c2)
 {
     if (c1.loc.xCell == c2.loc.xCell)
         return Math.Abs(c1.loc.zCell - c2.loc.zCell) + 1;
     else if (c1.loc.zCell == c2.loc.zCell)
         return Math.Abs(c1.loc.xCell - c2.loc.xCell) + 1;
     else
         throw new Exception(string.Format("CellDistance not defined for cells {0} and {1}", c1, c2));
 }
 internal GridCell NextCellAtHeight(GridCell cell, GridDirection direction)
 {
     int i = cell.loc.xCell + incrByDirection[(int)direction, 0];
     int j = cell.loc.zCell + incrByDirection[(int)direction, 1];
     if (i < 0 || i >= xCount || j < 0 || j >= zCount)
         return null;
     GridCell other = FindCellAtHeight(i, j, cell.loc.height);
     if (other != null)
         return other;
     else
         return null;
 }
 ///<summary>
 ///    Return the nth neighbor in the given direction.  We've 
 ///    already counted the neighbors, so there should be no 
 ///    case of a null neighbor.
 ///</summary>
 internal GridCell NthNeighbor(GridCell cell, GridDirection direction, int n)
 {
     GridCell temp = NthNeighborOrNull(cell, direction, n);
     if (temp == null)
         throw new Exception(string.Format("Null neighbor in PathGenerator.NthNeighbor({0}, {1}, {2})",
                                           cell.ToString(), (int)direction, n));
     return temp;
 }
 internal void MarkNeighborsInaccessible(GridCell cell, GridDirection direction, int count)
 {
     cell.used = true;
     GridCell temp = cell;
     for (int i=0; i<count; i++) {
         temp = NextCellAtHeight(temp, direction);
         if (temp == null)
             return;
         temp.used = true;
         temp.status = CellStatus.Inaccessible;
     }
 }
 ///<summary>
 ///    Create a grid edge, given the starting and ending cell,
 ///    and the outside direction.  Note bene: The order of the
 ///    corners is important.  c1 is lower left; c2 is lower
 ///    right; c3 is upper right; c4 is upper left.
 ///</summary>
 internal GridPolygon MakeGridPolygon(CellStatus status, GridCell c1, GridCell c2, GridCell c3, GridCell c4)
 {
     return new GridPolygon(status, c1, c2, c3, c4,
         CoordinatesOfCell(c1, -1, -1),
         CoordinatesOfCell(c2, 1, -1),
         CoordinatesOfCell(c3, 1, 1),
         CoordinatesOfCell(c4, -1, 1));
 }
 ///<summary>
 ///    Create a grid edge, given the starting and ending cell,
 ///    and the outside direction.
 ///</summary>
 internal GridEdge MakeGridEdge(GridCell start, GridCell end, GridDirection forward, GridDirection outside)
 {
     // Canonicalize the directions
     if (forward == GridDirection.MinusX) {
         forward = GridDirection.PlusX;
         GridCell temp = start;
         start = end;
         end = temp;
     }
     else if (forward == GridDirection.MinusZ) {
         forward = GridDirection.PlusZ;
         GridCell temp = start;
         start = end;
         end = temp;
     }
     return new GridEdge(start, end, forward, outside);
 }
 ///<summary>
 ///    Find the largest polygon that can be formed out of
 ///    unused neighbor cells, where the cell argument is the
 ///    cell in the upper left-hand corner
 ///</summary>
 internal GridPolygon FindMaximumPolygon(GridCell cell)
 {
     // Get the length of the rows in the plus X and plus Z
     // directions
     int maxRowLength = CountUnusedNeighbors(cell, GridDirection.PlusX, xCount);
     int maxColumnLength = CountUnusedNeighbors(cell, GridDirection.PlusZ, zCount);
     // Discover the maximum number in each direction from each
     // axis
     int [] rowLengths = new int[maxColumnLength];
     int [] columnLengths = new int[maxRowLength];
     GridCell next = cell;
     for (int i=0; i<maxRowLength; i++) {
         next = next.neighbors[(int)GridDirection.PlusX];
         columnLengths[i] = CountUnusedNeighbors(next, GridDirection.PlusZ, maxColumnLength);
     }
     next = cell;
     for (int i=0; i<maxColumnLength; i++) {
         next = next.neighbors[(int)GridDirection.PlusZ];
         rowLengths[i] = CountUnusedNeighbors(next, GridDirection.PlusX, maxRowLength);
     }
     // Now loop through all pairs of counts, z counts fastest,
     // finding the polygon with the biggest area
     float area = 0;
     int bestX = maxRowLength;
     int bestZ = 0;
     int minRowLength = maxRowLength;
     for (int i=0; i<maxColumnLength; i++) {
         minRowLength = Math.Min(minRowLength, rowLengths[i]);
         int minColumnLength = maxColumnLength;
         for (int j=0; j<minRowLength; j++) {
             minColumnLength = Math.Min(minColumnLength, columnLengths[j]);
             float newArea = (float)(minColumnLength + 1) * (float)(j + 1);
             if (newArea > area) {
                 area = newArea;
                 bestX = j + 1;
                 bestZ = minColumnLength;
             }
         }
     }
     GridCell lastXCell = NthNeighbor(cell, GridDirection.PlusX, bestX);
     GridPolygon result = MakeGridPolygon(cell.status, cell, lastXCell,
         NthNeighbor(lastXCell, GridDirection.PlusZ, bestZ),
         NthNeighbor(cell, GridDirection.PlusZ, bestZ));
     // Mark all these cells as used
     cell.used = true;
     next = cell;
     GridCell nextZ;
     for (int i=0; i<bestX+1; i++) {
         nextZ = next;
         for (int j=0; j<bestZ+1; j++) {
             nextZ.used = true;
             nextZ.polygon = result;
             nextZ = nextZ.neighbors[(int)GridDirection.PlusZ];
         }
         next = next.neighbors[(int)GridDirection.PlusX];
     }
     return result;
 }
 ///<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;
 }