package com.oracle.Astar;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.TreeMap;
class Node implements Comparable<Node>{
private int cross;
private int column;
private int h_score;
private int g_score;
private int f_score;
Node(int cross,int column) {
this.cross = cross;
this.column = column;
}
Node(int cross,int column,int h_score,int g_score) {
this.cross = cross;
this.column = column;
this.h_score = h_score;
this.g_score = g_score;
this.f_score = h_score + g_score;
}
public int getCross() {
return cross;
}
public void setCross(int cross) {
this.cross = cross;
}
public int getColumn() {
return column;
}
public void setColumn(int column) {
this.column = column;
}
public int getH_score() {
return h_score;
}
public void setH_score(int h_score) {
this.h_score = h_score;
}
public int getG_score() {
return g_score;
}
public void setG_score(int g_score) {
this.g_score = g_score;
}
public int getF_score() {
return f_score;
}
public void setF_score(int f_score) {
this.f_score = f_score;
}
@Override
public boolean equals(Object obj) {
Node o = (Node)obj;
if(this.cross == o.cross && this.column == o.column) {
return true;
}else {
return false;
}
}
@Override
public String toString() {
return "("+this.cross+","+this.column+")";
}
@Override
public int compareTo(Node o) {
if(this.f_score > o.f_score) {
return 1;
}else if(this.f_score == o.f_score){
return 0;
}else {
return -1;
}
}
@Override
public int hashCode() {
String str = this.cross+""+this.column;
return str.hashCode();
}
}
class Astart{
private Node start;
private Node goal;
public List<Node> openset = new ArrayList<>();
public List<Node> closeset = new ArrayList<>();
public Map<Node, Node> comaFrom = new HashMap<>();
public List<Node> path = new ArrayList<>();
public int[][] nodes;
Astart(int[][] nodes, Node start, Node goal) {
this.nodes = nodes;
this.start = start;
this.goal = goal;
}
/**
* 搜索路径
* */
public void find(Node node){
ArrayList<Node> Neighbors = getNeighbors(nodes, node);
if(closeset.contains(goal)) {
//关闭表中有goal点,搜索完成
}else {
if(node.equals(this.start)) {
//考虑起始点
closeset.add(node);
}
for (Node n : Neighbors) {
n.setG_score(getG_score(n, node));
n.setH_score(getH_score(n, goal));
n.setF_score(getF_score(n));
if(openset.contains(n)) {
//G值判定
if(n.getG_score() < openset.get(openset.indexOf(n)).getG_score()) {
openset.add(n);
comaFrom.put(n, node);
}
}else {
if( !(nodes[n.getCross()][n.getColumn()] == 1 || closeset.contains(n)) ){
//考虑相邻点为关闭点或者墙时
openset.add(n);
comaFrom.put(n, node);
}else {
closeset.add(n);
}
}
}
Collections.sort(openset);
node = openset.get(0);
openset.remove(node);
closeset.add(node);
find(node);
}
}
/**
* 得到某点的所有相邻点
* */
public ArrayList<Node> getNeighbors(int[][] nodes, Node node) {
int cross = node.getCross();
int column = node.getColumn();
ArrayList<Node> neighborNodes = new ArrayList<Node>();
for(int i = cross-1; i <= cross+1; i++) {
for(int j = column-1; j <= column+1; j++) {
if(i>=0 && j>=0 && i<5 && j<7){
if(i == cross && j == column) {
}else{
neighborNodes.add(new Node(i,j));
}
}
}
}
return neighborNodes;
}
/**
* h_score-----当前点到目标点(B点)的理论移动距离,不考虑不可通过区域
* */
public int getH_score(Node node, Node goal) {
return Math.abs(node.getCross()-goal.getCross())+Math.abs(node.getColumn()-goal.getColumn());
}
/**
* g_score-----起始点(A点)到当前点的实际移动距离
* */
public int getG_score(Node node, Node fatherNode) {
if(fatherNode.getColumn() == node.getColumn() || fatherNode.getCross() == node.getCross()) {
return 10+fatherNode.getF_score();
}else {
return 14+fatherNode.getF_score();
}
}
/**
* f_score-----g_score + h_score
* */
public int getF_score(Node node) {
return node.getH_score() + node.getG_score();
}
/**
* 从comaFrom表中得到起点到终点的最短路径
* */
public void getPath(Node goal) {
Node fatherNode = comaFrom.get(goal);
if(fatherNode.equals(start)) {
path.add(fatherNode);
}else {
path.add(fatherNode);
getPath(fatherNode);
}
}
}
public class test {
public static void main(String[] args) {
int[][] nodes = {{0,0,0,0,0,0,0},
{0,0,0,1,0,0,0},
{0,0,0,1,0,0,0},
{0,0,0,1,0,0,0},
{0,0,0,0,0,0,0},};
Node start = new Node(2,1,0,0);
Node goal = new Node(2,5);
Astart a = new Astart(nodes, start, goal);
a.find(start);
a.getPath(goal);
Collections.reverse(a.path);
System.out.println(a.path);
}
}
Start算法实现
最新推荐文章于 2020-09-10 02:59:10 发布