Optimal Path Planning Based on Visibility