/// <summary>
/// Creates the plan connecting coordinates by ramped lines.
/// </summary>
/// <param name="trajectory">Trajectory which plan will be created.</param>
/// <returns>The created plan.</returns>
public PlanBuilder CreateRampedPlan(Trajectory4D trajectory)
{
var planBuilder = new PlanBuilder();
iterateDistances(trajectory, (p, u, v, x, y) => planBuilder.AddRampedLineUVXY(u, v, x, y, Configuration.MaxPlaneAcceleration, Configuration.MaxPlaneSpeed));
return(planBuilder);
}
/// <summary>
/// Creates the plan connecting coordinates constant speed without acceleration.
/// </summary>
/// <param name="trajectory">Trajectory which plan will be created.</param>
/// <returns>The created plan.</returns>
public PlanBuilder CreateConstantPlan(Trajectory4D trajectory)
{
var planBuilder = new PlanBuilder();
iterateDistances(trajectory, (p, x, y) => planBuilder.AddConstantSpeedTransitionXY(x, y, _transitionSpeed));
return(planBuilder);
}
/// <summary>
/// Creates the plan connecting coordinates constant speed without acceleration.
/// </summary>
/// <param name="trajectory">Trajectory which plan will be created.</param>
/// <returns>The created plan.</returns>
public PlanBuilder CreateConstantPlan(Trajectory4D trajectory, IEnumerable <Tuple <Speed, Speed> > segmentSpeeds = null)
{
if (segmentSpeeds == null)
{
segmentSpeeds = new Tuple <Speed, Speed> [0];
}
var speeds = new Queue <Tuple <Speed, Speed> >(segmentSpeeds);
var planBuilder = new PlanBuilder();
iterateDistances(trajectory, (p, u, v, x, y) =>
{
var speedUV = _transitionSpeed;
var speedXY = _transitionSpeed;
if (speeds.Any())
{
var speed = speeds.Dequeue();
speedUV = speed.Item1;
speedXY = speed.Item2;
}
planBuilder.AddConstantSpeedTransitionUVXY(u, v, speedUV, x, y, speedXY);
});
return(planBuilder);
}
/// <summary>
/// Draws coordinates from a provider by a continuous speed
/// </summary>
/// <param name="provider">Provider which creates coordinates.</param>
/// <param name="speed">Speed of drawing</param>
public static PlanBuilder DrawContinuousLines(CoordinateProvider provider, Speed speed = null)
{
if (speed == null)
{
speed = Constants.MaxPlaneSpeed;
}
var points = provider();
var trajectory = new Trajectory4D(points);
var planner = new StraightLinePlanner(speed);
return(planner.CreateContinuousPlan(trajectory));
}
/// <summary>
/// Draws coordinates from a provider by a constant speed
/// </summary>
/// <param name="provider">Provider which creates coordinates.</param>
/// <param name="speed">Speed of drawing</param>
public static PlanBuilder DrawByConstantSpeed(CoordinateProvider provider, Speed speed = null)
{
if (speed == null)
{
speed = Constants.ReverseSafeSpeed;
}
var points = provider();
var trajectory = new Trajectory4D(points);
var planner = new StraightLinePlanner(speed);
return(planner.CreateConstantPlan(trajectory));
}
/// <summary>
/// Draws coordinates from a provider by a constant speed
/// </summary>
/// <param name="provider">Provider which creates coordinates.</param>
/// <param name="speed">Speed of drawing</param>
public static PlanBuilder DrawByRampedLines(CoordinateProvider provider, Speed speed = null)
{
if (speed == null)
{
speed = Configuration.MaxPlaneSpeed;
}
var points = provider();
var trajectory = new Trajectory4D(points);
var planner = new StraightLinePlanner(speed);
return(planner.CreateRampedPlan(trajectory));
}
/// <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);
}
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;
}
}
