/// <summary>
/// Evaluates the trajectory at a given time.
/// </summary>
/// <param name="time">The simulated time</param>
/// <returns>An instance of <c>JointTrajectoryPoint</c> at the given time.</returns>
/// <exception cref="ArgumentOutOfRangeException">Thrown when <paramref name="time"/> is not represented by this trajectory.</exception>
public JointTrajectoryPoint EvaluateAt(TimeSpan time)
{
if (time < this[0].TimeFromStart || time > this[this.Count - 1].TimeFromStart)
{
throw new ArgumentOutOfRangeException("Time is out of bounds.");
}
else
{
int index = GetPointBefore(time);
int k = Math.Min(index + 1, this.Count - 1);
JointTrajectoryPoint p0 = this[index];
JointTrajectoryPoint p1 = this[k];
JointTrajectoryPoint q = p0.InterpolateCubic(p1, time);
return(q.WithTimeFromStart(time));
}
}
public JointTrajectoryPoint InterpolateCubic(JointTrajectoryPoint point1, TimeSpan time) => JointTrajectoryPoint.InterpolateCubic(this, point1, time);
