Bugfixed Ray Casting & finished RRT