JPS(Jump Point Search)寻路(Java版本)

参考https://github.com/qiao/PathFinding.js

package pathfind;

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

public class Grid {
	/**
	 * The number of rows of the grid.
	 */
	private int height;

	/**
	 * The number of columns of the grid.
	 */
	private int width;

	/**
	 * A 2D array of nodes.
	 */
	private Node[][] nodes;

	public Grid(int[][] matrix) {
		this.width = matrix[0].length;
		this.height = matrix.length;
		this.nodes = this.buildNodes(width, height, matrix);

	}

	private Node[][] buildNodes(int width, int height, int[][] matrix) {
		Node[][] currNodes = new Node[width][height];
		for (int i = 0; i < height; ++i) {
			for (int j = 0; j < width; ++j) {
				currNodes[i][j] = new Node(j, i);
				if (matrix[i][j] == 1) {
					currNodes[i][j].setWalkable(false);
				}
			}
		}
		return currNodes;
	}

	public Node getNodeAt(int x, int y) {
		return this.nodes[y][x];
	}

	public boolean isWalkableAt(int x, int y) {
		return this.isInside(x, y) && this.nodes[y][x].isWalkable();
	}

	private boolean isInside(int x, int y) {
		return (x >= 0 && x < this.width) && (y >= 0 && y < this.height);
	}

	public void setWalkableAt(int x, int y, boolean walkable) {
		this.nodes[y][x].setWalkable(walkable);
	}

	public List<Node> getNeighbors(Node node) {
		int x = node.getX();
		int y = node.getY();
		boolean s0 = false;
		boolean s1 = false;
		boolean s2 = false;
		boolean s3 = false;
		List<Node> neighbors = new ArrayList<Node>();
		// ↑
		if (this.isWalkableAt(x, y - 1)) {
			neighbors.add(this.nodes[y - 1][x]);
			s0 = true;
		}
		// →
		if (this.isWalkableAt(x + 1, y)) {
			neighbors.add(this.nodes[y][x + 1]);
			s1 = true;
		}
		// ↓
		if (this.isWalkableAt(x, y + 1)) {
			neighbors.add(this.nodes[y + 1][x]);
			s2 = true;
		}
		// ←
		if (this.isWalkableAt(x - 1, y)) {
			neighbors.add(this.nodes[y][x - 1]);
			s3 = true;
		}
		boolean d0 = s3 && s0;
		boolean d1 = s0 && s1;
		boolean d2 = s1 && s2;
		boolean d3 = s2 && s3;
		// ↖
		if (d0 && this.isWalkableAt(x - 1, y - 1)) {
			neighbors.add(this.nodes[y - 1][x - 1]);
		}
		// ↗
		if (d1 && this.isWalkableAt(x + 1, y - 1)) {
			neighbors.add(this.nodes[y - 1][x + 1]);
		}
		// ↘
		if (d2 && this.isWalkableAt(x + 1, y + 1)) {
			neighbors.add(this.nodes[y + 1][x + 1]);
		}
		// ↙
		if (d3 && this.isWalkableAt(x - 1, y + 1)) {
			neighbors.add(this.nodes[y + 1][x - 1]);
		}
		return neighbors;
	}

}
package pathfind;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.PriorityQueue;

public abstract class JumpPointFinderBase {

	protected PriorityQueue<Node> openList;
	protected Node startNode;
	protected Node endNode;
	protected Grid grid;

	public JumpPointFinderBase(int startX, int startY, int endX, int endY, Grid grid) {
		this.grid = grid;
		this.openList = new PriorityQueue<>();
		this.startNode = grid.getNodeAt(startX, startY);
		this.endNode = grid.getNodeAt(endX, endY);
		// push the start node into the open list
		this.openList.add(this.startNode);
		this.startNode.setOpened(true);
	}

