Implementation of RTT Algorithm for robot motion plannig with MATLAB and CoppeliaSim.

I defined a "goal area" to simulate the set goal, in order to get faster results.
I bias the sampler towards the goal, if the sampled point is within the tolerance (0.01 - 0.02), then it is consider as the goal.
This way the algorithm is much faster and finds the goal in almost all cases. The algorithm can be found in the Modern Robotics book.

Other functions that I use:

Closest Node: outputs Xnearest, calculates EuclideanD of Xsamp and all the nodes.
LocalPlanner: outputs Xnew and if there is any collisions. Checks if the new segment line collides with any circle.
InterX: function available in Matlab to find intersections of two curves ("polyxpoly" also available).
MakePath: retrieves the path from the goal to the start of the graph.
Sampler: random uniform distribution number between -0.5 and 0.5. If the number is within the "goal area" then Xsamp = Xgoal.
RandCost/Hcost: random uniform distribution number within certain limits.
EuclideanD: Euclidean distance of two points in space.

CoppeliaSim example:

MATLAB example 1:

MATLAB example 2: