New tests.
Fully tested.
Merge pull request #266 from pleroy/TrajectoryTest

Test the new functions in Trajectory