java实现A*算法

public class Point {
// 节点横坐标
private int x;
// 节点纵坐标
private int y;

// 节点值
private char value;
// 父节点
private Point father;

/**
* 构造函数
*
* @param x 节点横坐标
* @param y 节点纵坐标
*/
public Point(int x, int y) {
this.x = x;
this.y = y;
}

/**
* 构造函数
*
* @param x 节点横坐标
* @param y 节点纵坐标
* @param value 节点值
*/
public Point(int x, int y, char value) {
this.x = x;
this.y = y;
this.value = value;
}

public Point getFather() {
return father;
}

public void setFather(Point father) {
this.father = father;
}

public char getValue() {
return value;
}

public int getX() {
return x;
}

public int getY() {
return y;
}
}

import java.util.ArrayList;
import java.util.List;

public class Node {


private String id;

private List<Edge> outEdges;

public Node(String id) {
this.id = id;
outEdges = new ArrayList<Edge>();
}

public void addOutEdge(Edge edge) {
if(edge != null)
outEdges.add(edge);
}

public String getId() {
return id;
}

public void setId(String id) {
this.id = id;
}

public List<Edge> getOutEdges() {
return outEdges;
}

public void setOutEdges(List<Edge> outEdges) {
this.outEdges = outEdges;
}
}

import java.util.ArrayList;
import java.util.List;

public class Graph {


public List<Node> nodeList = null;

public Graph() {
nodeList = new ArrayList();
}

public List<Node> getNodeList() {
return nodeList;
}

// initialize
public void init() {
// ************************ Node A ***********************
Node aNode = new Node("A");
nodeList.add(aNode);
// A -> B
Edge a2bEdge = new Edge();
a2bEdge.setStartNodeID(aNode.getId());
a2bEdge.setEndNodeID("B");
a2bEdge.setWeight(10);
aNode.addOutEdge(a2bEdge);
// A -> C
Edge a2cEdge = new Edge();
a2cEdge.setStartNodeID(aNode.getId());
a2cEdge.setEndNodeID("C");
a2cEdge.setWeight(20);
aNode.addOutEdge(a2cEdge);
// A -> E
Edge a2eEdge = new Edge();
a2eEdge.setStartNodeID(aNode.getId());
a2eEdge.setEndNodeID("E");
a2eEdge.setWeight(30);
aNode.addOutEdge(a2eEdge);

// ************************ Node B ***********************
Node bNode = new Node("B");
nodeList.add(bNode);
// B -> C
Edge b2cEdge = new Edge();
b2cEdge.setStartNodeID(bNode.getId());
b2cEdge.setEndNodeID("C");
b2cEdge.setWeight(5);
bNode.addOutEdge(b2cEdge);
// B -> E
Edge b2eEdge = new Edge();
b2eEdge.setStartNodeID(bNode.getId());
b2eEdge.setEndNodeID("E");
b2eEdge.setWeight(10);
bNode.addOutEdge(b2eEdge);

// ************************ Node C ***********************
Node cNode = new Node("C");
nodeList.add(cNode);
// C -> D
Edge c2dEdge = new Edge();
c2dEdge.setStartNodeID(cNode.getId());
c2dEdge.setEndNodeID("D");
c2dEdge.setWeight(30);
cNode.addOutEdge(c2dEdge);

// ************************ Node D ***********************
Node dNode = new Node("D");
nodeList.add(dNode);

// ************************ Node E ***********************
Node eNode = new Node("E");
nodeList.add(eNode);
// E -> D
Edge e2dEdge = new Edge();
e2dEdge.setStartNodeID(eNode.getId());
e2dEdge.setEndNodeID("D");
e2dEdge.setWeight(20);
eNode.addOutEdge(e2dEdge);

}
}

public class Edge {

// 边的起始节点编号
private String startNodeID;
// 边的末尾节点编号
private String endNodeID;
// 边的权值
private double weight;

public String getStartNodeID() {
return startNodeID;
}
public void setStartNodeID(String startNodeID) {
this.startNodeID = startNodeID;
}
public String getEndNodeID() {
return endNodeID;
}
public void setEndNodeID(String endNodeID) {
this.endNodeID = endNodeID;
}
public double getWeight() {
return weight;
}
public void setWeight(double weight) {
this.weight = weight;
}
}


import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;

