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