	public List<int[]> findPath() {
		// while the open list is not empty
		while (!this.openList.isEmpty()) {
			// pop the position of node which has the minimum `f` value.
			Node node = this.openList.poll();
			node.setClosed(true);
			if (node == this.endNode) {
				return this.compressPath(this.backtrace(node));
			}
			this.identifySuccessors(node);
		}
		// fail to find the path
		return null;
	}

	private void identifySuccessors(Node node) {
		int x = node.getX();
		int y = node.getY();
		List<int[]> neighbors = this.findNeighbors(node);
		for (int i = 0, l = neighbors.size(); i < l; ++i) {
			int[] neighbor = neighbors.get(i);
			int[] jumpPoint = this.jump(neighbor[0], neighbor[1], x, y);
			if (jumpPoint != null) {
				int jx = jumpPoint[0];
				int jy = jumpPoint[1];
				Node jumpNode = this.grid.getNodeAt(jx, jy);
				if (jumpNode.isClosed()) {
					continue;
				}
				// include distance, as parent may not be immediately adjacent:
				int d = Util.octile(Math.abs(jx - x), Math.abs(jy - y));
				int ng = node.getG() + d; // next `g` value
				if (!jumpNode.isOpened() || ng < jumpNode.getG()) {
					jumpNode.setG(ng);
					if (jumpNode.getH() == 0) {
						int endX = this.endNode.getX();
						int endY = this.endNode.getY();
						jumpNode.setH(Util.heuristic(Math.abs(jx - endX), Math.abs(jy - endY)));
					}
					jumpNode.setF(jumpNode.getG() + jumpNode.getH());
					jumpNode.setParent(node);
					if (!jumpNode.isOpened()) {
						this.openList.add(jumpNode);
						jumpNode.setOpened(true);
					}
				}
			}
		}
	}

	private List<int[]> backtrace(Node node) {
		List<int[]> paths = new ArrayList<int[]>();
		paths.add(new int[] { node.getX(), node.getY() });
		while (node.getParent() != null) {
			node = node.getParent();
			paths.add(new int[] { node.getX(), node.getY() });
		}
		Collections.reverse(paths);
		return paths;
	}

	private List<int[]> compressPath(List<int[]> path) {
		if (path.size() < 3) {
			return path;
		}
		List<int[]> compressed = new ArrayList<>();
		int sx = path.get(0)[0]; // start x
		int sy = path.get(0)[1]; // start y
		int px = path.get(1)[0]; // second point x
		int py = path.get(1)[1]; // second point y
		int dx = px - sx; // direction between the two points
		int dy = py - sy; // direction between the two points
		int lx, ly;
		int ldx, ldy;
		int sq, i;
		sq = (int) Math.sqrt(dx * dx + dy * dy);
		dx /= sq;
		dy /= sq;
		compressed.add(new int[] { sx, sy });
		for (i = 2; i < path.size(); i++) {
			lx = px;
			ly = py;
			ldx = dx;
			ldy = dy;
			px = path.get(i)[0];
			py = path.get(i)[1];
			dx = px - lx;
			dy = py - ly;
			sq = (int) Math.sqrt(dx * dx + dy * dy);
			dx /= sq;
			dy /= sq;
			if (dx != ldx || dy != ldy) {
				compressed.add(new int[] { lx, ly });
			}
		}
		compressed.add(new int[] { px, py });
		return compressed;
	}

	protected abstract int[] jump(int i, int j, int x, int y);

	protected abstract List<int[]> findNeighbors(Node node);
}
package pathfind;

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

public class JPFMoveDiagonallyIfNoObstacles extends JumpPointFinderBase {

	public JPFMoveDiagonallyIfNoObstacles(int startX, int startY, int endX, int endY, Grid grid) {
		super(startX, startY, endX, endY, grid);
	}

