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));
}
