}//end of sortqueue
void setstatus(Close[,] a, int hp, int wp, int gx, int gy, int sx, int sy)
{
for (int j = 0; j < hp; j++)
{
for (int i = 0; i < wp; i++)
{
if (i == gx && j == gy)
{
a[i, j].Node.status = "GOAL";
}
else if (i == sx && j == sy)
{
a[i, j].Node.status = "START";
}
else if (a[i, j].Node.node_Type == false)
{
a[i, j].Node.status = "OBSTACLE";
}
else
{
a[i, j].Node.status = "CLEAR";
}
}
}
}//end of set status
}//end of set status
void settag(Close[,] a, int hp, int wp)
{
for (int j = 0; j < hp; j++)
{
for (int i = 0; i < wp; i++)
{
a[i, j].Node.tag = "NEW";
}
}
}//end of settag
/// <summary>
/// // 获取最短路径
/// </summary>
/// <returns></returns>
void GetShortestPath(List <MyPoint> route, Close[,] close)
{
Close p = (close[endX, endY]);
while (p != null) //转置路径
{
route.Add(new MyPoint(p.Node.x, p.Node.y, p.Node.direction));
p = p.From;
}
route.Reverse();
}
}//end of settag
//SET DIMATION VALUE OF MY NODE
void setdimval(Close[,] a, int hp, int wp)
{
for (int j = 0; j < hp; j++)
{
for (int i = 0; i < wp; i++)
{
a[i, j].Node.x = i;
a[i, j].Node.y = j;
}
}
}//end of setdimval
}//end of linemove
void sortqueue(Queue <Close> OpenList, Close[,] state)
{
int ind = OpenList.Count;
double[] kk = new double[ind];
int[] ii = new int[ind];
int[] jj = new int[ind];
Close d = new Close();
for (int i = 0; i < ind; i++)
{
d = (Close)OpenList.Dequeue();
kk[i] = d.G;
ii[i] = d.Node.x;
jj[i] = d.Node.y;
}
for (int i = 0; i < ind; i++)
{
for (int j = 0; j < ind; j++)
{
if (kk[i] < kk[j])
{
double tempk = kk[i];
kk[i] = kk[j];
kk[j] = tempk;
int tempi = ii[i];
ii[i] = ii[j];
ii[j] = tempi;
int tempj = jj[i];
jj[i] = jj[j];
jj[j] = tempj;
}
}
}
for (int i = 0; i < ind; i++)
{
OpenList.Enqueue(state[ii[i], jj[i]]);
}
}//end of sortqueue
public int astar()
{ // A*算法遍历
//int times = 0;
int i, curX, curY, surX, surY;
float surG;
Open q = new Open(); //Open表
Close p = new Close();
close = new Close[Height, Width];
initOpen(q);
initClose(close, srcX, srcY, dstX, dstY);
close[srcX, srcY].vis = true;
push(q, close, srcX, srcY, 0);
while (q.length > 0)
{ //times++;
p = shift(q);
curX = p.cur.x;
curY = p.cur.y;
if (p.H == 0)
{
return(Sequential);
}
for (i = 0; i < 8; i++)
{
if ((p.cur.sur & (1 << i)) == 0)
{
continue;
}
surX = curX + (int)dir[i].x;
surY = curY + (int)dir[i].y;
if (!close[surX, surY].vis)
{
close[surX, surY].vis = true;
close[surX, surY].from = p;
surG = p.G + (float)(Math.Abs(curX - surX) + Math.Abs(curY - surY));
push(q, close, surX, surY, surG);
}
}
}
//System.Console.Write("times: %d\n", times);
return(NoSolution); //无结果
}
public void push(Open q, Close[,] cls, int x, int y, float g)
{ //向优先队列（Open表）中添加元素
Close t;
int i, mintag;
cls[x, y].G = g; //所添加节点的坐标
cls[x, y].F = cls[x, y].G + cls[x, y].H;
q.Array[q.length++] = (cls[x, y]);
mintag = q.length - 1;
for (i = 0; i < q.length - 1; i++)
{
if (q.Array[i].F < q.Array[mintag].F)
{
mintag = i;
}
}
t = q.Array[q.length - 1];
q.Array[q.length - 1] = q.Array[mintag];
q.Array[mintag] = t; //将评价函数值最小节点置于队头
}
// 地图Close表初始化配置
void initClose(Close[,] cls, Node[,] graph)
{
int i, j;
for (i = 0; i < Height; i++)
{
for (j = 0; j < Width; j++)
{
cls[i, j] = new Close {
};
cls[i, j].Node = graph[i, j]; // Close表所指节点
cls[i, j].Node.isSearched = -1; // !(graph[i, j].node_Type); // 是否被访问
cls[i, j].From = null; // 所来节点
cls[i, j].G = 0; //需要根据前面的点计算
cls[i, j].H = Math.Abs(endX - i) + Math.Abs(endY - j); // 评价函数值
cls[i, j].F = 0; //cls[i, j].G + cls[i, j].H;
}
}
//cls[endX, endY].G = AstarUtil.Infinity; //移步花费代价值
//cls[beginX, beginY].F = cls[beginX, beginY].H; //起始点评价初始值
}
public static void DijkstraPush(List <Close> q, Close[,] cls, int x, int y, double g)
{ //向优先队列（Open表）中添加元素
Close t;
int i, mintag;
cls[x, y].G = g; //所添加节点的坐标
cls[x, y].F = cls[x, y].G; // + cls[x, y].H;
q.Add(cls[x, y]);
mintag = 0;
for (i = 0; i < q.Count - 1; i++)
{
if (q[i].F < q[mintag].F)
{
mintag = i;
}
}
t = q[0];
q[0] = q[mintag];
q[mintag] = t; //将评价函数值最小节点置于列表最前面
}
// 地图初始化操作
public void initClose(Close[,] cls, int sx, int sy, int dx, int dy)
{ // 地图Close表初始化配置
int i, j;
for (i = 0; i < Height; i++)
{
for (j = 0; j < Width; j++)
{
cls[i, j] = new Close {
};
cls[i, j].node = graph[i, j]; // Close表所指节点
cls[i, j].vis = !(graph[i, j].node_Type); // 是否被访问
cls[i, j].from = null; // 所来节点
cls[i, j].G = cls[i, j].F = 0;
cls[i, j].H = Math.Abs(dx - i) + Math.Abs(dy - j); // 评价函数值
}
}
cls[sx, sy].F = cls[sx, sy].H; //起始点评价初始值
// cls[sy,sy].G = 0; //移步花费代价值
cls[dx, dy].G = Infinity;
}
void InitGraph()
{
graph = new MapNode[size.y, size.x];
close = new Close[size.y, size.x];
//地图发生变化时重新构造地
for (int y = 0; y < size.y; y++)
{
for (int x = 0; x < size.x; x++)
{
graph[y, x] = new MapNode();
graph[y, x].pos.y = y; //地图坐标X
graph[y, x].pos.x = x; //地图坐标Y
graph[y, x].value = (NodeType)map[y, x];
graph[y, x].reachable = (graph[y, x].value == NodeType.Reachable); // 节点可到达性
graph[y, x].sur = 0; //邻接节点个数
if (!graph[y, x].reachable)
{
continue;
}
if (x > 0)
{
if (graph[y, x - 1].reachable) // left节点可以到达
{
graph[y, x].sur |= West;
graph[y, x - 1].sur |= East;
}
if (y > 0)
{
if (graph[y - 1, x - 1].reachable &&
graph[y - 1, x].reachable && // 左，上，左上都可到达时
graph[y, x - 1].reachable) // up-left节点可以到达
{
graph[y, x].sur |= North_West;
graph[y - 1, x - 1].sur |= South_East;
}
}
}
if (y > 0)
{
if (graph[y - 1, x].reachable) // up节点可以到达
{
graph[y, x].sur |= North;
graph[y - 1, x].sur |= South;
}
if (x < size.x - 1)
{
//print(y - 1);
//print(x + 1);
//print("0:" + graph.GetUpperBound(0));
//print("1:" + graph.GetUpperBound(1));
if (graph[y - 1, x + 1].reachable &&
graph[y - 1, x].reachable && // 上，右，上右都可到达时
(NodeType)map[y, x + 1] == NodeType.Reachable) // up-right节点可以到达
{
graph[y, x].sur |= North_East;
graph[y - 1, x + 1].sur |= South_West;
}
}
}
}
}
graph[src.y, src.x].value = NodeType.Source;
graph[dst.y, dst.x].value = NodeType.Destination;
}
int astar()
{ // A*算法遍历
//int times = 0;
int i, curX, curY, surX, surY;
float surG;
Open open = new Open(); //Open表
Close curPoint = new Close();
// curPoint.node = new MapNode();
//curPoint.node.direction = this.searchDir;
//curPoint.node.stopTime = 1;
close = new Close[Height, Width];
initOpen(open);
initClose(close, srcX, srcY, dstX, dstY);
close[srcX, srcY].vis = true;
push(open, close, srcX, srcY, 0);
while (open.length > 0)
{ //times++;
curPoint = shift(open);
curX = curPoint.node.x;
curY = curPoint.node.y;
if (curPoint.from == null)
{
curPoint.node.direction = this.searchDir;
}
else
{
curPoint.node.direction = getDirection(curPoint.from, curPoint);//0525
}
for (i = 0; i < 4; i++)
{
if ((curPoint.node.adjoinNodeCount & (1 << i)) == 0)
{
continue;
}
surX = curX + (int)dir[i].X;
surY = curY + (int)dir[i].Y;
//if (surX < 0 || surY < 0)
//{
// Console.WriteLine("走出场地外");
// continue;
//}
if (!close[surX, surY].vis)
{
close[surX, surY].vis = true;
close[surX, surY].from = curPoint;
Direction tempDir = new Direction();
int tempPassDifficulty = 2;
switch (i)
{
case 0:
tempDir = Direction.Right;
tempPassDifficulty = graph[curX, curY].rightDifficulty;
break;
case 1:
tempDir = Direction.Down;
tempPassDifficulty = graph[curX, curY].downDifficulty;
break;
case 2:
tempDir = Direction.Left;
tempPassDifficulty = graph[curX, curY].leftDifficulty;
break;
case 3:
tempDir = Direction.Up;
tempPassDifficulty = graph[curX, curY].upDifficulty;
break;
}
int directionCost = (tempDir == curPoint.node.direction) ? 0 : 1;
// curPoint.node.stopTime = 1+directionCost * 2;
int tempTraConges = graph[curX, curY].traCongesIntensity;
//curPoint.searchDir = close[surX, surY].searchDir;
surG = curPoint.G + (float)(Math.Abs(curX - surX) + Math.Abs(curY - surY)) + SWERVE_COST * (directionCost + tempTraConges) + tempPassDifficulty;
push(open, close, surX, surY, surG);
}
}
if (curPoint.H == 0)
{
return(Sequential);
}
}
//System.Console.Write("times: %d\n", times);
return(NoSolution); //无结果
}
} //end of k_min
private List <MyPoint> findpath(Close[,] map, int gx, int gy)
{
List <MyPoint> route = new List <MyPoint>();
double[] mink = new double[1]; mink[0] = 100000;
int[] mi = new int[1]; mi[0] = 0;
int[] mj = new int[1]; mj[0] = 0;
int[] neighbori = new int[8];
int[] neighborj = new int[8];
int[] Mi = new int[4];
int[] Mj = new int[4];
string[] y = new string[4];
string[] z = new string[4];
double pathcoast = 0.0;
pathcoast += map[correnti, correntj].H;
int goali = gx;
int goalj = gy;
bool ddd = true;
do
{
vs(neighbori, neighborj, correnti, correntj);
mink[0] = 10000;
for (int i = 0; i < 8; i++)
{
int row = neighbori[i]; int col = neighborj[i];
if ((row <= wp - 1 && row >= 0) && (col <= hp - 1 && col >= 0) && (map[row, col].Node.status != "OBSTACLE"))
{
k_min(row, col, map[row, col], mink, mi, mj);
}
}
int mii = mi[0]; int mjj = mj[0];
//To Here we find the minimum k value
Mi[0] = correnti + 1; Mj[0] = correntj + 1;
Mi[1] = correnti + 1; Mj[1] = correntj - 1;
Mi[2] = correnti - 1; Mj[2] = correntj + 1;
Mi[3] = correnti - 1; Mj[3] = correntj - 1;
if (correntj + 1 > hp - 1)
{
z[0] = "NULL";
}
else
{
z[0] = map[correnti, correntj + 1].Node.status;
}
if (correntj - 1 < 0)
{
z[1] = "NULL";
}
else
{
z[1] = map[correnti, correntj - 1].Node.status;
}
if (correnti - 1 < 0)
{
z[2] = "NULL";
}
else
{
z[2] = map[correnti - 1, correntj].Node.status;
}
z[3] = z[1];
if (correnti + 1 > wp - 1)
{
y[0] = "NULL";
}
else
{
y[0] = map[correnti + 1, correntj].Node.status;
}
y[1] = y[0];
if (correntj + 1 > hp - 1)
{
y[2] = "NULL";
}
else
{
y[2] = map[correnti, correntj + 1].Node.status;
}
if (correnti - 1 < 0)
{
y[3] = "NULL";
}
else
{
y[3] = map[correnti - 1, correntj].Node.status;
}
for (int ii = 0; ii < 4; ii++)
{
if (mii == Mi[ii] && mjj == Mj[ii])
{
if (y[ii] == "OBSTACLE" && z[ii] == "OBSTACLE")
{
map[mii, mjj].H = 10000;
map[mii, mjj].Node.status = "OBSTACLE";
insert(OpenList, map[mii, mjj]);
int[] si = new int[8];
int[] sj = new int[8];
vs(si, sj, mii, mjj);
for (int i = 0; i < 8; i++)
{
if (map[si[i], sj[i]].Node.tag == "CLOSE")
{
insert(OpenList, map[si[i], sj[i]]);
}
}
int iold, jold;
double kold;
bool good;
good = true;
sortqueue(OpenList, map);
do
{
Close d = new Close();
d = (Close)OpenList.Dequeue();
kold = d.G;
iold = d.Node.x;
jold = d.Node.y;
map[iold, jold].Node.tag = "CLOSE";
if (iold == correnti && jold == correntj)
{
good = false;
}
vs(si, sj, iold, jold);
if (kold == map[iold, jold].H)
{
for (int i = 0; i < 8; i++)
{
int r = si[i];
int c = sj[i];
if ((r <= wp - 1 && r >= 0) && (c <= hp - 1 && c >= 0))
{
double co = cost(iold, jold, r, c);
double tt = co + map[iold, jold].H;
if (map[r, c].Node.tag == "NEW")
{
map[r, c].From = map[iold, jold];
map[r, c].H = tt;
map[r, c].G = tt;
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
else if (((map[r, c].From == map[iold, jold]) && (map[r, c].H != tt)) || ((map[r, c].From != map[iold, jold]) && ((float)map[r, c].H > (float)tt)))
{
if (map[r, c].Node.status != "OBSTACLE")
{
map[r, c].From = map[iold, jold];
map[r, c].H = tt;
if (map[r, c].Node.tag == "CLOSE")
{
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
}
}
}
}
}
if (kold < map[iold, jold].H)
{
for (int i = 0; i < 8; i++)
{
int r = si[i];
int c = sj[i];
if ((r <= wp - 1 && r >= 0) && (c <= hp - 1 && c >= 0))
{
double co = cost(iold, jold, r, c);
double tt = co + map[iold, jold].H;
if (map[r, c].Node.tag == "NEW")
{
map[r, c].From = map[iold, jold];
map[r, c].H = tt;
map[r, c].G = tt;
insert(OpenList, map[r, c]);
}
else
{
if ((map[r, c].From == map[iold, jold]) && (map[r, c].H != tt))
{
map[r, c].From = map[iold, jold];
if (map[r, c].Node.tag == "CLOSE")
{
if (map[iold, jold].H == 10000)
{
map[r, c].H = 10000;
}
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
else
{
map[r, c].H = tt;
if (map[r, c].H > 10000)
{
map[r, c].H = 10000;
}
}
}
else
{
if ((map[r, c].From != map[iold, jold]) && (map[r, c].H > tt))
{
map[iold, jold].G = tt;
insert(OpenList, map[iold, jold]);
sortqueue(OpenList, map);
}
else
{
if (map[r, c].From != map[iold, jold])
{
if (map[iold, jold].H > map[r, c].H + cost(iold, jold, r, c))
{
if (map[r, c].Node.tag == "CLOSE")
{
if (map[r, c].H > kold)
{
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
}
}
}
}
}
}
}
}
}
} while (good);
bool end = true;
while (end)
{
Close d = new Close();
d = (Close)OpenList.Dequeue();
kold = d.G;
iold = d.Node.x;
jold = d.Node.y;
map[iold, jold].Node.tag = "CLOSE";
if (kold < map[correnti, correntj].H)
{
vs(si, sj, iold, jold);
if (kold == map[iold, jold].H)
{
for (int i = 0; i < 8; i++)
{
int r = si[i];
int c = sj[i];
if ((r <= wp - 1 && r >= 0) && (c <= hp - 1 && c >= 0))
{
double co = cost(iold, jold, r, c);
double tt = map[iold, jold].H + co;
if (map[r, c].Node.tag == "NEW")
{
map[r, c].From = map[iold, jold];
map[r, c].H = map[iold, jold].H + co;
map[r, c].G = map[r, c].H;
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
else if (((map[r, c].From == map[iold, jold]) && ((float)map[r, c].H != (float)tt)) || ((map[r, c].From != map[iold, jold]) && ((float)map[r, c].H > (float)tt)))
{
if (map[r, c].Node.status != "OBSTACLE")
{
map[r, c].From = map[iold, jold];
map[r, c].H = map[iold, jold].H + co;
map[r, c].G = map[r, c].H;
if (map[r, c].Node.tag == "CLOSE")
{
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
}
}
}
}
}
else if (kold < map[iold, jold].H)
{
for (int i = 0; i < 8; i++)
{
int r = si[i];
int c = sj[i];
if ((r <= 4 && r >= 0) && (c <= 4 && c >= 0))
{
double co = cost(iold, jold, r, c);
double tt = map[iold, jold].H + co;
if (map[r, c].Node.tag == "NEW")
{
map[r, c].From = map[iold, jold];
map[r, c].H = tt;
map[r, c].G = tt;
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
else if ((map[r, c].From == map[iold, jold]) && ((float)map[r, c].H != (float)tt))
{
map[r, c].From = map[iold, jold];
if (map[r, c].Node.tag == "CLOSE")
{
map[r, c].H = tt;
if (map[r, c].H > 10000)
{
map[r, c].H = 10000;
}
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
}
else if ((map[r, c].From != map[iold, jold]) && ((float)map[r, c].H > (float)tt))
{
if (map[iold, jold].Node.tag == "CLOSE")
{
map[iold, jold].G = map[iold, jold].H;
insert(OpenList, map[iold, jold]);
sortqueue(OpenList, map);
}
}
else if ((map[r, c].From != map[iold, jold]) && ((float)map[iold, jold].H > (float)(map[r, c].H + co)) && (map[r, c].Node.tag == "CLOSE") && (map[r, c].H > (float)kold))
{
insert(OpenList, map[r, c]);
sortqueue(OpenList, map);
}
}
}
}
}
else
{
end = false;
OpenList.Enqueue(map[iold, jold]);
sortqueue(OpenList, map);
}
}
mii = correnti;
mjj = correntj;
}
}
}
correnti = mii;
correntj = mjj;
if ((correnti == goali) && (correntj == goalj))
{
ddd = false;
}
// Form1.ActiveForm.Refresh();
pathcoast += map[correnti, correntj].H;
route.Add(new MyPoint(correnti, correntj, Direction.Left));
Console.Write("(" + correnti.ToString() + ", " + correntj.ToString() + ") - ");
} while (ddd);
Console.WriteLine();
return(route);
}