private _Object[] diagonalSuccessors(bool xN, bool xS, bool xE, bool xW, int N, int S, int E, int W, int[][] grid, int rows, int cols, _Object[] result, int i)
{
if (xN)
{
if (xE && grid[N][E] == 0)
{
result[i++] = new _Object(E, N);
}
if (xW && grid[N][W] == 0)
{
result[i++] = new _Object(W, N);
}
}
if (xS)
{
if (xE && grid[S][E] == 0)
{
result[i++] = new _Object(E, S);
}
if (xW && grid[S][W] == 0)
{
result[i++] = new _Object(W, S);
}
}
return(result);
}
private _Object[] diagonalSuccessorsFree(bool xN, bool xS, bool xE, bool xW, int N, int S, int E, int W, int[][] grid, int rows, int cols, _Object[] result, int i)
{
xN = N > -1;
xS = S < rows;
xE = E < cols;
xW = W > -1;
if (xE)
{
if (xN && grid[N][E] == 0)
{
result[i++] = new _Object(E, N);
}
if (xS && grid[S][E] == 0)
{
result[i++] = new _Object(E, S);
}
}
if (xW)
{
if (xN && grid[N][W] == 0)
{
result[i++] = new _Object(W, N);
}
if (xS && grid[S][W] == 0)
{
result[i++] = new _Object(W, S);
}
}
return(result);
}
private _Object[] diagonalSuccessorsFree(bool xN, bool xS, bool xE, bool xW, int N, int S, int E, int W, int[,] g, int rows, int cols, _Object[] res, int i)
{
xN = N > -1;
xS = S < rows;
xE = E < cols;
xW = W > -1;
if (xE)
{
if (xN && g[N, E] == 0)
{
res[i++] = new _Object(E, N);
}
if (xS && g[S, E] == 0)
{
res[i++] = new _Object(E, S);
}
}
if (xW)
{
if (xN && g[N, W] == 0)
{
res[i++] = new _Object(W, N);
}
if (xS && g[S, W] == 0)
{
res[i++] = new _Object(W, S);
}
}
return(res);
}
private double euclidean(_Object start, _Object end)
{
var x = start.x - end.x;
var y = start.y - end.y;
return(Mathf.Sqrt(x * x + y * y));
}
/* --------------------------------------------------------------------------------------------*/
/* --------------------------------------------------------------------------------------------*/
/* --------------------------------------------------------------------------------------------*/
/* -------------------------------------- UTIL FUNCTION ---------------------------------------*/
/* --------------------------------------------------------------------------------------------*/
/* --------------------------------------------------------------------------------------------*/
/* --------------------------------------------------------------------------------------------*/
private _Object[] diagonalSuccessors(bool xN, bool xS, bool xE, bool xW, int N, int S, int E, int W, int[,] g, int rows, int cols, _Object[] res, int i)
{
if (xN)
{
if (xE && g[N, E] == 0)
{
res[i++] = new _Object(E, N);
}
if (xW && g[N, W] == 0)
{
res[i++] = new _Object(W, N);
}
}
if (xS)
{
if (xE && g[S, E] == 0)
{
res[i++] = new _Object(E, S);
}
if (xW && g[S, W] == 0)
{
res[i++] = new _Object(W, S);
}
}
return(res);
}
public void TestDoubleConversion()
{
_Object a = 1;
a++;
Assert.AreEqual(2, (int)a);
Assert.AreEqual(2, (int)_Object.Identity(a++));
Assert.AreEqual(4, (int)_Object.Identity(++a));
}
private _Object[] successors(int x, int y, int[,] g, int rows, int cols)
{
int N = y - 1;
int S = y + 1;
int E = x + 1;
int W = x - 1;
bool xN = N > -1 && g[N, x] == 0;
bool xS = S < rows && g[S, x] == 0;
bool xE = E < cols && g[y, E] == 0;
bool xW = W > -1 && g[y, W] == 0;
int i = 0;
_Object[] res = new _Object[8];
if (xN)
{
res[i++] = new _Object(x, N);
}
if (xE)
{
res[i++] = new _Object(E, y);
}
if (xS)
{
res[i++] = new _Object(x, S);
}
if (xW)
{
res[i++] = new _Object(W, y);
}
_Object[] obj;
if (this.find == AstarMoveMode.Diagonal || this.find == AstarMoveMode.Euclidean)
{
obj = diagonalSuccessors(xN, xS, xE, xW, N, S, E, W, g, rows, cols, res, i);
}
else if (this.find == AstarMoveMode.DiagonalFree || this.find == AstarMoveMode.EuclideanFree)
{
obj = diagonalSuccessorsFree(xN, xS, xE, xW, N, S, E, W, g, rows, cols, res, i);
}
else
{
obj = res;
}
return(obj);
}
private _Object[] successors(int x, int y, int[][] grid, int rows, int cols)
{
int N = y - 1;
int S = y + 1;
int E = x + 1;
int W = x - 1;
bool xN = N > -1 && grid[N][x] == 0;
bool xS = S < rows && grid[S][x] == 0;
bool xE = E < cols && grid[y][E] == 0;
bool xW = W > -1 && grid[y][W] == 0;
int i = 0;
_Object[] result = new _Object[8];
if (xN)
{
result[i++] = new _Object(x, N);
}
if (xE)
{
result[i++] = new _Object(E, y);
}
if (xS)
{
result[i++] = new _Object(x, S);
}
if (xW)
{
result[i++] = new _Object(W, y);
}
_Object[] obj =
(this.find == "Diagonal" || this.find == "Euclidean") ? diagonalSuccessors(xN, xS, xE, xW, N, S, E, W, grid, rows, cols, result, i):
(this.find == "DiagonalFree" || this.find == "EuclideanFree")? diagonalSuccessorsFree(xN, xS, xE, xW, N, S, E, W, grid, rows, cols, result, i):
nothingToDo(xN, xS, xE, xW, N, S, E, W, grid, rows, cols, result, i);
return(obj);
}
private _Object[] successors(int x, int y, int[][] grid, int rows, int cols)
{
int N = y - 1;
int S = y + 1;
int E = x + 1;
int W = x - 1;
bool xN = N > -1 && grid[N][x] == 0;
bool xS = S < rows && grid[S][x] == 0;
bool xE = E < cols && grid[y][E] == 0;
bool xW = W > -1 && grid[y][W] == 0;
int i = 0;
_Object[] result = new _Object[8];
if (xN)
{
result[i++] = new _Object(x, N);
}
if (xE)
{
result[i++] = new _Object(E, y);
}
if (xS)
{
result[i++] = new _Object(x, S);
}
if (xW)
{
result[i++] = new _Object(W, y);
}
_Object[] obj = result;
return(obj);
}
public Pathfinder(int[][] grid, int[] s, int[] e, string f)
{
this.find = (f == null) ? "Diagonal" : f;
int cols = grid[0].Length;
int rows = grid.Length;
int limit = cols * rows;
int length = 1;
List <_Object> open = new List <_Object>();
open.Add(new _Object(s[0], s[1]));
open[0].f = 0;
open[0].g = 0;
open[0].v = s[0] + s[1] * cols;
_Object current;
List <int> list = new List <int>();
double distanceS;
double distanceE;
int i;
int j;
double max;
int min;
_Object[] next;
_Object adj;
_Object end = new _Object(e[0], e[1]);
end.v = e[0] + e[1] * cols;
bool inList;
do
{
max = limit;
min = 0;
for (i = 0; i < length; i++)
{
if (open[i].f < max)
{
max = open[i].f;
min = i;
}
}
current = open[min];
open.RemoveAt(min);
if (current.v != end.v)
{
--length;
next = successors(current.x, current.y, grid, rows, cols);
for (i = 0, j = next.Length; i < j; ++i)
{
if (next[i] == null)
{
continue;
}
(adj = next[i]).p = current;
adj.f = adj.g = 0;
adj.v = adj.x + adj.y * cols;
inList = false;
foreach (int key in list)
{
if (adj.v == key)
{
inList = true;
}
}
if (!inList)
{
if (this.find == "DiagonalFree" || this.find == "Diagonal")
{
distanceS = diagonal(adj, current);
distanceE = diagonal(adj, end);
}
else if (this.find == "Euclidean" || this.find == "EuclideanFree")
{
distanceS = euclidean(adj, current);
distanceE = euclidean(adj, end);
}
else
{
distanceS = manhattan(adj, current);
distanceE = manhattan(adj, end);
}
adj.f = (adj.g = current.g + distanceS) + distanceE;
open.Add(adj);
list.Add(adj.v);
length++;
}
}
}
else
{
i = length = 0;
do
{
this.result.Add(new Vector2(current.x, current.y));
}while ((current = current.p) != null);
result.Reverse();
}
}while (length != 0);
}
private double manhattan(_Object start, _Object end)
{
return(Mathf.Abs(start.x - end.x) + Mathf.Abs(start.y - end.y));
}
private double diagonal(_Object start, _Object end)
{
return(Mathf.Max(Mathf.Abs(start.x - end.x), Mathf.Abs(start.y - end.y)));
}
private double sneaky(_Object start, _Object end)
{
return(towerMatrix[end.y][end.x]);
}
public static _Object Identity(_Object o)
{
return(o);
}
private void CalculatePath()
{
int cols = this.grid.GetLength(1);
int rows = this.grid.GetLength(0);
int limit = cols * rows;
int length = 1;
List <_Object> open = new List <_Object>();
open.Add(new _Object(this.from[0], this.from[1]));
open[0].f = 0;
open[0].g = 0;
open[0].v = this.from[0] + this.from[1] * cols;
_Object current;
List <int> list = new List <int>();
double distanceS;
double distanceE;
int i;
int j;
double max;
int min;
_Object[] next;
_Object adj;
_Object end = new _Object(this.to[0], this.to[1]);
end.v = this.to[0] + this.to[1] * cols;
bool inList;
do
{
max = limit;
min = 0;
for (i = 0; i < length; ++i)
{
if (open[i].f < max)
{
max = open[i].f;
min = i;
}
}
current = open[min];
open.RemoveAt(min);
if (current.v != end.v)
{
--length;
next = successors(current.x, current.y, this.grid, rows, cols);
for (i = 0, j = next.Length; i < j; ++i)
{
if (next[i] == null)
{
continue;
}
adj = next[i];
adj.p = current;
adj.f = adj.g = 0;
adj.v = adj.x + adj.y * cols;
inList = false;
foreach (int key in list)
{
if (adj.v == key)
{
inList = true;
}
}
if (!inList)
{
if (this.find == AstarMoveMode.DiagonalFree || this.find == AstarMoveMode.Diagonal)
{
distanceS = diagonal(adj, current);
distanceE = diagonal(adj, end);
}
else if (this.find == AstarMoveMode.Euclidean || this.find == AstarMoveMode.EuclideanFree)
{
distanceS = euclidean(adj, current);
distanceE = euclidean(adj, end);
}
else
{
distanceS = manhattan(adj, current);
distanceE = manhattan(adj, end);
}
adj.f = (adj.g = current.g + distanceS) + distanceE;
open.Add(adj);
list.Add(adj.v);
length++;
}
}
}
else
{
i = length = 0;
do
{
this.result.Add(new Vector2(current.x, current.y));
}while((current = current.p) != null);
this.result.Reverse();
}
}while(length != 0);
}
