Added method to compute the minimum distance(toGoal) point on the obstacel surface