public static WavefrontFormat DrawClothoRadius(this WavefrontFormat wf,
bool invertY, double a,
string radiusColor)
{
List <int> indices = new List <int>();
double lmax = ClothoUtils.GetMaxL(a);
int c = 100;
for (int i = -c; i <= c; i++)
{
double s = i * lmax / c;
double r = ClothoUtils.ClothoRadius(s, invertY, a);
if (double.IsInfinity(r))
{
r = SysMath.Sign(r) * 1000;
}
int v0 = wf.AddVertex(new Point2d(s, r));
indices.Add(v0);
//double dx, dy;
//MathUtils.DClotho(s, r, a, out dx, out dy);
}
wf.UseMaterial(radiusColor);
wf.AddLines(indices, false);
return(wf);
}
public override Vector2d GetFirstDerivative(double t)
{
/*
* Maxima:
* s: t * m + n;
* x: a*sqrt(%pi)*fresnel_c(s/(a*sqrt(%pi)));
* y: a*sqrt(%pi)*fresnel_s(s/(a*sqrt(%pi)));
* z: 1;
* result: diff([ x, y, z ], t, 1);
*
* result: [m*cos((m*t+n)^2 / (2*a^2)),
* m*sin((m*t+n)^2 / (2*a^2)),
* 0]
*
* m*[cos((m*t+n)^2 / (2*a^2)),
* sin((m*t+n)^2 / (2*a^2)),
* 0]
*
* m*[cos(s^2 / (2*a^2)),
* sin(s^2 / (2*a^2)),
* 0]
*/
double m = this.ttransform.A;
double dl = this.GetL(t);
Vector2d v = ClothoUtils.DClotho(dl, this.InvertY, this.A);
v = v.Mul(m);
return(this.transform.DoTransform(v));
}
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 override Vector2d GetThirdDerivative(double t)
{
/*
* Maxima:
* s: t * m + n;
* x: a*sqrt(%pi)*fresnel_c(s/(a*sqrt(%pi)));
* y: a*sqrt(%pi)*fresnel_s(s/(a*sqrt(%pi)));
* z: 1;
* result: diff([ x, y, z ], t, 3);
*
* result: [-(m^3*sin((m*t+n)^2 / (2*a^2))) / a^2 - (m^3*(m*t+n)^2*cos((m*t+n)^2 / (2*a^2))) / a^4,
* (m^3*cos((m*t+n)^2 / (2*a^2))) / a^2 - (m^3*(m*t+n)^2*sin((m*t+n)^2 / (2*a^2))) / a^4,
* 0]
*
* m^3*[-(sin((m*t+n)^2 / (2*a^2))) / a^2 - ((m*t+n)^2*cos((m*t+n)^2 / (2*a^2))) / a^4,
* (cos((m*t+n)^2 / (2*a^2))) / a^2 - ((m*t+n)^2*sin((m*t+n)^2 / (2*a^2))) / a^4,
* 0]
*
* m^3*[-(sin(s^2 / (2*a^2))) / a^2 - (s^2*cos(s^2 / (2*a^2))) / a^4,
* (cos(s^2 / (2*a^2))) / a^2 - (s^2*sin(s^2 / (2*a^2))) / a^4,
* 0]
*/
double m = this.ttransform.A;
double m3 = m * m * m;
double dl = this.GetL(t);
Vector2d v = ClothoUtils.DClotho3(dl, this.InvertY, this.A);
v = v.Mul(m3);
return(this.transform.DoTransform(v));
}
/// <summary>
/// Resuelve el parametro de la clotoide dado los radios <c>r0</c> y <c>r1</c> Con una distancia <c>d</c> entre los
/// puntos.
/// </summary>
private static double SolveParam(double d, double r0, double r1)
{
Func <double, double> f = (a) =>
{
double fs0, fc0;
ClothoUtils.Fresnel(a / (r0 * sqrtpi), out fs0, out fc0);
double fs1, fc1;
ClothoUtils.Fresnel(a / (r1 * sqrtpi), out fs1, out fc1);
double fc10 = (fc1 - fc0);
double fs10 = (fs1 - fs0);
return(a * a * SysMath.PI * (fc10 * fc10 + fs10 * fs10) - d * d);
};
//UnivariateSolver solver = new BisectionSolver(DEFAULT_ABSOLUTE_ACCURACY);
//UnivariateSolver solver = new SecantSolver(DEFAULT_ABSOLUTE_ACCURACY);
UnivariateSolver solver = new BrentSolver(DEFAULT_ABSOLUTE_ACCURACY);
int maxEval = 50; // 30
try
{
double v = solver.solve(maxEval, new DelegateUnivariateFunction(f), 0, SysMath.Min(SysMath.Abs(r0), SysMath.Abs(r1)) * ClothoUtils.MAX_L);
return(v);
}
catch (TooManyEvaluationsException e)
{
Console.WriteLine(e);
throw;
}
}
public static WavefrontFormat DrawClotho(this WavefrontFormat wf,
bool invertY, double a,
string clothoColor)
{
List <int> indices = new List <int>();
int c = 100;
for (int i = -c; i <= c; i++)
{
double lmax = ClothoUtils.GetMaxL(a);
double s = i * lmax / c;
double x, y;
ClothoUtils.Clotho(s, invertY, a, out x, out y);
int v0 = wf.AddVertex(new Point2d(x, y));
indices.Add(v0);
//double dx, dy;
//MathUtils.DClotho(s, r, a, out dx, out dy);
}
wf.UseMaterial(clothoColor);
wf.AddLines(indices, false);
return(wf);
}
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 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 void Test1()
{
for (double a = 0.5; a < 10; a += 0.5)
{
foreach (bool invertY in new[] { false, true })
{
int ysign = invertY ? -1 : 1;
double[] angles =
{
ysign * 0,
ysign *SysMath.PI / 4,
ysign *SysMath.PI / 2,
ysign * 3 *SysMath.PI / 4
};
foreach (double angle in angles)
{
Vec2d v = vecMath.NewRotate(angle);
double l = ClothoUtils.FindTangent(invertY, a, v);
double l_v2 = ClothoUtils.FindTangent(invertY, a, angle);
// Se comprueba que las busquedas de tangente sean equivalentes.
Assert.IsTrue(l.EpsilonEquals(l_v2));
// Se comprueba que la solucion corresponde con el vector tangente.
Vec2d d_pos = ClothoUtils.DClotho(l, invertY, a);
Assert.IsTrue(d_pos.Cross(v).EpsilonEquals(0));
Vec2d d_neg = ClothoUtils.DClotho(-l, invertY, a);
Assert.IsTrue(d_neg.Cross(v).EpsilonEquals(0));
}
}
}
}
/// <summary>
/// Resuelve el parametro de la clotoide dado los radios <c>r0</c> y <c>r1</c> Con una distancia <c>d</c> entre los
/// puntos.
/// </summary>
private static double SolveParam(double d, double r0, double r1)
{
Func <double, double> f = (a) =>
{
double fs0, fc0;
ClothoUtils.Fresnel(a / (r0 * sqrtpi), out fs0, out fc0);
double fs1, fc1;
ClothoUtils.Fresnel(a / (r1 * sqrtpi), out fs1, out fc1);
double fc10 = (fc1 - fc0);
double fs10 = (fs1 - fs0);
return(a * a * SysMath.PI * (fc10 * fc10 + fs10 * fs10) - d * d);
};
int maxEval = 50; // 30
try
{
double min = 0;
double max = SysMath.Min(SysMath.Abs(r0), SysMath.Abs(r1)) * ClothoUtils.MAX_L;
double v = Solver.Solve(f, min, max, Solver.Type.BrentSolver, DEFAULT_ABSOLUTE_ACCURACY, maxEval);
return(v);
}
catch (Exception e)
{
Debug.WriteLine(e);
throw;
}
}
public void Test1()
{
const double error = 1e-8;
// desarrollo, r, parametro, x, y, c, normal ¿o direccion?,
for (int i = 0; i < this.testData1.Length;)
{
double l = this.testData1[i++];
double r = this.testData1[i++];
double a = this.testData1[i++];
double x = this.testData1[i++];
double y = this.testData1[i++];
double radio = this.testData1[i++];
double tg = this.testData1[i++];
bool invertY = ((l < 0 && r > 0) || (l > 0 && r < 0));
double x2, y2;
ClothoUtils.Clotho(l, invertY, a, out x2, out y2);
double radio2 = ClothoUtils.ClothoRadius(l, invertY, a);
double tg2 = ClothoUtils.ClothoTangent(l, invertY, a);
Assert.IsTrue(x.EpsilonEquals(x2, error));
Assert.IsTrue(y.EpsilonEquals(y2, error));
Assert.IsTrue((double.IsInfinity(radio) && double.IsInfinity(radio2)) || radio.EpsilonEquals(radio2, error));
Assert.IsTrue(AngleUtils.Ensure0To2Pi(tg).EpsilonEquals(AngleUtils.Ensure0To2Pi(tg2), error));
}
}
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)));
}
private double GetL(double t)
{
t = t.Clamp(this.tmin, this.tmax);
double dl = this.ttransform.Get(t);
if (SysMath.Abs(dl) > ClothoUtils.GetMaxL(this.a))
{
throw new Exception("Longitud del arco por encima del máximo permitido.");
}
return(dl);
}
public override Vec2d GetPosition(double t)
{
/*
* Maxima:
* s: t * m + n;
* x: a*sqrt(%pi)*fresnel_c(s/(a*sqrt(%pi)));
* y: a*sqrt(%pi)*fresnel_s(s/(a*sqrt(%pi)));
* z: 1;
* result: [ x, y, z ];
*
* result: [sqrt(%pi)*a*fresnel_c((m*t+n)/(sqrt(%pi)*a)),sqrt(%pi)*a*fresnel_s((m*t+n)/(sqrt(%pi)*a)),1]
*/
double dl = this.GetL(t);
Vec2d pt = ClothoUtils.Clotho(dl, this.InvertY, this.A);
return(this.transform.TransformPoint(pt));
}
public void Test3()
{
int i = 0;
while (i < this.testData3_sc.Length)
{
double v1 = this.testData3_sc[i++];
double s1 = ClothoUtils.FresnelS(v1);
double c1 = ClothoUtils.FresnelC(v1);
double s2 = this.testData3_sc[i++];
double c2 = this.testData3_sc[i++];
Assert.IsTrue(s1.EpsilonEquals(s2));
Assert.IsTrue(c1.EpsilonEquals(c2));
}
}
public override Vec2d GetSecondDerivative(double t)
{
/*
* Maxima:
* s: t * m + n;
* x: a*sqrt(%pi)*fresnel_c(s/(a*sqrt(%pi)));
* y: a*sqrt(%pi)*fresnel_s(s/(a*sqrt(%pi)));
* z: 1;
* result: diff([ x, y, z ], t, 2);
*
* result: [-(m^2*(m*t+n)*sin((m*t+n)^2/(2*a^2)))/a^2,(m^2*(m*t+n)*cos((m*t+n)^2/(2*a^2)))/a^2,0]
*/
double m = this.ttransform.A;
double m2 = m * m;
double dl = this.GetL(t);
Vec2d v = ClothoUtils.DClotho2(dl, this.InvertY, this.A);
v = v.Mul(m2);
return(this.transform.TransformVector(v));
}
public void Test2()
{
double a = 10;
bool invertY = true;
double maxL = ClothoUtils.GetMaxL(a);
int ysign = invertY ? -1 : 1;
double[] angles =
{
ysign * 0,
ysign *SysMath.PI / 4,
ysign *SysMath.PI / 2,
ysign * 3 *SysMath.PI / 4
};
using (MaterialFormat mf = new MaterialFormat(@"C:\Temp\Default.mtl"))
{
mf.DefaultColors();
}
using (WavefrontFormat wf = new WavefrontFormat(@"C:\Temp\ClothoidArc2dTest_Test2.obj"))
{
wf.LoadMaterialLib("Default.mtl");
wf.DrawClotho(invertY, a, "Red");
foreach (double angle in angles)
{
Vec2d v = vecMath.NewRotate(angle);
double l = ClothoUtils.FindTangent(invertY, a, v);
wf.DrawLine("Green", ClothoUtils.Clotho(l, invertY, a), v);
wf.DrawLine("Magenta", ClothoUtils.Clotho(-l, invertY, a), v);
}
//wf.DrawLine("Yellow", new Vec2d(0, 0), new Vec2d(1, 0), 100);
//wf.DrawLine("Yellow", new Vec2d(0, 0), new Vec2d(0, 1), 100);
}
}
public void Test1()
{
const double error = 1e-8;
// desarrollo, r, parametro, x, y, c, normal ¿o direction?,
for (int i = 0; i < this.testData1.Length;)
{
double s = this.testData1[i++];
double r = this.testData1[i++];
double a = this.testData1[i++];
double x = this.testData1[i++];
double y = this.testData1[i++];
double radio = this.testData1[i++];
double tg = this.testData1[i++];
// NOTA: Clip toma las clotoides con s < 0 invertidas en Y.
if (s < 0)
{
y = -y;
tg = 2 * SysMath.PI - tg;
radio = -radio;
}
double x2, y2;
ClothoUtils.Clotho(s, r < 0, a, out x2, out y2);
double radio2 = ClothoUtils.ClothoRadious(s, r < 0, a);
double tg2 = ClothoUtils.ClothoTangent(s, r < 0, a);
double direction2 = ClothoUtils.ClothoTangent(s, r < 0, a);
Assert.IsTrue(x.EpsilonEquals(x2, error));
Assert.IsTrue(y.EpsilonEquals(y2, error));
Assert.IsTrue((double.IsInfinity(radio) && double.IsInfinity(radio2)) || radio.EpsilonEquals(radio2, error));
//Assert.IsTrue(AngleUtils.Ensure0To2Pi(normal).EpsilonEquals(AngleUtils.Ensure0To2Pi(normal2), error));
//Assert.IsTrue(AngleUtils.Ensure0To2Pi(tg).EpsilonEquals(AngleUtils.Ensure0To2Pi(tg2), error));
}
}
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 });
}
public static WavefrontFormat DrawClotho(this WavefrontFormat wf,
bool invertY, double a,
string clothoColor,
string dirColor,
string normalColor,
string radColor)
{
List <int> indices = new List <int>();
List <Tuple <int, int> > normals = new List <Tuple <int, int> >();
List <Tuple <int, int> > dirs = new List <Tuple <int, int> >();
List <Tuple <int, int> > radius = new List <Tuple <int, int> >();
int c = 100;
for (int i = -c; i <= c; i++)
{
double lmax = ClothoUtils.GetMaxL(a);
double s = i * lmax / c;
Point2d xy = ClothoUtils.Clotho(s, invertY, a);
int v0 = wf.AddVertex(xy);
indices.Add(v0);
Vector2d n = ClothoUtils.ClothoLeftNormal(s, invertY, a).Unit;
int v1 = wf.AddVertex(xy.Add(n));
normals.Add(Tuple.Create(v0, v1));
double dir = ClothoUtils.ClothoTangent(s, invertY, a);
Vector2d d = Vector2d.NewRotate(dir);
int v2 = wf.AddVertex(xy.Add(d.Mul(5)));
dirs.Add(Tuple.Create(v0, v2));
double r = ClothoUtils.ClothoRadius(s, invertY, a);
if (double.IsInfinity(r))
{
r = SysMath.Sign(r) * 100;
}
int v3 = wf.AddVertex(xy.Add(n.Mul(r)));
radius.Add(Tuple.Create(v0, v3));
//double dx, dy;
//MathUtils.DClotho(s, r, a, out dx, out dy);
}
wf.UseMaterial(clothoColor);
wf.AddLines(indices, false);
wf.UseMaterial(normalColor);
foreach (Tuple <int, int> normal in normals)
{
wf.AddLines(new[] { normal.Item1, normal.Item2 }, false);
}
wf.UseMaterial(dirColor);
foreach (Tuple <int, int> dir in dirs)
{
wf.AddLines(new[] { dir.Item1, dir.Item2 }, false);
}
wf.UseMaterial(radColor);
foreach (Tuple <int, int> rr in radius)
{
wf.AddLines(new[] { rr.Item1, rr.Item2 }, false);
}
return(wf);
}
/// <summary>
/// Prueba el constructor ClothoidArc2d(double,Vec2d,Vec2d,double,double).
/// </summary>
private static void TestClotho(double a, bool invertY, bool negX, double tg0, double tg1, Vec2d p0, Vec2d 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);
Vec2d pp0 = ClothoUtils.Clotho(l0, invertY, a);
Vec2d pp1 = ClothoUtils.Clotho(l1, invertY, a);
//Vec2d p0 = new Vec2d(5, 5);
Vec2d 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", Vec2d.Zero, new Vec2d(1, 0), 50);
wf.DrawLine("Yellow", Vec2d.Zero, new Vec2d(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);
}
}
}
public double GetRadius(double t)
{
double dl = this.GetL(t);
return(ClothoUtils.ClothoRadious(dl, this.InvertY, this.A));
}
public new double GetTangent(double t)
{
double dl = this.GetL(t);
return(ClothoUtils.ClothoTangent(dl, this.InvertY, this.A));
}
