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]); }