	public int[] jump(int x, int y, int px, int py) {
		if (!super.grid.isWalkableAt(x, y)) {
			return null;
		}
		if (super.grid.getNodeAt(x, y) == super.endNode) {
			return new int[] { x, y };
		}
		int dx = x - px;
		int dy = y - py;
		// check for forced neighbors
		// along the diagonal
		if (dx != 0 && dy != 0) {
			// when moving diagonally, must check for vertical/horizontal jump points
			if (this.jump(x + dx, y, x, y) != null || this.jump(x, y + dy, x, y) != null) {
				return new int[] { x, y };
			}
		}
		// horizontally/vertically
		else {
			if (dx != 0) {
				if ((super.grid.isWalkableAt(x, y - 1) && !super.grid.isWalkableAt(x - dx, y - 1))
						|| (super.grid.isWalkableAt(x, y + 1) && !super.grid.isWalkableAt(x - dx, y + 1))) {
					return new int[] { x, y };
				}
			} else if (dy != 0) {
				if ((super.grid.isWalkableAt(x - 1, y) && !super.grid.isWalkableAt(x - 1, y - dy))
						|| (super.grid.isWalkableAt(x + 1, y) && !super.grid.isWalkableAt(x + 1, y - dy))) {
					return new int[] { x, y };
				}
			}
		}
		// moving diagonally, must make sure one of the vertical/horizontal
		// neighbors is open to allow the path
		if (super.grid.isWalkableAt(x + dx, y) && super.grid.isWalkableAt(x, y + dy)) {
			return this.jump(x + dx, y + dy, x, y);
		} else {
			return null;
		}
	}

	protected List<int[]> findNeighbors(Node node) {
		List<int[]> neighbors = new ArrayList<>();
		Node parent = node.getParent();
		// directed pruning: can ignore most neighbors, unless forced.
		if (parent != null) {
			int px = parent.getX();
			int py = parent.getY();
			int x = node.getX();
			int y = node.getY();
			// get the normalized direction of travel
			int dx = (x - px) / Math.max(Math.abs(x - px), 1);
			int dy = (y - py) / Math.max(Math.abs(y - py), 1);
			// search diagonally
			if (dx != 0 && dy != 0) {
				if (super.grid.isWalkableAt(x, y + dy)) {
					neighbors.add(new int[] { x, y + dy });
				}
				if (super.grid.isWalkableAt(x + dx, y)) {
					neighbors.add(new int[] { x + dx, y });
				}
				if (super.grid.isWalkableAt(x, y + dy) && super.grid.isWalkableAt(x + dx, y)) {
					neighbors.add(new int[] { x + dx, y + dy });
				}
			}
			// search horizontally/vertically
			else {
				boolean isNextWalkable;
				if (dx != 0) {
					isNextWalkable = super.grid.isWalkableAt(x + dx, y);
					boolean isTopWalkable = super.grid.isWalkableAt(x, y + 1);
					boolean isBottomWalkable = super.grid.isWalkableAt(x, y - 1);
					if (isNextWalkable) {
						neighbors.add(new int[] { x + dx, y });
						if (isTopWalkable) {
							neighbors.add(new int[] { x + dx, y + 1 });
						}
						if (isBottomWalkable) {
							neighbors.add(new int[] { x + dx, y - 1 });
						}
					}
					if (isTopWalkable) {
						neighbors.add(new int[] { x, y + 1 });
					}
					if (isBottomWalkable) {
						neighbors.add(new int[] { x, y - 1 });
					}
				} else if (dy != 0) {
					isNextWalkable = super.grid.isWalkableAt(x, y + dy);
					boolean isRightWalkable = super.grid.isWalkableAt(x + 1, y);
					boolean isLeftWalkable = super.grid.isWalkableAt(x - 1, y);
					if (isNextWalkable) {
						neighbors.add(new int[] { x, y + dy });
						if (isRightWalkable) {
							neighbors.add(new int[] { x + 1, y + dy });
						}
						if (isLeftWalkable) {
							neighbors.add(new int[] { x - 1, y + dy });
						}
					}
					if (isRightWalkable) {
						neighbors.add(new int[] { x + 1, y });
					}
					if (isLeftWalkable) {
						neighbors.add(new int[] { x - 1, y });
					}
				}
			}
		}
		// return all neighbors
		else {
			List<Node> neighborNodes = super.grid.getNeighbors(node);
			for (int i = 0, l = neighborNodes.size(); i < l; ++i) {
				Node neighborNode = neighborNodes.get(i);
				neighbors.add(new int[] { neighborNode.getX(), neighborNode.getY() });
			}
		}
		return neighbors;
	}

}
package pathfind;

