路径导航与启发式搜索

while (true) {
 if (open.isEmpty())
  return false;
 Node currentNode = open.poll();
 closed.add(currentNode);
 if (isTarget(currentNode)) {
  latest = currentNode;
  return true;
 }
 ArrayList<Node> m = expand(currentNode);
 if (m.isEmpty()) 
  continue;
 else 
  setPointer(m, currentNode);
}

if (!open.contains(node) && !closed.contains(node)) {
 
} else if (open.contains(node)) {
 if (newDissipative < node.getDissipative()) {
  
 }
} else if (newDissipative < node.getDissipative()) {
 
}

/**
 * 评估函数的h(x)的值
 * @param source 起点
 * @param terminal 终点
 * @param parent 当前的点的父亲
 * @param node 当前的点
 * @return h(x)的值
 */
private double heuristic(Node source, Node terminal, Node parent, Node node) 

/**
 * 获得给定的点的邻居，邻居的定义是周围8个点
 * @param node 给定的点
 * @return 给定点的邻居
 */
private ArrayList<Node> getNeighbours(Node node) 

/**
 * 判断当前点是不是合法，合法的定义是不超过地图边界，且不是障碍物
 * @param x 横坐标
 * @param y 纵坐标
 * @return 如果合法，返回true，否则，false
 */
private boolean isValid(int x, int y) 

/**
 * 判断当前点是不是终点
 * @param node 当前点
 * @return 如果已经到了终点，返回true，否则，false
 */
private boolean isTarget(Node node) 

/**
 * 以链表的形式返回最终找到的路径
 * @return 最终找到的路径
 */
public LinkedList<Node> getResult() 

/**
 * 以链表的形式获得曾经试图访问过的点
 * @return 曾经试图访问过的点
 */
public LinkedList<Node> getProgress()

对于图G中的每一个点V
初始化d[v] = Infinity，previous[v] = undefined   
d[s] = 0
S = empty
Q = 所有的V
while Q 非空
u = 取出(Q)中最小的点
把u加入到S
对于u能够走到的每一个v
if d[v] > d[u] + w(u,v)
d[v] = d[u] + w(u,v)，previous[v] = u

for (Node node : m) {
 double newDissipative = 计算出新的评估值;
 if (!open.contains(node) && !closed.contains(node)) {
  node.setDissipative(newDissipative);
  open.add(node);
  node.setParent(parent);
 } else if (open.contains(node)) {
  if (newDissipative < node.getDissipative()) {
   node.setDissipative(newDissipative);
   node.setParent(parent);
  }
 } else if (newDissipative < node.getDissipative()) {
  node.setDissipative(newDissipative);
  node.setParent(parent);
  closed.remove(node);
  open.add(node);
 }
}

dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
h(n) = dx + dy

dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
h(n) = D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
h(n) = D * sqrt(dx * dx + dy * dy)

heuristic *= (1.0 + p)
p <(minimum cost of taking one step)/(expected maximum path length)

dx1 = current.x - goal.x
dy1 = current.y - goal.y
dx2 = start.x - goal.x
dy2 = start.y - goal.y
cross = abs(dx1*dy2 - dx2*dy1)
heuristic += cross*0.001

OPEN=(s)
f(s)=g(s)+h(s)
LOOP:
if OPEN为空 return FAIL
取出OPEN表的第一个元素n
if n是目标 return SUCCESS
从OPEN表移除n，加入到CLOSED表
扩展n为{m}
计算f(n,mi)=g(n,mi)+h(mi)
标记m到n的指针
mj加入到OPEN表
if f(n,mk)<f(mk) f(mk)=f(n,mk)
if f(n,ml)<f(ml) f(ml)=f(n,ml)并重新放回OPEN表
OPEN表按照f值从小到大排序（如果使用优先级队列自动完成，这一步省略）
GO LOOP

package main;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.PriorityQueue;

import handler.ConstantValue;
import handler.Draw;
import handler.InputHandler;;

public class Main {

