public TmGrid()
{
this.G = 0.0;
this.H = 1.0;
this.F = this.G + this.H;
this.type = GridType.Sky;
this.bObstacle = false;
this.parent = null;
}
private double GetCostG(TmGrid neighour, TmGrid grid)
{
double cost = 1.0;
if (grid.x != neighour.x && grid.z != neighour.z)
{
cost = 1.4;
}
return(cost);
}
private void AssignNeighbour(int row, int column, ArrayList neighbours, TmGrid[,] grids)
{
if (row > -1 && column > -1 && row < grids.GetLength(0) && column < grids.GetLength(1))
{
TmGrid grid = grids[row, column];
if (!grid.bObstacle)
{
neighbours.Add(grid);
}
}
}
public TmGrid(TmGrid grid)
{
this.x = grid.x;
this.z = grid.z;
this.G = grid.G;
this.H = grid.H;
this.F = grid.F;
this.type = grid.type;
this.bObstacle = grid.bObstacle;
this.parent = grid.parent;
}
private ArrayList CalculatePath(TmGrid goal)
{
ArrayList list = new ArrayList();
while (goal != null)
{
list.Add(goal);
goal = goal.parent;
}
list.Reverse();
return(list);
}
public ArrayList FindPath(TmGrid start, TmGrid goal, TmGrid[,] grids)
{
//if (grids[start.x, start.z].bObstacle && grids[goal.x, goal.z].bObstacle) return null;
openList = new TmPriorityQueue();
closedList = new TmPriorityQueue();
openList.Add(start);
start.G = 0.0;
start.H = Math.Abs(goal.x - start.x) + Math.Abs(goal.z - start.z) + start.bH;
start.F = start.G + start.H;
TmGrid grid = null;
while (openList.Length != 0)
{
grid = openList.First();
if (grid == null)
{
return(null);
}
if (grid.x == goal.x && grid.z == goal.z)
{
return(CalculatePath(grid));
}
ArrayList neighbours = GetNeighbours(grid, grids);
for (int i = 0; i < neighbours.Count; i++)
{
TmGrid neighGrid = (TmGrid)neighbours[i];
if (!closedList.Contains(neighGrid))
{
double costG = GetCostG(neighGrid, grid);
//double costH = Math.Abs(goal.x - neighGrid.x) + Math.Abs(goal.z - neighGrid.z);
double costH = GetCostH(neighGrid, goal);
neighGrid.G = grid.G + costG;
neighGrid.H = costH;
neighGrid.F = neighGrid.G + neighGrid.H;
neighGrid.parent = grid;
if (!openList.Contains(neighGrid))
{
openList.Add(neighGrid);
}
}
}
closedList.Add(grid);
openList.Remove(grid);
}
if (grid.x != goal.x && grid.z != goal.z)
{
Console.WriteLine("Goal Not Find.");
return(null);
}
return(CalculatePath(grid));
}
public int CompareTo(object obj)
{
TmGrid grid = (TmGrid)obj;
if (this.F < grid.F)
{
return(-1);
}
if (this.F > grid.F)
{
return(1);
}
return(0);
}
private double GetCostH(TmGrid neighour, TmGrid goal)
{
double cost = 0.0;
double xx = Math.Abs(goal.x - neighour.x);
double yy = Math.Abs(goal.z - neighour.z);
if (xx >= yy)
{
cost = xx - yy + yy * 1.4 + neighour.bH;
}
else
{
cost = yy - xx + xx * 1.4 + neighour.bH;
}
return(cost);
}
private ArrayList GetNeighbours(TmGrid grid, TmGrid[,] grids)
{
ArrayList list = new ArrayList();
int row = grid.z;
int column = grid.x;
AssignNeighbour(row - 1, column - 1, list, grids); //左上
AssignNeighbour(row - 1, column, list, grids); //左上
AssignNeighbour(row - 1, column + 1, list, grids); //左上
AssignNeighbour(row, column - 1, list, grids); //左上
AssignNeighbour(row, column + 1, list, grids); //左上
AssignNeighbour(row + 1, column - 1, list, grids); //左上
AssignNeighbour(row + 1, column, list, grids); //左上
AssignNeighbour(row + 1, column + 1, list, grids); //左上
return(list);
}
public void Remove(TmGrid grid)
{
this.grids.Remove(grid);
this.grids.Sort();
}
public void Add(TmGrid grid)
{
this.grids.Add(grid);
this.grids.Sort();
}
