新增有障碍物的路径规划算法和优化没有障碍物的路径算法

张世豪

6 天以前 1cf1ecbc75c6d14b40efb3161e7db0b8b64f7de2

 src/zhangaiwu/Obstacledraw.java

@@ -10,21 +10,28 @@

/**
 * 障碍物绘制类 - 负责绘制地块中的障碍物
 * 
 * 注意：障碍物图层需要处于地块和导航路径上方。
 * 在 MapRenderer.renderMap() 中，绘制顺序应为：
 * 1. 地块边界（底层）
 * 2. 导航路径（中层）
 * 3. 障碍物（顶层，显示在地块和导航路径上方）
 */
public class Obstacledraw {
    
    // 颜色定义
    private static final Color CIRCLE_FILL_COLOR = new Color(255, 182, 193, 120); // 圆形填充色 - 浅粉红
    private static final Color CIRCLE_FILL_COLOR = new Color(128, 128, 128, 128); // 圆形填充色 - 灰色，透明度50%
    private static final Color CIRCLE_BORDER_COLOR = new Color(199, 21, 133);    // 圆形边框色 - 深粉红
    private static final Color POLYGON_FILL_COLOR = new Color(173, 216, 230, 120); // 多边形填充色 - 浅蓝
    private static final Color POLYGON_FILL_COLOR = new Color(128, 128, 128, 128); // 多边形填充色 - 灰色，透明度50%
    private static final Color POLYGON_BORDER_COLOR = new Color(25, 25, 112);    // 多边形边框色 - 深蓝
    private static final Color OBSTACLE_LABEL_COLOR = Color.BLACK;
    private static final Color OBSTACLE_POINT_COLOR = Color.RED;
    
    // 尺寸定义
    private static final double OBSTACLE_POINT_SIZE = 0.1; // 障碍物控制点大小（米）
    private static final float DEFAULT_BORDER_WIDTH = 1.0f;
    private static final float SELECTED_BORDER_WIDTH = 2.5f;
    // 边界线宽度与地块边界线宽度一致：3 / Math.max(0.5, scale)
    private static final float BOUNDARY_STROKE_BASE = 3.0f; // 与地块边界线宽度一致
    private static final float SELECTED_WIDTH_MULTIPLIER = 1.5f; // 选中时的宽度倍数
    
    /**
     * 绘制地块的所有障碍物
@@ -109,13 +116,15 @@
        g2d.setColor(CIRCLE_FILL_COLOR);
        g2d.fill(circle);
        
        // 设置边框颜色和宽度
        // 设置边框颜色和宽度（与地块边界线宽度一致）
        float strokeWidth = (float)(BOUNDARY_STROKE_BASE / Math.max(0.5, scale));
        if (isSelected) {
            g2d.setColor(CIRCLE_BORDER_COLOR.darker());
            g2d.setStroke(new BasicStroke(SELECTED_BORDER_WIDTH / (float)scale));
            // 选中时稍微加粗
            g2d.setStroke(new BasicStroke(strokeWidth * SELECTED_WIDTH_MULTIPLIER));
        } else {
            g2d.setColor(CIRCLE_BORDER_COLOR);
            g2d.setStroke(new BasicStroke(DEFAULT_BORDER_WIDTH / (float)scale));
            g2d.setStroke(new BasicStroke(strokeWidth));
        }
        g2d.draw(circle);
        
@@ -153,13 +162,15 @@
        g2d.setColor(POLYGON_FILL_COLOR);
        g2d.fill(polygon);
        
        // 设置边框颜色和宽度
        // 设置边框颜色和宽度（与地块边界线宽度一致）
        float strokeWidth = (float)(BOUNDARY_STROKE_BASE / Math.max(0.5, scale));
        if (isSelected) {
            g2d.setColor(POLYGON_BORDER_COLOR.darker());
            g2d.setStroke(new BasicStroke(SELECTED_BORDER_WIDTH / (float)scale));
            // 选中时稍微加粗
            g2d.setStroke(new BasicStroke(strokeWidth * SELECTED_WIDTH_MULTIPLIER));
        } else {
            g2d.setColor(POLYGON_BORDER_COLOR);
            g2d.setStroke(new BasicStroke(DEFAULT_BORDER_WIDTH / (float)scale));
            g2d.setStroke(new BasicStroke(strokeWidth));
        }
        g2d.draw(polygon);
        
@@ -202,6 +213,7 @@
    
    /**
     * 绘制障碍物标签
     * 文字大小与"缩放"文字一致（11号字体），且不随地图缩放变化
     */
    private static void drawObstacleLabel(Graphics2D g2d, Obstacledge.Obstacle obstacle, 
                                          double scale) {
@@ -210,42 +222,92 @@
            return;
        }
        
