public static QuadraticSpeedDistanceCurve Build_A_Safe_Backward(AccelerationSpeedDistanceSurface A_V_D,
FlatSpeedDistanceCurve MRSP)
{
if (debug)
{
Log.InfoFormat("#######################################################");
Log.InfoFormat("## Build_A_Safe_Backward_Surface#######################");
Log.InfoFormat("#######################################################");
}
int debugging_counter = 0;
QuadraticSpeedDistanceCurve result = new QuadraticSpeedDistanceCurve();
/***********************************************************
* The ending point is the first point in the MRSP
***********************************************************/
SiDistance end_position = MRSP[0].X.X0;
/***********************************************************
* Go forward in the MRSP until we find the point with
* minimal speed. This shall be our starting point
**********************************************************/
SiDistance current_position = SiDistance.Zero;
SiSpeed current_speed = SiSpeed.MaxValue;
if (debug)
{
Log.DebugFormat(" Search the position of the minimal speed in the MRSP");
}
for (int i = 0; i < MRSP.SegmentCount; i++)
{
ConstantCurveSegment <SiDistance, SiSpeed> segment = MRSP[i];
if (segment.Y < current_speed)
{
current_speed = segment.Y;
current_position = segment.X.X1;
if (debug)
{
Log.DebugFormat(" new start position V={0,7:F2} at={1,7:F2} ", current_speed.ToUnits(),
current_position.ToUnits());
}
}
}
if (debug)
{
Log.DebugFormat(" end position is at={0,7:F2} ", end_position.ToUnits());
}
/*************************************************************************/
/* Starting from the right side of curves, go back to the left side. */
/* Build small curves arcs where the acceleration is constant on each one*/
/*************************************************************************/
while (current_position > end_position)
{
if (debug)
{
Log.DebugFormat("#######################################################");
Log.DebugFormat("### Loop {0} #########################################", debugging_counter);
Log.DebugFormat("#######################################################");
}
/************************************************************
* Based on current speed and position, search on wich tile
* of A_V_D tile we are
***********************************************************/
SurfaceTile current_tile = A_V_D.GetTileAt(current_speed + new SiSpeed(0.01),
current_position - new SiDistance(0.1));
SiAcceleration current_acceleration = current_tile.V.Y;
/***************************************************************************/
/* If at previous loop wi did 'hit' the vertical part of the MRSP,
* we might have a speed above the current MRSP segment.*/
/***************************************************************************/
if (current_speed >
MRSP.GetValueAt(current_position - new SiDistance(0.1), BrakingCurveDirectionEnum.Backwards))
{
current_speed = MRSP.GetValueAt(current_position - new SiDistance(0.1),
BrakingCurveDirectionEnum.Backwards);
}
/*******************************************************************
* We build a quadratic arc with current train position, speed
* and acceleration. The arc domain [0..current_position] is not valid yet.
* We must find out the domain left limit.
*****************************************************************/
QuadraticCurveSegment current_curve = new QuadraticCurveSegment(SiDistance.Zero,
current_position,
current_acceleration,
current_speed,
current_position);
if (debug)
{
Log.DebugFormat(" current_acceleration = {0,7:F2} from a_tile {1}", current_acceleration.ToUnits(),
current_tile.ToString());
Log.DebugFormat(" current_speed = {0,7:F2} ", current_speed.ToUnits());
Log.DebugFormat(" current_position = {0,7:F2} ", current_position.ToUnits());
Log.DebugFormat(" --> current_curve = {0} ", current_curve.ToString());
}
/********************************************************************/
/* The current_curve may 'hit' one of these 4 items:
* 1) The upper border of the tile (because of a new acceleration)
* 2) The left border of the tile (because of a gradient?)
* 3) A vertical segment of the MRSP
* 4) An horizontal segment of the MRSP
* Text all of them and update the next_position accordingly.
*************************************************************************/
SiDistance next_position = SiDistance.Zero;
/* 1) The distance at wich our temporary arc intersects the top (V2) segment of the AVD tile */
{
SiDistance d = current_curve.IntersectAt(current_tile.V.X.X1);
if (debug)
{
Log.DebugFormat(" intersection with tile (V_TOP) at {0,7:F2} ", d.ToUnits());
}
if (d >= next_position)
{
if (debug)
{
Log.DebugFormat(" --> case_1 next_position {0,7:F2} -> {1,7:F2}", next_position.ToUnits(),
d.ToUnits());
}
next_position = d;
}
}
/* 2) The distance at wich our temporary arc intersects the left (D0) segment of the AVD tile */
{
SiDistance d = current_tile.D.X.X0;
if (debug)
{
Log.DebugFormat(" intersection with tile (D0) at {0,7:F2} ", d.ToUnits());
}
if (d >= next_position)
{
if (debug)
{
Log.DebugFormat(" --> case_2 next_position {0,7:F2} -> {1,7:F2}", next_position.ToUnits(),
d.ToUnits());
}
next_position = d;
}
}
/*Since the MRSP is continous, the following cannot fail. */
ConstantCurveSegment <SiDistance, SiSpeed> speed_limit_here =
MRSP.Intersect(current_position - new SiDistance(0.1), current_curve);
if (debug)
{
Log.DebugFormat(" MRSP segment {0} ", speed_limit_here.ToString());
}
/* 3) Do we hit the vertical segment of the MRSP ? */
{
if (speed_limit_here.X.X0 >= next_position)
{
if (debug)
{
Log.DebugFormat(" --> case_3 next_position {0,7:F2} -> {1,7:F2}", next_position.ToUnits(),
speed_limit_here.X.X0.ToUnits());
}
next_position = speed_limit_here.X.X0;
}
}
/* 4) Do we hit the horizontal segment of the MRSP */
{
if (current_speed + new SiSpeed(0.01) < speed_limit_here.Y)
{
SiDistance d = current_curve.IntersectAt(speed_limit_here.Y);
if (d >= next_position)
{
if (debug)
{
Log.DebugFormat(" --> case_4a next_d {0,7:F2} -> {1,7:F2}",
next_position.ToUnits(), d.ToUnits());
}
next_position = d;
}
}
else
{
if (debug)
{
Log.DebugFormat(" --> case_4b next_acc_0 {0,7:F2} -> {1,7:F2}", next_position.ToUnits(),
speed_limit_here.X.X0.ToUnits());
}
current_acceleration = SiAcceleration.Zero;
next_position = speed_limit_here.X.X0;
}
}
/* Finally we can add the segment because next_position has been computed. */
result.Add(next_position, current_position, current_acceleration, current_speed, current_position);
result.Dump("result so far ");
/* 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);
/*************************************************************/
/* If this exception is thrown, you'd better call Juan */
/*************************************************************/
if (debugging_counter++ > 200)
{
throw new Exception("Algorithm is broken");
}
}
return(result);
}
public void Build_A_Safe_BackwardTestMethod()
{
// TODO : Fill the acceleration surface
AccelerationSpeedDistanceSurface acceleration = new AccelerationSpeedDistanceSurface();
for (int i = 0; i < 5; i++)
{
double StartDistance = i * 1000;
double EndDistance = StartDistance + 1000;
for (int j = 0; j < 4; j++)
{
double StartSpeed = j * 60;
double EndSpeed = StartSpeed + 60;
acceleration.Tiles.Add(new SurfaceTile(
new SiDistance(StartDistance),
new SiDistance(EndDistance),
new SiSpeed(StartSpeed, SiSpeed_SubUnits.KiloMeter_per_Hour),
new SiSpeed(EndSpeed, SiSpeed_SubUnits.KiloMeter_per_Hour),
new SiAcceleration(-(0.5 + (i - j) / 8))
));
}
}
// TODO : Fill the mrsp
FlatSpeedDistanceCurve mrsp = new FlatSpeedDistanceCurve();
mrsp.AddSegment(new ConstantCurveSegment <SiDistance, SiSpeed>(
new SiDistance(0),
new SiDistance(650),
new SiSpeed(80, SiSpeed_SubUnits.KiloMeter_per_Hour)
));
mrsp.AddSegment(new ConstantCurveSegment <SiDistance, SiSpeed>(
new SiDistance(650),
new SiDistance(1500),
new SiSpeed(60, SiSpeed_SubUnits.KiloMeter_per_Hour)
));
mrsp.AddSegment(new ConstantCurveSegment <SiDistance, SiSpeed>(
new SiDistance(1500),
new SiDistance(2500),
new SiSpeed(120, SiSpeed_SubUnits.KiloMeter_per_Hour)
));
mrsp.AddSegment(new ConstantCurveSegment <SiDistance, SiSpeed>(
new SiDistance(2500),
new SiDistance(4000),
new SiSpeed(55, SiSpeed_SubUnits.KiloMeter_per_Hour)
));
mrsp.AddSegment(new ConstantCurveSegment <SiDistance, SiSpeed>(
new SiDistance(4000),
new SiDistance(5000),
new SiSpeed(0)
));
// Compute the deceleration curve using the previous algorithm
QuadraticSpeedDistanceCurve deceleration =
EtcsBrakingCurveBuilder_Obsolete.Build_A_Safe_Backward(acceleration, mrsp);
// Compute the deceleration curve using the new algorithm
// TODO : Implement the new algorithm and use it
QuadraticSpeedDistanceCurve deceleration2 = EtcsBrakingCurveBuilder.Build_A_Safe_Backward(acceleration, mrsp);
TestOutput = new StringBuilder();
// Compare the deceleration curves
for (double d = 0.0; d < 5000.0; d += 1)
{
Assert.AreEqual(
deceleration.GetValueAt(new SiDistance(0.0 + d), BrakingCurveDirectionEnum.Backwards),
deceleration2.GetValueAt(new SiDistance(0.0 + d), BrakingCurveDirectionEnum.Backwards),
"Value at " + d + " should be equal"
);
TestOutput.Append(d.ToString());
TestOutput.Append("\t");
TestOutput.Append(mrsp.GetValueAt(new SiDistance(d), BrakingCurveDirectionEnum.Backwards).Value);
TestOutput.Append("\t");
TestOutput.Append(deceleration.GetValueAt(new SiDistance(d), BrakingCurveDirectionEnum.Backwards).Value);
TestOutput.Append("\t");
TestOutput.Append(deceleration2.GetValueAt(new SiDistance(d), BrakingCurveDirectionEnum.Backwards).Value);
TestOutput.Append("\n");
}
File.WriteAllText("ResultsCompare.csv", TestOutput.ToString());
}
/// <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);
}
public MiddleData()
{
MRSP = new FlatSpeedDistanceCurve();
MA = new FlatSpeedDistanceCurve();
TSR = new FlatSpeedDistanceCurve();
}
/// <summary>
/// Method used to get the full EBD and SBD curves from the MRSP
/// </summary>
/// <param name="A_V_D"></param>
/// <param name="MRSP"></param>
/// <returns></returns>
public static QuadraticSpeedDistanceCurve Build_A_Safe_Backward(AccelerationSpeedDistanceSurface A_V_D,
FlatSpeedDistanceCurve MRSP)
{
debugging_counter = 0;
if (debug)
{
Log.InfoFormat("#######################################################");
Log.InfoFormat("## Build_A_Safe_Backward_Surface#######################");
Log.InfoFormat("#######################################################");
}
QuadraticSpeedDistanceCurve result = new QuadraticSpeedDistanceCurve();
/***********************************************************
* The ending point is the first point in the MRSP
***********************************************************/
SiDistance end_position = MRSP[0].X.X0;
/***********************************************************
* Go forward in the MRSP until we find the point with
* minimal speed. This shall be our starting point
**********************************************************/
SiDistance current_position = SiDistance.Zero;
SiSpeed current_speed = SiSpeed.MaxValue;
if (debug)
{
Log.DebugFormat(" Search the position of the minimal speed in the MRSP");
}
for (int i = 0; i < MRSP.SegmentCount; i++)
{
ConstantCurveSegment <SiDistance, SiSpeed> segment = MRSP[i];
if (segment.Y < current_speed)
{
current_speed = segment.Y;
current_position = segment.X.X1;
if (debug)
{
Log.DebugFormat(" new start position V={0,7:F2} at={1,7:F2} ", current_speed.ToUnits(),
current_position.ToUnits());
}
}
}
if (debug)
{
Log.DebugFormat(" end position is at={0,7:F2} ", end_position.ToUnits());
}
SiDistance next_position = SiDistance.Zero;
/*************************************************************************/
/* Starting from the right side of curves, go back to the left side. */
/* Build small curves arcs where the acceleration is constant on each one*/
/*************************************************************************/
while (current_position > end_position)
{
Compute_Curve(A_V_D, result, MRSP, ref current_position, ref next_position, ref current_speed,
BrakingCurveDirectionEnum.Backwards);
/* 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);
/*************************************************************/
/* If this exception is thrown, you'd better call Juan */
/*************************************************************/
if (debugging_counter++ > 200)
{
throw new Exception("Algorithm is broken");
}
}
return(result);
}
/// <summary>
/// Method to return the curve segment at the current location and speed, going in the specified direction.
/// </summary>
/// <param name="current_tile"></param>
/// <param name="current_position"></param>
/// <param name="current_speed"></param>
/// <param name="MRSP"></param>
/// <param name="dir"></param>
/// <returns></returns>
private static QuadraticCurveSegment Build_One_Curve_Segment(SurfaceTile current_tile,
SiDistance current_position,
SiSpeed current_speed,
FlatSpeedDistanceCurve MRSP,
BrakingCurveDirectionEnum dir)
{
SiAcceleration current_acceleration = current_tile.V.Y;
SiDistance MRSP_end = MRSP[MRSP.SegmentCount - 1].X.X1;
SiDistance curve_start = new SiDistance();
SiDistance curve_end = new SiDistance();
switch (dir)
{
case BrakingCurveDirectionEnum.Backwards:
curve_start = SiDistance.Zero;
curve_end = current_position;
break;
case BrakingCurveDirectionEnum.Forwards:
curve_start = current_position;
curve_end = MRSP_end;
break;
}
QuadraticCurveSegment current_curve = new QuadraticCurveSegment(curve_start,
curve_end,
current_acceleration,
current_speed,
current_position);
if (debug)
{
Log.DebugFormat(" current_acceleration = {0,7:F2} from a_tile {1}", current_acceleration.ToUnits(),
current_tile.ToString());
Log.DebugFormat(" current_speed = {0,7:F2} ", current_speed.ToUnits());
Log.DebugFormat(" current_position = {0,7:F2} ", current_position.ToUnits());
Log.DebugFormat(" --> current_curve = {0} ", current_curve.ToString());
}
/********************************************************************/
/* The current_curve may 'hit' one of these 4 items:
* 1) The upper border of the tile (because of a new acceleration)
* 2) The left border of the tile (because of a gradient?)
* 3) A vertical segment of the MRSP
* 4) An horizontal segment of the MRSP
* Text all of them and update the next_position accordingly.
*************************************************************************/
SiDistance next_position = SiDistance.Zero;
/* The distance at which our temporary arc intersects a segment of the AVD tile */
{
next_position = Tile_Intersect(current_position, current_tile, current_curve, dir);
}
/* The MRSP checks only need to be performed if the curve is being computed backwards */
if (dir == BrakingCurveDirectionEnum.Backwards)
{
/*Since the MRSP is continous, the following cannot fail. */
ConstantCurveSegment <SiDistance, SiSpeed> speed_limit_here =
MRSP.Intersect(current_position - new SiDistance(0.1), current_curve);
if (debug)
{
Log.DebugFormat(" MRSP segment {0} ", speed_limit_here.ToString());
}
/* 3) Do we hit the vertical segment of the MRSP ? */
{
next_position = IntersectMRSPSpeed(next_position, speed_limit_here);
}
/* 4) Do we hit the horizontal segment of the MRSP */
{
if (current_speed + new SiSpeed(0.01) < speed_limit_here.Y)
{
SiDistance d = current_curve.IntersectAt(speed_limit_here.Y);
if (d >= next_position)
{
if (debug)
{
Log.DebugFormat(" --> case_4a next_d {0,7:F2} -> {1,7:F2}",
next_position.ToUnits(), d.ToUnits());
}
next_position = d;
}
}
else
{
if (debug)
{
Log.DebugFormat(" --> case_4b next_acc_0 {0,7:F2} -> {1,7:F2}", next_position.ToUnits(),
speed_limit_here.X.X0.ToUnits());
}
current_acceleration = SiAcceleration.Zero;
next_position = speed_limit_here.X.X0;
}
}
}
SiDistance result_start = new SiDistance();
SiDistance result_end = new SiDistance();
switch (dir)
{
case BrakingCurveDirectionEnum.Backwards:
result_start = next_position;
result_end = current_position;
break;
case BrakingCurveDirectionEnum.Forwards:
result_start = current_position;
result_end = next_position;
break;
}
QuadraticCurveSegment result = new QuadraticCurveSegment(result_start,
result_end,
current_acceleration,
current_speed,
current_position);
return(result);
}