/**
 * A node in grid.
 * 
 * @author Administrator
 *
 */
public class Node implements Comparable<Node> {

	/**
	 * The x coordinate of the node on the grid.
	 */
	private int x;
	/**
	 * The y coordinate of the node on the grid.
	 */
	private int y;
	/**
	 * Whether this node can be walked through.
	 */
	private boolean walkable = true;
	private int f;
	private int g;
	private int h;
	private boolean opened = false;
	private boolean closed = false;
	private Node parent;

	public Node(int x, int y) {
		this(x, y, true);
	}

	public Node(int x, int y, boolean walkable) {
		this.setX(x);
		this.setY(y);
		this.setWalkable(walkable);
	}

	public int getX() {
		return x;
	}

	public void setX(int x) {
		this.x = x;
	}

	public int getY() {
		return y;
	}

	public void setY(int y) {
		this.y = y;
	}

	public boolean isWalkable() {
		return walkable;
	}

	public void setWalkable(boolean walkable) {
		this.walkable = walkable;
	}

	public int getG() {
		return g;
	}

	public void setG(int g) {
		this.g = g;
	}

	public int getF() {
		return f;
	}

	public void setF(int f) {
		this.f = f;
	}

	public boolean isOpened() {
		return opened;
	}

	public void setOpened(boolean opened) {
		this.opened = opened;
	}

	public boolean isClosed() {
		return closed;
	}

	public void setClosed(boolean closed) {
		this.closed = closed;
	}

	public Node getParent() {
		return parent;
	}

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

	public int getH() {
		return h;
	}

	public void setH(int h) {
		this.h = h;
	}

	@Override
	public int compareTo(Node o) {

		return this.f - o.getF();
	}

	@Override
	public String toString() {
		return "x=" + x + " y=" + y + " walkable=" + walkable + " g=" + g + " f=" + f + " h=" + h + " opend=" + opened
				+ " closed=" + closed;
	}

}

package pathfind;

public class Util {
	public static int octile(int dx, int dy) {
		return (int) (Math.max(dx, dy) + (Math.sqrt(2) - 1) * Math.min(dx, dy));
	}

	public static int heuristic(int dx, int dy) {
		return (int) Math.sqrt(dx * dx + dy * dy);
	}
}
package pathfind;

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

public class Test {
	public static void main(String[] args) {
		int[][] matrix = { 
				{ 0, 0, 0, 0, 0, 0 }, 
				{ 1, 1, 1, 1, 1, 0 }, 
				{ 0, 0, 1, 0, 0, 0 }, 
				{ 0, 0, 0, 1, 0, 0 },
				{ 0, 0, 0, 0, 0, 0 }, 
				{ 0, 0, 0, 0, 0, 0 }
				};
		int startX = 5;
		int startY = 4;
		int endX = 0;
		int endY = 0;
		showStartPath("start", matrix, startX, startY, endX, endY);
		List<int[]> paths = new JPFMoveDiagonallyIfNoObstacles(startX, startY, endX, endY, new Grid(matrix)).findPath();
		if (paths == null) {
			System.out.println("Path not found!");
		} else {
			showEndPath("end", matrix, paths);
		}
	}

