参考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 + "++++++++++++++++++++++");
}
}
旁边没有障碍物，就可以按对角线走。。。
扫一扫
203
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