public class Dijkstra {

private String startNodeID;
private List<String> sourceNodeIDList = null;
private List<String> desNodeIDList = null;

private Map<String,Double> nodeidShortestRouteMapping = null;
private Map<String,String> nodeidLastNodeidMapping = null;
private Map<String,Node> idNodeMapping = null;

public Dijkstra(Graph graph, String startNodeID) {
this.startNodeID = startNodeID;

sourceNodeIDList = new ArrayList<String>();
desNodeIDList = new ArrayList<String>();
nodeidShortestRouteMapping = new HashMap<String,Double>();
nodeidLastNodeidMapping = new HashMap<String,String>();
idNodeMapping = new HashMap();

for(Node node : graph.getNodeList()) {
if(node.getId().equals(startNodeID)) {
desNodeIDList.add(startNodeID);
nodeidShortestRouteMapping.put(startNodeID, 0d);
}
else {
sourceNodeIDList.add(node.getId());
nodeidShortestRouteMapping.put(node.getId(), Double.MAX_VALUE);
}
nodeidLastNodeidMapping.put(node.getId(), null);
idNodeMapping.put(node.getId(), node);
}
}

public void start() {
Node nextNode = null;
Node currentNode = null;
String nextNodeID = "";
do {
if(nextNode == null) {
currentNode = idNodeMapping.get(startNodeID);
}
else {
currentNode = nextNode;
}
nextNodeID = getNextNode(currentNode);

nextNode = idNodeMapping.get(nextNodeID);
System.out.println("nextNode.id:" + nextNode.getId());

sourceNodeIDList.remove(nextNode.getId());
System.out.println("sourceNodeIDList:" + sourceNodeIDList.toString());
} while(sourceNodeIDList.size() > 0);
}


public String getNextNode(Node currentNode) {
System.out.println("============= currentNode.id: " + currentNode.getId() + " =============");
double shortestPath = Double.MAX_VALUE;
String nextNodeID = "";
for(Edge nextEdge : currentNode.getOutEdges()) {
System.out.println("nextEdge.endNodeid:" + nextEdge.getEndNodeID());
if((nextEdge.getWeight() + nodeidShortestRouteMapping.get(currentNode.getId()))
< nodeidShortestRouteMapping.get(nextEdge.getEndNodeID())) {
nodeidShortestRouteMapping.put(nextEdge.getEndNodeID(),
nextEdge.getWeight() + nodeidShortestRouteMapping.get(currentNode.getId()));
nodeidLastNodeidMapping.put(nextEdge.getEndNodeID(),
currentNode.getId());
}
}

for(String nodeID : sourceNodeIDList) {
if(nodeidShortestRouteMapping.get(nodeID) < shortestPath) {
shortestPath = nodeidShortestRouteMapping.get(nodeID);
nextNodeID = nodeID;
}
}
if(sourceNodeIDList.size() == 1)
return sourceNodeIDList.get(0);
return nextNodeID;
}


public Map<String,Double> getNodeidShortestRouteMapping() {
return nodeidShortestRouteMapping;
}

public Map<String,String> getNodeidLastNodeidMapping() {
return nodeidLastNodeidMapping;
}

public static void main(String[] args) {
Graph graph = new Graph();
graph.init();

Dijkstra dijkstra = new Dijkstra(graph, "A");
dijkstra.start();

Iterator<String> it = dijkstra.getNodeidShortestRouteMapping().keySet().iterator();
while(it.hasNext()) {
String id = it.next();
System.out.println("nodeId: " + id + ", shortest length: " + dijkstra.getNodeidShortestRouteMapping().get(id)
+ ", last nodeId: " + dijkstra.getNodeidLastNodeidMapping().get(id));
}
}
}


import java.util.Iterator;
import java.util.LinkedList;
import java.util.Queue;


