public static WavefrontFormat DrawLine(this WavefrontFormat wf,
string color,
Vec2d point,
Vec2d v,
double ext = 1000)
{
wf.UseMaterial(color);
Vec2d p0 = point.Sub(v.Mul(ext));
Vec2d p1 = point.Add(v.Mul(ext));
wf.AddLines(new Vec2d[] { p0, p1 }, false);
return(wf);
}
public override Vec2d GetFirstDerivative(double t)
{
/*
* s: t * m + n;
* x: p0_x +(p1_x - p0_x)*s;
* y: p0_y +(p1_y - p0_y)*s;
* z: 1;
* result: diff([ x, y, z ], t, 1);
*
* result: [m*(p1_x-p0_x),m*(p1_y-p0_y),0]
*/
double m = this.ttransform.A;
Vec2d v = this.v01;
v = v.Mul(m);
return(v);
}
/// <summary>
/// Calcula el centro de la circunferencia que pasa por los 3 puntos.
/// </summary>
public static Vec2d GetCenter(Vec2d p1, Vec2d p2, Vec2d p3)
{
Vec2d v1 = p2.Sub(p1);
Vec2d v2 = p3.Sub(p1);
Vec2d v3 = p3.Sub(p2);
double l1 = v1.Length2;
double l2 = v2.Length2;
double l3 = v3.Length2;
LenP1P2Dir[] vs =
{
Tuple.Create(l1, p1, p2, v1),
Tuple.Create(l2, p1, p3, v2),
Tuple.Create(l3, p2, p3, v3)
};
Array.Sort(vs, Length2Comparar.Instance);
// Para mejorar el calculo, se toman los segmentos mas alejados.
Vec2d pm_a = vecMath.Interpolate(vs[1].Item2, vs[1].Item3, 0.5);
Vec2d d_a = vecMath.PerpLeft(vs[1].Item4);
Vec2d pm_b = vecMath.Interpolate(vs[2].Item2, vs[2].Item3, 0.5);
Vec2d d_b = vecMath.PerpLeft(vs[2].Item4);
// Se resuelve la ecuacion: d_a * t1 + pm_a = d_b * t2 + pm_b
// Maxima:
// e1: d_ax * t1 + pm_ax = d_bx * t2 + pm_bx;
// e2: d_ay * t1 + pm_ay = d_by * t2 + pm_by;
// algsys ([e1, e2], [t1, t2]);
double div = (d_a.Y * d_b.X - d_a.X * d_b.Y);
if (div.EpsilonEquals(0))
{
throw new Exception("Cálculo imposible");
}
double t1 = (d_b.X * (pm_b.Y - pm_a.Y) - d_b.Y * (pm_b.X - pm_a.X)) / div;
//double t2 = (d_a.X * (pm_b.Y - pm_a.Y) - d_a.Y * (pm_b.X - pm_a.X)) / div;
return(d_a.Mul(t1).Add(pm_a));
}
public override Vec2d 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]
*/
double m = this.ttransform.A;
double dl = this.GetL(t);
Vec2d v = ClothoUtils.DClotho(dl, this.InvertY, this.A);
v = v.Mul(m);
return(this.transform.TransformVector(v));
}
public override Vec2d 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]
*/
double m = this.ttransform.A;
double m3 = m * m * m;
double dl = this.GetL(t);
Vec2d v = ClothoUtils.DClotho3(dl, this.InvertY, this.A);
v = v.Mul(m3);
return(this.transform.TransformVector(v));
}
/// <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);
}
}
}