 public static void main(String[] args) throws Exception {
  // 处理输入
  InputHandler inputHandler = new InputHandler();
  inputHandler.input();
  boolean[][] maze = inputHandler.getMaze();
  Node source = inputHandler.getSource();
  Node terminal = inputHandler.getTerminal();
  // 开始搜索
  Search s = new Search(source, terminal, maze);
  if (s.start()) {
   // 找到结果，就获得结果，并绘制，然后输出
   LinkedList<Node> progress = s.getProgress();
   LinkedList<Node> result = s.getResult();
   Draw.draw(maze, progress, result);
   System.out.println("总共走了 " + result.size() + " 步。");
   System.out.println("找到路径，请查看图形用户界面。");
  } else {
   // 找不到结果，就是答案不存在
   // Draw.draw(maze, new LinkedList<Node>(), new LinkedList<Node>());
   System.out.println("不存在这样一条路径！");
  }

 }
}

class Search {

 private Node source; // 起点
 private Node terminal; // 终点
 private boolean[][] maze = new boolean[ConstantValue.SIZE][ConstantValue.SIZE]; // 迷宫

 public Search(Node source, Node terminal, boolean[][] maze) {
  this.source = source;
  this.terminal = terminal;
  this.maze = maze;
 }

 PriorityQueue<Node> open = new PriorityQueue<Node>(); // OPEN表
 LinkedList<Node> closed = new LinkedList<Node>(); // CLOSED表
 LinkedList<Node> progress = new LinkedList<Node>(); // 哪些区域是被搜索过的，用来比较算法优劣
 Node latest;

 public boolean start() {
  /*
   * 起点的权重定义为0 事实上，因为起点一定在路径中，所以无所谓它的g(x)和h(x)到底是什么，直接定义为0就好了
   * 
   */
  source.setDissipative(0);

  open.add(source);

  while (true) {
   if (open.isEmpty()) {
    return false;
   }
   Node currentNode = open.poll();
   progress.add(currentNode);
   closed.add(currentNode);
   if (isTarget(currentNode)) {
    latest = currentNode;
    return true;
   }
   ArrayList<Node> m = expand(currentNode);
   if (m.isEmpty()) {
    continue;
   } else {
    setPointer(m, currentNode);
   }
  }
 }

 private void setPointer(ArrayList<Node> m, Node parent) {
  for (Node node : m) {

   double newDissipative;

   /*
    * 三种算法的区别只是这里的评估值不一样
    */

   // // 最短路径算法
   // newDissipative = parent.getDissipative() + 1;

   // A*算法
   newDissipative = parent.getDissipative() + 1;
   newDissipative += heuristic(source, terminal, parent, node);

   // // 局部搜索算法
   // newDissipative = heuristic(source, terminal, parent, node);

   // 如果不在OPEN表，且不在CLOSED表，更新评估值，设置父结点，然后加入OPEN表
   if (!open.contains(node) && !closed.contains(node)) {
    node.setDissipative(newDissipative);
    open.add(node);
    node.setParent(parent);
   } else if (open.contains(node)) {
    // 如果在OPEN表中，且新的评估值小于旧的评估值，更新评估值，然后更新父结点
    if (newDissipative < node.getDissipative()) {
     node.setDissipative(newDissipative);
     node.setParent(parent);
    }
   } else if (newDissipative < node.getDissipative()) {
    // 不然就是在CLOSED表中，如果新的评估值小于旧的评估值，然后更新父结点，然后从CLOSED表中移除并加入OPEN表
    node.setDissipative(newDissipative);
    node.setParent(parent);
    closed.remove(node);
    open.add(node);
   }
  }
 }

 /**
  * 评估函数的h(x)的值
  * 
  * @param source
  *            起点
  * @param terminal
  *            终点
  * @param parent
  *            当前的点的父亲
  * @param node
  *            当前的点
  * @return h(x)的值
  */
 private double heuristic(Node source, Node terminal, Node parent, Node node) {
  return Math.abs(terminal.x - node.x) + Math.abs(terminal.y - node.y)
    + (Math.sqrt(2) - 2) * Math.min(Math.abs(terminal.x - node.x), Math.abs(terminal.y - node.y));
 }

 private ArrayList<Node> expand(Node node) {
  return getNeighbours(node);
 }

