Fix bug in trajectory discretization function.