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); //无结果
}
Direction searchDir; //当前搜索方向
// 优先队列基本操作
public void initOpen(Open q) //优先队列初始化
{
q.length = 0; // 队内元素数初始为0
}
public Close shift(Open q)
{
return(q.Array[--q.length]);
}