 /**
  * 获得给定的点的邻居，邻居的定义是周围8个点
  * 
  * @param node
  *            给定的点
  * @return 给定点的邻居
  */
 private ArrayList<Node> getNeighbours(Node node) {
  ArrayList<Node> m = new ArrayList<Node>();
  for (int i = -1; i <= 1; ++i) {
   for (int j = -1; j <= 1; ++j) {
    if (i == 0 && j == 0)
     continue;
    int x = node.x + i;
    int y = node.y + j;

    if (isValid(x, y)) {
     m.add(new Node(x, y));
    }
   }
  }
  return m;
 }

 /**
  * 判断当前点是不是合法，合法的定义是不超过地图边界，且不是障碍物
  * 
  * @param x
  *            横坐标
  * @param y
  *            纵坐标
  * @return 如果合法，返回true，否则，false
  */
 private boolean isValid(int x, int y) {
  if (x < 0 || x >= ConstantValue.SIZE || y < 0 || y >= ConstantValue.SIZE)
   return false;
  return (maze[x][y]);
 }

 /**
  * 判断当前点是不是终点
  * 
  * @param node
  *            当前点
  * @return 如果已经到了终点，返回true，否则，false
  */
 private boolean isTarget(Node node) {
  return (node.x == terminal.x && node.y == terminal.y);
 }

 /**
  * 以链表的形式返回最终找到的路径
  * 
  * @return 最终找到的路径
  */
 public LinkedList<Node> getResult() {
  LinkedList<Node> result = new LinkedList<Node>();
  Node node = latest;
  while (node != null) {
   result.addFirst(node);
   node = node.getParent();
  }
  return result;
 }

 /**
  * 以链表的形式获得曾经试图访问过的点
  * 
  * @return 曾经试图访问过的点
  */
 public LinkedList<Node> getProgress() {
  return progress;
 }
}

package main;

import java.awt.Point;

public class Node extends Point implements Comparable<Node> {

 private static final long serialVersionUID = 1L;

 private Node parent;
 private double dissipative;

 public double getDissipative() {
  return dissipative;
 }

 public void setDissipative(double dissipative) {
  this.dissipative = dissipative;
 }

 public Node getParent() {
  return parent;
 }

 public void setParent(Node parent) {
  this.parent = parent;
 }

 public Node(int x, int y) {
  super(x, y);
  parent = null;
 }

 @Override
 public int compareTo(Node o) {
  if (dissipative < o.dissipative)
   return -1;
  else if (dissipative == o.dissipative)
   return 0;
  else
   return 1;
 }

}

package gui;

public class Field {
 private int width;
 private int height;
 private Road[][] field;
 
 public Field(int width, int height) {
  this.width = width;
  this.height = height;
  field = new Road[height][width];
 }
 
 public int getWidth() { return width; }
 public int getHeight() { return height; }
 
 public Road replace(int row, int col, Road newRoad) {
  Road ret = field[row][col];
  field[row][col] = newRoad;
  return ret;
 }
 
 public Road get(int row, int col) {
  return field[row][col];
 }
 
 public void clear() {
  for ( int i=0; i<height; i++ ) {
   for ( int j=0; j<width; j++ ) {
    field[i][j] = null;
   }
  }
 }
}

package gui;

import java.awt.Color;
import java.awt.Graphics;
 
public class Road {
 
 
 private Color color;
 
 public Road(Color color) {
  this.color = color;
 }

 public void draw(Graphics g, int x, int y, int size) {
  g.setColor(color);
  g.drawRect(x, y, size, size);
  g.fillRect(x, y, size, size);
 }
}

package gui;

import java.awt.Dimension;
import java.awt.Graphics;

import javax.swing.JPanel;

import handler.ConstantValue;

public class View extends JPanel {

 private static final long serialVersionUID = 1L;
 
 private Field theField;
 
 public View(Field field) {
  theField = field;
 }

