Feature/optimize traj

This shouldn't be merged until the distance manager is merged