Created the p2p trajectory, and added some iterators to the other trajectories. Now thinking of using a virtual protected interface in waypoint-container-base class.