/// <summary>Creates a new instance of PathFinder.</summary>
/// <param name="numRows">The number of rows in the grid.</param>
/// <param name="numCols">The number of columns in the grid.</param>
/// <param name="inWorld">The world that contains this path finder.</param>
public PathFinder(int numRows, int numCols, World inWorld)
{
clear();
InWorld = inWorld;
Grid = new PathNode[numRows, numCols];
Nodes = new PathNode[numRows * numCols];
NodeSpacing = (VectorD)inWorld.Size / new VectorD((double)numCols, (double)numRows);
// build grid
PathNode node = null; // temporary variable
// initialize each node in the grid
for (int row = 0; row < numRows; row++)
{
for (int col = 0; col < numCols; col++)
{
node = new PathNode();
node.Index = row * NumCols + col;
node.Position = new VectorD((double)col * NodeSpacing.X, (double)row * NodeSpacing.Y);
Grid[row, col] = node;
Nodes[node.Index] = node;
}
}
// connect nodes to each other, if not blocked
for (int row = 0; row < numRows; row++)
{
for (int col = 0; col < numCols; col++)
{
// use collision detection to rule out connections (only geos can block)
node = Grid[row, col];
PathNode destNode = null;
VectorF nodePos = (VectorF)node.Position;
if (col < numCols - 1)
{
// upper-right
if (row > 0)
{
destNode = Grid[row - 1, col + 1];
if (!inWorld.Collision.segCollision(nodePos, (VectorF)destNode.Position, null, null, false, false, true))
{
node.addLast(new PathEdge(this, node.Index, destNode.Index));
destNode.addLast(new PathEdge(this, destNode.Index, node.Index));
}
}
// right
destNode = Grid[row, col + 1];
if (!inWorld.Collision.segCollision(nodePos, (VectorF)destNode.Position, null, null, false, false, true))
{
node.addLast(new PathEdge(this, node.Index, destNode.Index));
destNode.addLast(new PathEdge(this, destNode.Index, node.Index));
}
// lower right
if (row < numRows - 1)
{
destNode = Grid[row + 1, col + 1];
if (!inWorld.Collision.segCollision(nodePos, (VectorF)destNode.Position, null, null, false, false, true))
{
node.addLast(new PathEdge(this, node.Index, destNode.Index));
destNode.addLast(new PathEdge(this, destNode.Index, node.Index));
}
}
}
// bottom
if (row < numRows - 1)
{
destNode = Grid[row + 1, col];
if (!inWorld.Collision.segCollision(nodePos, (VectorF)destNode.Position, null, null, false, false, true))
{
node.addLast(new PathEdge(this, node.Index, destNode.Index));
destNode.addLast(new PathEdge(this, destNode.Index, node.Index));
}
}
}
}
}