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