public class AStar {
Point[][] maze;
Point start;
Point goal;

Queue<Point> openQueue = null;
Queue<Point> closedQueue = null;

int[][] FList = null;
int[][] GList = null;
int[][] HList = null;


public void printPath() {
System.out.println("================ printPath ================");
Point father_point = null;
char[][] result = new char[7][8];
for (int i = 0; i < 7; i++) {
for (int j = 0; j < 8; j++) {
result[i][j] = '.';
}
}

int step = 0;
father_point = maze[goal.getX()][goal.getY()];
while (father_point != null) {
if(father_point.equals(start))
result[father_point.getX()][father_point.getY()] = 'r';
else if(father_point.equals(goal)) {
result[father_point.getX()][father_point.getY()] = 'a';
step++;
}
else if(father_point.getValue() == 'x') {
result[father_point.getX()][father_point.getY()] = 'x';
step += 2;
}
else {
result[father_point.getX()][father_point.getY()] = '*';
step++;
}
father_point = father_point.getFather();
}
System.out.println("step is : " + step);
for (int i = 0; i < 7; i++) {
for (int j = 0; j < 8; j++) {
System.out.print(result[i][j] + " ");
}
System.out.println();
}
}

public AStar(Point[][] maze, Point start, Point goal) {
this.maze = maze;
this.start = start;
this.goal = goal;

openQueue = new LinkedList<Point>();
closedQueue = new LinkedList<Point>();

FList = new int[maze.length][maze[0].length];
GList = new int[maze.length][maze[0].length];
HList = new int[maze.length][maze[0].length];

for (int i = 0; i < maze.length; i++) {
for (int j = 0; j < maze[0].length; j++) {
FList[i][j] = Integer.MAX_VALUE;
GList[i][j] = Integer.MAX_VALUE;
HList[i][j] = Integer.MAX_VALUE;
}
}

init();
}

private void init() {
openQueue.offer(start);
int start_x = start.getX();
int start_y = start.getY();
int goal_x = goal.getX();
int goal_y = goal.getY();

GList[start_x][start_y] = 0;
HList[start_x][start_y] = getDistance(start_x, start_y, goal_x, goal_y);
// f(x) = g(x) + h(x)
FList[start_x][start_y] = GList[start_x][start_y]
+ HList[start_x][start_y];
}
public void start() {
Point currentPoint;

while ((currentPoint = findShortestFPoint()) != null) {
if (currentPoint.getX() == goal.getX()
&& currentPoint.getY() == goal.getY())
return;
updateNeighborPoints(currentPoint);
}
}

public static void main(String[] args) {


// char[][] mazeRaw = { { '#', '.', '#', '#', '#', '#', '#', '.' },
// { '#', '.', 'a', '#', '.', '.', 'r', '.' },
// { '#', '.', '.', '#', 'x', '.', '.', '.' },
// { '.', '.', '#', '.', '.', '#', '.', '#' },
// { '#', '.', '.', '.', '#', '#', '.', '.' },
// { '.', '#', '.', '.', '.', '.', '.', '.' },
// { '.', '.', '.', '.', '.', '.', '.', '.' } };

char[][] mazeRaw = { { '#', '.', '#', '#', '#', '#', '#', '.' },
{ '#', '.', '.', '#', '.', '.', '.', '.' },
{ '#', '.', '.', '#', 'x', '.', '.', '.' },
{ '.', '.', '#', '.', '.', '#', '.', '#' },
{ '#', '.', '.', '.', '#', '#', '.', '.' },
{ '.', '#', '.', '.', '.', '.', 'r', '.' },
{ '.', '.', '.', '.', '.', '.', '.', 'a' } };

System.out.println("mazeRaw.length"+mazeRaw.length+"---"+mazeRaw[0].length);


Point[][] maze = new Point[mazeRaw.length][mazeRaw[0].length];
for (int i = 0; i < maze.length; i++) {
for (int j = 0; j < maze[0].length; j++) {
maze[i][j] = new Point(i, j, mazeRaw[i][j]);
}
}

Point start = maze[5][6];

Point goal = maze[6][7];

AStar astar = new AStar(maze, start, goal);
astar.start();
astar.printPath();

}

private boolean checkPosValid(int x, int y) {
if ((x >= 0 && x < maze.length) && (y >= 0 && y < maze[0].length)
&& (maze[x][y].getValue() != '#')) {
Iterator<Point> it = closedQueue.iterator();
Point point = null;
while (it.hasNext()) {
if ((point = it.next()) != null) {
if (point.getX() == x && point.getY() == y)
return false;
}
}
return true;
}
return false;
}

private int getDistance(int current_x, int current_y, int goal_x, int goal_y) {
return Math.abs(current_x - goal.getX())
+ Math.abs(current_y - goal.getY());
}

private Point findShortestFPoint() {
Point currentPoint = null;
Point shortestFPoint = null;
int shortestFValue = Integer.MAX_VALUE;

Iterator<Point> it = openQueue.iterator();
while (it.hasNext()) {
currentPoint = it.next();
if (FList[currentPoint.getX()][currentPoint.getY()] <= shortestFValue) {
shortestFPoint = currentPoint;
shortestFValue = FList[currentPoint.getX()][currentPoint.getY()];
}
}

if (shortestFValue != Integer.MAX_VALUE) {
openQueue.remove(shortestFPoint);
closedQueue.offer(shortestFPoint);
}

return shortestFPoint;
}

private void updateNeighborPoints(Point currentPoint) {
int current_x = currentPoint.getX();
int current_y = currentPoint.getY();

if (checkPosValid(current_x - 1, current_y)) {
updatePoint(maze[current_x][current_y],
maze[current_x - 1][current_y]);
}
if (checkPosValid(current_x + 1, current_y)) {
updatePoint(maze[current_x][current_y],
maze[current_x + 1][current_y]);
}
if (checkPosValid(current_x, current_y - 1)) {
updatePoint(maze[current_x][current_y],
maze[current_x][current_y - 1]);
}
if (checkPosValid(current_x, current_y + 1)) {
updatePoint(maze[current_x][current_y],
maze[current_x][current_y + 1]);
}
}

private void updatePoint(Point lastPoint, Point currentPoint) {
int last_x = lastPoint.getX();
int last_y = lastPoint.getY();
int current_x = currentPoint.getX();
int current_y = currentPoint.getY();

int temp_g = GList[last_x][last_y] + 1;
if (maze[current_x][current_y].getValue() == 'x') // 锟斤拷锟角帮拷诘锟斤拷强锟斤拷锟�
++temp_g;
int temp_h = getDistance(current_x, current_y, goal.getX(), goal.getY());
// f(x) = g(x) + h(x)
int temp_f = temp_g + temp_h;

if (!openQueue.contains(currentPoint)) {
openQueue.offer(currentPoint);
currentPoint.setFather(lastPoint);

GList[current_x][current_y] = temp_g;
HList[current_x][current_y] = temp_h;
// f(x) = g(x) + h(x)
FList[current_x][current_y] = temp_f;
} else {

if (temp_f < FList[current_x][current_y]) {
GList[current_x][current_y] = temp_g;
HList[current_x][current_y] = temp_h;
// f(x) = g(x) + h(x)
FList[current_x][current_y] = temp_f;
currentPoint.setFather(lastPoint);
}
}
}
}