Waypoints and playcontroller

fixed bug in playcontroller. However, it is not tested thoroughly yet.
Working on waypoints.