Visibility-based Optimal Path and Motion Planning