FIxed bugs
getCurrentStepNo does not search in the complete trajectory.
Fixed bug
When inserting a trajectory immediately, the trajectory start some points in the past.