public void Step1_SetMap(PixelMap map)
{
m_map = map;
m_mask = m_map.CreateMask();
m_heap = m_map.CreateHeap();
m_pathNodes = m_map.CreatePathNodes();
}
public static List<int> SearchMap(PixelMap map, PathFindingTask pathFinding, bool[] mask, IndexHeap heap, PathNodes nodes)
{
var width = map.Width;
var height = map.Height;
var goalPosX = pathFinding.TargetX;
var goalPosY = pathFinding.TargetY;
heap.Compare = (int a, int b) =>
(nodes.Data[a].Travelled + nodes.Data[a].DistanceToGoal).CompareTo(
nodes.Data[b].Travelled + nodes.Data[b].DistanceToGoal);
IsVisitedDelegate IsVisited = (int x, int y) =>
x < 0 || y < 0 || x >= width || y >= height || mask[x + y * width];
// Contains the id of previous node.
int node = -1;
AddDelegate Add = delegate(int x, int y) {
int id = x + y * width;
nodes.Data[id].PreviousId = node;
int dx = goalPosX - x;
int dy = goalPosY - y;
// Use square of length because it is faster.
int dist = dx * dx + dy * dy;
nodes.Data[id].DistanceToGoal = dist;
if (node == -1) {
nodes.Data[id].Travelled = 0;
} else {
dx = x - node % width;
dy = y - node / width;
// Use square of length because it is faster.
dist = dx * dx + dy * dy;
nodes.Data[id].Travelled = nodes.Data[node].Travelled + dist;
}
return id;
};
var start = Add(pathFinding.StartX, pathFinding.StartY);
heap.Push(start);
// Set values from the walls to the mask.
mask.Initialize();
var walls = map.Walls;
int walls_length = walls.Count >> 1;
int i, j, e, s;
for (i = walls_length - 1; i >= 0; i--) {
s = walls[i << 1];
e = walls[(i << 1) + 1];
for (j = e - 1; j >= s; j--) {
mask[j] = true;
}
}
mask[start] = true;
int n;
int nodex;
int nodey;
while (heap.Cursor > 1) {
node = heap.Pop();
nodey = node / width;
nodex = node - nodey * width;
if (nodex == goalPosX &&
nodey == goalPosY) {
var list = new List<int>();
while (nodes.Data[node].PreviousId != -1) {
list.Add(node);
node = nodes.Data[node].PreviousId;
}
return list;
}
if (!IsVisited(nodex + 1, nodey)) {
n = Add(nodex + 1, nodey);
heap.Push(n);
mask[n] = true;
}
if (!IsVisited(nodex, nodey + 1)) {
n = Add(nodex, nodey + 1);
heap.Push(n);
mask[n] = true;
}
if (!IsVisited(nodex - 1, nodey)) {
n = Add(nodex - 1, nodey);
heap.Push(n);
mask[n] = true;
}
if (!IsVisited(nodex, nodey - 1)) {
n = Add(nodex, nodey - 1);
heap.Push(n);
mask[n] = true;
}
}
return null;
}