| | |
|---|
| | | package lujing; |
|---|
| | | |
|---|
| | | import java.util.*; |
|---|
| | | import java.util.Set; |
|---|
| | | import java.util.HashSet; |
|---|
| | | |
|---|
| | | /** |
|---|
| | | * 异形草地路径规划 - 凹多边形兼容优化版 V5.0 |
|---|
| | |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static int getEdgeIndex(Point p, List<Point> poly) { |
|---|
| | | int bestIdx = -1; |
|---|
| | | double minD = Double.MAX_VALUE; |
|---|
| | | for (int i = 0; i < poly.size(); i++) { |
|---|
| | | Point p1 = poly.get(i); |
|---|
| | | Point p2 = poly.get((i + 1) % poly.size()); |
|---|
| | | if (distToSegment(p, p1, p2) < 1e-3) return i; |
|---|
| | | double d = distToSegment(p, p1, p2); |
|---|
| | | if (d < minD) { |
|---|
| | | minD = d; |
|---|
| | | bestIdx = i; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | return -1; |
|---|
| | | // 只要找到最近的边即可，放宽阈值以应对浮点误差和旋转变形 |
|---|
| | | // 如果距离过大（例如超过1米），可能确实不在边界上，但在路径规划上下文中， |
|---|
| | | // 这些点是由扫描线生成的，理论上一定在边界上，所以强制吸附是安全的。 |
|---|
| | | return minD < 1.0 ? bestIdx : -1; |
|---|
| | | } |
|---|
| | | |
|---|
| | | private static double distToSegment(Point p, Point s, Point e) { |
|---|
