Finish C++ Trajectory refactor