added verlet code and fixed up TrajectoryPoint