I have implemented a RRT algorithm in C++ for 2D path planning in a continuous state space. I have also created a visualization of the algorithm using the SFML library.  This allows for an interactive experience in which the user can add obstacles to the state space before starting the algorithm. The algorithm achieves 2D continuous path planning through the following procedure: 

1. Randomly sample any point in the state space

2. Ensure that the sampled point does not intersect with an obstacle (pink). 

3. Find the closest node in the tree to the newly sampled point.  Create a vector projection that points from the closest node to the state space sample.  The magnitude of this vector is normalized to a configurable growth factor.  The growth factor can be decreased to create a smoother path at the cost of requiring more iterations of the algorithm to reach the goal, increasing compute requirements. 

4. Discretize the line from the closest node in the tree to the newly sampled and normalized point in the state space that is slated to be added to the tree. For each discretization, traverse from the node in the tree to the proposed new node and ensure that each point along the line does not intersect with an obstacle. This process is intended to prune nodes that would cause a collision with an obstacle. 

   • The number of discretizations between the node in the tree and the proposed new node is also configurable. Decreasing it will speed up execution at the cost of increasing the chances of colliding with a skinny obstacle as a discretization may skip over it. 

5. If there are no obstacle collisions between the node in the tree and the new node, add it to the tree by setting the parent of the new node to a pointer to the node in the tree. 

Once a node is added to the tree that has a euclidean distance to the goal position that is less than the set goal tolerance, the algorithm has concluded.  At this point, the path from start to goal can be found by traversing from the final node to the start node as each node has a pointer to its parent.