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