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>
/// Sets the next point to be reached.
/// </summary>
/// <param name="point">Point where the tracer will run to.</param>
internal void StartNextPoint(Point4Dstep point)
{
if (_nextPoint != null)
{
throw new InvalidOperationException("Cannot set next point until profile of previous one is complete");
}
_nextPoint = point;
_context = new TraceContext(_context);
}
private double getTheta(Point4Dstep point1, Point4Dstep point2, Point4Dstep point3)
{
var Ax = 1.0 * point2.X - point1.X;
var Ay = 1.0 * point2.Y - point1.Y;
var A = new Vector(Ax, Ay);
var Bx = 1.0 * point2.X - point3.X;
var By = 1.0 * point2.Y - point3.Y;
var B = new Vector(Bx, By);
return(Vector.AngleBetween(A, B));
}
private void iterateDistances(Trajectory4D trajectory, Action <Point4Dstep, int, int> planner)
{
Point4Dstep lastPoint = null;
foreach (var point in trajectory.Points)
{
if (lastPoint == null)
{
lastPoint = point;
continue;
}
var distanceX = point.X - lastPoint.X;
var distanceY = point.Y - lastPoint.Y;
planner(point, distanceX, distanceY);
lastPoint = point;
}
}
