private Dictionary <Point4Dstep, double> createPointSpeeds(IEnumerable <Point4Dstep> points)
{
var junctionLimits = createJunctionLimits(points);
var calculationPlan = new PlanBuilder();
var speedLimits = new Dictionary <Point4Dstep, double>();
var currentSpeedF = 1.0 * Configuration.TimerFrequency / Configuration.StartDeltaT;
var stepAcceleration = 1;
Point4Dstep previousPoint = null;
foreach (var point in points.Reverse())
{
var junctionLimitF = junctionLimits[point];
var newSpeedF = currentSpeedF + stepAcceleration;
if (previousPoint != null)
{
var currentSpeed = new Speed((int)(currentSpeedF), Configuration.TimerFrequency);
var junctionSpeed = new Speed((int)(junctionLimitF), Configuration.TimerFrequency);
var stepsX = previousPoint.X - point.X;
var stepsY = previousPoint.Y - point.Y;
var newSpeed = calculationPlan.AddLineXY(stepsX, stepsY, currentSpeed, Configuration.MaxPlaneAcceleration, junctionSpeed);
newSpeedF = 1.0 * newSpeed.StepCount / newSpeed.Ticks * Configuration.TimerFrequency;
}
currentSpeedF = Math.Min(newSpeedF, junctionLimitF);
if (double.IsNaN(currentSpeedF))
{
throw new NotSupportedException("invalid computation");
}
speedLimits[point] = currentSpeedF;
previousPoint = point;
}
previousPoint = null;
currentSpeedF = 1.0 * Configuration.TimerFrequency / Configuration.StartDeltaT;
foreach (var point in points)
{
var speedLimitF = speedLimits[point];
var newSpeedF = currentSpeedF + stepAcceleration;
if (previousPoint != null)
{
var currentSpeed = new Speed((int)(currentSpeedF), Configuration.TimerFrequency);
var limitSpeed = new Speed((int)(speedLimitF), Configuration.TimerFrequency);
var stepsX = previousPoint.X - point.X;
var stepsY = previousPoint.Y - point.Y;
var newSpeed = calculationPlan.AddLineXY(stepsX, stepsY, currentSpeed, Configuration.MaxPlaneAcceleration, limitSpeed);
newSpeedF = 1.0 * newSpeed.StepCount / newSpeed.Ticks * Configuration.TimerFrequency;
}
currentSpeedF = Math.Min(newSpeedF, speedLimitF);
speedLimits[point] = currentSpeedF;
previousPoint = point;
//Debug.WriteLine(currentVelocity);
}
return(speedLimits);
}
/// <summary>
/// Creates the plan connecting coordinates by lines with continuous speed.
/// </summary>
/// <param name="trajectory">Trajectory which plan will be created.</param>
/// <returns>The created plan.</returns>
public PlanBuilder CreateContinuousPlan(Trajectory4D trajectory)
{
var pointSpeeds = createPointSpeeds(trajectory.Points);
var planBuilder = new PlanBuilder();
var currentSpeed = Speed.Zero;
iterateDistances(trajectory, (p, x, y) =>
{
var targetSpeedF = pointSpeeds[p];
var targetSpeed = new Speed((int)(targetSpeedF), Configuration.TimerFrequency);
planBuilder.AddLineXY(x, y, currentSpeed, Configuration.MaxPlaneAcceleration, targetSpeed);
currentSpeed = targetSpeed;
});
return(planBuilder);
}
