/******************************************************************************************************/ protected void Emit_d_v_a(StreamWriter swd, SiDistance d, SiSpeed V, SiAcceleration a) { swd.WriteLine("{0} {1} {2}", d.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture), V.ToSubUnits(SiSpeed_SubUnits.KiloMeter_per_Hour).ToString(System.Globalization.CultureInfo.InvariantCulture), a.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture)); }
/// <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> /// Takes three SiDistances and determines which of the second two is closest to the first one /// </summary> /// <param name="reference">The point the others are compared to.</param> /// <param name="first">The first compared distance.</param> /// <param name="second">The second compared distance.</param> /// <returns></returns> private static SiDistance Closest_To(SiDistance reference, SiDistance first, SiDistance second) { SiDistance result = first; if (Math.Abs(reference.Value - first.Value) > Math.Abs(reference.Value - second.Value)) { result = second; } return(result); }
/// <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> /// 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); }
/************************************************************/ public SurfaceTile GetTileAt(SiSpeed V, SiDistance D) { foreach (SurfaceTile T in Tiles) { if (T.D.X.Contains(D)) { if (T.V.X.Contains(V)) return T; } } return null; }
static int CompareDistances(SiDistance a, SiDistance b) { if (a.ToUnits() < b.ToUnits()) { return(-1); } else if (a.ToUnits() > b.ToUnits()) { return(1); } return(0); }
/// <summary> /// Finds the distance at which the curve intersects the MRSP speed /// </summary> /// <param name="next_position"></param> /// <param name="speed_limit_here"></param> /// <param name="dir"></param> /// <returns></returns> private static SiDistance IntersectMRSPSpeed(SiDistance next_position, ConstantCurveSegment <SiDistance, SiSpeed> speed_limit_here) { 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; } return(next_position); }
/************************************************************/ public SurfaceTile GetTileAt(SiSpeed V, SiDistance D) { foreach (SurfaceTile T in Tiles) { if (T.D.X.Contains(D)) { if (T.V.X.Contains(V)) { return(T); } } } return(null); }
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()); }
/// <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); }
/// <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(SurfaceTile current_tile, BrakingCurveDirectionEnum dir) { SiDistance result = SiDistance.Zero; switch (dir) { case BrakingCurveDirectionEnum.Backwards: result = current_tile.D.X.X0; break; case BrakingCurveDirectionEnum.Forwards: result = current_tile.D.X.X1; break; } return(result); }
/******************************************************************************************************/ public static List <SurfaceTripple> Collect_And_Sort_Surface(AccelerationSpeedDistanceSurface A_V_D) { List <SurfaceTripple> Tripples = new List <SurfaceTripple>(); /* We stretch the tiles a bit in order to avoid vertical parts in the drawn surface */ SiDistance dx = new SiDistance(1.0); SiSpeed dv = new SiSpeed(1.0, SiSpeed_SubUnits.KiloMeter_per_Hour); for (int i = 0; i < A_V_D.Tiles.Count; i++) { Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X0, A_V_D.Tiles[i].V.X.X0, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X0, A_V_D.Tiles[i].V.X.X1 - dv, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X1 - dx, A_V_D.Tiles[i].V.X.X0, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X1 - dx, A_V_D.Tiles[i].V.X.X1 - dv, A_V_D.Tiles[i].D.Y)); } Tripples.Sort(SurfaceTripple.CompareTripple); return(Tripples); }
/******************************************************************************************************/ protected void Emit_Segment(StreamWriter swd, SiSpeed v0, SiSpeed v1, SiDistance d, SiSpeed v_offset, SiDistance d_offset) { Emit_d_V(swd, d + d_offset, v0 + v_offset); SiSpeed delta = v1 - v0; if (delta > SiSpeed.One) { delta = new SiSpeed(0.5); SiSpeed v = v0 + delta; do { Emit_d_V(swd, d + d_offset, v + v_offset); v += delta; } while (v < v1); } Emit_d_V(swd, d + d_offset, v1 + v_offset); }
/******************************************************************************************************/ public static List<SurfaceTripple> Collect_And_Sort_Surface(AccelerationSpeedDistanceSurface A_V_D) { List<SurfaceTripple> Tripples = new List<SurfaceTripple>(); /* We stretch the tiles a bit in order to avoid vertical parts in the drawn surface */ SiDistance dx = new SiDistance(1.0); SiSpeed dv = new SiSpeed(1.0, SiSpeed_SubUnits.KiloMeter_per_Hour); for (int i = 0; i < A_V_D.Tiles.Count; i++) { Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X0, A_V_D.Tiles[i].V.X.X0, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X0, A_V_D.Tiles[i].V.X.X1 - dv, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X1 - dx, A_V_D.Tiles[i].V.X.X0, A_V_D.Tiles[i].D.Y)); Tripples.Add(new SurfaceTripple(A_V_D.Tiles[i].D.X.X1 - dx, A_V_D.Tiles[i].V.X.X1 - dv, A_V_D.Tiles[i].D.Y)); } Tripples.Sort(SurfaceTripple.CompareTripple); return Tripples; }
/******************************************************************************************************/ protected void Emit_Segment(StreamWriter swd, SiDistance d0, SiDistance d1, SiSpeed v, SiSpeed v_offset, SiDistance d_offset) { Emit_d_V(swd, d0 + d_offset, v + v_offset); SiDistance delta = d1 - d0; if (delta > SiDistance.One) { delta = new SiDistance(1.0); SiDistance d = d0 + delta; do { Emit_d_V(swd, d + d_offset, v + v_offset); d += delta; } while (d < d1); } Emit_d_V(swd, d1 + d_offset, v + v_offset); }
/******************************************************************************************************/ protected void Emit_Constant_Segment(StreamWriter swd, ConstantCurveSegment <SiDistance, SiSpeed> 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); }
/******************************************************************************************************/ 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); }
/// <summary> /// Determines whether a tile conatins the provided speed and distance, based on the direction in which we are looking /// </summary> /// <param name="tile"></param> /// <param name="current_location"></param> /// <param name="current_speed"></param> /// <param name="dir"></param> /// <returns></returns> private static bool TileContains(SurfaceTile tile, SiDistance current_location, SiSpeed current_speed, BrakingCurveDirectionEnum dir) { bool result = false; switch (dir) { case BrakingCurveDirectionEnum.Backwards: result = (tile.D.X.X0.ToUnits() < current_location.ToUnits() && current_location.ToUnits() <= tile.D.X.X1.ToUnits() && tile.V.X.Contains(current_speed)); break; case BrakingCurveDirectionEnum.Forwards: result = (tile.D.X.Contains(current_location) && tile.V.X.X0.ToUnits() < current_speed.ToUnits() && current_speed.ToUnits() <= tile.V.X.X0.ToUnits()); break; } return(result); }
/// <summary> /// Finds the distance at which the curve intersects the MRSP speed /// </summary> /// <param name="next_position"></param> /// <param name="speed_limit_here"></param> /// <param name="dir"></param> /// <returns></returns> private static SiDistance IntersectMRSPSpeed(SiDistance next_position, ConstantCurveSegment<SiDistance, SiSpeed> speed_limit_here) { 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; } return next_position; }
/// <summary> /// Determines whether a tile conatins the provided speed and distance, based on the direction in which we are looking /// </summary> /// <param name="tile"></param> /// <param name="current_location"></param> /// <param name="current_speed"></param> /// <param name="dir"></param> /// <returns></returns> private static bool TileContains(SurfaceTile tile, SiDistance current_location, SiSpeed current_speed, BrakingCurveDirectionEnum dir) { bool result = false; switch (dir) { case BrakingCurveDirectionEnum.Backwards: result = (tile.D.X.X0.ToUnits() < current_location.ToUnits() && current_location.ToUnits() <= tile.D.X.X1.ToUnits() && tile.V.X.Contains(current_speed)); break; case BrakingCurveDirectionEnum.Forwards: result = (tile.D.X.Contains(current_location) && tile.V.X.X0.ToUnits() < current_speed.ToUnits() && current_speed.ToUnits() <= tile.V.X.X0.ToUnits()); break; } return result; }
public SinglePoint(SiDistance anX, SiSpeed anY) { X = anX; Y = anY; }
/******************************************************************************************************/ protected void Emit_d_a(StreamWriter swd, SiDistance d, SiAcceleration a) { swd.WriteLine("{0} {1}", d.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture), a.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture)); }
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); }
public SurfaceTripple(SiDistance oneD, SiSpeed oneV, SiAcceleration oneA) { A = oneA; D = oneD; V = oneV; }
/// <summary> /// Provides the current tile based on the direction we are looking in /// </summary> /// <param name="A_V_D"></param> /// <param name="location"></param> /// <param name="dir"></param> /// <returns></returns> private static SurfaceTile Get_Current_Tile(AccelerationSpeedDistanceSurface A_V_D, SiDistance current_location, SiSpeed current_speed, BrakingCurveDirectionEnum dir) { foreach (SurfaceTile Tile in A_V_D.Tiles) { if (TileContains(Tile, current_location, current_speed, dir)) { return(Tile); } } return(null); }
private static int CompareDistances(SiDistance a, SiDistance b) { if (a.ToUnits() < b.ToUnits()) { return -1; } else if (a.ToUnits() > b.ToUnits()) { return 1; } return 0; }
/// <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; }
public SurfaceTile(SiDistance d0, SiDistance d1, SiSpeed v0, SiSpeed v1, SiAcceleration a) { myD = new ConstantCurveSegment<SiDistance, SiAcceleration>(d0, d1, a); myV = new ConstantCurveSegment<SiSpeed, SiAcceleration>(v0, v1, a); }
/******************************************************************************************************/ protected void Emit_Constant_Segment(StreamWriter swd, ConstantCurveSegment<SiDistance, SiSpeed> 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> /// Creates a graph for the function /// </summary> /// <param name="context"></param> /// <param name="parameter"></param> /// <param name="explain"></param> /// <returns></returns> public override Graph CreateGraph(InterpretationContext context, Parameter parameter, ExplanationPart explain) { Graph retVal = new Graph(); StructureValue LocationStruct = context.FindOnStack(Target).Value as StructureValue; SiDistance location; SiSpeed speed; if (LocationStruct != null) { IVariable locationField = LocationStruct.Val["Location"] as IVariable; location = new SiDistance((locationField.Value as DoubleValue).Val); IVariable speedField = LocationStruct.Val["Speed"] as IVariable; speed = new SiSpeed((speedField.Value as DoubleValue).Val, SiSpeed_SubUnits.KiloMeter_per_Hour); Function decelerationFactor = context.FindOnStack(DecelerationFactor).Value as Function; if (decelerationFactor != null) { Surface DecelerationSurface = decelerationFactor.CreateSurface(context, explain); if (DecelerationSurface != null) { AccelerationSpeedDistanceSurface accelerationSurface = DecelerationSurface.createAccelerationSpeedDistanceSurface(double.MaxValue, double.MaxValue); QuadraticSpeedDistanceCurve BrakingCurve = null; try { BrakingCurve = EtcsBrakingCurveBuilder.Build_Deceleration_Curve(accelerationSurface, speed, location); } catch (Exception e) { retVal.AddSegment(new Graph.Segment(0, double.MaxValue, new Graph.Segment.Curve(0, 0, 0))); } SiSpeed finalSpeed = new SiSpeed(GetDoubleValue(context.FindOnStack(EndSpeed).Value), SiSpeed_SubUnits.KiloMeter_per_Hour); for (int i = 0; i < BrakingCurve.SegmentCount; i++) { QuadraticCurveSegment segment = BrakingCurve[i]; SiSpeed endSpeed = Max(finalSpeed, segment.Get(segment.X.X1)); SiDistance endDistance; if (endSpeed == finalSpeed) { // Ensures that a braking curve is calculated until the finalSpeed // but not further than the end of the curve segment SiSpeed tmp = Max(segment.Get(segment.X.X1), endSpeed - new SiSpeed(0.001)); endDistance = segment.IntersectAt(tmp); } else { endDistance = segment.X.X1; } Graph.Segment newSegment = new Graph.Segment( segment.X.X0.ToUnits(), endDistance.ToUnits(), new Graph.Segment.Curve( segment.A.ToSubUnits(SiAcceleration_SubUnits.Meter_per_SecondSquare), segment.V0.ToSubUnits(SiSpeed_SubUnits.KiloMeter_per_Hour), segment.D0.ToSubUnits(SiDistance_SubUnits.Meter) ) ); retVal.AddSegment(newSegment); if (endSpeed == finalSpeed) { break; } } } } } return(retVal); }
/******************************************************************************************************/ protected void Emit_d_d(StreamWriter swd, SiDistance d1, SiDistance d2) { swd.WriteLine("{0} {1}", d1.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture), d2.ToUnits().ToString(System.Globalization.CultureInfo.InvariantCulture)); }
/// <summary> /// Takes three SiDistances and determines which of the second two is closest to the first one /// </summary> /// <param name="reference">The point the others are compared to.</param> /// <param name="first">The first compared distance.</param> /// <param name="second">The second compared distance.</param> /// <returns></returns> private static SiDistance Closest_To(SiDistance reference, SiDistance first, SiDistance second) { SiDistance result = first; if (Math.Abs(reference.Value - first.Value) > Math.Abs(reference.Value - second.Value)) { result = second; } 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 "); }
/// <summary> /// Provides the current tile based on the direction we are looking in /// </summary> /// <param name="A_V_D"></param> /// <param name="location"></param> /// <param name="dir"></param> /// <returns></returns> private static SurfaceTile Get_Current_Tile(AccelerationSpeedDistanceSurface A_V_D, SiDistance current_location, SiSpeed current_speed, BrakingCurveDirectionEnum dir) { foreach (SurfaceTile Tile in A_V_D.Tiles) { if (TileContains(Tile, current_location, current_speed, dir)) return Tile; } return null; }
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()); }
/// <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); }
/// <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; }
/******************************************************************************************************/ 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(); }
/// <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; }
public SurfaceTile(SiDistance d0, SiDistance d1, SiSpeed v0, SiSpeed v1, SiAcceleration a) { myD = new ConstantCurveSegment <SiDistance, SiAcceleration>(d0, d1, a); myV = new ConstantCurveSegment <SiSpeed, SiAcceleration>(v0, v1, a); }
/// <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> /// Creates a graph for the function /// </summary> /// <param name="context"></param> /// <param name="parameter"></param> /// <param name="explain"></param> /// <returns></returns> public override Graph CreateGraph(InterpretationContext context, Parameter parameter, ExplanationPart explain) { Graph retVal = new Graph(); StructureValue LocationStruct = context.FindOnStack(Target).Value as StructureValue; SiDistance location; SiSpeed speed; if (LocationStruct != null) { IVariable locationField = LocationStruct.Val["Location"] as IVariable; location = new SiDistance((locationField.Value as DoubleValue).Val); IVariable speedField = LocationStruct.Val["Speed"] as IVariable; speed = new SiSpeed((speedField.Value as DoubleValue).Val, SiSpeed_SubUnits.KiloMeter_per_Hour); Function decelerationFactor = context.FindOnStack(DecelerationFactor).Value as Function; if (decelerationFactor != null) { Surface DecelerationSurface = decelerationFactor.CreateSurface(context, explain); if (DecelerationSurface != null) { AccelerationSpeedDistanceSurface accelerationSurface = DecelerationSurface.createAccelerationSpeedDistanceSurface(double.MaxValue, double.MaxValue); QuadraticSpeedDistanceCurve BrakingCurve = null; try { BrakingCurve = EtcsBrakingCurveBuilder.Build_Deceleration_Curve(accelerationSurface, speed, location); } catch (Exception e) { retVal.AddSegment(new Graph.Segment(0, double.MaxValue, new Graph.Segment.Curve(0, 0, 0))); } SiSpeed finalSpeed = new SiSpeed(GetDoubleValue(context.FindOnStack(EndSpeed).Value), SiSpeed_SubUnits.KiloMeter_per_Hour); for (int i = 0; i < BrakingCurve.SegmentCount; i++) { QuadraticCurveSegment segment = BrakingCurve[i]; SiSpeed endSpeed = Max(finalSpeed, segment.Get(segment.X.X1)); SiDistance endDistance; if (endSpeed == finalSpeed) { // Ensures that a braking curve is calculated until the finalSpeed // but not further than the end of the curve segment SiSpeed tmp = Max(segment.Get(segment.X.X1), endSpeed - new SiSpeed(0.001)); endDistance = segment.IntersectAt(tmp); } else { endDistance = segment.X.X1; } Graph.Segment newSegment = new Graph.Segment( segment.X.X0.ToUnits(), endDistance.ToUnits(), new Graph.Segment.Curve( segment.A.ToSubUnits(SiAcceleration_SubUnits.Meter_per_SecondSquare), segment.V0.ToSubUnits(SiSpeed_SubUnits.KiloMeter_per_Hour), segment.D0.ToSubUnits(SiDistance_SubUnits.Meter) ) ); retVal.AddSegment(newSegment); if (endSpeed == finalSpeed) { break; } } } } } return retVal; }