public void MoveToDestination(Vector destination, ulong currentTime) { var currentMotion = m_Motion.GetCurrentMotion(currentTime); var initialPosition = currentMotion.GetCurrentPosition(currentTime); var initialVelocity = currentMotion.GetVelocity(currentTime); var newPath = CreatePathTo(destination, initialVelocity, initialPosition, currentTime); if (newPath[0].GetCurrentPosition(currentTime) != initialPosition) { throw new InvalidOperationException("The positions do not match up"); } m_Motion = new CombinedMotion(new List <IMotion>(newPath)); }