/// <summary>
/// Builds a full deceleration curve corresponding to a given target(location, speed)
/// </summary>
/// <param name="A_V_D"></param>
/// <param name="TargetSpeed"></param>
/// <param name="TargetDistance"></param>
/// <returns></returns>
public static QuadraticSpeedDistanceCurve Build_Deceleration_Curve(AccelerationSpeedDistanceSurface A_V_D,
SiSpeed TargetSpeed, SiDistance TargetDistance)
{
debugging_counter = 0;
QuadraticSpeedDistanceCurve result = new QuadraticSpeedDistanceCurve();
// Build a MRSP for this target
FlatSpeedDistanceCurve mrsp = new FlatSpeedDistanceCurve();
mrsp.Add(SiDistance.Zero, TargetDistance, new SiSpeed(400, SiSpeed_SubUnits.KiloMeter_per_Hour));
mrsp.Add(TargetDistance, new SiDistance(20000), TargetSpeed);
// Add to result by calculating backwards then forwards
SiDistance current_position = TargetDistance;
SiSpeed current_speed = TargetSpeed;
BrakingCurveDirectionEnum dir = BrakingCurveDirectionEnum.Backwards;
SiDistance next_position = SiDistance.Zero;
SiDistance end_position = mrsp[0].X.X0;
while (current_position > end_position)
{
Compute_Curve(A_V_D, result, mrsp, ref current_position, ref next_position, ref current_speed, dir);
/* Next loop starts from our new position.
* We do not need to update current_acceleration because
* it is done at the beginning of the loop*/
current_position = next_position;
current_speed = result.GetValueAt(current_position, BrakingCurveDirectionEnum.Backwards);
}
current_position = TargetDistance;
current_speed = TargetSpeed;
dir = BrakingCurveDirectionEnum.Forwards;
while (current_speed > minimal_speed_threshold)
{
Compute_Curve(A_V_D, result, mrsp, ref current_position, ref next_position, ref current_speed, dir);
/* Next loop starts from our new position.
* We do not need to update current_acceleration because
* it is done at the beginning of the loop*/
current_position = next_position;
current_speed = result.GetValueAt(current_position, dir);
}
result.Add(current_position, mrsp[mrsp.SegmentCount - 1].X.X1, SiAcceleration.Zero, SiSpeed.Zero,
current_position);
return(result);
}
/// <summary>
/// Provides the graph of the minimal value between this graph and another graph
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public static FlatSpeedDistanceCurve Min(FlatSpeedDistanceCurve left, FlatSpeedDistanceCurve right)
{
/* Here goes the result */
FlatSpeedDistanceCurve result = new FlatSpeedDistanceCurve();
/* Early exit to avoid accessing unexisting elements */
if (left.SegmentCount == 0)
{
for (int i = 0; i < right.SegmentCount; i++)
{
result.Add(right[i].X.X0, right[i].X.X1, right[i].Y);
}
return(result);
}
if (right.SegmentCount == 0)
{
for (int i = 0; i < left.SegmentCount; i++)
{
result.Add(left[i].X.X0, left[i].X.X1, left[i].Y);
}
return(result);
}
/* We collect all x positions */
List <SiDistance> x_positions = new List <SiDistance>();
/* Collect all X positions from left */
x_positions.Add(left[0].X.X0);
for (int i = 0; i < left.SegmentCount; i++)
{
x_positions.Add(left[i].X.X1);
}
/* Collect all X positions from right */
x_positions.Add(right[0].X.X0);
for (int i = 0; i < right.SegmentCount; i++)
{
x_positions.Add(right[i].X.X1);
}
x_positions.Sort(CompareDistances);
for (int i = 1; i < x_positions.Count; i++)
{
SiDistance x0 = x_positions[i - 1];
SiDistance x1 = x_positions[i];
/* Caution GetSegmentAt might return null */
ConstantCurveSegment <SiDistance, SiSpeed> left_segment = left.GetSegmentAt(x0);
ConstantCurveSegment <SiDistance, SiSpeed> right_segment = right.GetSegmentAt(x0);
if (left_segment == null)
{
result.Add(x0, x1, right_segment.Y);
}
else if (right_segment == null)
{
result.Add(x0, x1, left_segment.Y);
}
else
{
result.Add(x0, x1, SiSpeed.Min(left_segment.Y, right_segment.Y));
}
}
return(result);
}