rrt.java

@@ -0,0 +1,209 @@
+package DemonKing;
+
+/**
+ * <p>
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import java.util.Random;
+
+ public class RRTStar {
+
+    private List<Point> nodes;
+    private static final double step = 20.0;
+    private static final double goalTolerance = 5;
+    private static final double obstacleRadius = 1.0;
+    private static final double expandDistance = 5.0;
+    private static final int maxIter = 10000;
+
+    public static void main(String[] args) {
+        // 设置起点和终点
+        Point start = new Point(0, 0);
+        Point goal = new Point(90, 80);
+        // 设置障碍
+        Point[] obstacles = new Point[]{new Point(40, 30), new Point(70, 50), new Point(30, 70)};
+        RRTStar rrt = new RRTStar();
+        List<Point> path = rrt.plan(start, goal, obstacles);
+        // 输出路径
+        for (Point p : path) {
+            System.out.println(p);
+        }
+    }
+
+    public List<Point> plan(Point start, Point goal, Point[] obstacles) {
+        // 创建节点列表，将起点加入列表
+        nodes = new ArrayList<>();
+        nodes.add(start);
+        // 开始迭代
+        for (int i = 0; i < maxIter; i++) {
+            // 随机生成一个点
+            Point randomPoint = getRandomPoint(goal);
+            // 找到距离该点最近的节点
+            int closestPointIndex = getClosestPointIndex(randomPoint);
+            Point closestPoint = nodes.get(closestPointIndex);
+            // 获取朝该点移动的向量
+            Point newPoint = getNewPoint(closestPoint, randomPoint);
+            // 判断是否有障碍物
+            if(!detectObstacle(closestPoint, newPoint, obstacles)){
+                // 查找距离新点最近的一些点
+                List<Point> nearbyPoints = getNearbyPoints(newPoint);
+                // 找到距离新点最优的父节点
+                int parentIndex = getBestParent(nearbyPoints, newPoint);
+                Point parentNode = nodes.get(parentIndex);
+                // 把新节点加入树中
+                nodes.add(newPoint);
+                // 设置新节点的父节点
+                newPoint.parent = parentNode;
+                newPoint.cost = parentNode.cost + newPoint.distance(parentNode);
+                // 重新排序nearbyPoints
+                nearbyPoints.remove(newPoint.parent);
+                for(Point nearbyPoint : nearbyPoints) {
+                    double distance = nearbyPoint.distance(newPoint);
+                    double cost = newPoint.cost + distance;
+                    if(cost < nearbyPoint.cost && !detectObstacle(nearbyPoint, newPoint, obstacles)){
+                        nearbyPoint.parent = newPoint;
+                        nearbyPoint.cost = cost;
+                    }
+                }
+            }
+            // 判断是否接近终点
+            if (newPoint.distance(goal) < goalTolerance) {
+                goal.parent = newPoint;
+                nodes.add(goal);
+                return getPath(goal);
+            }
+        }
+        // 未找到路径，返回空
+        return null;
+    }
+
+    // 随机生成一个点
+    private Point getRandomPoint(Point goal) {
+        Random random = new Random();
+        if (random.nextDouble() < 0.1) {
+            return goal;
+        } else {
+            Point p = new Point(random.nextDouble()*100, random.nextDouble()*100);
+            return p;
+        }
+    }
+
+    // 获取距离该点最近的节点
+    private int getClosestPointIndex(Point point) {
+        int index = 0;
+        double minDistance = point.distance(nodes.get(0));
+        for (int i = 1; i < nodes.size(); i++) {
+            double distance = point.distance(nodes.get(i));
+            if (distance < minDistance) {
+                minDistance = distance;
+                index = i;
+            }
+        }
+        return index;
+    }
+
+    // 获取朝该点移动的向量
+    private Point getNewPoint(Point start, Point end) {
+        if(start.distance(end) < step){
+            return end;
+        } else {
+            double theta = Math.atan2(end.y - start.y, end.x - start.x);
+            double x = start.x + step * Math.cos(theta);
+            double y = start.y + step * Math.sin(theta);
+            return new Point(x, y);
+        }
+    }
+
+    // 判断是否有障碍物
+    private boolean detectObstacle(Point start, Point end, Point[] obstacles) {
+        double distance = start.distance(end);
+        for(Point obstacle : obstacles){
+            if(obstacle.distance(start) < obstacleRadius || obstacle.distance(end) < obstacleRadius){
+                return true;
+            }
+            Point v1 = new Point(end.x - start.x, end.y - start.y);
+            Point v2 = new Point(obstacle.x - start.x, obstacle.y - start.y);
+            double cosTheta = v1.dotProduct(v2) / (v1.length() * v2.length());
+            double theta = Math.acos(cosTheta);
+            double distToLine = Math.sin(theta) * v2.length();
+            if(distToLine < obstacleRadius && distance > v1.length()){
+                return true;
+            }
+        }
+        return false;
+    }
+
+    // 查找距离新点最近的一些点
+    private List<Point> getNearbyPoints(Point point){
+        List<Point> nearbyPoints = new ArrayList<>();
+        for(Point p : nodes){
+            if(p.distance(point) < expandDistance){
+                nearbyPoints.add(p);
+            }
+        }
+        return nearbyPoints;
+    }
+
+    // 找到距离新点最优的父节点
+    private int getBestParent(List<Point> nearbyPoints, Point point) {
+        int bestParent = nodes.size() - 1;
+        double bestCost = Double.POSITIVE_INFINITY;
+        for (int i = 0; i < nearbyPoints.size(); i++) {
+            Point p = nearbyPoints.get(i);
+            if (!detectObstacle(p, point, new Point[0])) {
+                double cost = p.cost + point.distance(p);
+                if (cost < bestCost) {
+                    bestCost = cost;
+                    bestParent = nodes.indexOf(p);
+                }
+            }
+        }
+        return bestParent;
+    }
+
+    // 获取路径
+    private List<Point> getPath(Point goal) {
+        List<Point> path = new ArrayList<>();
+        path.add(goal);
+        Point p = goal.parent;
+        while (p != null) {
+            path.add(p);
+            p = p.parent;
+        }
+        Collections.reverse(path);
+        return path;
+    }
+}
+
+class Point {
+    public double x;
+    public double y;
+    public double cost;
+    public Point parent;
+
+    public Point(double x, double y) {
+        this.x = x;
+        this.y = y;
+        this.cost = 0;
+        this.parent = null;
+    }
+
+    public double distance(Point other){
+        double dx = this.x - other.x;
+        double dy = this.y - other.y;
+        return Math.sqrt(dx*dx + dy*dy);
+    }
+
+    public double dotProduct(Point other){
+        return this.x * other.x + this.y * other.y;
+    }
+
+    public double length(){
+        return Math.sqrt(this.x * this.x + this.y * this.y);
+    }
+
+    public String toString(){
+        return "(" + x + "," + y + ")";
+    }
+}