├── AnytimeRRT.gif ├── BasicRRT.gif ├── README.md ├── RRTstar.gif ├── geometry.h └── rrt.cpp /AnytimeRRT.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nikhilchandak/Rapidly-Exploring-Random-Trees/c8291ad33a5ed7bb2134630b213a935de4e82748/AnytimeRRT.gif -------------------------------------------------------------------------------- /BasicRRT.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nikhilchandak/Rapidly-Exploring-Random-Trees/c8291ad33a5ed7bb2134630b213a935de4e82748/BasicRRT.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Rapidly-Exploring-Random-Trees 2 | My C++ implementations of RRT, RRT* and Anytime RRT (path planning algorithm used in robotics). 3 | 4 | ## Visualization 5 | 6 | In the following maps, pink dot represents the starting point while blue dot is the destination. Few polygonal obstacles (filled rectangles) are also present. 7 | 8 | ### RRT 9 | 10 | RRT is based on incremental construction of search trees which rapidly and uniformly explore the state space. The path found may not be optimal but the algorithm is probablistically complete. 11 | 12 | ![Basic RRT](BasicRRT.gif) 13 | 14 | ### RRT* 15 | 16 | RRT* is an improvement over RRT. It is asymptotically optimal and probalisticaly complete. It continuously refines the paths formed to reach optimal path configuration. Due to the constant improvement, it is slower than RRT. 17 | 18 | ![RRT*](RRTstar.gif) 19 | 20 | ### *Anytime* RRT 21 | 22 | My implementation of Anytime RRT is a combination of RRT and RRT*. First, RRT is run to find any feasible path from start to destination. Once a path is found, RRT* is run to refine the alreafy explored search space and future paths. Thus, this hybrid prioritizes finding any path at first, later optimizing its length. 23 | 24 | ![Anytime RRT](AnytimeRRT.gif) 25 | 26 | ## Requirements 27 | 28 | C++ 11+ and SFML (Simple and Fast Multimedia Library) is required to run the codes. 29 | 30 | ## Installation 31 | 32 | Run the following command to install SFML on Linux (For other OS, please check the guide(s) available online) 33 | 34 | ``` 35 | $ sudo apt-get install libsfml-dev 36 | ``` 37 | 38 | ## How to Run 39 | 40 | ``` 41 | $ g++ -std=c++11 -c geometry.h rrt.cpp 42 | $ g++ rrt.o -o sfml-app -lsfml-graphics -lsfml-window -lsfml-system 43 | $ ./sfml-app 44 | ``` 45 | 46 | ## Usage 47 | 48 | Example of interaction with the program: 49 | 50 | ``` 51 | NOTE: 52 | Height of screen: 600 pixels. Width of screeen: 800 pixels. 53 | Maximum distance by which algorithm jumps from one point to another: 32 units 54 | If you would like to change of any of these, please make modifications in code 55 | Please provide your inputs keeping this in mind. 56 | 57 | Which type of RRT would you like to watch? 1 for RRT, 2 for RRT*, 3 for Anytime RRT 58 | ``` 59 | ``` 60 | 3 61 | ``` 62 | ``` 63 | Input co-ordinates of starting and ending point respectively in this format X1 Y1 X2 Y2 64 | ``` 65 | ``` 66 | 100 70 67 | 600 400 68 | ``` 69 | ``` 70 | How many obstacles? 71 | ``` 72 | ``` 73 | 2 74 | ``` 75 | ``` 76 | How many points in 1th polygon? 77 | ``` 78 | ``` 79 | 4 80 | ``` 81 | ``` 82 | Input co-ordinates of 1th polygon in clockwise order 83 | ``` 84 | ``` 85 | 200 480 86 | 200 100 87 | 250 100 88 | 250 480 89 | ``` 90 | ``` 91 | How many points in 2th polygon? 92 | ``` 93 | ``` 94 | 5 95 | ``` 96 | ``` 97 | Input co-ordinates of 2th polygon in clockwise order 98 | ``` 99 | ``` 100 | 400 0 101 | 300 100 102 | 350 250 103 | 450 250 104 | 500 100 105 | ``` 106 | ## Config 107 | 108 | Configuration of the screen size, jump size of algorithm, radius of circle of starting/destination points, goal sampling probability can be changed in the rrt.cpp file in the section shown below: 109 | 110 | ``` 111 | const int WIDTH = 800 ; 112 | const int HEIGHT = 600 ; 113 | const int RADIUS = 5 ; 114 | const double GOAL_SAMPLING_PROB = 0.05; 115 | const double INF = 1e18; 116 | 117 | const double JUMP_SIZE = (WIDTH/100.0 * HEIGHT/100.0)/1.5; 118 | const double DISK_SIZE = JUMP_SIZE ; // Ball radius around which nearby points are found 119 | ``` 120 | -------------------------------------------------------------------------------- /RRTstar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nikhilchandak/Rapidly-Exploring-Random-Trees/c8291ad33a5ed7bb2134630b213a935de4e82748/RRTstar.gif -------------------------------------------------------------------------------- /geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_H 2 | #define GEOMETRY_H 3 | // ^To make sure I don't declare any function more than once by including the header multiple times. 4 | 5 | #include 6 | #include 7 | using namespace std ; 8 | 9 | // Type of data type to be used for all calculations (Ex: long double) 10 | #define ftype double 11 | 12 | /* NOTE: Most of the calculations are done using EPS as a factor of difference 13 | since double/long double doesn't store floating point values precisely (limited precision) */ 14 | const ftype EPS = 1e-6; 15 | 16 | struct Point { 17 | ftype x, y; 18 | Point() {} 19 | Point(ftype x, ftype y): x(x), y(y) {} 20 | Point& operator+=(const Point &t) { 21 | x += t.x; 22 | y += t.y; 23 | return *this; 24 | } 25 | Point& operator-=(const Point &t) { 26 | x -= t.x; 27 | y -= t.y; 28 | return *this; 29 | } 30 | Point& operator*=(ftype t) { 31 | x *= t; 32 | y *= t; 33 | return *this; 34 | } 35 | Point& operator/=(ftype t) { 36 | x /= t; 37 | y /= t; 38 | return *this; 39 | } 40 | Point operator+(const Point &t) const { 41 | return Point(*this) += t; 42 | } 43 | Point operator-(const Point &t) const { 44 | return Point(*this) -= t; 45 | } 46 | Point operator*(ftype t) const { 47 | return Point(*this) *= t; 48 | } 49 | Point operator/(ftype t) const { 50 | return Point(*this) /= t; 51 | } 52 | ftype dot(const Point &t) const { 53 | return (x*t.x + y*t.y); 54 | } 55 | ftype cross(const Point& t) const { 56 | return x * t.y - y * t.x; 57 | } 58 | ftype cross(const Point& a, const Point& b) const { 59 | return (a - *this).cross(b - *this); 60 | } 61 | ftype distance(const Point &t) const { 62 | const double x_diff = x - t.x, y_diff = y - t.y ; 63 | return sqrt(x_diff * x_diff + y_diff * y_diff); 64 | } 65 | Point steer(const Point& t, ftype DELTA) { 66 | if(this->distance(t) < DELTA) { 67 | return t ; 68 | } 69 | else { 70 | double theta = atan2(t.y - y, t.x - x); 71 | return Point(x + DELTA * cos(theta), y + DELTA * sin(theta)); 72 | } 73 | } 74 | bool operator==(const Point& rhs) const 75 | { 76 | return fabs(x - rhs.x) < EPS and fabs(y - rhs.y) < EPS ; // or another approach as above 77 | } 78 | }; 79 | 80 | Point operator*(ftype a, Point b) { 81 | return b * a; 82 | } 83 | 84 | ftype distance(Point& a, Point &b) { 85 | const ftype x_diff = a.x - b.x, y_diff = a.y - b.y ; 86 | return sqrt(x_diff * x_diff + y_diff * y_diff); 87 | } 88 | 89 | ftype dot(Point a, Point b) { 90 | return (a.x*b.x + a.y*b.y); 91 | } 92 | 93 | ftype cross(Point a, Point b) { 94 | return (a.x*b.y - b.x*a.y); 95 | } 96 | 97 | /* Returns a point in the direction of (p2 - p1) vector such that 98 | the new point is within a DELTA distance of point1 */ 99 | Point stepNear(Point& p1, Point& p2, ftype DELTA) { 100 | if( (distance(p1, p2) - DELTA) <= EPS) 101 | return p2 ; 102 | else { 103 | ftype theta = atan2(p2.y - p1.y, p2.x - p1.x); 104 | return Point(p1.x + DELTA * cos(theta), p1.y + DELTA * sin(theta)); 105 | } 106 | } 107 | 108 | // Return minimum distance between line segment vw and point p 109 | ftype minimum_distance(Point v, Point w, Point p) { 110 | ftype l2 = distance(v, w); l2 *= l2 ; // i.e. |w-v|^2 - avoid a sqrt 111 | if (l2 < EPS) return distance(p, v); // v == w case 112 | 113 | // Consider the line extending the segment, parameterized as v + t (w - v). 114 | // We find projection of point p onto the line. 115 | // It falls where t = [(p-v) . (w-v)] / |w-v|^2 116 | // We clamp t from [0,1] to handle points outside the segment vw. 117 | const ftype t = max(0.0, min(1.0, dot(p - v, w - v) / l2)); 118 | 119 | Point projection = v + t * (w - v); // Projection falls on the segment 120 | return distance(p, projection); 121 | } 122 | 123 | struct Polygon { 124 | vector < Point > points ; // Assumes clockwise/anti-clockwise points input 125 | int pointCnt = 0 ; 126 | 127 | void addPoint(const Point pnt) { 128 | points.push_back(pnt); pointCnt++ ; 129 | } 130 | vector < Point > getPoints() { 131 | return points ; 132 | } 133 | bool pointInside(const Point point) { // Can be done in log(N) 134 | int i, j, nvert = points.size(); 135 | bool c = false; 136 | for(i = 0, j = nvert - 1; i < nvert; j = i++) { 137 | if( ( (points[i].y >= point.y ) != (points[j].y >= point.y) ) && 138 | (point.x <= (points[j].x - points[i].x) * (point.y - points[i].y) / (points[j].y - points[i].y) + points[i].x) 139 | ) 140 | c = !c; 141 | } 142 | return c; 143 | } 144 | }; 145 | 146 | /* Return true if the given line segment intersects the circle whose center 147 | is at location */ 148 | bool checkCollision(Point lineFrom, Point lineTo, Point location, ftype radius) 149 | { 150 | location += Point(radius, radius); // Adjust location from top-left corner to center coordinates 151 | ftype ab2, acab, h2; 152 | Point ac = location - lineFrom; 153 | Point ab = lineTo - lineFrom; 154 | ab2 = dot(ab, ab); acab = dot(ac, ab); 155 | ftype t = acab / ab2; 156 | 157 | if (t < 0) t = 0; 158 | else if (t > 1) t = 1; 159 | 160 | Point h = ((ab * t) + lineFrom) - location; 161 | h2 = dot(h, h); 162 | return (h2 <= (radius * radius)); 163 | } 164 | 165 | // taken from stackoverflow: https://stackoverflow.com/questions/11716268/point-in-polygon-algorithm 166 | // this can be done in log(N) though 167 | bool PointInPolygon(Point point, Polygon polygon) { 168 | vector points = polygon.getPoints(); 169 | int i, j, nvert = points.size(); 170 | bool c = false; 171 | 172 | for(i = 0, j = nvert - 1; i < nvert; j = i++) { 173 | if( ( (points[i].y >= point.y ) != (points[j].y >= point.y) ) && 174 | (point.x <= (points[j].x - points[i].x) * (point.y - points[i].y) / (points[j].y - points[i].y) + points[i].x) 175 | ) 176 | c = !c; 177 | } 178 | return c; 179 | } 180 | 181 | // helper function 182 | int sign(const ftype x) { 183 | return x >= 0 ? x ? 1 : 0 : -1; 184 | } 185 | 186 | /* Returns true if two line segments on the same line intersect. 187 | (a, b) denote the endpoints of first line segment and 188 | (c, d) denotes the endpoints of the second lint segment */ 189 | bool intersectOnLine(ftype a, ftype b, ftype c, ftype d) { 190 | if ((a - b) > EPS) swap(a, b); 191 | if ((c - d) > EPS) swap(c, d); 192 | return max(a, c) <= min(b, d); 193 | } 194 | 195 | // Returns true if the two line segments (a, b) and (c, d) intersect 196 | bool check_intersection(const Point a, const Point b, const Point c, const Point d) { 197 | // Check if both line segments lie on the same line 198 | if (c.cross(a, d) == 0 && c.cross(b, d) == 0) 199 | return intersectOnLine(a.x, b.x, c.x, d.x) && intersectOnLine(a.y, b.y, c.y, d.y); 200 | 201 | // Check if a and b both lie on different side of line segment CD 202 | // Similarly check for c and d to lie on different side of line segment AC 203 | return sign(a.cross(b, c)) != sign(a.cross(b, d)) && sign(c.cross(d, a)) != sign(c.cross(d, b)); 204 | } 205 | 206 | /* Returns true if the given line segment represented by ba intersects with any 207 | side of the polygon */ 208 | bool lineSegmentIntersectsPolygon(Point a, Point b, Polygon& polygon) { 209 | // PQ is merely a point not enough distance for it be line segment 210 | if( a.distance(b) < EPS ) 211 | return PointInPolygon( (a+b)/2.0, polygon); 212 | 213 | int num = polygon.pointCnt ; 214 | vector < Point > points = polygon.getPoints(); 215 | for(int i = 0; i < num; i++) { 216 | int nxt = i+1; if(nxt == num) nxt = 0 ; 217 | if(check_intersection(a, b, points[i], points[nxt])) return true ; 218 | } 219 | return false ; 220 | } 221 | 222 | #endif -------------------------------------------------------------------------------- /rrt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "geometry.h" 6 | 7 | using namespace std ; 8 | 9 | const int WIDTH = 800 ; 10 | const int HEIGHT = 600 ; 11 | const int RADIUS = 5 ; 12 | const double GOAL_SAMPLING_PROB = 0.05; 13 | const double INF = 1e18; 14 | 15 | const double JUMP_SIZE = (WIDTH/100.0 * HEIGHT/100.0)/1.5; 16 | const double DISK_SIZE = JUMP_SIZE ; // Ball radius around which nearby points are found 17 | 18 | int whichRRT = 3 ; 19 | 20 | vector < Polygon > obstacles ; 21 | Point start, stop ; 22 | int obstacle_cnt = 1 ; 23 | 24 | vector < Point > nodes ; 25 | vector < int > parent, nearby ; 26 | vector < double > cost, jumps ; 27 | int nodeCnt = 0, goalIndex = -1 ; 28 | 29 | vector polygons ; 30 | sf::CircleShape startingPoint, endingPoint ; 31 | bool pathFound = 0 ; 32 | 33 | void getInput() { 34 | cout << "NOTE:" << endl ; 35 | cout << "Height of screen: " << HEIGHT << " pixels." ; 36 | cout << " Width of screeen: " << WIDTH << " pixels." << endl ; 37 | cout << "Maximum distance by which algorithm jumps from one point to another: " << JUMP_SIZE << " units" << endl ; 38 | cout << "If you would like to change of any of these, please make modifications in code" << endl ; 39 | cout << "Please provide your inputs keeping this in mind. " << endl << endl ; 40 | 41 | cout << "Which type of RRT would you like to watch? 1 for RRT, 2 for RRT*, 3 for Anytime RRT" << endl ; 42 | cin >> whichRRT ; 43 | cout << "Input co-ordinates of starting and ending point respectively in this format X1 Y1 X2 Y2" << endl ; 44 | cin >> start.x >> start.y >> stop.x >> stop.y ; 45 | cout << "How many obstacles?" << endl ; 46 | cin >> obstacle_cnt ; 47 | 48 | obstacles.resize(obstacle_cnt); 49 | int pnts = 0 ; Point pnt ; 50 | vector < Point > poly ; 51 | 52 | for(int i = 0; i < obstacle_cnt; i++) { 53 | poly.clear(); 54 | cout << "How many points in " << i+1 << "th polygon?" << endl ; 55 | cin >> pnts ; 56 | poly.resize(pnts); 57 | 58 | cout << "Input co-ordinates of " << i+1 << "th polygon in clockwise order" << endl ; 59 | for(int j = 0; j < pnts; j++) { 60 | cin >> pnt.x >> pnt.y ; 61 | obstacles[i].addPoint(pnt); 62 | } 63 | } 64 | } 65 | 66 | // Prepares SFML objects of starting, ending point and obstacles 67 | void prepareInput() { 68 | // Make starting and ending point circles ready 69 | startingPoint.setRadius(RADIUS); endingPoint.setRadius(RADIUS); 70 | startingPoint.setFillColor(sf::Color(208, 0, 240)); endingPoint.setFillColor(sf::Color::Blue); 71 | startingPoint.setPosition(start.x, start.y); endingPoint.setPosition(stop.x, stop.y); 72 | startingPoint.setOrigin(RADIUS/2, RADIUS/2); endingPoint.setOrigin(RADIUS/2, RADIUS/2); 73 | 74 | // Prepare polygon of obstacles 75 | polygons.resize(obstacle_cnt); 76 | for(int i = 0; i < obstacle_cnt; i++) { 77 | polygons[i].setPointCount(obstacles[i].pointCnt); 78 | polygons[i].setFillColor(sf::Color(89, 87, 98)); 79 | for(int j = 0; j < obstacles[i].pointCnt; j++) 80 | polygons[i].setPoint(j, sf::Vector2f(obstacles[i].points[j].x, obstacles[i].points[j].y)); 81 | } 82 | } 83 | 84 | void draw(sf::RenderWindow& window) { 85 | sf::Vertex line[2]; sf::CircleShape nodeCircle; 86 | 87 | // Uncomment if circular nodes are to be drawn 88 | /* 89 | for(auto& node: nodes) { 90 | nodeCircle.setRadius(RADIUS/2.5); // nodeCircle.setRadius(min(2.0, RADIUS/2.0)); 91 | nodeCircle.setOrigin(RADIUS/2.5, RADIUS/2.5); 92 | nodeCircle.setFillColor(sf::Color(0, 255, 171)); nodeCircle.setPosition(node.x, node.y); 93 | window.draw(nodeCircle); 94 | } 95 | */ 96 | 97 | // Draw obstacles 98 | for(auto& poly : polygons) window.draw(poly); 99 | 100 | // Draw edges between nodes 101 | for(int i = (int)nodes.size() - 1; i; i--) { 102 | Point par = nodes[parent[i]] ; 103 | line[0] = sf::Vertex(sf::Vector2f(par.x, par.y)); 104 | line[1] = sf::Vertex(sf::Vector2f(nodes[i].x, nodes[i].y)); 105 | window.draw(line, 2, sf::Lines); 106 | } 107 | 108 | window.draw(startingPoint); window.draw(endingPoint); 109 | 110 | // If destination is reached then path is retraced and drawn 111 | if(pathFound) { 112 | int node = goalIndex; 113 | while(parent[node] != node) { 114 | int par = parent[node]; 115 | line[0] = sf::Vertex(sf::Vector2f(nodes[par].x, nodes[par].y)); 116 | line[1] = sf::Vertex(sf::Vector2f(nodes[node].x, nodes[node].y)); 117 | line[0].color = line[1].color = sf::Color::Red; // orange color 118 | window.draw(line, 2, sf::Lines); 119 | node = par ; 120 | } 121 | } 122 | } 123 | 124 | template // Returns a random number in [low, high] 125 | T randomCoordinate(T low, T high){ 126 | random_device random_device; 127 | mt19937 engine{random_device()}; 128 | uniform_real_distribution dist(low, high); 129 | return dist(engine); 130 | } 131 | 132 | // Returns true if the line segment ab is obstacle free 133 | bool isEdgeObstacleFree(Point a, Point b) { 134 | for(auto& poly: obstacles) 135 | if(lineSegmentIntersectsPolygon(a, b, poly)) 136 | return false ; 137 | return true ; 138 | } 139 | 140 | // Returns a random point with some bias towards goal 141 | Point pickRandomPoint() { 142 | double random_sample = randomCoordinate(0.0, 1.0); 143 | if((random_sample - GOAL_SAMPLING_PROB) <= EPS and !pathFound) return stop + Point(RADIUS, RADIUS) ; 144 | return Point(randomCoordinate(0, WIDTH), randomCoordinate(0, HEIGHT)); 145 | } 146 | 147 | void checkDestinationReached() { 148 | sf::Vector2f position = endingPoint.getPosition(); 149 | if(checkCollision(nodes[parent[nodeCnt - 1]], nodes.back(), Point(position.x, position.y), RADIUS)) { 150 | pathFound = 1 ; 151 | goalIndex = nodeCnt - 1; 152 | cout << "Reached!! With a distance of " << cost.back() << " units. " << endl << endl ; 153 | } 154 | } 155 | 156 | /* Inserts nodes on the path from rootIndex till Point q such 157 | that successive nodes on the path are not more than 158 | JUMP_SIZE distance away */ 159 | void insertNodesInPath(int rootIndex, Point& q) { 160 | Point p = nodes[rootIndex] ; 161 | if(!isEdgeObstacleFree(p, q)) return ; 162 | while(!(p == q)) { 163 | Point nxt = p.steer(q, JUMP_SIZE); 164 | nodes.push_back(nxt); 165 | parent.push_back(rootIndex); 166 | cost.push_back(cost[rootIndex] + distance(p, nxt)); 167 | rootIndex = nodeCnt++ ; 168 | p = nxt ; 169 | } 170 | } 171 | 172 | /* Rewires the parents of the tree greedily starting from 173 | the new node found in this iterationsation as the parent */ 174 | void rewire() { 175 | int lastInserted = nodeCnt - 1 ; 176 | for(auto nodeIndex: nearby) { 177 | int par = lastInserted, cur = nodeIndex; 178 | 179 | // Rewire parents as much as possible (greedily) 180 | while( ((cost[par] + distance(nodes[par], nodes[cur])) - cost[cur]) <= EPS) { 181 | int oldParent = parent[cur] ; 182 | parent[cur] = par; cost[cur] = cost[par] + distance(nodes[par], nodes[cur]); 183 | par = cur, cur = oldParent; 184 | } 185 | } 186 | } 187 | 188 | /* Runs one iteration of RRT depending on user choice 189 | At least one new node is added on the screen each iteration. */ 190 | void RRT() { 191 | Point newPoint, nearestPoint, nextPoint ; bool updated = false ; int cnt = 0 ; 192 | int nearestIndex = 0 ; double minCost = INF; nearby.clear(); jumps.resize(nodeCnt); 193 | 194 | while(!updated) { 195 | newPoint = pickRandomPoint(); 196 | 197 | // Find nearest point to the newPoint such that the next node 198 | // be added in graph in the (nearestPoint, newPoint) while being obstacle free 199 | nearestPoint = *nodes.begin(); nearestIndex = 0; 200 | for(int i = 0; i < nodeCnt; i++) { 201 | if(pathFound and randomCoordinate(0.0, 1.0) < 0.25) // Recalculate cost once in a while 202 | cost[i] = cost[parent[i]] + distance(nodes[parent[i]], nodes[i]); 203 | 204 | // Make smaller jumps sometimes to facilitate passing through narrow passages 205 | jumps[i] = randomCoordinate(0.3, 1.0) * JUMP_SIZE ; 206 | auto pnt = nodes[i] ; 207 | if((pnt.distance(newPoint) - nearestPoint.distance(newPoint)) <= EPS and isEdgeObstacleFree(pnt, pnt.steer(newPoint, jumps[i]))) 208 | nearestPoint = pnt, nearestIndex = i ; 209 | } 210 | nextPoint = stepNear(nearestPoint, newPoint, jumps[nearestIndex]); 211 | if(!isEdgeObstacleFree(nearestPoint, nextPoint)) continue ; 212 | 213 | if( (whichRRT == 1) or (!pathFound and whichRRT == 3)) { 214 | // This is where we don't do any RRT* optimization part 215 | updated = true ; 216 | nodes.push_back(nextPoint); nodeCnt++; 217 | parent.push_back(nearestIndex); 218 | cost.push_back(cost[nearestIndex] + distance(nearestPoint, nextPoint)); 219 | if(!pathFound) checkDestinationReached(); 220 | continue ; 221 | } 222 | 223 | // Find nearby nodes to the new node as center in ball of radius DISK_SIZE 224 | for(int i = 0; i < nodeCnt; i++) 225 | if((nodes[i].distance(nextPoint) - DISK_SIZE) <= EPS and isEdgeObstacleFree(nodes[i], nextPoint)) 226 | nearby.push_back(i); 227 | 228 | // Find minimum cost path to the new node 229 | int par = nearestIndex; minCost = cost[par] + distance(nodes[par], nextPoint); 230 | for(auto nodeIndex: nearby) { 231 | if( ( (cost[nodeIndex] + distance(nodes[nodeIndex], nextPoint)) - minCost) <= EPS) 232 | minCost = cost[nodeIndex] + distance(nodes[nodeIndex], nextPoint), par = nodeIndex; 233 | } 234 | 235 | parent.push_back(par); cost.push_back(minCost); 236 | nodes.push_back(nextPoint); nodeCnt++; 237 | updated = true ; 238 | if(!pathFound) checkDestinationReached(); 239 | rewire(); 240 | } 241 | } 242 | 243 | int main() { 244 | getInput(); prepareInput(); 245 | sf::RenderWindow window(sf::VideoMode(WIDTH, HEIGHT), "Basic Anytime RRT"); 246 | 247 | nodeCnt = 1; nodes.push_back(start); int iterations = 0 ; 248 | parent.push_back(0); cost.push_back(0); 249 | sf::Time delayTime = sf::milliseconds(5); 250 | 251 | cout << endl << "Starting node is in Pink and Destination node is in Blue" << endl << endl ; 252 | while (window.isOpen()) 253 | { 254 | sf::Event event; 255 | while (window.pollEvent(event)) 256 | { 257 | if (event.type == sf::Event::Closed) 258 | { 259 | window.close(); 260 | return 0; exit(0); 261 | } 262 | } 263 | RRT(); iterations++; 264 | 265 | if(iterations % 500 == 0) { 266 | cout << "Iterations: " << iterations << endl ; 267 | if(!pathFound) cout << "Not reached yet :( " << endl ; 268 | else cout << "Shortest distance till now: " << cost[goalIndex] << " units." << endl ; 269 | cout << endl ; 270 | } 271 | 272 | //sf::sleep(delayTime); 273 | window.clear(); 274 | draw(window); 275 | window.display(); 276 | } 277 | } 278 | 279 | /* SOME SAMPLE INPUTS ARE SHOWN BELOW (only cin part) without any RRT preference */ 280 | 281 | /* 282 | 100 70 283 | 600 400 284 | 2 285 | 4 286 | 200 480 287 | 200 100 288 | 250 100 289 | 250 480 290 | 5 291 | 400 0 292 | 300 100 293 | 350 250 294 | 450 250 295 | 500 100 296 | */ 297 | 298 | /* 299 | 50 50 300 | 750 180 301 | 3 302 | 4 303 | 100 0 304 | 200 0 305 | 200 400 306 | 100 400 307 | 4 308 | 250 200 309 | 350 200 310 | 350 600 311 | 250 600 312 | 4 313 | 400 50 314 | 550 50 315 | 550 250 316 | 400 250 317 | */ 318 | 319 | /* 320 | 50 50 321 | 750 580 322 | 3 323 | 4 324 | 100 0 325 | 200 0 326 | 200 400 327 | 100 400 328 | 4 329 | 250 200 330 | 350 200 331 | 350 600 332 | 250 600 333 | 4 334 | 400 32 335 | 700 32 336 | 700 575 337 | 400 575 338 | */ 339 | 340 | /* 341 | 190 30 342 | 660 500 343 | 5 344 | 3 345 | 200 50 346 | 200 200 347 | 350 200 348 | 3 349 | 220 50 350 | 370 50 351 | 370 200 352 | 3 353 | 400 250 354 | 400 450 355 | 600 450 356 | 3 357 | 430 250 358 | 630 250 359 | 630 450 360 | 3 361 | 640 470 362 | 640 550 363 | 680 550 364 | */ 365 | 366 | /* 367 | 190 55 368 | 660 500 369 | 9 370 | 4 371 | 740 360 372 | 750 350 373 | 680 540 374 | 670 530 375 | 3 376 | 710 290 377 | 780 350 378 | 630 380 379 | 3 380 | 520 450 381 | 620 540 382 | 349 580 383 | 4 384 | 450 70 385 | 700 70 386 | 700 240 387 | 450 240 388 | 3 389 | 200 50 390 | 200 200 391 | 350 200 392 | 3 393 | 220 50 394 | 370 50 395 | 370 200 396 | 3 397 | 400 250 398 | 400 450 399 | 600 450 400 | 3 401 | 430 250 402 | 630 250 403 | 630 450 404 | 3 405 | 640 470 406 | 640 550 407 | 680 550 408 | */ --------------------------------------------------------------------------------