I am trying to find a solution in S(1)*R^2 (x,y, orientation) with obstacles (refer to image) using RRT star and Dubins Model.
The code takes a lot of time to find a suitable random sample with x,y, theta such that a successful Dubins path can be connected between the two points without the vehicle (a rectangle colliding any of the obstacles). The fact that the random sample needs to be at the correct angle so that the vehicle's path is collision free is 1 out of 100,000 random samples. This makes the code very slow even when my computer is at its full processing power. None of my internal codes take much time. I timed all of them, only the fact of achieving that 1 out of 100,000 sample causes the code to take so much time. I tried decreasing my discretization space by half but the problem still exists.