/**
* Determine the distance between two neighbor Cells
* as used by the AStar algorithm.
*
* @param cell1 any Cell
* @param cell2 any of cell1's neighbors
* @return Float - the distance between the two neighbors
*/
public float getDistanceBetween(AStarCell cell1, AStarCell cell2)
{
//if the cells are on top or next to each other, return 1
if (cell1.getX() == cell2.getX() || cell1.getY() == cell2.getY())
{
return 1;//*(mapHeight+mapWith);
}
else
{ //if they are diagonal to each other return diagonal distance: sqrt(1^2+1^2)
return SQRT2;//*(mapHeight+mapWith);
}
}
int CellRiskDegree(AStarCell cell)
{
int safeDistance = 6;
if (someoneIsArrested)
{
safeDistance = 16;
}
if (cell.pos[1] != policePosition[1])
{
return(0);
}
else if (Distance(cell.pos, policePosition) >= safeDistance)
{
return(0);
}
else
{
return(safeDistance - Distance(cell.pos, policePosition));
}
}
public Vector2 Raycast(Vector2 startPos, Vector2 endPos)
{
Point startCell = PathFinder.posToCell(startPos, astarMap.getCellSize());
Point goalCell = PathFinder.posToCell(endPos, astarMap.getCellSize());
// exception: start is obstacle. Now just return start
if (AStarCell.isObstacle(astarMap.getCell(startCell.x, startCell.y)))
{
return(startPos);
}
if (startCell.Equals(goalCell))
{
return(endPos);
}
Point hitCell = pathFinder.raycast(startPos, endPos, astarMap.getCellSize());
Vector2 hitPos = PathFinder.cellToPos(hitCell, astarMap.getCellSize());
Vector2 dir = (endPos - startPos).normalized;
hitPos = PathFinder.getNextPosBeforeNextCell(hitPos, dir, astarMap.getCellSize());
return(hitPos);
}
public Point getCellWithErrorTolerate(float x, float y)
{
int cellX = (int)Math.Floor((x + map.getErrorTolerate()) / map.getCellSize());
int cellY = (int)Math.Floor((y + map.getErrorTolerate()) / map.getCellSize());
AStarCell cell = map.getCell(cellX, cellY);
if (!AStarCell.isObstacle(cell))
{
return(new Point(cellX, cellY));
}
cellX = (int)Math.Floor((x - map.getErrorTolerate()) / map.getCellSize());
cellY = (int)Math.Floor((y - map.getErrorTolerate()) / map.getCellSize());
cell = map.getCell(cellX, cellY);
if (!AStarCell.isObstacle(cell))
{
return(new Point(cellX, cellY));
}
cellX = (int)Math.Floor((x + map.getErrorTolerate()) / map.getCellSize());
cellY = (int)Math.Floor((y - map.getErrorTolerate()) / map.getCellSize());
cell = map.getCell(cellX, cellY);
if (!AStarCell.isObstacle(cell))
{
return(new Point(cellX, cellY));
}
cellX = (int)Math.Floor((x - map.getErrorTolerate()) / map.getCellSize());
cellY = (int)Math.Floor((y + map.getErrorTolerate()) / map.getCellSize());
cell = map.getCell(cellX, cellY);
if (!AStarCell.isObstacle(cell))
{
return(new Point(cellX, cellY));
}
return(null);
}
public Vector3 getNextDirection(Vector3[] bricks, Vector2 startpos, Vector2 startdir, Vector3[] coins)
{
// Initialize the AStar grid
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
asgrid[i, j] = new AStarCell(new Vector2(i, j));
}
}
// Set stones and bricks locations
for (int i = 0; i < stones.Length; i++) asgrid[(int)stones[i].X, (int)stones[i].Y].setType(2, 100);
for (int i = 0; i < bricks.Length; i++) asgrid[(int)bricks[i].X, (int)bricks[i].Y].setType(1, (int)bricks[i].Z);
// Set coin locations
goals = new AStarCell[coins.Length];
for (int i = 0; i < coins.Length; i++)
{
goals[i] = asgrid[(int)coins[i].X, (int)coins[i].Y];
goals[i].profit = (int)coins[i].Z;
}
// Start cell
scell = asgrid[(int)startpos.X, (int)startpos.Y];
scell.dir = startdir;
SimulateAStar();
Vector3 move= AStarMove();
if (asgrid[(int)(startpos.X + move.X), (int)(startpos.Y + move.Y)].cost > 1)
{
move.X = 0;
move.Y = 0;
}
return move;
}
List <AStarCell> FindWay(int[] startPosition, int[] endPosition) //, List<List<List<int?>>> map, int[] mapSize
{
int floor = startPosition[1];
List <AStarCell> track = new List <AStarCell>();
List <List <AStarCell> > AStarMap = new List <List <AStarCell> >();
foreach (int x in Range(mapSize[0]))
{
List <AStarCell> nextX = new List <AStarCell>();
foreach (int z in Range(mapSize[2]))
{
nextX.Add(new AStarCell(new int[] { x, floor, z }));
}
AStarMap.Add(nextX);
}
List <AStarCell> openSet = new List <AStarCell>();
AStarMap[startPosition[0]][startPosition[2]].state = "start";
AStarMap[startPosition[0]][startPosition[2]].disStart = 0;
AStarMap[startPosition[0]][startPosition[2]].disEnd = Distance(startPosition, endPosition);
AStarMap[startPosition[0]][startPosition[2]].disSum = 0 + Distance(startPosition, endPosition);
openSet.Add(AStarMap[startPosition[0]][startPosition[2]]);
while (openSet != null)
{
openSet = openSet.OrderBy(m => m.disSum).ToList();
if ((openSet[0].pos[0] == endPosition[0]) && (openSet[0].pos[1] == endPosition[1]) && (openSet[0].pos[2] == endPosition[2]))
{
//logger.info("[FIND!!!]");
AStarCell cellBack = openSet[0];
track.Add(cellBack);
while (cellBack.disStart != 0)
{
switch (cellBack.direction)
{
case "U":
cellBack = AStarMap[cellBack.pos[0]][cellBack.pos[2] - 1];
track.Add(cellBack);
break;
case "D":
cellBack = AStarMap[cellBack.pos[0]][cellBack.pos[2] + 1];
track.Add(cellBack);
break;
case "R":
cellBack = AStarMap[cellBack.pos[0] - 1][cellBack.pos[2]];
track.Add(cellBack);
break;
case "L":
cellBack = AStarMap[cellBack.pos[0] + 1][cellBack.pos[2]];
track.Add(cellBack);
break;
}
}
track.Reverse();
foreach (int i in Range(track.Count() - 1))
{
track[i].direction = track[i + 1].direction;
}
track.Remove(track.Last());
break;
}
else
{
int[] pos = openSet[0].pos;
int[,] dir = new int[4, 2] {
{ 0, 1 }, { 0, -1 }, { 1, 0 }, { -1, 0 }
};
AStarMap[pos[0]][pos[2]].state = "close";
openSet.Remove(openSet[0]);
foreach (int i in Range(4))
{
if ((pos[0] + dir[i, 0] >= 0) && (pos[0] + dir[i, 0] < mapSize[0]) &&
(pos[2] + dir[i, 1] >= 0) && (pos[2] + dir[i, 1] < mapSize[2]))
{
if (map[pos[0] + dir[i, 0]][floor - 1][pos[2] + dir[i, 1]] != 1)
{
if (AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].state == "unreached")
{
int[] posRound = { pos[0] + dir[i, 0], floor, pos[2] + dir[i, 1] };
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].state = "open";
openSet.Add(AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]]);
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart = AStarMap[pos[0]][pos[2]].disStart + 1;
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disEnd = Distance(posRound, endPosition);
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disSum = AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart + AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disEnd;
switch (i)
{
case 0:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "U";
break;
case 1:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "D";
break;
case 2:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "R";
break;
case 3:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "L";
break;
}
}
else if (AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].state == "open")
{
if ((AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart > AStarMap[pos[0]][pos[2]].disStart + 1) ||
((AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart == AStarMap[pos[0]][pos[2]].disStart + 1) && (Distance(AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].pos, policePosition) < Distance(AStarMap[pos[0]][pos[2]].pos, policePosition))))
{
int[] posRound = { pos[0] + dir[i, 0], floor, pos[2] + dir[i, 1] };
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart = AStarMap[pos[0]][pos[2]].disStart + 1;
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disEnd = Distance(posRound, endPosition);
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disSum = AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disStart + AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].disEnd;
switch (i)
{
case 0:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "U";
break;
case 1:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "D";
break;
case 2:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "R";
break;
case 3:
AStarMap[pos[0] + dir[i, 0]][pos[2] + dir[i, 1]].direction = "L";
break;
}
}
}
}
}
}
}
}
return(track);
}
/**
* Sets up the Cells of the map with the With and Height specified in the constructor
* or set methods.
*/
private void createMap(int[][] obstacleMap)
{
map.Clear();
for (int x = 0; x < mapWith; x++)
{
for (int y = 0; y < mapHeight; y++)
{
bool obstacle = (obstacleMap[y][x] == 1);
if (obstacle)
{
continue;
}
AStarCell cell = new AStarCell(this, x, y);
int cellId = y << 16 | x;
map.Add(cellId, cell);
}
}
}
private List<Point> reconstructPath(AStarCell cell)
{
List<Point> path = new List<Point>();
while (!(cell.getPreviousCell() == null))
{
path.Insert(0, cell.getPoint());
cell = cell.getPreviousCell();
}
path.Insert(0, cell.getPoint());
return path;
}
