private static void RefineIntersectionPoint(Ellipse2D e1, Ellipse2D e2, ref GeoPoint2DWithParameter ip)
{ // hier mir Param arbeiten, da es sich auch um einen Ellipsenbogen handeln kann
double par1 = e1.ParamOf(ip.p);
double par2 = e2.ParamOf(ip.p);
GeoPoint2D p1 = e1.PointAtParam(par1);
GeoPoint2D p2 = e2.PointAtParam(par2);
int counter = 0;
while (!Precision.IsEqual(p1, p2))
{
GeoVector2D d1 = e1.DirectionAtParam(par1);
GeoVector2D d2 = e2.DirectionAtParam(par2);
GeoPoint2D p;
if (Geometry.IntersectLL(p1, d1, p2, d2, out p))
{
par1 = e1.ParamOf(p);
par2 = e2.ParamOf(p);
p1 = e1.PointAtParam(par1);
p2 = e2.PointAtParam(par2);
ip.p = p;
}
else
{
break;
}
++counter;
if (counter > 100)
{
break;
}
}
ip.par1 = e1.PositionOf(ip.p); // richtige Werte auch für Arc
ip.par2 = e2.PositionOf(ip.p);
}
public static Ellipse2D FromFivePoints(GeoPoint2D[] p, bool isFull)
{
Ellipse2D res = FromFivePoints(p);
if (res != null)
{
double mindist = double.MaxValue;
double p0 = res.PositionOf(p[0]);
for (int i = 1; i < 5; i++)
{
double p1 = res.PositionOf(p[i]);
if (Math.Abs(p1 - p0) < Math.Abs(mindist))
{
mindist = p1 - p0;
}
p0 = p1;
}
if (mindist < 0)
{
res.Reverse();
}
if (!isFull)
{
double sp = res.PositionOf(p[0]);
double ep = res.PositionOf(p[4]);
double mp = res.PositionOf(p[2]);
if (sp < ep)
{
if (sp < mp && mp < ep)
{
res = res.Trim(sp, ep) as Ellipse2D;
}
else
{
res = res.Trim(ep, sp) as Ellipse2D;
}
}
else
{ // to be tested!
if (sp < mp && mp < ep)
{
res = res.Trim(ep, sp) as Ellipse2D;
}
else
{
res = res.Trim(sp, ep) as Ellipse2D;
}
}
}
}
return(res);
}
/// <summary>
/// Overrides <see cref="CADability.Curve2D.GeneralCurve2D.Intersect (ICurve2D)"/>
/// </summary>
/// <param name="IntersectWith"></param>
/// <returns></returns>
public override GeoPoint2DWithParameter[] Intersect(ICurve2D IntersectWith)
{ // gesucht sind alle Schnittpunkte, also auch in der Verlängerung!
Line2D l2d = IntersectWith as Line2D;
if (l2d != null)
{
GeoPoint2D ip;
if (Geometry.IntersectLL(startPoint, endPoint, l2d.StartPoint, l2d.EndPoint, out ip))
{
double pos1 = this.PositionOf(ip);
double pos2 = l2d.PositionOf(ip);
GeoPoint2DWithParameter pwp = new GeoPoint2DWithParameter();
pwp.p = ip;
pwp.par1 = pos1;
pwp.par2 = pos2;
return(new GeoPoint2DWithParameter[] { pwp });
}
else
{
return(new GeoPoint2DWithParameter[0]);
}
}
Circle2D c2d = IntersectWith as Circle2D;
if (c2d != null)
{
GeoPoint2D[] isp = Geometry.IntersectLC(startPoint, endPoint, c2d.Center, c2d.Radius);
GeoPoint2DWithParameter[] res = new GeoPoint2DWithParameter[isp.Length];
for (int i = 0; i < isp.Length; ++i)
{
res[i].p = isp[i];
res[i].par1 = this.PositionOf(isp[i]);
res[i].par2 = c2d.PositionOf(isp[i]);
}
return(res);
}
Ellipse2D e2d = IntersectWith as Ellipse2D;
if (e2d != null)
{
GeoPoint2D[] isp = Geometry.IntersectEL(e2d.center, e2d.majorAxis.Length, e2d.minorAxis.Length, e2d.majorAxis.Angle, startPoint, endPoint);
GeoPoint2DWithParameter[] res = new GeoPoint2DWithParameter[isp.Length];
for (int i = 0; i < isp.Length; ++i)
{
res[i].p = isp[i];
res[i].par1 = this.PositionOf(isp[i]);
res[i].par2 = e2d.PositionOf(isp[i]);
}
return(res);
}
Polyline2D p2d = IntersectWith as Polyline2D;
if (p2d != null)
{
GeoPoint2DWithParameter[] res = p2d.Intersect(this); // sorum geht es
for (int i = 0; i < res.Length; ++i)
{
double t = res[i].par1;
res[i].par1 = res[i].par2;
res[i].par2 = t;
}
return(res);
}
Path2D pa2d = IntersectWith as Path2D;
if (pa2d != null)
{
GeoPoint2DWithParameter[] res = pa2d.Intersect(this); // sorum geht es
for (int i = 0; i < res.Length; ++i)
{
double t = res[i].par1;
res[i].par1 = res[i].par2;
res[i].par2 = t;
}
return(res);
}
BSpline2D b2d = IntersectWith as BSpline2D;
if (b2d != null)
{
GeoPoint2DWithParameter[] res = b2d.Intersect(this); // sorum geht es
for (int i = 0; i < res.Length; ++i)
{
double t = res[i].par1;
res[i].par1 = res[i].par2;
res[i].par2 = t;
}
return(res);
}
return(base.Intersect(IntersectWith));
}
/// <summary>
/// Overrides <see cref="CADability.Curve2D.GeneralCurve2D.Intersect (ICurve2D)"/>
/// </summary>
/// <param name="IntersectWith"></param>
/// <returns></returns>
public override GeoPoint2DWithParameter[] Intersect(ICurve2D IntersectWith)
{ // gesucht sind alle Schnittpunkte, also auch in der Verlängerung!
Line2D l2d = IntersectWith as Line2D;
if (l2d != null)
{
GeoPoint2DWithParameter[] res = l2d.Intersect(this); // die Linie kanns
// aber noch die Rollen vertauschen
for (int i = 0; i < res.Length; ++i)
{ // par1 und par2 vertauschen
double tmp = res[i].par1;
res[i].par1 = res[i].par2;
res[i].par2 = tmp;
}
return(res);
}
else if (IntersectWith is Circle2D) // geht auch für Arc2D
{
GeoPoint2D[] ip = Geometry.IntersectEC(center, majrad, minrad, majorAxis.Angle, (IntersectWith as Circle2D).Center, (IntersectWith as Circle2D).Radius);
GeoPoint2DWithParameter[] res = new GeoPoint2DWithParameter[ip.Length];
double error = 0.0;
for (int i = 0; i < ip.Length; ++i)
{
GeoPoint2D p0 = toUnitCircle * ip[i];
error += Math.Abs(1.0 - p0.ToVector().Length);
error += Math.Abs(1.0 - (ip[i] | (IntersectWith as Circle2D).Center) / (IntersectWith as Circle2D).Radius);
res[i].p = ip[i];
res[i].par1 = PositionOf(ip[i]);
res[i].par2 = IntersectWith.PositionOf(ip[i]); // liefert auch für Arc2D den richtigen Wert
}
if (error < 1e-7) // auf Radius 1 normierter fehler
{ // wenn nicht, dann unten mit dreiecksschnitten lösen
return(res);
}
}
else if (IntersectWith is Ellipse2D) // geht auch für EllipseArc2D
{
Ellipse2D elli = (IntersectWith as Ellipse2D);
Ellipse2D ellinorm = this.GetModified(elli.toUnitCircle) as Ellipse2D;
GeoPoint2D[] ip = Geometry.IntersectEC(ellinorm.center, ellinorm.majrad, ellinorm.minrad, ellinorm.majorAxis.Angle, elli.toUnitCircle * elli.center, 1.0);
GeoPoint2DWithParameter[] res = new GeoPoint2DWithParameter[ip.Length];
for (int i = 0; i < ip.Length; ++i)
{
res[i].p = elli.fromUnitCircle * ip[i];
res[i].par1 = PositionOf(res[i].p);
res[i].par2 = IntersectWith.PositionOf(res[i].p);
}
for (int i = 0; i < res.Length; i++)
{
RefineIntersectionPoint(this, elli, ref res[i]);
}
if (((ellinorm.Center | GeoPoint2D.Origin) < Precision.eps) && res.Length == 0)
{ // gleicher Mittelpunkt und keine Lösung, könnte zusammenhängender Bogen zweier identischen Ellipsen sein
bool connected = false;
GeoPoint2D p = GeoPoint2D.Origin;
double par0 = 0.0;
double par1 = 0.0;
if ((this.EndPoint | elli.StartPoint) < Precision.eps)
{
p = this.EndPoint;
par0 = 1.0;
par1 = 0.0;
connected = true;
}
if ((this.EndPoint | elli.EndPoint) < Precision.eps)
{
p = this.EndPoint;
par0 = 1.0;
par1 = 1.0;
connected = true;
}
if ((this.StartPoint | elli.StartPoint) < Precision.eps)
{
p = this.StartPoint;
par0 = 0.0;
par1 = 0.0;
connected = true;
}
if ((this.StartPoint | elli.EndPoint) < Precision.eps)
{
p = this.StartPoint;
par0 = 0.0;
par1 = 1.0;
connected = true;
}
if (connected)
{
res = new GeoPoint2DWithParameter[1];
res[0] = new GeoPoint2DWithParameter(p, par0, par1);
}
}
return(res);
}
// auch wenn die Ellipsenschnittpunkte schlecht waren
return(base.Intersect(IntersectWith));
}
public static Ellipse2D FromFivePoints(GeoPoint2D[] p)
{
Matrix m = new DenseMatrix(5, 5);
Vector b = new DenseVector(5);
for (int i = 0; i < 5; ++i)
{
m[i, 0] = 1;
m[i, 1] = 2 * p[i].x;
m[i, 2] = 2 * p[i].y;
m[i, 3] = 2 * p[i].x * p[i].y;
m[i, 4] = p[i].y * p[i].y;
b[i] = -p[i].x * p[i].x;
}
Vector x = (Vector)m.Solve(b);
if (!x.IsValid())
{
return(null);
}
double l1, l2;
if (Geometry.quadgl(1, -(1 + x[4]), x[4] - x[3] * x[3], out l1, out l2) == 0)
{
l1 = l2 = (1 + x[4]) / 2.0;
}
if (l1 == 0.0 || l2 == 0.0)
{
return(null);
}
Angle MajorAngle;
if (Math.Abs(l1 - 1) > Math.Abs(l2 - 1))
{
MajorAngle = Math.Atan2(l1 - 1, x[3]);
}
else
{
MajorAngle = Math.Atan2(l2 - 1, x[3]) + Math.PI / 2.0;
}
double b1 = x[1] * Math.Cos(MajorAngle) + x[2] * Math.Sin(MajorAngle);
double b2 = -x[1] * Math.Sin(MajorAngle) + x[2] * Math.Cos(MajorAngle);
double MajorRadius = Math.Sqrt(Math.Abs((b1 * b1 / l1 + b2 * b2 / l2 - x[0]) / l1));
double MinorRadius = Math.Sqrt(Math.Abs((b1 * b1 / l1 + b2 * b2 / l2 - x[0]) / l2));
Matrix a = new DenseMatrix(2, 2);
Vector c = new DenseVector(2);
a[0, 0] = 1;
a[1, 0] = a[0, 1] = x[3];
a[1, 1] = x[4];
c[0] = -x[1];
c[1] = -x[2];
Vector mp = (Vector)a.Solve(c);
if (!mp.IsValid())
{
return(null);
}
//{ // ein Test, ob die Lösung auch stimmt. Es gibt Lösungen
// // die wohl eher Hyperbeln oder so was liefern und trotzdem alle
// // durch die Gleichungen gegebenen Bedingungen erfüllen
// if (MajorRadius < 1e-13) return null;
// if (MinorRadius < 1e-13) return null;
// double s = Math.Sin(MajorAngle);
// double cos = Math.Cos(MajorAngle);
// ModOp2D tuc = new ModOp2D(cos / MajorRadius, s / MajorRadius, (-cos * mp[0, 0] - s * mp[1, 0]) / MajorRadius,
// -s / MinorRadius, cos / MinorRadius, (s * mp[0, 0] - cos * mp[1, 0]) / MinorRadius);
// // bildet auf den Einheitskreis ab
// for (int i = 0; i < 5; ++i)
// {
// GeoPoint2D pp = tuc * p[i];
// double e = Math.Abs(pp.x * pp.x + pp.y * pp.y - 1.0);
// if (e > 1e-5) return null;
// }
//}
// hier ist nun alles bestimmt:
Angle MinorAngle = MajorAngle + SweepAngle.ToLeft;
Ellipse2D res = new Ellipse2D(new GeoPoint2D(mp[0], mp[1]), MajorRadius * new GeoVector2D(MajorAngle), MinorRadius * new GeoVector2D(MinorAngle));
return(res);
}
