import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.*;
public class OrthogonalPathFinder {
private static final float GRID_SIZE = 0.5f; // 网格步长，可根据需要调整
private static final float EPSILON = 1e-5f; // 浮点数比较容差
static class Node implements Comparable<Node> {
Point2D.Float point;
Node parent;
double gScore; // 从起点到当前节点的实际代价
double fScore; // 总估计代价 (gScore + 启发式代价)
Node(Point2D.Float point, Node parent, double gScore, double fScore) {
this.point = point;
this.parent = parent;
this.gScore = gScore;
this.fScore = fScore;
}
@Override
public int compareTo(Node other) {
return Double.compare(this.fScore, other.fScore);
}
}
public static List<Point2D.Float> findPath(List<Rectangle2D.Float> obstacles, Point2D.Float start, Point2D.Float end) {
// 检查起点和终点是否在障碍物内
if (isPointInObstacle(start, obstacles) || isPointInObstacle(end, obstacles)) {
return Collections.emptyList();
}
// 计算搜索边界
float[] bounds = calculateBounds(obstacles, start, end);
float minX = bounds[0], minY = bounds[1], maxX = bounds[2], maxY = bounds[3];
// A* 算法初始化
PriorityQueue<Node> openSet = new PriorityQueue<>();
Map<Point2D.Float, Double> gScoreMap = new HashMap<>();
Map<Point2D.Float, Node> nodeMap = new HashMap<>();
Node startNode = new Node(start, null, 0, heuristic(start, end));
openSet.add(startNode);
gScoreMap.put(start, 0.0);
while (!openSet.isEmpty()) {
Node current = openSet.poll();
Point2D.Float currentPoint = current.point;
// 到达终点，回溯路径
if (isEqual(currentPoint, end)) {
return reconstructPath(current);
}
// 生成四个方向的邻居点
Point2D.Float[] neighbors = {
new Point2D.Float(currentPoint.x + GRID_SIZE, currentPoint.y), // 右
new Point2D.Float(currentPoint.x - GRID_SIZE, currentPoint.y), // 左
new Point2D.Float(currentPoint.x, currentPoint.y + GRID_SIZE), // 上
new Point2D.Float(currentPoint.x, currentPoint.y - GRID_SIZE) // 下
};
for (Point2D.Float neighbor : neighbors) {
// 检查邻居点是否在边界内
if (neighbor.x < minX || neighbor.x > maxX || neighbor.y < minY || neighbor.y > maxY) {
continue;
}
// 检查线段是否与障碍物相交
if (isLineIntersectingObstacle(currentPoint, neighbor, obstacles)) {
continue;
}
// 计算新的代价
double tentativeGScore = current.gScore + GRID_SIZE;
double oldGScore = gScoreMap.getOrDefault(neighbor, Double.POSITIVE_INFINITY);
// 如果新路径更好，更新节点
if (tentativeGScore < oldGScore) {
double fScore = tentativeGScore + heuristic(neighbor, end);
Node neighborNode = new Node(neighbor, current, tentativeGScore, fScore);
gScoreMap.put(neighbor, tentativeGScore);
openSet.add(neighborNode);
}
}
}
return Collections.emptyList(); // 无路径
}
// 回溯重建路径
private static List<Point2D.Float> reconstructPath(Node endNode) {
LinkedList<Point2D.Float> path = new LinkedList<>();
Node current = endNode;
while (current != null) {
path.addFirst(current.point);
current = current.parent;
}
return path;
}
// 计算曼哈顿距离作为启发式函数
private static double heuristic(Point2D.Float a, Point2D.Float b) {
return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
}
// 检查点是否在障碍物内
private static boolean isPointInObstacle(Point2D.Float point, List<Rectangle2D.Float> obstacles) {
for (Rectangle2D.Float rect : obstacles) {
if (rect.contains(point.x, point.y)) {
return true;
}
}
return false;
}
// 检查线段是否与障碍物相交
private static boolean isLineIntersectingObstacle(Point2D.Float a, Point2D.Float b, List<Rectangle2D.Float> obstacles) {
if (Math.abs(a.y - b.y) < EPSILON) { // 水平线段
float y = a.y;
float x1 = Math.min(a.x, b.x);
float x2 = Math.max(a.x, b.x);
for (Rectangle2D.Float rect : obstacles) {
if (y >= rect.getMinY() && y <= rect.getMaxY()) {
if (x1 <= rect.getMaxX() && x2 >= rect.getMinX()) {
return true;
}
}
}
} else if (Math.abs(a.x - b.x) < EPSILON) { // 垂直线段
float x = a.x;
float y1 = Math.min(a.y, b.y);
float y2 = Math.max(a.y, b.y);
for (Rectangle2D.Float rect : obstacles) {
if (x >= rect.getMinX() && x <= rect.getMaxX()) {
if (y1 <= rect.getMaxY() && y2 >= rect.getMinY()) {
return true;
}
}
}
}
return false;
}
// 计算搜索边界
private static float[] calculateBounds(List<Rectangle2D.Float> obstacles, Point2D.Float start, Point2D.Float end) {
float minX = Math.min(start.x, end.x);
float minY = Math.min(start.y, end.y);
float maxX = Math.max(start.x, end.x);
float maxY = Math.max(start.y, end.y);
for (Rectangle2D.Float rect : obstacles) {
minX = Math.min(minX, rect.x);
minY = Math.min(minY, rect.y);
maxX = Math.max(maxX, rect.x + rect.width);
maxY = Math.max(maxY, rect.y + rect.height);
}
// 扩展边界
minX -= 1.0f;
minY -= 1.0f;
maxX += 1.0f;
maxY += 1.0f;
return new float[]{minX, minY, maxX, maxY};
}
// 浮点数比较（容差）
private static boolean isEqual(Point2D.Float a, Point2D.Float b) {
return Math.abs(a.x - b.x) < EPSILON && Math.abs(a.y - b.y) < EPSILON;
}
}
扫一扫
2475
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
