package lujing;
|
|
import org.locationtech.jts.algorithm.Angle;
|
import org.locationtech.jts.geom.*;
|
import org.locationtech.jts.operation.distance.DistanceOp;
|
import org.locationtech.jts.operation.union.CascadedPolygonUnion;
|
|
import java.util.*;
|
import java.util.stream.Collectors;
|
|
/**
|
* 障碍物路径规划器
|
* * 优化方案：
|
* 1. 预规划：先不考虑障碍物，规划出覆盖全图的平行线路径（弓字形）。
|
* 2. 几何切割：根据障碍物的外扩（安全距离）几何体，将路径切断，移除落在障碍物内的部分。
|
* 3. 区域重组：将剩余的路径段按连通性聚类为若干个“连续区域”，每个区域内部通过弓字形连接。
|
* 4. 全局连接：使用避障算法（A*可视图）将这些孤立的区域串联起来。
|
* * * 修复问题：
|
* 1. 修复路径穿越障碍物的问题（通过更严格的 Interior Intersection 检查）。
|
* 2. 修复路径超出地块边界的问题（通过加入 Boundary Covers 约束）。
|
* 3. [本次修复] 修复 IntersectionMatrix 类型报错，使用正确的矩阵单元格检查替代不存在的方法。
|
* * * 二次优化（针对用户反馈）：
|
* 1. 引入 Tolerance Buffer (内缩) 用于相交检测，解决“接触即相交”导致的合法边缘路径被误判问题。
|
* 2. 增强 isLineSafe 的边界约束，确保路径严格在 Boundary 内部。
|
* 3. 优化 findSafePath，增加点位校验与吸附（Snap），防止因浮点误差导致的寻路失败而回退到直线。
|
*/
|
public class ObstaclePathPlanner {
|
private final List<Coordinate> polygon; // 割草区域边界点集
|
private final double width; // 割草宽度
|
private final String mode; // 模式
|
private final List<List<Coordinate>> obstacles; // 障碍物列表
|
private final double safetyDistance; // 安全距离
|
private final GeometryFactory gf = new GeometryFactory();
|
|
// 保存地块的几何形状，用于边界约束检查
|
private Geometry boundaryGeom;
|
// 用于检测的障碍物几何体（可能经过微调）
|
private Geometry checkObstacleGeom;
|
|
public ObstaclePathPlanner(List<Coordinate> polygon, double width, String mode,
|
List<List<Coordinate>> obstacles, double safetyDistance) {
|
this.polygon = polygon;
|
this.width = width;
|
this.mode = mode;
|
this.obstacles = obstacles != null ? obstacles : new ArrayList<>();
|
this.safetyDistance = safetyDistance;
|
|
initBoundaryGeom();
|
}
|
|
/**
|
* 初始化边界几何体，用于后续的约束检查
|
*/
|
private void initBoundaryGeom() {
|
if (polygon == null || polygon.size() < 3) {
|
this.boundaryGeom = gf.createPolygon();
|
return;
|
}
|
List<Coordinate> closed = new ArrayList<>(polygon);
|
if (!closed.get(0).equals2D(closed.get(closed.size() - 1))) {
|
closed.add(new Coordinate(closed.get(0)));
|
}
|
LinearRing ring = gf.createLinearRing(closed.toArray(new Coordinate[0]));
|
Polygon poly = gf.createPolygon(ring);
|
// 确保几何有效性
|
this.boundaryGeom = poly.isValid() ? poly : poly.buffer(0);
|
}
|
|
/**
|
* 生成路径的主入口
|
*/
|
public List<Lunjingguihua.PathSegment> generate() {
|
// 1. 生成初始的完整平行线路径（忽略障碍物）
|
List<Lunjingguihua.PathSegment> fullPath = generateFullPathWithoutObstacles();
|
if (fullPath.isEmpty()) return fullPath;
|
|
// 2. 构建障碍物的外扩安全区域 (Buffer)
|
Geometry obstacleBuffer = createObstacleBuffer();
|
|
// 初始化用于检测的几何体（内缩一点点，允许路径贴边）
|
if (!obstacleBuffer.isEmpty()) {
|
this.checkObstacleGeom = obstacleBuffer.buffer(-0.01); // 内缩1cm，容忍浮点误差
|
} else {
|
this.checkObstacleGeom = obstacleBuffer;
|
}
|
|
if (obstacleBuffer.isEmpty()) return fullPath;
|
|
// 3. 切割路径：移除与障碍物重叠的部分
|
List<Lunjingguihua.PathSegment> clippedSegments = clipPathWithObstacles(fullPath, obstacleBuffer);
|
if (clippedSegments.isEmpty()) return new ArrayList<>();
|
|
// 4. 重新规划：将碎片化的线段重组成连续的弓字形路径，并进行避障连接
|
List<Lunjingguihua.PathSegment> finalPath = reorganizeAndConnectPath(clippedSegments, obstacleBuffer);
|
|
// 5. 后处理（标记起终点等）
|
postProcessPath(finalPath);
|
|
return finalPath;
|
}
|
|
/**
|
* 步骤1：调用核心库生成无障碍的平行线路径
|
*/
|
private List<Lunjingguihua.PathSegment> generateFullPathWithoutObstacles() {
|
Lunjingguihua.PlannerCore tempPlanner = new Lunjingguihua.PlannerCore(
|
polygon, width, mode, new ArrayList<>());
|
return tempPlanner.generateParallelPath();
|
}
|
|
/**
|
* 步骤2：创建所有障碍物的并集 + 安全距离外扩
|
*/
|
private Geometry createObstacleBuffer() {
|
if (obstacles.isEmpty()) return gf.createPolygon();
|
|
List<Geometry> geoms = new ArrayList<>();
|
for (List<Coordinate> obs : obstacles) {
|
if (obs == null || obs.size() < 3) continue;
|
List<Coordinate> closed = new ArrayList<>(obs);
|
if (!closed.get(0).equals2D(closed.get(closed.size() - 1))) {
|
closed.add(new Coordinate(closed.get(0)));
|
}
|
LinearRing ring = gf.createLinearRing(closed.toArray(new Coordinate[0]));
|
Polygon poly = gf.createPolygon(ring);
|
if (!poly.isValid()) poly = (Polygon) poly.buffer(0);
|
geoms.add(poly);
|
}
|
|
if (geoms.isEmpty()) return gf.createPolygon();
|
|
Geometry union = CascadedPolygonUnion.union(geoms);
|
// 对合并后的障碍物进行外扩（Buffer）
|
Geometry buffered = union.buffer(safetyDistance, 8);
|
return buffered.isValid() ? buffered : buffered.buffer(0);
|
}
|
|
/**
|
* 步骤3：用障碍物几何体切割路径
|
*/
|
private List<Lunjingguihua.PathSegment> clipPathWithObstacles(List<Lunjingguihua.PathSegment> fullPath, Geometry obstacleBuffer) {
|
List<Lunjingguihua.PathSegment> validSegments = new ArrayList<>();
|
|
for (Lunjingguihua.PathSegment seg : fullPath) {
|
if (!seg.isMowing) continue;
|
|
LineString line = gf.createLineString(new Coordinate[]{seg.start, seg.end});
|
Geometry diff;
|
try {
|
diff = line.difference(obstacleBuffer);
|
} catch (Exception e) {
|
continue;
|
}
|
|
if (diff.isEmpty()) continue;
|
|
for (int i = 0; i < diff.getNumGeometries(); i++) {
|
Geometry g = diff.getGeometryN(i);
|
if (g instanceof LineString) {
|
Coordinate[] coords = g.getCoordinates();
|
for (int k = 0; k < coords.length - 1; k++) {
|
Coordinate p1 = coords[k];
|
Coordinate p2 = coords[k+1];
|
if (p1.distance(p2) > 0.1) {
|
validSegments.add(new Lunjingguihua.PathSegment(p1, p2, true));
|
}
|
}
|
}
|
}
|
}
|
return validSegments;
|
}
|
|
/**
|
* 步骤4：核心重组逻辑
|
*/
|
private List<Lunjingguihua.PathSegment> reorganizeAndConnectPath(List<Lunjingguihua.PathSegment> segments, Geometry obstacleBuffer) {
|
if (segments.isEmpty()) return new ArrayList<>();
|
|
// --- A. 分析扫描线方向 ---
|
Lunjingguihua.PathSegment firstSeg = segments.get(0);
|
double angle = Math.atan2(firstSeg.end.y - firstSeg.start.y, firstSeg.end.x - firstSeg.start.x);
|
double sin = Math.sin(angle);
|
double cos = Math.cos(angle);
|
|
// --- B. 给每个线段分配扫描线索引 (Grid Index) ---
|
double gridStep = width;
|
|
class IndexedSegment {
|
final Lunjingguihua.PathSegment segment;
|
final int gridIndex;
|
final double projectVal;
|
|
IndexedSegment(Lunjingguihua.PathSegment s) {
|
this.segment = s;
|
double cx = (s.start.x + s.end.x) / 2;
|
double cy = (s.start.y + s.end.y) / 2;
|
double perpDist = -cx * sin + cy * cos;
|
this.gridIndex = (int) Math.floor(perpDist / gridStep);
|
this.projectVal = cx * cos + cy * sin;
|
}
|
}
|
|
List<IndexedSegment> indexedSegments = segments.stream()
|
.map(IndexedSegment::new)
|
.sorted(Comparator.comparingInt((IndexedSegment s) -> s.gridIndex)
|
.thenComparingDouble(s -> s.projectVal))
|
.collect(Collectors.toList());
|
|
// --- C. 构建“区域链” (Zones) ---
|
List<List<Lunjingguihua.PathSegment>> zones = new ArrayList<>();
|
Set<IndexedSegment> visited = new HashSet<>();
|
|
while (visited.size() < indexedSegments.size()) {
|
IndexedSegment startNode = null;
|
for (IndexedSegment is : indexedSegments) {
|
if (!visited.contains(is)) {
|
startNode = is;
|
break;
|
}
|
}
|
if (startNode == null) break;
|
|
List<Lunjingguihua.PathSegment> zone = new ArrayList<>();
|
zone.add(startNode.segment);
|
visited.add(startNode);
|
|
IndexedSegment current = startNode;
|
boolean lookingForNext = true;
|
|
while (lookingForNext) {
|
IndexedSegment bestNext = null;
|
double minDistance = Double.MAX_VALUE;
|
|
// 搜索最佳后续线段
|
for (int i = 0; i < indexedSegments.size(); i++) {
|
IndexedSegment candidate = indexedSegments.get(i);
|
if (visited.contains(candidate)) continue;
|
|
if (Math.abs(candidate.gridIndex - current.gridIndex) > 1) continue;
|
|
double d = current.segment.end.distance(candidate.segment.start);
|
|
if (d > width * 3.0) continue;
|
|
if (d < minDistance) {
|
// 使用 checkObstacleGeom 进行检测
|
if (isLineSafe(current.segment.end, candidate.segment.start)) {
|
minDistance = d;
|
bestNext = candidate;
|
}
|
}
|
}
|
|
if (bestNext != null) {
|
zone.add(bestNext.segment);
|
visited.add(bestNext);
|
current = bestNext;
|
} else {
|
lookingForNext = false;
|
}
|
}
|
zones.add(zone);
|
}
|
|
// --- D. 连接所有 Zones ---
|
List<Lunjingguihua.PathSegment> resultPath = new ArrayList<>();
|
|
List<List<Lunjingguihua.PathSegment>> remainingZones = new ArrayList<>(zones);
|
List<Lunjingguihua.PathSegment> currentProcessingZone = remainingZones.remove(0);
|
|
addZoneToPath(resultPath, currentProcessingZone, obstacleBuffer, false);
|
|
while (!remainingZones.isEmpty()) {
|
Lunjingguihua.PathSegment lastSeg = resultPath.get(resultPath.size() - 1);
|
Coordinate currentPos = lastSeg.end;
|
|
int bestZoneIdx = -1;
|
double minDist = Double.MAX_VALUE;
|
|
for (int i = 0; i < remainingZones.size(); i++) {
|
List<Lunjingguihua.PathSegment> z = remainingZones.get(i);
|
if (z.isEmpty()) continue;
|
double d = currentPos.distance(z.get(0).start);
|
if (d < minDist) {
|
minDist = d;
|
bestZoneIdx = i;
|
}
|
}
|
|
if (bestZoneIdx != -1) {
|
List<Lunjingguihua.PathSegment> nextZone = remainingZones.remove(bestZoneIdx);
|
addZoneToPath(resultPath, nextZone, obstacleBuffer, true);
|
} else {
|
break;
|
}
|
}
|
|
return resultPath;
|
}
|
|
/**
|
* 将一个 Zone 添加到结果路径中
|
*/
|
private void addZoneToPath(List<Lunjingguihua.PathSegment> path,
|
List<Lunjingguihua.PathSegment> zone,
|
Geometry obstacleBuffer,
|
boolean needConnectToZoneStart) {
|
if (zone.isEmpty()) return;
|
|
// 1. 连接到 Zone 的起点
|
if (needConnectToZoneStart && !path.isEmpty()) {
|
Coordinate from = path.get(path.size() - 1).end;
|
Coordinate to = zone.get(0).start;
|
List<Coordinate> travel = findSafePath(from, to, obstacleBuffer);
|
if (travel.size() > 1) {
|
for (int i = 0; i < travel.size() - 1; i++) {
|
path.add(new Lunjingguihua.PathSegment(travel.get(i), travel.get(i+1), false));
|
}
|
}
|
}
|
|
// 2. 处理 Zone 内部
|
for (int i = 0; i < zone.size(); i++) {
|
Lunjingguihua.PathSegment seg = zone.get(i);
|
|
if (i > 0) {
|
Coordinate prevEnd = zone.get(i-1).end;
|
Coordinate currStart = seg.start;
|
if (!prevEnd.equals2D(currStart)) {
|
if (isLineSafe(prevEnd, currStart)) {
|
path.add(new Lunjingguihua.PathSegment(prevEnd, currStart, false));
|
} else {
|
List<Coordinate> detour = findSafePath(prevEnd, currStart, obstacleBuffer);
|
if (detour.size() > 1) {
|
for (int k = 0; k < detour.size() - 1; k++) {
|
path.add(new Lunjingguihua.PathSegment(detour.get(k), detour.get(k+1), false));
|
}
|
}
|
}
|
}
|
}
|
path.add(seg);
|
}
|
}
|
|
/**
|
* 检查两点连线是否安全
|
* 修改点：
|
* 1. 严格检查 Boundary (Covers)
|
* 2. 使用 checkObstacleGeom (内缩版) 检查障碍物，允许贴边
|
* 3. [Fix] 使用 matrix.get() 代替不存在的 isIntersects(int)
|
*/
|
private boolean isLineSafe(Coordinate p1, Coordinate p2) {
|
if (p1.equals2D(p2)) return true;
|
LineString line = gf.createLineString(new Coordinate[]{p1, p2});
|
|
// 1. 边界约束：线段必须完全在地块内部
|
if (boundaryGeom != null && !boundaryGeom.covers(line)) {
|
return false;
|
}
|
|
// 2. 避障约束：使用内缩后的 buffer 检查
|
// 如果 checkObstacleGeom 为空（无障碍），则安全
|
if (checkObstacleGeom == null || checkObstacleGeom.isEmpty()) return true;
|
|
IntersectionMatrix matrix = line.relate(checkObstacleGeom);
|
|
// 我们要检查：线段的任何部分（Interior）是否穿过障碍物内部（Interior）
|
// 或者 线段的端点（Boundary）是否在障碍物内部（Interior）
|
// 如果两者都是 Dimension.FALSE (-1)，则说明没有穿过内部
|
|
boolean interiorIntersects = matrix.get(Location.INTERIOR, Location.INTERIOR) != Dimension.FALSE;
|
boolean boundaryIntersects = matrix.get(Location.BOUNDARY, Location.INTERIOR) != Dimension.FALSE;
|
|
return !interiorIntersects && !boundaryIntersects;
|
}
|
|
/**
|
* 寻找两点间的安全路径
|
* 修改点：
|
* 1. 增加点位校验与吸附（Snap），确保起点终点有效
|
* 2. 移除直线强制回退，若寻路失败则返回空（或保留起点），避免穿墙
|
*/
|
private List<Coordinate> findSafePath(Coordinate start, Coordinate end, Geometry obstacleBuffer) {
|
// 0. 数据清洗：吸附起点终点到合法区域
|
Coordinate safeStart = snapPointToValid(start);
|
Coordinate safeEnd = snapPointToValid(end);
|
|
List<Coordinate> path = new ArrayList<>();
|
|
// 1. 尝试直连
|
if (isLineSafe(safeStart, safeEnd)) {
|
path.add(safeStart);
|
path.add(safeEnd);
|
return path;
|
}
|
|
// 2. 构建可视图
|
Set<Coordinate> nodes = new HashSet<>();
|
nodes.add(safeStart);
|
nodes.add(safeEnd);
|
|
// 提取障碍物顶点
|
addPolygonVertices(obstacleBuffer, nodes);
|
// 提取边界顶点（关键：处理凹形边界）
|
addPolygonVertices(boundaryGeom, nodes);
|
|
List<Coordinate> nodeList = new ArrayList<>(nodes);
|
|
// 构建邻接表
|
Map<Coordinate, List<Coordinate>> graph = new HashMap<>();
|
for (Coordinate c1 : nodeList) {
|
for (Coordinate c2 : nodeList) {
|
if (c1 == c2) continue;
|
if (isLineSafe(c1, c2)) {
|
graph.computeIfAbsent(c1, k -> new ArrayList<>()).add(c2);
|
}
|
}
|
}
|
|
// Dijkstra 寻路
|
Map<Coordinate, Double> dist = new HashMap<>();
|
Map<Coordinate, Coordinate> prev = new HashMap<>();
|
PriorityQueue<Coordinate> pq = new PriorityQueue<>(Comparator.comparingDouble(dist::get));
|
|
for (Coordinate n : nodeList) dist.put(n, Double.MAX_VALUE);
|
dist.put(safeStart, 0.0);
|
pq.add(safeStart);
|
|
while (!pq.isEmpty()) {
|
Coordinate u = pq.poll();
|
if (u.equals2D(safeEnd)) break;
|
if (dist.get(u) == Double.MAX_VALUE) break;
|
|
if (graph.containsKey(u)) {
|
for (Coordinate v : graph.get(u)) {
|
double alt = dist.get(u) + u.distance(v);
|
if (alt < dist.get(v)) {
|
dist.put(v, alt);
|
prev.put(v, u);
|
pq.add(v);
|
}
|
}
|
}
|
}
|
|
// 重构路径
|
if (prev.containsKey(safeEnd)) {
|
LinkedList<Coordinate> p = new LinkedList<>();
|
Coordinate curr = safeEnd;
|
while (curr != null) {
|
p.addFirst(curr);
|
curr = prev.get(curr);
|
}
|
return p;
|
}
|
|
// 寻路失败，返回空（避免错误的直线穿越）
|
return path;
|
}
|
|
// 辅助：验证并吸附点到合法区域
|
private Coordinate snapPointToValid(Coordinate p) {
|
Point point = gf.createPoint(p);
|
boolean inBoundary = (boundaryGeom == null) || boundaryGeom.covers(point);
|
boolean outObstacle = (checkObstacleGeom == null) || !checkObstacleGeom.contains(point); // 使用 contains 而不是 intersects interior，稍微严格点
|
|
if (inBoundary && outObstacle) return p;
|
|
// 如果点无效，尝试找最近的有效点（边界或障碍物边缘）
|
// 这里简化处理：如果在障碍物内，吸附到障碍物边界；如果在边界外，吸附到边界
|
// 实际上 JTS DistanceOp.nearestPoints 可以做这个
|
|
Geometry target = boundaryGeom;
|
if (!outObstacle && checkObstacleGeom != null) {
|
// 在障碍物内，优先吸附出障碍物
|
Coordinate[] nearest = DistanceOp.nearestPoints(point, checkObstacleGeom.getBoundary());
|
return nearest[1];
|
}
|
|
if (!inBoundary && boundaryGeom != null) {
|
Coordinate[] nearest = DistanceOp.nearestPoints(point, boundaryGeom);
|
return nearest[1];
|
}
|
|
return p;
|
}
|
|
private void addPolygonVertices(Geometry geom, Set<Coordinate> nodes) {
|
if (geom == null) return;
|
if (geom instanceof Polygon) {
|
Collections.addAll(nodes, ((Polygon) geom).getExteriorRing().getCoordinates());
|
for(int i=0; i<((Polygon)geom).getNumInteriorRing(); i++) {
|
Collections.addAll(nodes, ((Polygon)geom).getInteriorRingN(i).getCoordinates());
|
}
|
} else if (geom instanceof MultiPolygon) {
|
MultiPolygon mp = (MultiPolygon) geom;
|
for (int i = 0; i < mp.getNumGeometries(); i++) {
|
addPolygonVertices(mp.getGeometryN(i), nodes);
|
}
|
} else if (geom instanceof GeometryCollection) {
|
GeometryCollection gc = (GeometryCollection) geom;
|
for (int i = 0; i < gc.getNumGeometries(); i++) {
|
addPolygonVertices(gc.getGeometryN(i), nodes);
|
}
|
}
|
}
|
|
/**
|
* 后处理：移除短线段，标记起终点
|
*/
|
private void postProcessPath(List<Lunjingguihua.PathSegment> path) {
|
if (path.isEmpty()) return;
|
path.removeIf(seg -> seg.start.distance(seg.end) < 0.05);
|
for (Lunjingguihua.PathSegment seg : path) {
|
seg.isStartPoint = false;
|
seg.isEndPoint = false;
|
}
|
boolean startFound = false;
|
for (Lunjingguihua.PathSegment seg : path) {
|
if (seg.isMowing) {
|
seg.setAsStartPoint();
|
startFound = true;
|
break;
|
}
|
}
|
for (int i = path.size() - 1; i >= 0; i--) {
|
Lunjingguihua.PathSegment seg = path.get(i);
|
if (seg.isMowing) {
|
seg.setAsEndPoint();
|
break;
|
}
|
}
|
}
|
}
|
