Example #1
0
        public ClothoidArc2(double l0,
                            Vec2d point0, Vec2d point1,
                            double radius0, double radius1,
                            double a)
        {
            // Correccion sobre los radios.
            if (SysMath.Sign(radius1) != SysMath.Sign(radius0))
            {
                if (double.IsInfinity(radius0))
                {
                    radius0 = SysMath.Sign(radius1) * double.PositiveInfinity;
                }
                else if (double.IsInfinity(radius1))
                {
                    radius1 = SysMath.Sign(radius0) * double.PositiveInfinity;
                }
                else
                {
                    // Se deja tal cual.
                    //// Se toma el radio inicio como infinito.
                    ////radius0 = SysMath.Sign(radius1) * double.PositiveInfinity;
                }
            }

            if (SysMath.Abs(radius0) > SysMath.Abs(radius1))
            { // t positivas
                this.invertY = radius1 < 0;
            }
            else
            { // t negativa
                this.invertY = radius1 > 0;
            }

            // Diferencia de puntos en coordenadas reales.
            Vec2d v01 = point1.Sub(point0);

            this.a = a;

            // Desarrollo segun el radio en el punto (0) y (1).
            double l0_n = ClothoUtils.ClothoL(radius0, this.invertY, this.a);
            double l1_n = ClothoUtils.ClothoL(radius1, this.invertY, this.a);

            // Coordenadas en el punto (0) y (1) para una clotoide normalizada.
            Vec2d p0_n = ClothoUtils.Clotho(l0_n, this.invertY, this.a);
            Vec2d p1_n = ClothoUtils.Clotho(l1_n, this.invertY, this.a);

            Vec2d v01_n = p1_n.Sub(p0_n);

            // Rotacion de v01_n -> v01.
            double r = vecMath.Angle(v01_n, v01);

            // Transformacion a aplicar.
            this.transform = Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y)
                             .Mult(Transform2.Rotate(p1_n.X, p1_n.Y, r));

            this.l0 = l0_n;
            this.l1 = l1_n;

            this.SetTInterval(l0, l0 + (this.l1 - this.l0));
        }
Example #2
0
        public ClothoidArc2(double l0,
                            Vector2d point0, Vector2d point1,
                            double radius0, double radius1)
        {
            // Correccion sobre los radios.
            if (SysMath.Sign(radius1) != SysMath.Sign(radius0))
            {
                if (double.IsInfinity(radius0))
                {
                    radius0 = SysMath.Sign(radius1) * double.PositiveInfinity;
                }
                else if (double.IsInfinity(radius1))
                {
                    radius1 = SysMath.Sign(radius0) * double.PositiveInfinity;
                }
                else
                {
                    // No se permite cambio de signo en el radio. Funcionaria???
                    Contract.Assert(false);
                }
            }

            if (SysMath.Abs(radius0) > SysMath.Abs(radius1))
            {
                // t positivas
                this.InvertY = radius1 < 0;
            }
            else
            {
                // t negativa
                this.InvertY = radius1 > 0;
            }

            // Diferencia de puntos en coordenadas reales.
            Vector2d v01 = point1.Sub(point0);

            this.A = SolveParam(v01.Length, radius0, radius1);

            // Desarrollo segun el radio en el punto (0) y (1).
            double l0_n = ClothoUtils.ClothoL(radius0, this.InvertY, this.A);
            double l1_n = ClothoUtils.ClothoL(radius1, this.InvertY, this.A);

            // Coordenadas en el punto (0) y (1) para una clotoide normalizada.
            Point2d p0_n = ClothoUtils.Clotho(l0_n, this.InvertY, this.A);
            Point2d p1_n = ClothoUtils.Clotho(l1_n, this.InvertY, this.A);

            Vector2d v01_n = p1_n.Sub(p0_n);

            // Rotacion de v01_n -> v01.
            double r = v01_n.AngleTo(v01);

            // Transformacion a aplicar.
            this.transform = Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y)
                             .Concat(Transform2.Rotate(p1_n.X, p1_n.Y, r));

            this.l0 = l0_n;
            this.l1 = l1_n;

            this.SetTInterval(l0, l0 + (this.l1 - this.l0));
        }
Example #3
0
        public static ClothoidArc2[] Split(double l0,
                                           Vec2d point0, Vec2d point1,
                                           double radius0, double radius1,
                                           double a)
        {
            bool invertY = (radius1 < 0);

            double l0_n = ClothoUtils.ClothoL(radius0, invertY, a);
            double l1_n = ClothoUtils.ClothoL(radius1, invertY, a);

            Contract.Assert(l0_n < 0 && l1_n > 0);

            // Coordenadas en el punto (0) y (1) para una clotoide normalizada.
            Vec2d p0_n = ClothoUtils.Clotho(l0_n, invertY, a);
            Vec2d p1_n = ClothoUtils.Clotho(l1_n, invertY, a);

            // Diferencia de puntos en coordenadas reales.
            Vec2d v01 = point1.Sub(point0);

            Vec2d v01_n = p1_n.Sub(p0_n);

            // Rotacion de v01_n -> v01.
            double r = vecMath.Angle(v01_n, v01);

            // Transformacion a aplicar.
            Transform2 transform = Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y)
                                   .Mult(Transform2.Rotate(p1_n.X, p1_n.Y, r));

            ClothoidArc2 left  = new ClothoidArc2(transform, l0_n, 0, invertY, a);
            ClothoidArc2 right = new ClothoidArc2(transform, 0, l1_n, invertY, a);

            left.SetTInterval(l0, l0 + (-l0_n)); // l0_n < 0
            right.SetTInterval(l0 + (-l0_n), l0 + (-l0_n) + l1_n);

            return(new[] { left, right });
        }