Calculate detour waypoints based on robot radius and trigenometry functions
-needs further extensive scenario testing