/// <summary>
/// Crea un arco de circunferencia indicando puntoInicial, puntoFinal, centro, radio y sentido de giro (cw).
/// </summary>
public static CircleArc2 NewArc(Point2d pt1, Point2d pt2, Point2d center, double radius, bool cw)
{
double a1 = pt1.Sub(center).Angle;
double a2 = pt2.Sub(center).Angle;
a1 = AngleUtils.Ensure0To2Pi(a1);
a2 = AngleUtils.Ensure0To2Pi(a2);
if (cw)
{
if (a2 > a1)
{
a2 -= 2 * SysMath.PI;
}
}
else
{
if (a2 < a1)
{
a2 += 2 * SysMath.PI;
}
}
return(new CircleArc2(center, radius, a1, a2));
}
public double Project01(Point2d p)
{
Vector2d diff = p.Sub(this.p0);
//return this.dir.Dot(diff) / this.len;
return(this.dirN.Dot(diff));
}
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));
}
/// <summary>
/// Crea un arco de circunferencia indicando puntoInicial, puntoFinal, centro, radio y sentido de giro (cw).
/// </summary>
public static CircleArc2 NewArc(Point2d pt1, Point2d pt2, Point2d center, double radius, ArcDirection dir)
{
double angle1 = AngleUtils.Ensure0To2Pi(pt1.Sub(center).Angle, false);
double angle2 = AngleUtils.Ensure0To2Pi(pt2.Sub(center).Angle, false);
if (dir == ArcDirection.Clockwise)
{
if (angle2 > angle1)
{
angle2 -= 2.0 * System.Math.PI;
}
}
else if (angle2 < angle1)
{
angle2 += 2.0 * System.Math.PI;
}
if (angle2 > 2.0 * System.Math.PI)
{
angle1 -= 2.0 * System.Math.PI;
angle2 -= 2.0 * System.Math.PI;
}
else if (angle2 < -2.0 * System.Math.PI)
{
angle1 += 2.0 * System.Math.PI;
angle2 += 2.0 * System.Math.PI;
}
return(new CircleArc2(center, radius, angle1, angle2));
}
/// <summary>
/// Indica si los puntos estan alineados.
/// </summary>
/// <param name="pt0">Punto 1.</param>
/// <param name="pt1">Punto 2.</param>
/// <param name="pt2">Punto 3.</param>
/// <returns>
/// Indica si los puntos estan al
/// ineados.
/// </returns>
public static bool AreAligned(Point2d pt0, Point2d pt1, Point2d pt2)
{
Vector2d v13 = pt2.Sub(pt0);
Vector2d v12 = pt1.Sub(pt0);
double area = v13.Cross(v12) / 2;
return(area.EpsilonEquals(0));
}
/// <summary>
/// Calcula el centro de la circunferencia que pasa por los 3 puntos.
/// </summary>
public static Point2d GetCenter(Point2d p1, Point2d p2, Point2d p3)
{
Vector2d v1 = p2.Sub(p1);
Vector2d v2 = p3.Sub(p1);
Vector2d v3 = p3.Sub(p2);
double l1 = v1.LengthSquared;
double l2 = v2.LengthSquared;
double l3 = v3.LengthSquared;
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.
Point2d pm_a = vs[1].Item2.Lerp(vs[1].Item3, 0.5);
Vector2d d_a = vs[1].Item4.PerpLeft;
Point2d pm_b = vs[2].Item2.Lerp(vs[2].Item3, 0.5);
Vector2d d_b = vs[2].Item4.PerpLeft;
// 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(pm_a.Add(d_a.Mul(t1)));
}
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 static WavefrontFormat DrawLine(this WavefrontFormat wf,
string color,
Point2d point,
Vector2d v,
double ext = 1000)
{
wf.UseMaterial(color);
Point2d p0 = point.Sub(v.Mul(ext));
Point2d p1 = point.Add(v.Mul(ext));
wf.AddLines(new Point2d[] { p0, p1 }, false);
return(wf);
}
/// <summary>
/// Obtiene el centro de la circunferencia que pasa por los 2 puntos, con el radio indicado.
/// Dependiendo del criterio (leftRule), se interpreta el signo del radio:
/// Si leftRule entonces el radio se multiplica por la normal a la izquierda (leftNormal) para obtener el centro de la
/// circunferencia.
/// Si rightRule (!leftRule) entonces el radio se multiplica por la normal a la derecha (rightNormal) para obtener el
/// centro de la circunferencia.
/// Clip utiliza un criterio rightRule.
/// <p />
/// Criterio rightRule:
/// <c><![CDATA[
/// _ _
/// _ / \c
/// _/ O pf O
/// _/ / / \
/// / _/ radio - _/ |
/// / /| radio + <-- /| |
/// | / --> / /
/// / / / _/
/// |/ / _/
/// O pi O__/
/// \ _/
/// ]]></c>
/// <![CDATA[
/// p1 a/2 pm a/2 p2
/// x---------+-------->x
/// \ | /
/// \ | /
/// \ | /
/// \ |b /
/// \ | / r
/// \ | /
/// \ | /
/// \ | /
/// \|/
/// pc
/// ]]>
/// Teniendo como dato p1, p2 y r, tenemos que obtener el centro del circulo que pasa por
/// p1 y p2, con radio r.
/// Con el vector 1-2 obtenemos la distancia a.
/// b es calculado mediante la fórmula b = raizcua(r * r + a/2 * a/2).
/// Creamos un vector perpendicular al 1-2 a una distacion a/2 desde p1 y obtenemos
/// el punto central de la circunferencia.
/// Con este dato y conociendo el radio ya simplemente calculamos la esquina del rectangulo.
/// Si el radio es positivo, avanza en sentido contrario a las agujas del reloj.
/// Si el radio es negativo, avanza en sentido de las agujas del reloj.
/// <![CDATA[
/// + p2
/// /|\
/// |
/// /____ | ____\
/// \ \___ | ___/ /
/// \__ | __/
/// \_ | _/
/// radio - \ | / radio +
/// (giro \ | / (giro horario)
/// antihorario) \|/
/// |
/// + p1
/// ]]>
/// </summary>
/// <exception cref="CalculoImposibleException">
/// Indica que no se puede calcular el
/// centro.
/// </exception>
public static Point2d EvaluateCenter(Point2d pt0, Point2d pt1, double radius, bool leftRule)
{
// Vector direccion normalizado y longitud.
Vector2d dir = pt1.Sub(pt0);
double a = dir.Length;
if (a.EpsilonEquals(0))
{
throw new Exception("CalculoImposible: Puntos coincidentes.");
}
dir = dir.Div(a);
// Punto medio.
Point2d pm = pt0.Lerp(pt1, 0.5);
// Se tratan radios erroneos que no permitan generar un circunferencia.
double v = radius * radius - a * a / 4;
if (v.EpsilonEquals(0))
{
return(pm);
}
if (v < 0)
{
throw new Exception("CalculoImposible: Radio erroneo.");
}
double b = SysMath.Sign(radius) * SysMath.Sqrt(v);
Vector2d normal;
if (leftRule)
{
// Vector normal a la izquierda.
normal = dir.PerpLeft;
}
else
{
// Vector normal a la derecha.
normal = dir.PerpLeft;
}
Vector2d vectorm1 = normal.Mul(b);
return(pm.Add(vectorm1));
}
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 void TestPointInPolyNonZero_Extended_Robust()
{
Circle2 c = new Circle2(new Point2d(0, 0), 5);
double tmin = c.TMin;
double tmax = c.TMax;
double tinc = (tmax - tmin) / 5;
Point2d p0 = c.GetPosition(tmin);
Point2d p1 = c.GetPosition(tmin + tinc);
Point2d p2 = c.GetPosition(tmin + 2 * tinc);
Point2d p3 = c.GetPosition(tmin + 3 * tinc);
Point2d p4 = c.GetPosition(tmin + 4 * tinc);
IList <Point2d> points = DuplicatePoints(AsList(p0, p2, p4, p1, p3));
PointInPoly pip1 = PolygonUtils.PointInPolyNonZero(points, new Point2d(4, 0), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip1);
PointInPoly pip2 = PolygonUtils.PointInPolyNonZero(points, new Point2d(-2, 2), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip2);
PointInPoly pip3 = PolygonUtils.PointInPolyNonZero(points, new Point2d(-2, -2), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip3);
PointInPoly pip4 = PolygonUtils.PointInPolyNonZero(points, new Point2d(0, 0), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip4);
PointInPoly pip5 = PolygonUtils.PointInPolyNonZero(points, new Point2d(10, 0), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Outside, pip5);
PointInPoly pip6 = PolygonUtils.PointInPolyNonZero(points, new Point2d(4, 3), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Outside, pip6);
PointInPoly pip7 = PolygonUtils.PointInPolyNonZero(points, new Point2d(4, -3), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Outside, pip7);
PointInPoly pip8 = PolygonUtils.PointInPolyNonZero(points, new Point2d(1, 1), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip8);
PointInPoly pip9 = PolygonUtils.PointInPolyNonZero(points, new Point2d(1, -1), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.Inside, pip9);
PointInPoly pip10 = PolygonUtils.PointInPolyNonZero(points, p4.Add(p1.Sub(p4).Unit.Mul(0.7)), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.On, pip10);
PointInPoly pip11 = PolygonUtils.PointInPolyNonZero(points, p4.Add(p1.Sub(p4).Unit.Mul(0.3)), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.On, pip11);
PointInPoly pip12 = PolygonUtils.PointInPolyNonZero(points, p1.Add(p3.Sub(p1).Unit.Mul(0.7)), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.On, pip12);
PointInPoly pip13 = PolygonUtils.PointInPolyNonZero(points, p1.Add(p3.Sub(p1).Unit.Mul(0.3)), true, true, MathUtils.EPSILON);
Assert.AreEqual(PointInPoly.On, pip13);
}
/**
* This method projects the point <code>p</code> on the segment <code>p0:0 to p1:length</code> and return the parameter t.
*
* @param p Point.
* @return Parameter.
*/
public double Project0L(Point2d p)
{
Vector2d diff = p.Sub(this.p0);
return(this.directionUnit.Dot(diff));
}
/// <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);
}
}
}
/// <summary>
/// Arco que pasa por los puntos <c>pinicial</c>, <c>ppaso</c>, <c>pfinal</c>.
/// </summary>
/// <exception cref="PuntosAlineadosException">
/// Indica que los puntos estan
/// alineados.
/// </exception>
public static CircleArc2 ThreePoints(Point2d p1, Point2d pp, Point2d p2)
{
Point2d pcenter = GetCenter(p1, pp, p2);
double radius = pcenter.DistanceTo(p1);
Vector2d v1 = p1.Sub(pcenter);
Vector2d vp = pp.Sub(pcenter);
Vector2d v2 = p2.Sub(pcenter);
double alpha1 = v1.Angle;
double alphap = vp.Angle;
double alpha2 = v2.Angle;
{
// Se prueba el sentido CCW, alpha1 < alphap < alpha2
double a1 = alpha1;
double ap = alphap;
double a2 = alpha2;
while (ap < a1)
{
ap += 2 * SysMath.PI;
}
while (a2 < ap)
{
a2 += 2 * SysMath.PI;
}
if (a2 - a1 > 2 * SysMath.PI)
{
// Se ha dado mas de una vuelta, solucion no valida.
}
else
{
return(NewArcWithAdvance(pcenter, radius, AngleUtils.Ensure0To2Pi(a1), a2 - a1));
}
}
{
// Se prueba el sentido CW, alpha1 > alphap > alpha2
double a1 = alpha1;
double ap = alphap;
double a2 = alpha2;
while (ap > a1)
{
ap -= 2 * SysMath.PI;
}
while (a2 > ap)
{
a2 -= 2 * SysMath.PI;
}
if (a1 - a2 > 2 * SysMath.PI)
{
// Se ha dado mas de una vuelta, solucion no valida.
}
else
{
return(NewArcWithAdvance(pcenter, radius, AngleUtils.Ensure0To2Pi(a1), a2 - a1));
}
}
throw new Exception();
#if false
double alpha2 = vecMath.Angle(v1, v2);
alpha1 = AngleUtils.Ensure0To2Pi(alpha1);
// Existen 2 soluciones que dependen de angulo de pp:
// si alpha2 > 0 : alpha2 y alpha2 - 2*PI
// si alpha2 < 0 : alpha2 y alpha2 + 2*PI
if (alpha2 > 0)
{
double alpha3 = vecMath.Angle(pp);
}
else
{
}
return(NewArcWithAdvance(pcenter, radius, alpha1, alpha2));
#endif
}