        // 计算标签位置（中心点）
        double centerX = 0;
        double centerY = 0;
        // 保存当前变换
        AffineTransform originalTransform = g2d.getTransform();
        
        for (Obstacledge.XYCoordinate coord : xyCoords) {
            centerX += coord.getX();
            centerY += coord.getY();
        double centerX;
        double centerY;

        Obstacledge.ObstacleShape shape = obstacle.getShape();
        if (shape == Obstacledge.ObstacleShape.CIRCLE) {
            Obstacledge.XYCoordinate centerCoord = xyCoords.get(0);
            centerX = centerCoord.getX();
            centerY = centerCoord.getY();
        } else {
            Point2D.Double centroid = computePolygonCentroid(xyCoords);
            centerX = centroid.x;
            centerY = centroid.y;
        }
        
        centerX /= xyCoords.size();
        centerY /= xyCoords.size();
        // 将世界坐标转换为屏幕坐标
        Point2D.Double worldPoint = new Point2D.Double(centerX, centerY);
        Point2D.Double screenPoint = new Point2D.Double();
        originalTransform.transform(worldPoint, screenPoint);
        
        // 获取障碍物名称和形状
        // 恢复原始变换以使用屏幕坐标绘制
        g2d.setTransform(new AffineTransform());
        
        // 获取障碍物名称
        String obstacleName = obstacle.getObstacleName();
        String shapeDesc = obstacle.getShape().getDescription();
        if (obstacleName == null || obstacleName.trim().isEmpty()) {
            obstacleName = "障碍物";
        } else {
            obstacleName = obstacleName.trim();
        }
        
        // 设置字体和颜色
        // 设置字体和颜色（与"缩放"文字一致）
        g2d.setColor(OBSTACLE_LABEL_COLOR);
        
        // 根据缩放比例调整字体大小
        int fontSize = (int)(10 / scale);
        fontSize = Math.max(8, Math.min(fontSize, 14)); // 限制字体大小范围
        
        g2d.setFont(new Font("微软雅黑", Font.PLAIN, fontSize));
        g2d.setFont(new Font("微软雅黑", Font.PLAIN, 11)); // 与"缩放"文字大小一致
        
        // 绘制标签
        String label = String.format("%s(%s)", obstacleName, shapeDesc);
        String label = obstacleName;
        FontMetrics metrics = g2d.getFontMetrics();
        int textWidth = metrics.stringWidth(label);
        int textHeight = metrics.getHeight();
        
        // 在中心点绘制标签
        int textX = (int)(centerX - textWidth / 2.0);
        int textY = (int)(centerY + textHeight / 4.0); // 稍微向下偏移
        // 在屏幕坐标中心点绘制标签
        int textX = (int)(screenPoint.x - textWidth / 2.0);
        int textY = (int)(screenPoint.y + textHeight / 4.0); // 稍微向下偏移
        
        g2d.drawString(label, textX, textY);
        
        // 恢复原始变换
        g2d.setTransform(originalTransform);
    }

    private static Point2D.Double computePolygonCentroid(List<Obstacledge.XYCoordinate> xyCoords) {
        double area = 0.0;
        double cx = 0.0;
        double cy = 0.0;
        int n = xyCoords.size();
        for (int i = 0; i < n; i++) {
            Obstacledge.XYCoordinate current = xyCoords.get(i);
            Obstacledge.XYCoordinate next = xyCoords.get((i + 1) % n);
            double x0 = current.getX();
            double y0 = current.getY();
            double x1 = next.getX();
            double y1 = next.getY();
            double cross = x0 * y1 - x1 * y0;
            area += cross;
            cx += (x0 + x1) * cross;
            cy += (y0 + y1) * cross;
        }

        double areaFactor = area * 0.5;
        if (Math.abs(areaFactor) < 1e-9) {
            double avgX = 0.0;
            double avgY = 0.0;
            for (Obstacledge.XYCoordinate coord : xyCoords) {
                avgX += coord.getX();
                avgY += coord.getY();
            }
            int size = Math.max(1, xyCoords.size());
            return new Point2D.Double(avgX / size, avgY / size);
        }

        double centroidX = cx / (6.0 * areaFactor);
        double centroidY = cy / (6.0 * areaFactor);
        return new Point2D.Double(centroidX, centroidY);
    }
    
    /**