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