fixed bug in getCurrentWayPoint