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