 @Override
 public void paint(Graphics g) {
  super.paint(g);
  for ( int row = 0; row<theField.getHeight(); row++ ) {
   for ( int col = 0; col<theField.getWidth(); col++ ) {
    Road road = theField.get(row, col);
    if ( road != null ) {
     road.draw(g, col*ConstantValue.GRID_SIZE, row*ConstantValue.GRID_SIZE, ConstantValue.GRID_SIZE);
    }
   }
  }
 }

 @Override
 public Dimension getPreferredSize() {
  return new Dimension(theField.getWidth()*ConstantValue.GRID_SIZE+1, theField.getHeight()*ConstantValue.GRID_SIZE+1);
 }

}

package handler;

import java.awt.Color;

public class ConstantValue {
 public static final int SIZE = 100;
 public static final int GRID_SIZE = 500 / SIZE;
 public static final Color COLOR_FOR_PROGRESS = new Color(0x3C, 0xF5, 0xEB);
 public static final Color COLOR_FOR_RESULT = Color.RED;
 public static final Color COLOR_FOR_BLOCK = Color.BLACK;
 public static final Color COLOR_FOR_PASSABLE_AREA = Color.WHITE;
 public static final String GUI_TITLE = "路径导航 by 张臻炜";

 private ConstantValue() {

 }

}

package handler;

import java.awt.Color;
import java.util.LinkedList;

import javax.swing.JFrame;

import gui.Field;
import gui.Road;
import gui.View;
import main.Node;

public class Draw {

 public static void draw(boolean[][] maze, LinkedList<Node> progress, LinkedList<Node> result) {
  Field field = new Field(ConstantValue.SIZE, ConstantValue.SIZE);
  for (int row = 0; row < field.getHeight(); row++) {
   for (int col = 0; col < field.getWidth(); col++) {
    Color c;
    if (maze[row][col] == true) {
     c = ConstantValue.COLOR_FOR_PASSABLE_AREA;
    } else {
     c = ConstantValue.COLOR_FOR_BLOCK;
    }
    field.replace(row, col, new Road(c));
   }
  }

  for (Node node : progress) {
   Color c = ConstantValue.COLOR_FOR_PROGRESS;
   field.replace(node.x, node.y, new Road(c));
  }

  for (Node node : result) {
   Color c = ConstantValue.COLOR_FOR_RESULT;
   field.replace(node.x, node.y, new Road(c));
  }

  View view = new View(field);
  JFrame frame = new JFrame();
  frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
  frame.setResizable(false);
  frame.setTitle(ConstantValue.GUI_TITLE);
  frame.add(view);
  frame.pack();
  frame.setVisible(true);

 }

}

package handler;

import java.io.File;
import java.io.FileNotFoundException;
import java.util.Scanner;

import main.Node;

public class InputHandler {
 String fileName;
 Node source;
 Node terminal;

 public void input() {
  Scanner in = new Scanner(System.in);
  System.out.println("请输入地图的文件名：");
  fileName = in.nextLine();
  System.out.println("请输入起点的横纵坐标，以一个空格分隔，以回车结束：");
  source = new Node(in.nextInt(), in.nextInt());
  System.out.println("请输入终点的横纵坐标，以一个空格分隔，以回车结束：");
  terminal = new Node(in.nextInt(), in.nextInt());
  in.close();
 }

 public String getFileName() throws Exception {
  if (fileName == null) {
   throw new Exception("请先输入文件名");
  } else {
   return fileName;
  }

 }

 public Node getSource() throws Exception {
  if (source == null) {
   throw new Exception("请先输入起点！");
  } else {
   return source;
  }

 }

 public Node getTerminal() throws Exception {
  if (terminal == null) {
   throw new Exception("请先输入终点！");
  } else {
   return terminal;
  }

 }

 public boolean[][] getMaze() throws FileNotFoundException {
  Scanner in = new Scanner(new File(fileName));
  boolean[][] maze = new boolean[ConstantValue.SIZE][ConstantValue.SIZE];
  for (int i = 0; i < ConstantValue.SIZE; ++i) {
   String s = in.nextLine();
   for (int j = 0; j < ConstantValue.SIZE; ++j) {
    maze[i][j] = (s.charAt(j) == '0');
   }
  }
  in.close();
  return maze;
 }

}