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; } }