Near optimal path planning for vehicle with heading and curvature constraints