public static ClothoidArc2 BuildSimple(double radius0, double radius1, double a, ITransform2 transform)
{
// 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);
}
}
// Desarrollo segun el radio en el punto (0) y (1).
double l0 = ClothoUtils.ClothoL(radius0, false, a);
double l1 = ClothoUtils.ClothoL(radius1, false, a);
ClothoidArc2 clotho = new ClothoidArc2(transform, l0, l1, false, a);
clotho.SetTInterval(0, Math.Abs(l1 - l0));
return(clotho);
}
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 tmin,
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.
ITransform2 transform = Transform2.Translate(point1.X - p1_n.X, point1.Y - p1_n.Y)
.Concat(Transform2.Rotate(p1_n.X, p1_n.Y, r));
double l0 = l0_n;
double l1 = l1_n;
ClothoidArc2 clotho = new ClothoidArc2(transform, l0, l1, invertY, a);
clotho.SetTInterval(tmin, tmin + Math.Abs(l1 - l0));
return(clotho);
}
/// <summary>
/// Prueba el constructor ClothoidArc2d(double,Point2d,Point2d,double,double).
/// </summary>
private static void TestClotho(double a, bool invertY, bool negX, double tg0, double tg1, Point2d p0, Vector2d dir,
string fileName = null, bool toWavefront = false)
{
//double a = 5;
//bool invertY = true;
//double tg0 = -4 * SysMath.PI / 10;
//double tg1 = -SysMath.PI / 10;
// Si se indica negX, se invierten las tangentes.
int sign = 1;
if (negX)
{
sign = -1;
}
double l0 = sign * ClothoUtils.FindTangent(invertY, a, tg0);
double l1 = sign * ClothoUtils.FindTangent(invertY, a, tg1);
double r0 = ClothoUtils.ClothoRadious(l0, invertY, a);
double r1 = ClothoUtils.ClothoRadious(l1, invertY, a);
Point2d pp0 = ClothoUtils.Clotho(l0, invertY, a);
Point2d pp1 = ClothoUtils.Clotho(l1, invertY, a);
//Point2d p0 = new Point2d(5, 5);
Point2d p1 = p0.Add(dir.Mul(pp1.Sub(pp0).Length));
ClothoidArc2 arc = new ClothoidArc2(l0, p0, p1, r0, r1);
Assert.IsTrue(arc.Point0.EpsilonEquals(p0, ERROR));
Assert.IsTrue(arc.Point1.EpsilonEquals(p1, ERROR));
Assert.IsTrue(arc.GetRadius(arc.TMin).EpsilonEquals(r0, ERROR));
Assert.IsTrue(arc.GetRadius(arc.TMax).EpsilonEquals(r1, ERROR)); // <-
Assert.IsTrue(arc.InvertY == invertY);
Assert.IsTrue(arc.A.EpsilonEquals(a, ERROR));
// Salida por fichero de la prueba.
if ((fileName != null) && toWavefront)
{
double figSize = 0.5;
string mtl = Path.ChangeExtension(fileName, "mtl");
using (MaterialFormat mf = new MaterialFormat(mtl))
{
mf.DefaultColors();
}
using (WavefrontFormat wf = new WavefrontFormat(fileName))
{
wf.LoadMaterialLib(Path.GetFileName(mtl));
wf.DrawLine("Yellow", Point2d.Zero, new Point2d(1, 0), 50);
wf.DrawLine("Yellow", Point2d.Zero, new Point2d(0, 1), 50);
wf.DrawFigure("Blue", WaveFigure.X, ClothoUtils.Clotho(l0, invertY, a), figSize);
wf.DrawFigure("Olive", WaveFigure.X, ClothoUtils.Clotho(l1, invertY, a), figSize);
wf.DrawClotho(invertY, a, "Red");
wf.DrawClotho("Magenta", arc);
wf.DrawFigure("Blue", WaveFigure.X, p0, figSize);
wf.DrawFigure("Olive", WaveFigure.X, p1, figSize);
}
}
}
