fix: Reimplement FollowTrajectoryWithSpeedControlUnit

• fixed bug when the entity is updated for the first time but the current time is not equal to the time of the first vertex
• there were multiple cases where the entity didn't follwo the trajectory with the correct speed and finished early
• reimplemented control unit and removed traverser. instead the position is now calculated directly from the timing in the points.