/* Find the intersection beween this FlatCurve and a Quadratic one */
/******************************************************************************/
/**@brief Returns the segment wich intersects the arc a_quadratic_segment. */
/******************************************************************************/
public ConstantCurveSegment <SiDistance, SiSpeed> Intersect(SiDistance d_max,
QuadraticCurveSegment a_quadratic_segment)
{
ConstantCurveSegment <SiDistance, SiSpeed> matching_flat_segment = null;
/* Find the right most one. */
for (int i = this.SegmentCount - 1; i >= 0; i--)
{
matching_flat_segment = this[i];
SiDistance d_intersection = a_quadratic_segment.IntersectAt(matching_flat_segment.Y);
if (debug)
{
Log.DebugFormat(" FlatSD [{0:D2}/{1:D2}] Q:{2} intersects F:{3} at {4,7:F2} d_max:{5,7:F2}",
i,
this.SegmentCount,
a_quadratic_segment.ToString(),
matching_flat_segment.ToString(),
d_intersection.ToUnits(),
d_max.ToUnits());
}
if (matching_flat_segment.X.X0 <= d_max)
{
return(matching_flat_segment);
}
}
return(this[0]);
}
/// <summary>
/// Computes the curve from a point
/// </summary>
/// <param name="A_V_D"></param>
/// <param name="result"></param>
/// <param name="mrsp"></param>
/// <param name="current_position"></param>
/// <param name="next_position"></param>
/// <param name="current_speed"></param>
/// <param name="dir"></param>
private static void Compute_Curve(AccelerationSpeedDistanceSurface A_V_D,
QuadraticSpeedDistanceCurve result,
FlatSpeedDistanceCurve mrsp,
ref SiDistance current_position,
ref SiDistance next_position,
ref SiSpeed current_speed,
BrakingCurveDirectionEnum dir)
{
int Direction = Get_Direction(dir);
SiSpeed speed_step = (-1) * Direction * minimal_speed_threshold;
SiDistance distance_step = Direction * minimal_distance_threshold;
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 + speed_step, current_position + distance_step);
/***************************************************************************/
/* 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 - minimal_distance_threshold, BrakingCurveDirectionEnum.Backwards))
{
current_speed = mrsp.GetValueAt(current_position - minimal_distance_threshold,
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 = Build_One_Curve_Segment(current_tile, current_position, current_speed,
mrsp, dir);
next_position = Distance_Edge(current_curve, dir);
SiAcceleration current_acceleration = current_curve.A;
/* Finally we can add the segment because next_position has been computed. */
SiDistance refLocation = current_curve.X.X0;
SiSpeed refSpeed = current_curve.Get(refLocation);
result.Add(current_curve.X.X0, current_curve.X.X1, current_acceleration, refSpeed, refLocation);
// result.Dump("result so far ");
}
/// <summary>
/// Finds the tile edge that is intersected in the chosen direction
/// </summary>
/// <param name="current_tile"></param>
/// <param name="current_curve"></param>
/// <param name="dir"></param>
/// <returns></returns>
private static SiDistance Tile_Intersect(SiDistance current_position, SurfaceTile current_tile,
QuadraticCurveSegment current_curve, BrakingCurveDirectionEnum dir)
{
SiDistance result = SiDistance.Zero;
SiDistance SpdEdge = Speed_Edge_Location(current_tile, current_curve, dir);
SiDistance DistEdge = Distance_Edge(current_tile, dir);
result = Closest_To(current_position, DistEdge, SpdEdge);
return(result);
}
/// <summary>
/// Provides the distance limit of a tile in the requested direction
/// </summary>
/// <param name="current_tile"></param>
/// <param name="dir"></param>
/// <returns></returns>
private static SiDistance Distance_Edge(QuadraticCurveSegment current_curve, BrakingCurveDirectionEnum dir)
{
SiDistance result = SiDistance.Zero;
switch (dir)
{
case BrakingCurveDirectionEnum.Backwards:
result = current_curve.X.X0;
break;
case BrakingCurveDirectionEnum.Forwards:
result = current_curve.X.X1;
break;
}
return(result);
}
/******************************************************************************************************/
protected void Emit_Quadratic_Segment(StreamWriter swd, QuadraticCurveSegment aSegment, SiSpeed v_offset, SiDistance d_offset)
{
SiDistance d0 = aSegment.X.X0;
SiDistance d1 = aSegment.X.X1;
Emit_d_V(swd, d0 + d_offset, aSegment.Get(d0) + v_offset);
if (d1 - d0 > SiDistance.One)
{
SiDistance delta = SiDistance.One;
SiDistance d = d0 + delta;
do
{
Emit_d_V(swd, d + d_offset, aSegment.Get(d) + v_offset);
d += delta;
} while (d < d1);
}
Emit_d_V(swd, d1 + d_offset, aSegment.Get(d1) + v_offset);
}
/// <summary>
/// Provides the distance at which the curve intersects the horizontal edge of the tile in the given direction
/// </summary>
/// <param name="current_tile"></param>
/// <param name="current_curve"></param>
/// <param name="dir"></param>
/// <returns></returns>
private static SiDistance Speed_Edge_Location(SurfaceTile current_tile, QuadraticCurveSegment current_curve,
BrakingCurveDirectionEnum dir)
{
SiDistance result = SiDistance.Zero;
SiSpeed limit = SiSpeed.Zero;
switch (dir)
{
case BrakingCurveDirectionEnum.Backwards:
limit = current_tile.V.X.X1;
break;
case BrakingCurveDirectionEnum.Forwards:
limit = current_tile.V.X.X0;
break;
}
result = current_curve.IntersectAt(limit);
return(result);
}
/******************************************************************************************************/
protected override void Build_Gnuplot_Files(string job_filename, string png_filename)
{
int label_counter = 0;
StreamWriter swj = new StreamWriter(job_filename);
swj.WriteLine("reset ");
swj.WriteLine("set terminal png size {0}, {1}", my_bitmap_width, my_bitmap_height);
swj.WriteLine("set output '{0}'", png_filename);
swj.WriteLine("set palette gray ");
swj.WriteLine("set style data points ");
swj.WriteLine("set xlabel 'm' ");
swj.WriteLine("set xrange [{0}:{1}] ",
min_x_is_set ? min_x.ToSubUnits(SiDistance_SubUnits.Meter).ToString() : "*",
max_x_is_set ? max_x.ToSubUnits(SiDistance_SubUnits.Meter).ToString() : "*");
swj.WriteLine("set ylabel 'km/h' ");
swj.WriteLine("set autoscale y ");
//swj.WriteLine ("yrange [0:*] ");
//swj.WriteLine ("set yzeroaxis ");
/* When the Y2 axis is involved and mixx is used, GnuPlot doen not generate anything... */
/* This must be FIXED in order to plot the Gradient profile along with the braking
* curves */
{
//swj.WriteLine ("set x2label 'm2'");
//swj.WriteLine ("set x2range [{0}:{1}] ",
// min_x_is_set ? min_x.ToSubUnits(SiDistance_SubUnits.Meter).ToString() : "*",
// max_x_is_set ? max_x.ToSubUnits(SiDistance_SubUnits.Meter).ToString() : "*" );
//swj.WriteLine ("set x2tics auto ");
//swj.WriteLine ("set y2range [0:*] ");
// swj.WriteLine ("set autoscale y2 ");
// swj.WriteLine ("set y2label '%' ");
// swj.WriteLine ("set y2tics auto ");
// swj.WriteLine ("set y2zeroaxis ");
// swj.WriteLine ("set x2zeroaxis ");
}
swj.WriteLine("set key left bottom ");
swj.WriteLine("set grid ");
// swj.WriteLine ("set samples 100 ");
// swj.WriteLine ("set view map ");
List <string> GPPlot = new List <string>();
/* Let's plot all curves */
for (int item_idx = 0; item_idx < Items.Count; item_idx++)
{
SiSpeed v_offset = 0.0 * new SiSpeed((double)(item_idx) * -5.0, SiSpeed_SubUnits.KiloMeter_per_Hour);
SiDistance d_offset = 0.0 * new SiDistance((double)(item_idx) * -10.0);
PlottedItem pi = Items[item_idx];
/************************ Quadratic ******************************/
if (pi.theCurve is QuadraticSpeedDistanceCurve)
{
QuadraticSpeedDistanceCurve Q_Curve = pi.theCurve as QuadraticSpeedDistanceCurve;
if (ShowColouredSegments)
{
for (int segment_idx = 0; segment_idx < Q_Curve.SegmentCount; segment_idx++)
{
QuadraticCurveSegment aSegment = Q_Curve[segment_idx];
string title = "notitle";
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}_{2:D2}.dat", my_base_name, item_idx, segment_idx));
StreamWriter swd = new StreamWriter(filename_dat);
Emit_Quadratic_Segment(swd, aSegment, v_offset, d_offset);
swd.Close();
if (Add_Extended_Legend)
{
string the_text = String.Format("{0}_{1}", pi.theName, segment_idx);
if (aSegment.A < new SiAcceleration(0.0))
{
the_text += String.Format(" A{0,6:F1}{1}", aSegment.A.ToUnits(), aSegment.A.UnitString());
the_text += String.Format(" V{0,6:F1}..{1,6:F1} {2}",
aSegment.Get(aSegment.X.X0).ToUnits(),
aSegment.Get(aSegment.X.X1).ToUnits(),
aSegment.Get(aSegment.X.X0).UnitString());
}
else
{
the_text += String.Format(" V{0,6:F1}{1}", aSegment.V0.ToUnits(), aSegment.V0.UnitString());
}
SiDistance segment_length = aSegment.X.X1 - aSegment.X.X0;
the_text += String.Format(" ({0,4:F0}{1})", segment_length.ToUnits(), segment_length.UnitString());
title = String.Format("title \"{0}\" ", the_text);
}
else
{
if (segment_idx == 0)
{
title = String.Format("title \"{0}\" ", pi.theName);
}
}
GPPlot.Add(String.Format("'{0}' using 1:2 axes x1y1 {1} with lines", filename_dat, title));
}
}
else
{
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}.dat", my_base_name, item_idx));
string title = String.Format("title \"{0}\" ", pi.theName);
StreamWriter swd = new StreamWriter(filename_dat);
for (int segment_idx = 0; segment_idx < Q_Curve.SegmentCount; segment_idx++)
{
QuadraticCurveSegment aSegment = Q_Curve[segment_idx];
Emit_Quadratic_Segment(swd, aSegment, v_offset, d_offset);
}
swd.Close();
GPPlot.Add(String.Format("'{0}' using 1:2 axes x1y1 {1} with lines linecolor rgb \"{2}\"", filename_dat, title, pi.theColor));
}
if (Add_Acceleration_Overlay)
{
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}_acc.dat", my_base_name, item_idx));
string title = String.Format("title \"{0}_A(V,d)\" ", pi.theName);
StreamWriter swd = new StreamWriter(filename_dat);
for (int segment_idx = 0; segment_idx < Q_Curve.SegmentCount; segment_idx++)
{
QuadraticCurveSegment aSegment = Q_Curve[segment_idx];
Emit_d_a(swd, aSegment.X.X0, aSegment.A);
Emit_d_a(swd, aSegment.X.X1 - new SiDistance(0.1), aSegment.A);
}
swd.Close();
GPPlot.Add(String.Format("'{0}' using 1:2 axes x1y2 {1} with lines", filename_dat, title));
}
}
/************************ Flat curve ******************************/
else if (pi.theCurve is FlatSpeedDistanceCurve)
{
FlatSpeedDistanceCurve F_Curve = pi.theCurve as FlatSpeedDistanceCurve;
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}_speed_distance.dat", my_base_name, item_idx));
StreamWriter swd = new StreamWriter(filename_dat);
for (int segment_idx = 0; segment_idx < F_Curve.SegmentCount; segment_idx++)
{
Emit_Constant_Segment(swd, F_Curve[segment_idx], v_offset, d_offset);
}
swd.Close();
GPPlot.Add(String.Format("'{0}' using 1:2 axes x1y1 title \"{1}\" with lines linewidth 3 linecolor rgb \"{2}\"", filename_dat, pi.theName, pi.theColor));
}
/************************ Flat curve ******************************/
else if (pi.theCurve is AccelerationSpeedDistanceSurface)
{
AccelerationSpeedDistanceSurface AVD_Surface = pi.theCurve as AccelerationSpeedDistanceSurface;
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}_asd_surface.dat", my_base_name, item_idx));
{
StreamWriter swd = new StreamWriter(filename_dat);
for (int i = 0; i < AVD_Surface.Tiles.Count; i++)
{
SurfaceTile aTile = AVD_Surface.Tiles[i];
Emit_Segment(swd, aTile.D.X.X0, aTile.D.X.X1, aTile.V.X.X0, v_offset, d_offset);
Emit_Segment(swd, aTile.D.X.X0, aTile.D.X.X1, aTile.V.X.X1, v_offset, d_offset);
Emit_Segment(swd, aTile.V.X.X0, aTile.V.X.X1, aTile.D.X.X0, v_offset, d_offset);
Emit_Segment(swd, aTile.V.X.X0, aTile.V.X.X1, aTile.D.X.X1, v_offset, d_offset);
if (Show_ASD_AccelerationValues)
{
SiDistance cx = (aTile.D.X.X0 + aTile.D.X.X1) * 0.5;
SiSpeed cy = (aTile.V.X.X0 + aTile.V.X.X1) * 0.5;
label_counter++;
swj.WriteLine("set label {0} '{1} m/s²' at {2}, {3} center front",
label_counter,
aTile.V.Y.ToUnits().ToString("0.00", System.Globalization.CultureInfo.InvariantCulture),
cx.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture),
cy.ToSubUnits(SiSpeed_SubUnits.KiloMeter_per_Hour).ToString(System.Globalization.CultureInfo.InvariantCulture)
);
}
}
swd.Close();
GPPlot.Add(String.Format("'{0}' using 1:2 axes x1y1 title \"{1}\" with points pointtype 5 pointsize 0.5 linecolor rgb \"gray\"", filename_dat, pi.theName));
}
}
/************************ A single Point ****************************/
else if (pi.theCurve is SinglePoint)
{
string filename_dat = BuildTempFile(true, String.Format("{0}_{1:D2}.dat", my_base_name, item_idx));
StreamWriter swd = new StreamWriter(filename_dat);
SinglePoint aPoint = pi.theCurve as SinglePoint;
Emit_d_V(swd, aPoint.X, aPoint.Y);
swd.Close();
GPPlot.Add(String.Format("'{0}' axes x1y1 title \"{1}\" with points pointsize 2 pointtype 7 linecolor rgb \"{2}\" ",
filename_dat,
pi.theName,
pi.theColor));
}
/************************ WTF is that ? ***************************/
else
{
throw new ArgumentException("Unsuported curve type" + pi.theCurve);
}
}
swj.Write("plot ");
for (int i = 0; i < GPPlot.Count - 1; i++)
{
swj.WriteLine(" {0},\\", GPPlot[i]);
}
swj.WriteLine(" {0}", GPPlot[GPPlot.Count - 1]);
swj.Close();
}
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);
}
/// <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);
}
