A* 寻路算法给出奇怪的路径（文本地图 - 无 GUI）
A* Pathfinding algorithm giving weird path (Text Map - No GUI)
我正在尝试解决我的 A* 路径查找算法。我有三个 类：Menu、Grid 和 Node。
如果你 运行 我的程序，你会看到它打印出一条不寻常的螺旋、跳跃、兔子跳跃路径。我相信意外行为与以下功能有关：
printAStarPath(int startx, int starty, int endx, int endy)
在我看来，我认为问题与父节点设置不当有关。我很确定 Node 和 Menu 工作正常。
输入：
菜单功能主要管理用户输入。用户可以添加墙壁、开始位置和结束位置，以及网格的大小。我还在菜单中包含了一些测试（因此您不必每次测试都重新输入所有内容）。 printAStarPath(...) 函数接受起始 x,y 位置和结束 x,y 位置。
输出：
我想要这样打印网格：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
不幸的是，我很疯狂：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][*]
[ ][S][ ][X][ ][E][*]
[ ][ ][*][X][ ][ ][*]
[ ][ ][ ][*][*][*][ ]
另一个不同输入会发生什么的例子：
[E][ ][ ][ ][ ]
[ ][*][ ][ ][*]
[ ][ ][X][ ][*]
[ ][ ][ ][ ][*]
[ ][*][ ][*][S]
我的一些网格看起来像指向右下的箭头。有些看起来像是向右下方然后螺旋上升并向左。
总结：
我正在使用 A* 路径查找方法，并且正在通过曼哈顿方法计算启发式（或 H 成本）。我使用递归来获取准确的 G 成本以及从结束位置追溯到开始位置。
Here's the article that I followed.
代码如下：
Menu:
package pathFinding;
import java.util.*;
public class Menu {
private Grid board;
public Menu(){
board = new Grid(0, 0);
}//end constructor
static public void main (String[] args){
Menu pft = new Menu();
pft.boardMenu();
System.out.print("process terminated.");
}//end main
public void boardMenu(){
// very user error prone !
boolean kg = true;
Scanner in = new Scanner(System.in);
int input;
while(kg){
input = -1;
System.out.print("\n\n\n\n");
System.out.print(">>> Hi there,"
+ "\n(0) quit"
+ "\n(1) test 1"
+ "\n(2) test 2"
+ "\n(3) new board"
+ "\n>>> ");
input = in.nextInt();
if (input == 3){
this.initUserData();
} else if (input == 0){
kg = false;
} else if (input == 1){
board.setSize(7, 5);
board.setCollidable(3, 1);
board.setCollidable(3, 2);
board.setCollidable(3, 3);
board.printAStarPath(1, 2, 5, 2);
} else if (input == 2){
board.setSize(25, 25);
board.setCollidable(5, 4);
board.setCollidable(4, 5);
board.setCollidable(3, 3);
board.printAStarPath(15, 15, 4, 4);
}
} // end while
} // end boardMenu
public void initUserData(){
boolean kgTmp = true;
int xTmp, yTmp, iTmp, jTmp = 0;
// initiate input device
Scanner in = new Scanner(System.in);
// 0: determine board size
System.out.print("\nBoard width: ");
xTmp = in.nextInt();
System.out.print("\nBoard height: ");
yTmp = in.nextInt();
board.setSize(xTmp, yTmp);
// 1: determine obstruction locations
kgTmp = true;
while(kgTmp){
System.out.print("\nObstruction x Loc: ");
xTmp = in.nextInt();
System.out.print("\nObstruction y Loc: ");
yTmp = in.nextInt();
board.setCollidable(xTmp, yTmp);
System.out.print("\nMore Obstructions?(0=no;1=yes): ");
if(in.nextInt() == 0)
kgTmp = false;
} // end while
// 2: determine start location
System.out.print("\nStart x Loc: ");
xTmp = in.nextInt();
System.out.print("\nStart y Loc: ");
yTmp = in.nextInt();
// 3: determine end location
System.out.print("\nEnd x Loc: ");
iTmp = in.nextInt();
System.out.print("\nEnd y Loc: ");
jTmp = in.nextInt();
System.out.println("\nredy for astar");
// 4: determine and print A* path
board.printAStarPath(xTmp, yTmp, iTmp, jTmp);
System.out.println("\nastar shud be done?");
} // end initBoardData
} // end class def
Grid:
package pathFinding;
import java.util.*;
public class Grid {
private List<List<Node>> grid;
List<List<Integer>> path;
private int width;
private int height;
//------------------------------------------------------------------------
// Name: Constructor
// Desc: Takes in a width & height. Initializes stuff.
//------------------------------------------------------------------------
public Grid(int width, int height){
grid = new ArrayList<List<Node>>();
path = new ArrayList<List<Integer>>();
this.width = width;
this.height = height;
initGrid(width, height);
} // end constructor
//------------------------------------------------------------------------
// Name: initGrid
// Desc: initializes the grid with data
//------------------------------------------------------------------------
public void initGrid(int w, int h){
// add columns
for (int i=0;i<w;i++)
grid.add(new ArrayList<Node>());
// fill grid with nodes
for (int i=0;i<w;i++)
for (int j=0;j<h;j++)
grid.get(i).add(new Node(i, j));
} // end initGrid
//------------------------------------------------------------------------
// Name: setSize
// Desc: Sets the size of the grid
//------------------------------------------------------------------------
public void setSize(int w, int h){
this.width = w;
this.height = h;
// update the nodes
clearAll();
initGrid(width, height);
} // end setSize
//------------------------------------------------------------------------
// Name: clearAll
// Desc: Clears any data in grid and path
//------------------------------------------------------------------------
public void clearAll(){
// removes all rows/columns/nodes
grid.clear();
path.clear();
} // end clearAll
//------------------------------------------------------------------------
// Name: printGrid
// Desc: Prints the whole grid
//------------------------------------------------------------------------
public void printGrid(){
// prints every node's value
// loop thru columns
for (int j=0;j<height;j++){
// thru row
for (int i=0;i<width;i++)
grid.get(i).get(j).printText();
System.out.println();
} // end j loop
} // end printGrid
//------------------------------------------------------------------------
// Name: setCollidable
// Desc: Sets a node at x,y to collidable
//------------------------------------------------------------------------
public void setCollidable(int x, int y){
// makes a node at x,y collidable
grid.get(x).get(y).setCollidable(true);
grid.get(x).get(y).setText("[X]");
} // end setCollidable
//------------------------------------------------------------------------
// Name: printAStarPath
// Desc: Finds and prints the path from start to end
// Errr: This function only almost works :( ... oh well i tried
//------------------------------------------------------------------------
public void printAStarPath(int startx, int starty, int endx, int endy){
//========================================================
// PSEUDO CODE BRO.
//========================================================
// 1: Declarations
// PART ONE
// 2: Initialize:
// 1: Drop current node from openList
// Add current node to closedList
// 2: Set current node as parent for each neighbor
// Add neighbors to openList
//
// PART TWO
// 3: Loop: (thru openList)
//
// (openList should contain neighbors of closedList nodes here)
//
// EXAMPLE:
//
// n n n n n
// n n n n n>
// n S * * n-> E (the closest star is the current node)
// n n n n n>
// n n n n n
//
// 1: Set neighbor w/ lowest F-cost from the openList as current node
// 2: Add this new node to the closedList
// Remove from openList
// 3: Loop (for each neighbor):
// 1: Add openlist'less neighbors to openList
// Set current node as parent for neighbor node
// 2: If neighbor is already on the openList:
// 1: Get G-cost of neighbor IF: neighbor's parent is current node's parent (default)
// IF: neighbor's parent is current node
// 2: If the 2nd G-cost is less:
// 1: set neighbor's parent to current node
// 2: recalculate F and G costs (possibly you don't need this)
// 4: Stop: IF: end node is in closedList or,
// IF: end node is not in closedList and openList is empty
// 4: Save/Return Path
// 5: Print Results: (if you wanna print)
// 1: Fill grid with proper symbols
// 2: Print grid
//===========//
// 1 //
//===========//
List<List<Integer>> closedList = new ArrayList<List<Integer>>();
List<List<Integer>> openList = new ArrayList<List<Integer>>();
int x = startx;
int y = starty;
int gOrig = 0;
int gThru = 0;
boolean condition = false;
//===========//
// 2 //
//===========//
if (closedList.contains(Arrays.asList(x, y)) == false)
closedList.add(Arrays.asList(x, y));
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3 //
//===========//
while(condition == false){
//===========//
// 3.1 //
//===========//
// selecting lowest F-cost node
x = getLowestFCostNodePos(openList, endx, endy)[0];
y = getLowestFCostNodePos(openList, endx, endy)[1];
//===========//
// 3.2 //
//===========//
closedList.add(Arrays.asList(x, y));
openList.remove(Arrays.asList(x, y));
//===========//
// 3.3 //
//===========//
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
if (openList.contains(Arrays.asList(i, j)) == false){
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
}//end if (in openList?)
else{
// getting G-costs
gOrig = grid.get(i).get(j).getG();
grid.get(i).get(j).setParent(grid.get(x).get(y));
gThru = grid.get(i).get(j).getG();
// comparing G-costs
if (gOrig < gThru){
// revert parent back the way it was
grid.get(i).get(j).setParent(grid.get(x).get(y).getParent());
}//end if (G-costs)
// adding to openList
openList.add(Arrays.asList(i, j));
}//end else (in openList?)
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3.5 //
//===========//
if (openList.size() == 0){
condition = true;
System.out.print("\nNo Path.\n");
} else if (closedList.contains(Arrays.asList(endx, endy)) == true){
condition = true;
System.out.print("\nPath Found.\n");
}
}//end while loop (condition)
//===========//
// 4 //
//===========//
if (openList.size() > 0)
getNodePath(grid.get(endx).get(endy));
//===========//
// 5.1 //
//===========//
if (openList.size() > 0)
for (int i=0; i<path.size(); i++){
// setting symbols
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
}
// setting start/end
grid.get(startx).get(starty).setText("[S]");
grid.get(endx).get(endy).setText("[E]");
//===========//
// 5.2 //
//===========//
printGrid();
} // end printAStarPath
//------------------------------------------------------------------
// Name: getNodePath
// Desc: returns coordinates of path (in order) from start to end
//------------------------------------------------------------------
public void getNodePath(Node node){
// redo this function with the parent of node
if (node.getParent() != null){
// add a coordinate to path list
this.path.add(0, Arrays.asList(node.getX(), node.getY()));
// recur
getNodePath(node.getParent());
}//end if (recursive)
} // end getNodePath
//------------------------------------------------------------------
// Name: getLowestFCostNodePos
// Desc: returns coordinates of node with lowest F-cost in openList
//------------------------------------------------------------------
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int xTmp = 0;
int yTmp = 0;
int fMin = 1000000;
int[] cords = new int[2];
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
xTmp = openList.get(i).get(0);
yTmp = openList.get(i).get(1);
// compare F-values
if (fMin > grid.get(xTmp).get(yTmp).getF(endx, endy)){
// set temporary F-cost
fMin = grid.get(xTmp).get(yTmp).getF(endx, endy);
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = xTmp;
cords[1] = yTmp;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
} // end class def
Node:
package pathFinding;
public class Node {
private String text;
private int x;
private int y;
private boolean collidable;
private Node parent;
public Node(int x, int y){
text = "[ ]";
this.x = x;
this.y = y;
collidable = false;
parent = null;
} // end constructor
public void setText(String text){
this.text = text;
} // end setText
public int getX(){
return this.x;
} // end getX
public int getY(){
return this.y;
} // end getY
public void setCollidable(boolean arg0){
collidable = arg0;
} // end setCollidable
public boolean getCollidable(){
return collidable;
} // end getCollidable
public void setParent(Node n){
parent = n;
} // end setParent
public Node getParent(){
// for parent location: return new int[] {parent.getX(), parent.getY()};
return parent;
} // end getParent
public void printText(){
System.out.print(this.text);
} // end printText
public int getF(int endx, int endy){
return this.getG() + this.getH(endx, endy);
} // end getF
public int getG(){
// calculate exact distance from start
if (parent != null){
if (parent.getX()-this.x == 0 || parent.getY()-this.y == 0)
return parent.getG() + 10;
else
return parent.getG() + 14;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end (Manhattan distance)
return (Math.abs(this.x-endx) + Math.abs(this.y-endy))*10;
} // end getH
} // end class def
编辑：
一段时间后我又回到这段代码，我刚刚测试了一个新图表，不幸的是，它给了我这个：
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][X][ ][ ][ ][ ]
[ ][ ][X][X][X][X][ ][*][ ][ ]
[ ][ ][*][*][E][X][ ][ ][*][ ]
[ ][*][X][X][X][X][ ][ ][ ][S]
[ ][ ][*][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][*][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][*][ ][*][ ][ ][ ]
[ ][ ][ ][ ][ ][*][ ][ ][ ][ ]
谁能知道为什么会这样？
我没有阅读您的所有代码，但 getLowestFCostNodePos 函数中至少有一个错误。请注意，您 return 的坐标不是最小 FCost 的坐标，而是 openList 中最后一个节点的坐标，因为您更新了 xTmp 和 yTmp无条件。
好的，我已经仔细阅读了您的代码，并在此处其他人的评论的帮助下成功地使它工作。我只发布更改后的方法：
Grid.getLowestFCostNodePos 不跟踪具有最低 F 的节点的 X 和 Y 值：
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int fMin = 1000000;
int[] cords = new int[2];
int minX = -1;
int minY = -1;
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
int xTmp = openList.get(i).get(0);
int yTmp = openList.get(i).get(1);
int fCandidate = grid.get(xTmp).get(yTmp).getF(endx, endy);
// compare F-values
if (fMin > fCandidate) {
// set temporary F-cost
fMin = fCandidate;
minX = xTmp;
minY = yTmp;
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = minX;
cords[1] = minY;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
Node.getG() Node.getH()` 不使用相同的单位（H 认为步长为 1，G 步长为 10，N/S/E/W 对角线步长为 14步骤）和 H 不考虑对角线步骤。我将其标准化，使一步总是花费 1:
public int getG(){
// calculate exact distance from start
if (parent != null){
return parent.getG() + 1;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end
// Since we can walk diagonally we can cover the smallest of
// dx and dy while covering the longest. The distance is therefore
// the largest of the two
return Math.max(Math.abs(this.x - endx), Math.abs(this.y - endy));
} // end getH
测试板1：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
测试板2：
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][E][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][X][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][S][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
自定义板：
[S][X][ ][ ][ ]
[*][X][ ][*][ ]
[*][X][*][X][*]
[*][X][*][X][*]
[ ][*][ ][X][E]
就是说，您应该考虑实现 Point class 而不是到处使用 ArrayList 并使用局部变量和辅助方法，因为您的代码非常冗长且难以阅读。
像这样的行让我很头疼：
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
将 ArrayList<Integer> 更改为自定义 Point class 并使用两个局部变量大大提高了可读性：
Point point = path.get(i);
List<Node> row = grid.get(point.getX());
row.get(point.getY()).setText("[*]");
如果您要向 Grid 添加实用程序方法以从 Point 获取特定的 Node，您可以将其简化为：
getNode(path.get(i)).setText("[*]");
并且可以在很多地方使用该方法来提高可读性。
我正在尝试解决我的 A* 路径查找算法。我有三个 类：Menu、Grid 和 Node。
如果你 运行 我的程序，你会看到它打印出一条不寻常的螺旋、跳跃、兔子跳跃路径。我相信意外行为与以下功能有关：
printAStarPath(int startx, int starty, int endx, int endy)
在我看来，我认为问题与父节点设置不当有关。我很确定 Node 和 Menu 工作正常。
输入：
菜单功能主要管理用户输入。用户可以添加墙壁、开始位置和结束位置，以及网格的大小。我还在菜单中包含了一些测试（因此您不必每次测试都重新输入所有内容）。 printAStarPath(...) 函数接受起始 x,y 位置和结束 x,y 位置。
输出：
我想要这样打印网格：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
不幸的是，我很疯狂：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][*]
[ ][S][ ][X][ ][E][*]
[ ][ ][*][X][ ][ ][*]
[ ][ ][ ][*][*][*][ ]
另一个不同输入会发生什么的例子：
[E][ ][ ][ ][ ]
[ ][*][ ][ ][*]
[ ][ ][X][ ][*]
[ ][ ][ ][ ][*]
[ ][*][ ][*][S]
我的一些网格看起来像指向右下的箭头。有些看起来像是向右下方然后螺旋上升并向左。
总结：
我正在使用 A* 路径查找方法，并且正在通过曼哈顿方法计算启发式（或 H 成本）。我使用递归来获取准确的 G 成本以及从结束位置追溯到开始位置。
Here's the article that I followed.
代码如下：
Menu:
package pathFinding;
import java.util.*;
public class Menu {
private Grid board;
public Menu(){
board = new Grid(0, 0);
}//end constructor
static public void main (String[] args){
Menu pft = new Menu();
pft.boardMenu();
System.out.print("process terminated.");
}//end main
public void boardMenu(){
// very user error prone !
boolean kg = true;
Scanner in = new Scanner(System.in);
int input;
while(kg){
input = -1;
System.out.print("\n\n\n\n");
System.out.print(">>> Hi there,"
+ "\n(0) quit"
+ "\n(1) test 1"
+ "\n(2) test 2"
+ "\n(3) new board"
+ "\n>>> ");
input = in.nextInt();
if (input == 3){
this.initUserData();
} else if (input == 0){
kg = false;
} else if (input == 1){
board.setSize(7, 5);
board.setCollidable(3, 1);
board.setCollidable(3, 2);
board.setCollidable(3, 3);
board.printAStarPath(1, 2, 5, 2);
} else if (input == 2){
board.setSize(25, 25);
board.setCollidable(5, 4);
board.setCollidable(4, 5);
board.setCollidable(3, 3);
board.printAStarPath(15, 15, 4, 4);
}
} // end while
} // end boardMenu
public void initUserData(){
boolean kgTmp = true;
int xTmp, yTmp, iTmp, jTmp = 0;
// initiate input device
Scanner in = new Scanner(System.in);
// 0: determine board size
System.out.print("\nBoard width: ");
xTmp = in.nextInt();
System.out.print("\nBoard height: ");
yTmp = in.nextInt();
board.setSize(xTmp, yTmp);
// 1: determine obstruction locations
kgTmp = true;
while(kgTmp){
System.out.print("\nObstruction x Loc: ");
xTmp = in.nextInt();
System.out.print("\nObstruction y Loc: ");
yTmp = in.nextInt();
board.setCollidable(xTmp, yTmp);
System.out.print("\nMore Obstructions?(0=no;1=yes): ");
if(in.nextInt() == 0)
kgTmp = false;
} // end while
// 2: determine start location
System.out.print("\nStart x Loc: ");
xTmp = in.nextInt();
System.out.print("\nStart y Loc: ");
yTmp = in.nextInt();
// 3: determine end location
System.out.print("\nEnd x Loc: ");
iTmp = in.nextInt();
System.out.print("\nEnd y Loc: ");
jTmp = in.nextInt();
System.out.println("\nredy for astar");
// 4: determine and print A* path
board.printAStarPath(xTmp, yTmp, iTmp, jTmp);
System.out.println("\nastar shud be done?");
} // end initBoardData
} // end class def
Grid:
package pathFinding;
import java.util.*;
public class Grid {
private List<List<Node>> grid;
List<List<Integer>> path;
private int width;
private int height;
//------------------------------------------------------------------------
// Name: Constructor
// Desc: Takes in a width & height. Initializes stuff.
//------------------------------------------------------------------------
public Grid(int width, int height){
grid = new ArrayList<List<Node>>();
path = new ArrayList<List<Integer>>();
this.width = width;
this.height = height;
initGrid(width, height);
} // end constructor
//------------------------------------------------------------------------
// Name: initGrid
// Desc: initializes the grid with data
//------------------------------------------------------------------------
public void initGrid(int w, int h){
// add columns
for (int i=0;i<w;i++)
grid.add(new ArrayList<Node>());
// fill grid with nodes
for (int i=0;i<w;i++)
for (int j=0;j<h;j++)
grid.get(i).add(new Node(i, j));
} // end initGrid
//------------------------------------------------------------------------
// Name: setSize
// Desc: Sets the size of the grid
//------------------------------------------------------------------------
public void setSize(int w, int h){
this.width = w;
this.height = h;
// update the nodes
clearAll();
initGrid(width, height);
} // end setSize
//------------------------------------------------------------------------
// Name: clearAll
// Desc: Clears any data in grid and path
//------------------------------------------------------------------------
public void clearAll(){
// removes all rows/columns/nodes
grid.clear();
path.clear();
} // end clearAll
//------------------------------------------------------------------------
// Name: printGrid
// Desc: Prints the whole grid
//------------------------------------------------------------------------
public void printGrid(){
// prints every node's value
// loop thru columns
for (int j=0;j<height;j++){
// thru row
for (int i=0;i<width;i++)
grid.get(i).get(j).printText();
System.out.println();
} // end j loop
} // end printGrid
//------------------------------------------------------------------------
// Name: setCollidable
// Desc: Sets a node at x,y to collidable
//------------------------------------------------------------------------
public void setCollidable(int x, int y){
// makes a node at x,y collidable
grid.get(x).get(y).setCollidable(true);
grid.get(x).get(y).setText("[X]");
} // end setCollidable
//------------------------------------------------------------------------
// Name: printAStarPath
// Desc: Finds and prints the path from start to end
// Errr: This function only almost works :( ... oh well i tried
//------------------------------------------------------------------------
public void printAStarPath(int startx, int starty, int endx, int endy){
//========================================================
// PSEUDO CODE BRO.
//========================================================
// 1: Declarations
// PART ONE
// 2: Initialize:
// 1: Drop current node from openList
// Add current node to closedList
// 2: Set current node as parent for each neighbor
// Add neighbors to openList
//
// PART TWO
// 3: Loop: (thru openList)
//
// (openList should contain neighbors of closedList nodes here)
//
// EXAMPLE:
//
// n n n n n
// n n n n n>
// n S * * n-> E (the closest star is the current node)
// n n n n n>
// n n n n n
//
// 1: Set neighbor w/ lowest F-cost from the openList as current node
// 2: Add this new node to the closedList
// Remove from openList
// 3: Loop (for each neighbor):
// 1: Add openlist'less neighbors to openList
// Set current node as parent for neighbor node
// 2: If neighbor is already on the openList:
// 1: Get G-cost of neighbor IF: neighbor's parent is current node's parent (default)
// IF: neighbor's parent is current node
// 2: If the 2nd G-cost is less:
// 1: set neighbor's parent to current node
// 2: recalculate F and G costs (possibly you don't need this)
// 4: Stop: IF: end node is in closedList or,
// IF: end node is not in closedList and openList is empty
// 4: Save/Return Path
// 5: Print Results: (if you wanna print)
// 1: Fill grid with proper symbols
// 2: Print grid
//===========//
// 1 //
//===========//
List<List<Integer>> closedList = new ArrayList<List<Integer>>();
List<List<Integer>> openList = new ArrayList<List<Integer>>();
int x = startx;
int y = starty;
int gOrig = 0;
int gThru = 0;
boolean condition = false;
//===========//
// 2 //
//===========//
if (closedList.contains(Arrays.asList(x, y)) == false)
closedList.add(Arrays.asList(x, y));
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3 //
//===========//
while(condition == false){
//===========//
// 3.1 //
//===========//
// selecting lowest F-cost node
x = getLowestFCostNodePos(openList, endx, endy)[0];
y = getLowestFCostNodePos(openList, endx, endy)[1];
//===========//
// 3.2 //
//===========//
closedList.add(Arrays.asList(x, y));
openList.remove(Arrays.asList(x, y));
//===========//
// 3.3 //
//===========//
for (int i=x-1;i<x+2;i++){
for (int j=y-1;j<y+2;j++){
if (i>=0 && i<this.width){
if (j>=0 && j<this.height){
if (closedList.contains(Arrays.asList(i, j)) == false){
if (grid.get(i).get(j).getCollidable() == false){
//-----------------------------------------------------
if (openList.contains(Arrays.asList(i, j)) == false){
// setting parent
grid.get(i).get(j).setParent( grid.get(x).get(y) );
// adding to openList
openList.add(Arrays.asList(i, j));
}//end if (in openList?)
else{
// getting G-costs
gOrig = grid.get(i).get(j).getG();
grid.get(i).get(j).setParent(grid.get(x).get(y));
gThru = grid.get(i).get(j).getG();
// comparing G-costs
if (gOrig < gThru){
// revert parent back the way it was
grid.get(i).get(j).setParent(grid.get(x).get(y).getParent());
}//end if (G-costs)
// adding to openList
openList.add(Arrays.asList(i, j));
}//end else (in openList?)
//-----------------------------------------------------
}//end if (check collidable)
}//end if (in closedList?)
}//end if (check height)
}//end if (check width)
}//end j loop
}//end i loop
//===========//
// 3.5 //
//===========//
if (openList.size() == 0){
condition = true;
System.out.print("\nNo Path.\n");
} else if (closedList.contains(Arrays.asList(endx, endy)) == true){
condition = true;
System.out.print("\nPath Found.\n");
}
}//end while loop (condition)
//===========//
// 4 //
//===========//
if (openList.size() > 0)
getNodePath(grid.get(endx).get(endy));
//===========//
// 5.1 //
//===========//
if (openList.size() > 0)
for (int i=0; i<path.size(); i++){
// setting symbols
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
}
// setting start/end
grid.get(startx).get(starty).setText("[S]");
grid.get(endx).get(endy).setText("[E]");
//===========//
// 5.2 //
//===========//
printGrid();
} // end printAStarPath
//------------------------------------------------------------------
// Name: getNodePath
// Desc: returns coordinates of path (in order) from start to end
//------------------------------------------------------------------
public void getNodePath(Node node){
// redo this function with the parent of node
if (node.getParent() != null){
// add a coordinate to path list
this.path.add(0, Arrays.asList(node.getX(), node.getY()));
// recur
getNodePath(node.getParent());
}//end if (recursive)
} // end getNodePath
//------------------------------------------------------------------
// Name: getLowestFCostNodePos
// Desc: returns coordinates of node with lowest F-cost in openList
//------------------------------------------------------------------
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int xTmp = 0;
int yTmp = 0;
int fMin = 1000000;
int[] cords = new int[2];
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
xTmp = openList.get(i).get(0);
yTmp = openList.get(i).get(1);
// compare F-values
if (fMin > grid.get(xTmp).get(yTmp).getF(endx, endy)){
// set temporary F-cost
fMin = grid.get(xTmp).get(yTmp).getF(endx, endy);
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = xTmp;
cords[1] = yTmp;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
} // end class def
Node:
package pathFinding;
public class Node {
private String text;
private int x;
private int y;
private boolean collidable;
private Node parent;
public Node(int x, int y){
text = "[ ]";
this.x = x;
this.y = y;
collidable = false;
parent = null;
} // end constructor
public void setText(String text){
this.text = text;
} // end setText
public int getX(){
return this.x;
} // end getX
public int getY(){
return this.y;
} // end getY
public void setCollidable(boolean arg0){
collidable = arg0;
} // end setCollidable
public boolean getCollidable(){
return collidable;
} // end getCollidable
public void setParent(Node n){
parent = n;
} // end setParent
public Node getParent(){
// for parent location: return new int[] {parent.getX(), parent.getY()};
return parent;
} // end getParent
public void printText(){
System.out.print(this.text);
} // end printText
public int getF(int endx, int endy){
return this.getG() + this.getH(endx, endy);
} // end getF
public int getG(){
// calculate exact distance from start
if (parent != null){
if (parent.getX()-this.x == 0 || parent.getY()-this.y == 0)
return parent.getG() + 10;
else
return parent.getG() + 14;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end (Manhattan distance)
return (Math.abs(this.x-endx) + Math.abs(this.y-endy))*10;
} // end getH
} // end class def
编辑：
一段时间后我又回到这段代码，我刚刚测试了一个新图表，不幸的是，它给了我这个：
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][X][ ][ ][ ][ ]
[ ][ ][X][X][X][X][ ][*][ ][ ]
[ ][ ][*][*][E][X][ ][ ][*][ ]
[ ][*][X][X][X][X][ ][ ][ ][S]
[ ][ ][*][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][*][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][*][ ][*][ ][ ][ ]
[ ][ ][ ][ ][ ][*][ ][ ][ ][ ]
谁能知道为什么会这样？
我没有阅读您的所有代码，但 getLowestFCostNodePos 函数中至少有一个错误。请注意，您 return 的坐标不是最小 FCost 的坐标，而是 openList 中最后一个节点的坐标，因为您更新了 xTmp 和 yTmp无条件。
好的，我已经仔细阅读了您的代码，并在此处其他人的评论的帮助下成功地使它工作。我只发布更改后的方法：
Grid.getLowestFCostNodePos 不跟踪具有最低 F 的节点的 X 和 Y 值：
public int[] getLowestFCostNodePos(List<List<Integer>> openList, int endx, int endy){
// Declarations
int fMin = 1000000;
int[] cords = new int[2];
int minX = -1;
int minY = -1;
// look for lowest F-cost node
for (int i=0;i<openList.size();i++){
// setting possible position
int xTmp = openList.get(i).get(0);
int yTmp = openList.get(i).get(1);
int fCandidate = grid.get(xTmp).get(yTmp).getF(endx, endy);
// compare F-values
if (fMin > fCandidate) {
// set temporary F-cost
fMin = fCandidate;
minX = xTmp;
minY = yTmp;
}//end if (compare F)
}//end i loop
// just in case
if (openList.size() > 0){
cords[0] = minX;
cords[1] = minY;
return cords;
} else{
System.out.print("openList is empty!");
return null;
}
} // end getLowestFCostNodePos
Node.getG() Node.getH()` 不使用相同的单位（H 认为步长为 1，G 步长为 10，N/S/E/W 对角线步长为 14步骤）和 H 不考虑对角线步骤。我将其标准化，使一步总是花费 1:
public int getG(){
// calculate exact distance from start
if (parent != null){
return parent.getG() + 1;
}//end if
return 0;
} // end getG
public int getH(int endx, int endy){
// calculate estimated distance to end
// Since we can walk diagonally we can cover the smallest of
// dx and dy while covering the longest. The distance is therefore
// the largest of the two
return Math.max(Math.abs(this.x - endx), Math.abs(this.y - endy));
} // end getH
测试板1：
[ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ]
[ ][S][ ][X][ ][E][ ]
[ ][ ][*][X][*][ ][ ]
[ ][ ][ ][*][ ][ ][ ]
测试板2：
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][E][X][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][X][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][*][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][S][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ][ ]
自定义板：
[S][X][ ][ ][ ]
[*][X][ ][*][ ]
[*][X][*][X][*]
[*][X][*][X][*]
[ ][*][ ][X][E]
就是说，您应该考虑实现 Point class 而不是到处使用 ArrayList 并使用局部变量和辅助方法，因为您的代码非常冗长且难以阅读。
像这样的行让我很头疼：
grid.get(path.get(i).get(0)).get(path.get(i).get(1)).setText("[*]");
将 ArrayList<Integer> 更改为自定义 Point class 并使用两个局部变量大大提高了可读性：
Point point = path.get(i);
List<Node> row = grid.get(point.getX());
row.get(point.getY()).setText("[*]");
如果您要向 Grid 添加实用程序方法以从 Point 获取特定的 Node，您可以将其简化为：
getNode(path.get(i)).setText("[*]");
并且可以在很多地方使用该方法来提高可读性。