/* 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>
 ///     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);
 }
        /******************************************************************************************************/
        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);
        }
Beispiel #4
0
        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);
        }
Beispiel #5
0
        /// <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);
        }
Beispiel #6
0
 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>
        ///     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);
        }