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 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 double GetRadius(double t)
{
double dl = this.GetL(t);
return(ClothoUtils.ClothoRadius(dl, this.InvertY, this.A));
}
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,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.ClothoRadius(l0, invertY, a);
double r1 = ClothoUtils.ClothoRadius(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, a);
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);
}
}
}