/// <summary>
/// Returns frames according to the specified framerate
/// </summary>
/// <param name="fps"></param>
/// <returns></returns>
public List <TimedPose2D> SampleFrames(int fps)
{
List <TimedPose2D> poses = new List <TimedPose2D>();
float frames = ((float)Duration.TotalSeconds * fps);
float duration = (float)Duration.TotalSeconds;
float delta = duration / frames;
float value = 0;
for (int f = 0; f < frames; f++)
{
TimedPose2D pose = this.GetPose(TimeSpan.FromSeconds(value));
poses.Add(pose);
value += delta;
}
return(poses);
}
/// <summary>
/// Estimates the next waypoint of a given path.
/// Returns the first point which is farer away than the specified distance
/// </summary>
/// <param name="globalPath"></param>
/// <param name="currentPosition"></param>
/// <param name="horizon">The time horizon for planning</param>
/// <returns></returns>
private Vector2 GetNextWaypoint(MotionTrajectory2D globalPath, Vector2 currentPosition, TimeSpan horizon)
{
Vector2 waypoint = currentPosition;
TimeSpan time = this.EstimateCurrentTime(currentPosition);
//Determine the next pose
TimedPose2D pose = this.trajectory.Poses.Last();
//Only interpolate if time is not exceeded
if (time + horizon < pose.Time)
{
pose = this.trajectory.GetPose(time + horizon);
}
//Set the waypoint
waypoint = new Vector2(pose.Position.x, pose.Position.y);
return(waypoint);
}
/// <summary>
/// Estimates the current time of the current position within the spline
/// </summary>
/// <returns></returns>
private TimeSpan EstimateCurrentTime(Vector2 position)
{
TimeSpan time = TimeSpan.Zero;
float minDist = float.MaxValue;
int bestIndex = this.currentIndex;
for (int i = this.currentIndex; i < this.discretePoses.Count; i++)
{
TimedPose2D frame = this.discretePoses[i];
float dist = (frame.Position - position).magnitude;
if (dist < minDist)
{
time = frame.Time;
minDist = dist;
bestIndex = i;
}
}
this.currentIndex = bestIndex;
return(time);
}
/// <summary>
/// Returns the pose at the given time.
/// New implementation utilizes binary search for improved performance.
/// </summary>
/// <param name="poses">The input list of poses</param>
/// <param name="time">The desired timestamp</param>
/// <param name="interpolate">Specifies whether interpolation is used to generate a suitable pose at the timestamp </param>
/// <returns></returns>
protected TimedPose2D ComputePose(List <TimedPose2D> poses, System.TimeSpan time, InterpolationMode mode = InterpolationMode.Linear, bool extrapolate = true)
{
//The first index which is smaller than the actual time
int from = 0;
int to = 0;
//Find the closest indices using binary search
GetClosestIndices(poses, time, out from, out to);
//First determine the weight
float weight = 0;
//Check if extrapoltion in positive direction is required
if (poses[to].Time < time)
{
if (extrapolate)
{
float deltaT = (float)(poses[to].Time - poses[from].Time).TotalSeconds;
//Use next point if both points are identical
if (deltaT == 0 && from > 0)
{
deltaT = (float)(poses[to].Time - poses[from - 1].Time).TotalSeconds;
}
float newDeltaT = (float)(time - poses[from].Time).TotalSeconds;
//Compute the weight
weight = newDeltaT / deltaT;
}
else
{
weight = 1f;
}
}
//Check if extrapolation in negative direction is required
else if (poses[from].Time > time)
{
if (extrapolate)
{
float deltaT = (float)(poses[from].Time - poses[to].Time).TotalSeconds;
//Use next point if both points are identical
if (deltaT == 0 && to < poses.Count - 1)
{
deltaT = (float)(poses[from].Time - poses[to + 1].Time).TotalSeconds;
}
float newDeltaT = (float)(poses[from].Time - time).TotalSeconds;
//Compute the weight
weight = newDeltaT / deltaT;
}
else
{
weight = 0f;
}
}
//Default behavior just interpolate between the two poses
else
{
//Compute the weight
float weightTo = (float)(time - poses[from].Time).Duration().TotalSeconds;
float weightFrom = (float)(poses[to].Time - time).Duration().TotalSeconds;
//Normalize the weight
float sum = weightFrom + weightTo;
//Check if sum is null
if (sum == 0)
{
weight = 0;
}
else
{
weight = weightTo / sum;
}
}
//Check the speicifc interpolation mode
switch (mode)
{
//Perform a linear interpolation
case InterpolationMode.Linear:
return(TimedPose2D.Interpolate(poses[from], poses[to], weight));
//Perform a cutmull rom interpolation
case InterpolationMode.CatmullRom:
//Only valid if 4 points are available
if (from > 0 && to < poses.Count - 1)
{
TimedPose2D result = new TimedPose2D();
result.Position = CatmullRom.ComputePoint(poses[from - 1].Position, poses[from].Position, poses[to].Position, poses[to + 1].Position, weight);
result.Time = System.TimeSpan.FromSeconds(CatmullRom.ComputePoint((float)poses[from - 1].Time.TotalSeconds, (float)poses[from].Time.TotalSeconds, (float)poses[to].Time.TotalSeconds, (float)poses[to + 1].Time.TotalSeconds, weight));
return(result);
}
//Otherwise interpolate linear
else
{
return(TimedPose2D.Interpolate(poses[from], poses[to], weight));
}
//Get the closest point
case InterpolationMode.None:
return((poses[from].Time - time).Duration() < (poses[to].Time - time).Duration() ? poses[from] : poses[to]);
}
return(poses.Last());
}
/// <summary>
/// Interpolates between two timed poses
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="toWeight"></param>
/// <returns></returns>
public static TimedPose2D Interpolate(TimedPose2D from, TimedPose2D to, float toWeight)
{
return(new TimedPose2D(Vector2.Lerp(from.Position, to.Position, toWeight), TimeSpan.FromSeconds(from.Time.TotalSeconds + (to.Time.TotalSeconds - from.Time.TotalSeconds) * toWeight)));
}
