public ClothoidArc2(double tmin, // It is only needed as start station Point2d point0, Point2d 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 { // 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; } 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. 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); Vector2d v01 = point1.Sub(point0); // 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(tmin, tmin + Math.Abs(this.l1 - this.l0)); }
public static ITransform2 EvaluateTransform(Point2d point0, Point2d 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 { // No se permite cambio de signo en el radio. Funcionaria??? Contract.Assert(false); } } bool invertY; if (SysMath.Abs(radius0) > SysMath.Abs(radius1)) { // t positivas invertY = radius1 < 0; } else { // t negativa invertY = radius1 > 0; } // Diferencia de puntos en coordenadas reales. Vector2d v01 = point1.Sub(point0); // Desarrollo segun el radio en el punto (0) y (1). double l0_n = ClothoUtils.ClothoL(radius0, invertY, a); double l1_n = ClothoUtils.ClothoL(radius1, invertY, a); // Coordenadas en el punto (0) y (1) para una clotoide normalizada. Point2d p0_n = ClothoUtils.Clotho(l0_n, invertY, a); Point2d p1_n = ClothoUtils.Clotho(l1_n, invertY, a); Vector2d v01_n = p1_n.Sub(p0_n); // Rotacion de v01_n -> v01. double r = v01_n.AngleTo(v01); // Transformacion a aplicar. return(Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y) .Concat(Transform2.Rotate(p1_n.X, p1_n.Y, r))); }
public static ClothoidArc2[] Split(double tmin, Point2d point0, Point2d 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. Point2d p0_n = ClothoUtils.Clotho(l0_n, invertY, a); Point2d p1_n = ClothoUtils.Clotho(l1_n, invertY, a); // Diferencia de puntos en coordenadas reales. Vector2d v01 = point1.Sub(point0); Vector2d v01_n = p1_n.Sub(p0_n); // Rotacion de v01_n -> v01. double r = v01_n.AngleTo(v01); // Transformacion a aplicar. ITransform2 transform = Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y) .Concat(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(tmin, tmin + (-l0_n)); // l0_n < 0 right.SetTInterval(tmin + (-l0_n), tmin + (-l0_n) + l1_n); return(new[] { left, right }); }
public static ClothoidArc2 BuildSimple(double radius0, double radius1, double a) { return(BuildSimple(radius0, radius1, a, Transform2.Identity())); }
public Transform2 Concat(Transform2 transform) { return((Transform2)this.Concat((ITransform2)transform)); }