	private static void showStartPath(String tips, int[][] shows, int startX, int startY, int endX, int endY) {
		String[][] showStrs = new String[shows[0].length][shows.length];
		for (int i = 0; i < shows[0].length; i++) {
			for (int j = 0; j < shows.length; j++) {
				showStrs[j][i] = shows[j][i] + " ";
			}
		}
		showStrs[startY][startX] = "S ";
		showStrs[endY][endX] = "E ";
		System.out.println("+++++++++++++++++++++" + tips + "++++++++++++++++++++++");
		System.out.println("start:"+Arrays.toString(new int[] {startY,startX}));
		System.out.println("end  :"+Arrays.toString(new int[] {endY,endX}));
		for (String[] showStr : showStrs) {
			System.out.println(Arrays.toString(showStr));
		}
		System.out.println("+++++++++++++++++++++" + tips + "++++++++++++++++++++++");
	}

	private static void showEndPath(String tips, int[][] shows, List<int[]> paths) {
		if (paths.size() <= 1) {
			return;
		}
		String[][] showStrs = new String[shows[0].length][shows.length];
		for (int i = 0; i < shows[0].length; i++) {
			for (int j = 0; j < shows.length; j++) {
				showStrs[j][i] = shows[j][i] + " ";
			}
		}
		List<String[]> pathStr = new ArrayList<>();
		for (int i = 1; i < paths.size(); i++) {
			int previousX = paths.get(i - 1)[0];
			int previousY = paths.get(i - 1)[1];
			int currX = paths.get(i)[0];
			int currY = paths.get(i)[1];
			int dx = currX - previousX;
			int dy = currY - previousY;
			if (dx != 0 && dy != 0) {
				if (dx > 0 && dy < 0) {
					while (currX != previousX && currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↗" });
						previousX++;
						previousY--;
					}
				} else if (dx > 0 && dy > 0) {
					while (currX != previousX && currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↘" });
						previousX++;
						previousY++;
					}
				} else if (dx < 0 && dy > 0) {
					while (currX != previousX && currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↙" });
						previousX--;
						previousY++;
					}
				} else if (dx < 0 && dy < 0) {
					while (currX != previousX && currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↖" });
						previousX--;
						previousY--;
					}
				}
			}
			if (dx == 0) {
				if (dy < 0) {
					while (currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↑" });
						previousY--;
					}
				} else {
					while (currY != previousY) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "↓" });
						previousY++;
					}
				}
			} else if (dy == 0) {
				if (dx < 0) {
					while (currX != previousX) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "←" });
						previousX--;
					}
				} else {
					while (currX != previousX) {
						pathStr.add(new String[] { String.valueOf(previousY), String.valueOf(previousX), "→" });
						previousX++;
					}
				}
			}
		}
		pathStr.add(new String[] { String.valueOf(paths.get(paths.size() - 1)[1]),
				String.valueOf(paths.get(paths.size() - 1)[0]), pathStr.get(pathStr.size() - 1)[2] });
		for (String[] str : pathStr) {
			showStrs[Integer.parseInt(str[0])][Integer.parseInt(str[1])] = str[2];
		}
		System.out.println("+++++++++++++++++++++" + tips + "++++++++++++++++++++++");
		List<int[]> pathStrs = new ArrayList<>();
		for(int[] path :paths) {
			pathStrs.add(new int[] {path[1],path[0]});
		}
		System.out.println("Path:" + Arrays.deepToString(pathStrs.toArray()));
		for (String[] showStr : showStrs) {
			System.out.println(Arrays.toString(showStr));
		}
		System.out.println("+++++++++++++++++++++" + tips + "++++++++++++++++++++++");
	}
}

旁边没有障碍物,就可以按对角线走。。。

  • 0
    点赞
  • 3
    收藏
  • 打赏
    打赏
  • 5
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:深蓝海洋 设计师:CSDN官方博客 返回首页
评论 5

打赏作者

xiaohailalala

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值