/// <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 "); }
public void Build_Deceleration_CurveTestMethod() { // TODO : Fill the acceleration surface AccelerationSpeedDistanceSurface acceleration = new AccelerationSpeedDistanceSurface(); for (int i = 0; i < 8; i++) { double StartDistance = i * 625; double EndDistance = StartDistance + 625; for (int j = 0; j < 6; j++) { double StartSpeed = j * 35; double EndSpeed = StartSpeed + 35; 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(-Math.Abs(0.5 * (1.2 + Math.Pow(-1, i - j)))) )); } } // the target SiSpeed TargetSpeed = new SiSpeed(50, SiSpeed_SubUnits.KiloMeter_per_Hour); SiDistance TargetDistance = new SiDistance(2250); // Compute the deceleration curve using the previous algorithm QuadraticSpeedDistanceCurve deceleration = EtcsBrakingCurveBuilder.Build_Deceleration_Curve(acceleration, TargetSpeed, TargetDistance); TestOutput = new StringBuilder(); // Compare the deceleration curves for (double d = 0.0; d < 5000.0; d += 1) { double spd = deceleration.GetValueAt(new SiDistance(d), BrakingCurveDirectionEnum.Backwards).Value; TestOutput.Append(d.ToString()); TestOutput.Append("\t"); TestOutput.Append(spd.ToString()); TestOutput.Append("\t"); TestOutput.Append(acceleration.GetTileAt(new SiSpeed(spd), new SiDistance(d)).V.Y.Value); TestOutput.Append("\n"); } File.WriteAllText("Results.csv", TestOutput.ToString()); }
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))) { current_speed = MRSP.GetValueAt(current_position - new SiDistance(0.1)); } /******************************************************************* 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); /*************************************************************/ /* If this exception is thrown, you'd better call Juan */ /*************************************************************/ if (debugging_counter++ > 200) { throw new Exception("Algorithm is broken"); } } return result; }
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> /// 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 "); }
public void Build_Deceleration_CurveTestMethod() { // TODO : Fill the acceleration surface AccelerationSpeedDistanceSurface acceleration = new AccelerationSpeedDistanceSurface(); for (int i = 0; i < 8; i++) { double StartDistance = i*625; double EndDistance = StartDistance + 625; for (int j = 0; j < 6; j++) { double StartSpeed = j*35; double EndSpeed = StartSpeed + 35; 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(-Math.Abs(0.5*(1.2 + Math.Pow(-1, i - j)))) )); } } // the target SiSpeed TargetSpeed = new SiSpeed(50, SiSpeed_SubUnits.KiloMeter_per_Hour); SiDistance TargetDistance = new SiDistance(2250); // Compute the deceleration curve using the previous algorithm QuadraticSpeedDistanceCurve deceleration = EtcsBrakingCurveBuilder.Build_Deceleration_Curve(acceleration, TargetSpeed, TargetDistance); TestOutput = new StringBuilder(); // Compare the deceleration curves for (double d = 0.0; d < 5000.0; d += 1) { double spd = deceleration.GetValueAt(new SiDistance(d), BrakingCurveDirectionEnum.Backwards).Value; TestOutput.Append(d.ToString()); TestOutput.Append("\t"); TestOutput.Append(spd.ToString()); TestOutput.Append("\t"); TestOutput.Append(acceleration.GetTileAt(new SiSpeed(spd), new SiDistance(d)).V.Y.Value); TestOutput.Append("\n"); } File.WriteAllText("Results.csv", TestOutput.ToString()); }