InterpolatedPath* RRTPlanner::runRRT(MotionInstant start, MotionInstant goal, const MotionConstraints& motionConstraints, const Geometry2d::ShapeSet* obstacles) { InterpolatedPath* path = new InterpolatedPath(); path->setStartTime(RJ::timestamp()); // Initialize two RRT trees FixedStepTree startTree; FixedStepTree goalTree; startTree.init(start.pos, obstacles); goalTree.init(goal.pos, obstacles); startTree.step = goalTree.step = .15f; // Run bi-directional RRT algorithm Tree* ta = &startTree; Tree* tb = &goalTree; for (unsigned int i = 0; i < _maxIterations; ++i) { Geometry2d::Point r = RandomFieldLocation(); Tree::Point* newPoint = ta->extend(r); if (newPoint) { // try to connect the other tree to this point if (tb->connect(newPoint->pos)) { // trees connected // done with global path finding // the path is from start to goal // runRRT will handle the rest break; } } swap(ta, tb); } Tree::Point* p0 = startTree.last(); Tree::Point* p1 = goalTree.last(); // sanity check if (!p0 || !p1 || p0->pos != p1->pos) { return path; } // extract path from RRTs // add the start tree first...normal order (aka from root to p0) startTree.addPath(*path, p0); // add the goal tree in reverse (aka p1 to root) goalTree.addPath(*path, p1, true); path = optimize(*path, obstacles, motionConstraints, start.vel, goal.vel); return path; }