Added optimizeTrajectory() method which culls unneccessary waypoints.