private void UpdateMinDistance(IntDouble newlyfoundpIndex)
{
List <IntDouble> nlist = GetNeigboursList(newlyfoundpIndex.X, newlyfoundpIndex.Y);
for (int i = 0; i < nlist.Count; i++)
{
IntDouble nindex = nlist[i];
visited[nindex.X, nindex.Y] = true;
if (flagMap_Close[nindex.X, nindex.Y])
{
continue;
}
int gPassp = gMap[newlyfoundpIndex.X, newlyfoundpIndex.Y] + GetWeight(newlyfoundpIndex.X, newlyfoundpIndex.Y, nindex.X, nindex.Y);
if (set_Open.Exist(nindex))
{
if (gPassp < gMap[nindex.X, nindex.Y])
{
int oldvalue = fMap[nindex.X, nindex.Y];
gMap[nindex.X, nindex.Y] = gPassp;
fMap[nindex.X, nindex.Y] = gPassp + GetH(nindex);
set_Open.UpdateKey(nindex, oldvalue);
prev[nindex.X, nindex.Y] = newlyfoundpIndex;
}
}
else
{
gMap[nindex.X, nindex.Y] = gPassp;
fMap[nindex.X, nindex.Y] = gPassp + GetH(nindex);
prev[nindex.X, nindex.Y] = newlyfoundpIndex;
set_Open.Add(nindex);
}
}
}
public ResultCollection GetResult()
{
ResultCollection ret = new ResultCollection();
this.FindShortestPath();
resultPath = GetPath();
ret.distanceMap = this.distanceMap;
int[,] retMap = Consts.Init2dArray <int>(size, 0);
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
if (map[i, j] == 1)
{
retMap[i, j] = 1;
}
if (map[i, j] == 2)
{
retMap[i, j] = 2;
}
if (map[i, j] == 3)
{
retMap[i, j] = 3;
}
if (map[i, j] == 0 && visited[i, j])
{
retMap[i, j] = 5;
}
}
}
for (int i = 0; i < resultPath.Count; i++)
{
IntDouble p = resultPath[i];
if (retMap[p.X, p.Y] == 5)
{
retMap[p.X, p.Y] = 4;
}
}
ret.resultMap = retMap;
ret.inputMap = map;
int[,] parent = Consts.Init2dArray <int>(size, -1);
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
IntDouble parentDir = prev[i, j];
if (parentDir.X != -1 && parentDir.Y != -1)
{
int index = Consts.OffsetToIndex[parentDir.X - i + 1, parentDir.Y - j + 1];
parent[i, j] = index;
}
else
{
parent[i, j] = -1;
}
}
}
ret.parentMap = parent;
return(ret);
}
public BFS_Manhattan(int[,] map, int size)
{
this.map = map;
this.size = size;
this.start = GetStartPos();
this.end = GetEndPos();
InitDistanceMap();
}
public IntDouble ExtractMin()
{
int insetIndex = GetMinIndex();
IntDouble ret = reachedSet[insetIndex];
RemoveAt(insetIndex);
return(ret);
}
private void Swap(int i, int j)
{
IntDouble temp = heapArray[i];
heapArray[i] = heapArray[j];
heapArray[j] = temp;
setIndexMap[heapArray[i].X, heapArray[i].Y] = i; //record new position
setIndexMap[heapArray[j].X, heapArray[j].Y] = j; //record new position
}
private void FindShortestPath()
{
bool[,] visited = new bool[size, size];
pathmap = new IntDouble[size, size];
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
pathmap[i, j].X = -1; pathmap[i, j].Y = -1;
visited[i, j] = false;
}
}
records_passedblocks = new List <IntDouble>();
Queue <IntDouble> queue = new Queue <IntDouble>();
queue.Enqueue(start);
visited[start.X, start.Y] = true;
distanceMap[start.X, start.Y] = 0;
IntDouble[] aroundPArray = new IntDouble[4];
path = new List <IntDouble>();
while (queue.Count != 0)
{
IntDouble p = queue.Dequeue();
InitAroundPos(aroundPArray, p);
for (int i = 0; i < aroundPArray.Length; i++)
{
IntDouble t = aroundPArray[i];
if (t.X == end.X && t.Y == end.Y)
{
pathmap[t.X, t.Y].X = p.X;
pathmap[t.X, t.Y].Y = p.Y;
distanceMap[t.X, t.Y] = distanceMap[p.X, p.Y] + 10;
IntDouble cur = p;
path.Add(end);
while (!(cur.X == start.X && cur.Y == start.Y))
{
path.Add(cur);
cur = pathmap[cur.X, cur.Y];
}
path.Add(start);
path.Reverse();
return;
}
if (InMap(t.X, t.Y) && !visited[t.X, t.Y] && map[t.X, t.Y] == 0)
{
visited[t.X, t.Y] = true;
distanceMap[t.X, t.Y] = distanceMap[p.X, p.Y] + 10;
queue.Enqueue(t);
pathmap[t.X, t.Y].X = p.X;
pathmap[t.X, t.Y].Y = p.Y;
records_passedblocks.Add(t);
}
}
}
}
public void UpdateKey(IntDouble pindex, int oldValue)
{
if (values[pindex.X, pindex.Y] > oldValue)
{
ShiftDown(setIndexMap[pindex.X, pindex.Y]);
}
else
{
ShiftUp(setIndexMap[pindex.X, pindex.Y]);
}
}
private List <IntDouble> GetPath()
{
IntDouble cur = end;
while (!(cur.X == start.X && cur.Y == start.Y))
{
resultPath.Add(cur);
cur = prev[cur.X, cur.Y];
}
resultPath.Add(start);
resultPath.Reverse();
return(resultPath);
}
public IntDouble ExtractMin()
{
if (GetCount() == 0)
{
return(new IntDouble(-1, -1));
}
IntDouble pindex = heapArray[0];
Swap(0, heapArray.Count - 1);
heapArray.RemoveAt(heapArray.Count - 1);
ShiftDown(0);
setIndexMap[pindex.X, pindex.Y] = -1;
return(pindex);
}
public DijkstraProcessor(int[,] map, int size)
{
this.map = map;
this.size = size;
this.tempList = new List <IntDouble>();
this.tempArray = new IntDouble[8];
this.resultPath = new List <IntDouble>();
this.start = GetStartPos();
this.end = GetEndPos();
this.prev = Consts.Init2dArray <IntDouble>(size, new IntDouble(-1, -1));
this.flagMap_Close = Consts.Init2dArray <bool>(size, false);
this.distanceMap = Consts.Init2dArray <int>(size, int.MaxValue);
this.set_Open = new DijkstraSet_Heap(size * size, distanceMap);
this.visited = Consts.Init2dArray <bool>(size, false);
}
public ResultCollection GetResult()
{
this.FindShortestPath();
ResultCollection ret = new ResultCollection();
ret.inputMap = map;
int[,] retMap = new int[size, size];
int[,] parentMap = new int[size, size];
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
retMap[i, j] = map[i, j];
parentMap[i, j] = -1;
}
}
for (int i = 0; i < records_passedblocks.Count; i++)
{
IntDouble p = records_passedblocks[i];
if (map[p.X, p.Y] == 0)
{
retMap[p.X, p.Y] = 5;
}
}
for (int i = 0; i < path.Count; i++)
{
IntDouble p = path[i];
if (map[p.X, p.Y] == 0)
{
retMap[p.X, p.Y] = 4;
}
}
ret.resultMap = retMap;
for (int i = 0; i < records_passedblocks.Count; i++)
{
IntDouble p = records_passedblocks[i];
IntDouble parentPos = pathmap[p.X, p.Y];
int index = Consts.OffsetToIndex[parentPos.X - p.X + 1, parentPos.Y - p.Y + 1];
parentMap[p.X, p.Y] = index;
}
int index2 = Consts.OffsetToIndex[pathmap[end.X, end.Y].X - end.X + 1, pathmap[end.X, end.Y].Y - end.Y + 1];
parentMap[end.X, end.Y] = index2;
ret.parentMap = parentMap;
ret.distanceMap = distanceMap;
return(ret);
}
private bool FindShortestPath()
{
set_Open.Add(start);
distanceMap[start.X, start.Y] = 0;
while (set_Open.GetCount() != 0)
{
IntDouble pindexnewlyfound = set_Open.ExtractMin(); // vertex with index "pindexnewlyfound" found its s-path
flagMap_Close[pindexnewlyfound.X, pindexnewlyfound.Y] = true; //mark it
if (pindexnewlyfound.X == end.X && pindexnewlyfound.Y == end.Y)
{
return(true);
}
UpdateMinDistance(pindexnewlyfound);// update its neighbour's s-distance
}
return(false);
}
private void InitAroundPos(IntDouble[] aroundPArray, IntDouble p)
{
aroundPArray[0].X = p.X - 1;
aroundPArray[0].Y = p.Y;
aroundPArray[1].X = p.X + 1;
aroundPArray[1].Y = p.Y;
aroundPArray[2].X = p.X;
aroundPArray[2].Y = p.Y - 1;
aroundPArray[3].X = p.X;
aroundPArray[3].Y = p.Y + 1;
if (aroundPArray.Length > 4)
{
aroundPArray[4].X = p.X - 1;
aroundPArray[4].Y = p.Y - 1;
aroundPArray[5].X = p.X + 1;
aroundPArray[5].Y = p.Y - 1;
aroundPArray[6].X = p.X - 1;
aroundPArray[6].Y = p.Y + 1;
aroundPArray[7].X = p.X + 1;
aroundPArray[7].Y = p.Y + 1;
}
}
private void UpdateMinDistance(IntDouble newlyfoundpIndex)
{
List <IntDouble> nlist = GetNeigboursList(newlyfoundpIndex.X, newlyfoundpIndex.Y);
for (int i = 0; i < nlist.Count; i++)
{
IntDouble nindex = nlist[i];
visited[nindex.X, nindex.Y] = true;
if (!flagMap_Close[nindex.X, nindex.Y])
{
if (distanceMap[nindex.X, nindex.Y] == int.MaxValue)
{
set_Open.Add(nindex);//newly approached vertex is pushed into set
}
if (distanceMap[nindex.X, nindex.Y] > distanceMap[newlyfoundpIndex.X, newlyfoundpIndex.Y] + GetWeight(newlyfoundpIndex.X, newlyfoundpIndex.Y, nindex.X, nindex.Y))
{
distanceMap[nindex.X, nindex.Y] = distanceMap[newlyfoundpIndex.X, newlyfoundpIndex.Y] + GetWeight(newlyfoundpIndex.X, newlyfoundpIndex.Y, nindex.X, nindex.Y);
set_Open.DecreaseKey(nindex);// vertex with index "nindex" update its s-distance,so it's position in heap should also be updated.
prev[nindex.X, nindex.Y] = newlyfoundpIndex;
}
}
}
}
private int GetH(IntDouble p)
{
return(Consts.GetEvaDistance_Euro(p.X, p.Y, end.X, end.Y));
}
public void Add(IntDouble pindex)
{
this.heapArray.Add(pindex);
setIndexMap[pindex.X, pindex.Y] = heapArray.Count - 1;
ShiftUp(heapArray.Count - 1);
}
public void DecreaseKey(IntDouble pindex)
{
//do nothing cuz this element distribution won't change with the key
}
public bool Exist(IntDouble pindex)
{
return(setIndexMap[pindex.X, pindex.Y] != -1);
}
public void Add(IntDouble p)
{
this.reachedSet.Add(p);
}
public void DecreaseKey(IntDouble pindex)
{
ShiftUp(setIndexMap[pindex.X, pindex.Y]);
}
