public static void PrintPath(Path fillPath, int x1, int y1)
{
int x = x1, y = y1;
Console.WriteLine(" " + x + ", " + y);
for (int i = 0; i < fillPath.NSteps; i++)
{
x += StepDX[(int)fillPath.Steps[i]];
y += StepDY[(int)fillPath.Steps[i]];
Console.WriteLine(" " + x + ", " + y);
}
}
public static int[][] UnrollPath(Path fillPath, int x1, int y1)
{
int[][] unrolled = new int[fillPath.NSteps + 1][];
int x = x1, y = y1;
for (int i = 0; i < fillPath.NSteps; i++)
{
unrolled[i] = new int[] {x, y};
x += StepDX[(int)fillPath.Steps[i]];
y += StepDY[(int)fillPath.Steps[i]];
}
unrolled[fillPath.NSteps] = new int[] {x, y};
return unrolled;
}
// returns a Path if there is one or null if not
public static Path BFSPath(int xStart, int yStart, int xEnd, int yEnd,
CanStep stepFrom, CanStep stepTo, StepCost costFrom, DirectedStepCost costTo,
MapRectangle limits, bool allowDiagonalSteps)
{
bool debug = false;
if (debug)
{
Console.WriteLine("BFS start");
}
// update bestCost to hold the Dijkstra map
if (bestCost == null || bestCost.Length < limits.w)
{
bestCost = new int[limits.w][];
visitedFrom = new eStep[limits.w][];
}
if (bestCost[0] == null || bestCost[0].Length < limits.h)
{
for (int x = 0; x < limits.w; x++)
{
bestCost[x] = new int[limits.h];
visitedFrom[x] = new eStep[limits.h];
}
}
// clear bestCost to -1 = unvisited
for (int x = 0; x < limits.w; x++)
{
for (int y = 0; y < limits.h; y++)
{
bestCost[x][y] = -1;
}
}
// forward pass
List<int> frontX = new List<int>();
List<int> frontY = new List<int>();
List<int> newFrontX = new List<int>();
List<int> newFrontY = new List<int>();
frontX.Add(xStart);
frontY.Add(yStart);
bestCost[xStart - limits.x][yStart - limits.y] = 0;
bool done = false;
while (!done)
{
if (debug)
{
Console.WriteLine("BFS new iteration, front size = " +
frontX.Count);
}
newFrontX.Clear();
newFrontY.Clear();
done = true;
for (int i = 0; i < frontX.Count; i++)
{
int baseCost = bestCost[frontX[i] - limits.x][
frontY[i] - limits.y];
if (costFrom != null)
{
baseCost += costFrom(frontX[i], frontY[i]);
}
if (stepFrom == null || stepFrom(frontX[i], frontY[i]))
{
for (int j = 0; j < 8; j += (allowDiagonalSteps ? 1 : 2))
{
int xNew = frontX[i] + StepDX[j];
int yNew = frontY[i] + StepDY[j];
if (debug)
{
Console.WriteLine(" attempt step from " + frontX[i] +
", " + frontY[i] + " dir " + j + " delta = " +
StepDX[j] + ", " + StepDY[j] + " to " + xNew + ", " +
yNew);
}
if (xNew >= limits.x && yNew >= limits.y &&
xNew <= limits.x2 && yNew <= limits.y2 &&
(stepTo == null || stepTo(xNew, yNew)))
{
int newCost = baseCost;
if (costTo != null)
{
newCost += costTo(xNew, yNew, (eStep)j);
}
int dx = xNew - limits.x;
int dy = yNew - limits.y;
int currentCost = bestCost[dx][dy];
if (currentCost == -1 ||
currentCost > newCost)
{
bestCost[dx][dy] = newCost;
visitedFrom[dx][dy] = ReverseStep[
(int)j];
newFrontX.Add(xNew);
newFrontY.Add(yNew);
if (debug)
{
Console.WriteLine(" step to " + xNew +
", " + yNew + " new cost = " +
newCost);
}
done = false;
}
}
}
}
}
if (!done)
{
List<int> swap = newFrontX; newFrontX = frontX; frontX = swap;
swap = newFrontY; newFrontY = frontY; frontY = swap;
}
}
if (bestCost[xEnd - limits.x][yEnd - limits.y] != -1)
{
// reverse pass and path gen
int x = xEnd, y = yEnd;
List<eStep> steps = new List<eStep>();
while (x != xStart || y != yStart)
{
eStep backStep = visitedFrom[x - limits.x][y - limits.y];
steps.Add(backStep);
int dx = StepDX[(int)backStep];
int dy = StepDY[(int)backStep];
x += dx;
y += dy;
}
Path solution = new Path(new eStep[steps.Count]);
for (int i = 0; i < steps.Count; i++)
{
solution.Steps[i] = ReverseStep[(int)steps[steps.Count - i - 1]];
}
return solution;
}
else
{
return null; // there is no path
}
}
// based on the Wikipedia implementation; will reuse fillPath to
// save allocations if possible. Null fillPath is ok.
public static Path GetBresenhamPath(int x1, int y1, int x2, int y2,
Path fillPath)
{
int dx = x2 - x1;
int dy = y2 - y1;
// handle the degenerate case
if (dx == 0 && dy == 0)
{
return new Path(new eStep[0]);
}
// move the problem to the first octant
eStep diagonalStep, straightStep;
if (dx < 0)
{
dx = -dx;
if (dy < 0)
{
dy = -dy;
if (dx >= dy)
{
diagonalStep = eStep.Southwest;
straightStep = eStep.West;
}
else
{
MathUtils.Swap(ref dx, ref dy);
diagonalStep = eStep.Southwest;
straightStep = eStep.South;
}
}
else
{
if (dx >= dy)
{
diagonalStep = eStep.Northwest;
straightStep = eStep.West;
}
else
{
MathUtils.Swap(ref dx, ref dy);
diagonalStep = eStep.Northwest;
straightStep = eStep.North;
}
}
}
else
{
if (dy < 0)
{
dy = -dy;
if (dx >= dy)
{
diagonalStep = eStep.Southeast;
straightStep = eStep.East;
}
else
{
MathUtils.Swap(ref dx, ref dy);
diagonalStep = eStep.Southeast;
straightStep = eStep.South;
}
}
else
{
if (dx >= dy)
{
diagonalStep = eStep.Northeast;
straightStep = eStep.East;
}
else
{
MathUtils.Swap(ref dx, ref dy);
diagonalStep = eStep.Northeast;
straightStep = eStep.North;
}
}
}
// allocate results
if (fillPath == null)
{
fillPath = new Path(new eStep[dx]);
}
else if (fillPath.Steps == null || fillPath.Steps.Length < dx)
{
fillPath.Steps = new eStep[dx];
fillPath.NSteps = dx;
}
else
{
fillPath.NSteps = dx;
}
// run the first-octant Bresenham line algorithm
int two_dy = dy + dy;
int two_dx = dx + dx;
int D = two_dy - dx;
for (int x = 0; x < dx; x++)
{
if (D > 0)
{
D -= two_dx;
fillPath.Steps[x] = diagonalStep;
}
else
{
fillPath.Steps[x] = straightStep;
}
D += two_dy;
}
return fillPath;
}