public static Circle2d[] LineLine(Line2d lin1, Line2d lin2, double radius)
{
// See page 39 in 'a programmers geometry' for explanation
double a1, b1, c1, a2, b2, c2;
if (!lin1.ToEquation(out a1, out b1, out c1))
{
return(LinePoint(lin2, lin1.MidPoint, radius)); //degenerate line 1 (if lin2 is degenerate as well, this is handled in LinePoint)
}
if (!lin2.ToEquation(out a2, out b2, out c2))
{
return(LinePoint(lin1, lin2.MidPoint, radius)); //degenerate line 2
}
double denom = a2 * b1 - a1 * b2;
if (MathUtil.IsZero(denom))
{
return(null); //parallel lines
}
Circle2d[] res = new Circle2d[4];
for (int l = 0; l < 4; l++)
{
// note: this computation assumes that the line equation ax+by+c is normalized so that Math.Sqrt(a1 * a1 + b1 * b1) equals 1 which simplifies the computation
int sign1, sign2;
sign1 = ((l & 1) == 0) ? 1:-1;
sign2 = ((l & 2) == 0) ? 1:-1;
double x = (b2 * (c1 + radius * sign1) - b1 * (c2 + radius * sign2)) / denom;
double y = (a2 * (c1 + radius * sign1) - a1 * (c2 + radius * sign2)) / -denom;
res[l] = new Circle2d(x, y, radius);
}
return(res.ToArray());
}
public static Circle2d[] LinePoint(Line2d lin, Point2d pnt, double radius)
{
//see 'a programmers geometry' page 36 for explanation
double a, b, c;
if (!lin.ToEquation(out a, out b, out c))
{
return(PointPoint(lin.MidPoint, pnt, radius)); //degenerate line
}
c = c + a * pnt.X + b * pnt.Y; //new c for line equation corresponding to pnt beeing transformed to origo
if (c < 0)
{
a = -a; b = -b; c = -c;
} //make sure c is posetive
double rt = c * (2.0 * radius - c);
if (rt < -MathUtil.Epsilon)
{
return(null); //point to far away from line, no solutions
}
else
{
if (rt < 0.0)
{
rt = 0.0; //protect from fuzzy span between -epsilon and zero
}
double xj = -a * (c - radius);
double yj = -b * (c - radius);
if (MathUtil.Equals(rt, 0.0))
{ //one solution
if (MathUtil.IsZero(c)) //point is on line, we can still have two solutions
{
return(new Circle2d[] {
new Circle2d(pnt.X + xj, pnt.Y + yj, radius),
new Circle2d(pnt.X - xj, pnt.Y - yj, radius),
});
}
else
{
return(new Circle2d[] { new Circle2d(xj + pnt.X, yj + pnt.Y, radius) });
}
}
else
{ //two solutions
rt = Math.Sqrt(rt);
return(new Circle2d[] {
new Circle2d(xj + b * rt + pnt.X, yj - a * rt + pnt.Y, radius),
new Circle2d(xj - b * rt + pnt.X, yj + a * rt + pnt.Y, radius)
});
}
}
}
public static Circle2d[] CircleLine(Circle2d ci, Line2d li, double radius)
{
// see 'a programmers geometry' page 42 for explanation
double a, b, c, rj, sa, sb, sc, xcons, ycons;
List <Circle2d> result = new List <Circle2d>();
if (!li.ToEquation(out a, out b, out c))
{
return(CirclePoint(ci, li.MidPoint, radius)); //line was degenerate
}
c = c + a * ci.Center.X + b * ci.Center.Y; //transform line so that circle center is at origo
for (int l = 0; l < 4; l++)
{
sa = ((l & 1) == 0) ? a:-a;
sb = ((l & 1) == 0) ? b : -b;
sc = ((l & 1) == 0) ? c : -c;
rj = ((l & 2) == 0) ? ci.Radius:-ci.Radius;
double root = (radius + rj) * (radius + rj) - (sc - radius) * (sc - radius);
xcons = ci.Center.X - sa * (sc - radius);
ycons = ci.Center.Y - sb * (sc - radius);
if (root < -MathUtil.Epsilon)
{
continue; //circle center to far away
}
else if (root < MathUtil.Epsilon)
{ //one solution possible
result.Add(new Circle2d(xcons, ycons, radius));
}
else
{//tw solutions
root = Math.Sqrt(root);
result.Add(new Circle2d(xcons + sb * root, ycons - sa * root, radius));
result.Add(new Circle2d(xcons - sb * root, ycons + sa * root, radius));
}
}
if (result.Count <= 0)
{
return(null);
}
return(result.ToArray());
}
public static Circle2d[] LineLineLine(Line2d li1, Line2d li2, Line2d li3)
{
// see http://www.arcenciel.co.uk/geometry/ for explanation
List <Circle2d> result = null;
double a1, b1, c1, a2, b2, c2, a3, b3, c3, rad, u, v, xcenter, ycenter, denom;
double t1 = 1.0, t2 = 1.0, t3 = 1.0;
if (!li1.ToEquation(out a1, out b1, out c1))
{
return(null);
}
if (!li2.ToEquation(out a2, out b2, out c2))
{
return(null);
}
if (!li3.ToEquation(out a3, out b3, out c3))
{
return(null);
}
//line 2 must not be vertical for this solver, if so, exchange lines
if (MathUtil.IsZero(b2))
{
MathUtil.Swap(ref a1, ref a2);
MathUtil.Swap(ref b1, ref b2);
MathUtil.Swap(ref c1, ref c2);
}
u = a2 * b1 - a1 * b2;
v = a3 * b2 - a2 * b3;
//loop for all 8 possible solutions (only 4 can actually be real solutions)
for (int signcase = 0; signcase < 8; signcase++)
{
t1 = ((signcase & 1) == 0) ? -1 : 1;
t2 = ((signcase & 2) == 0) ? -1 : 1;
t3 = ((signcase & 4) == 0) ? -1 : 1;
//compute radius
denom = (v * (b1 * t2 - b2 * t1) - u * (b2 * t3 - b3 * t2));
if (MathUtil.IsZero(denom))
{
continue;
}
rad = (u * (b3 * c2 - b2 * c3) - v * (b2 * c1 - b1 * c2)) / denom;
if (rad < minradius || rad > maxradius)
{
continue;
}
//compute center x
if (!MathUtil.IsZero(u))
{
xcenter = (b2 * c1 - b2 * rad * t1 - b1 * c2 + b1 * rad * t2) / u;
}
else if (!MathUtil.IsZero(v))
{
xcenter = (b3 * c2 + b3 * rad * t2 - b2 * c3 + b2 * rad * t3) / v;
}
else
{
continue;
}
//compute center y
if (b1 != 0.0)
{
ycenter = (-a1 * xcenter - c1 + rad * t1) / b1;
}
else if (b2 != 0.0)
{
ycenter = (-a2 * xcenter - c2 + rad * t2) / b2;
}
else
{
ycenter = (-a3 * xcenter - c3 + rad * t3) / b3;
}
AddResult(ref result, xcenter, ycenter, rad);
} //end loop signcase
if (result == null)
{
return(null);
}
return(result.ToArray());
}
public static Circle2d[] CircleCircleLine(Circle2d ci1, Circle2d ci2, Line2d li)
{
// see http://www.arcenciel.co.uk/geometry/ for explanation
List <Circle2d> result = null;
double a1, b1, c1, t, r2, r3, a, b, c, u, s;
double A, B, C, xc, yc;
//transform input so that c1 is at origo and c2 is on xaxis
Transform2d trans = Transform2d.Translate(Point2d.Origo - ci1.Center) * Transform2d.Rotate(-ci1.Center.Angle(ci2.Center));
ci1 = new Circle2d(ci1);
ci1.Transform(trans);
ci2 = new Circle2d(ci2);
ci2.Transform(trans);
li = new Line2d(li);
li.Transform(trans);
if (!li.ToEquation(out a1, out b1, out c1))
{
return(null); //degenerate line
}
for (int signcase = 0; signcase < 8; ++signcase)
{
t = ((signcase & 1) == 0) ? 1 : -1;
r2 = ((signcase & 2) == 0) ? ci1.Radius : -ci1.Radius;
r3 = ((signcase & 4) == 0) ? ci2.Radius : -ci2.Radius;
// Get constants
a = 2 * (a1 * (r2 - r3) - ci2.X * t);
b = 2 * b1 * (r2 - r3);
c = 2 * c1 * (r2 - r3) + t * (r2 * r2 - r3 * r3 + ci2.X * ci2.X);
if (!MathUtil.IsZero(b))
{
u = b1 * c - b * c1;
s = a1 * b - a * b1;
A = t * t * b * b * (a * a + b * b) - b * b * s * s;
B = 2 * (u * t * b * (a * a + b * b) + a * c * s * t * b - b * b * s * s * r2);
C = u * u * (a * a + b * b) + 2 * a * c * s * u + c * c * s * s - b * b * s * s * r2 * r2;
}
else
{
u = a1 * c - a * c1;
s = a * b1;
A = a * a * (t * t * a * a - s * s);
B = 2 * a * a * (u * t * a - s * s * r2);
C = u * u * a * a + c * c * s * s - a * a * s * s * r2 * r2;
}
// Calculate radius
double[] roots = RealPolynomial.SolveQuadric(A, B, C);
if (roots != null)
{
foreach (double radius in roots)
{
if (radius < minradius || radius > maxradius)
{
continue;
}
// compute x coordinates of centers
List <double> xsols = new List <double>();
if (!MathUtil.IsZero(ci2.X)) //circles are not concentric
{
xc = ((r2 + radius) * (r2 + radius) - (r3 + radius) * (r3 + radius) + ci2.X * ci2.X) / (2 * ci2.X);
xsols.Add(xc);
}
else // If circles are concentric there can be 2 solutions for x
{
A = (a1 * a1 + b1 * b1);
B = -2 * a1 * (radius * t - c1);
C = (radius * t - c1) * (radius * t - c1) - b1 * b1 * (r2 + radius) * (r2 + radius);
double[] roots2 = RealPolynomial.SolveQuadric(A, B, C);
if (roots2 != null)
{
foreach (double x in roots2)
{
xsols.Add(x);
}
}
}
// now compute y coordinates from the calculated x:es
// and input the final solution
foreach (double x in xsols)
{
if (!MathUtil.IsZero(b1))
{
yc = (-a1 * x - c1 + radius * t) / b1;
}
else
{
double ycSquare = (r2 + radius) * (r2 + radius) - (x * x);
if (ycSquare < 0.0)
{
continue;
}
yc = Math.Sqrt(ycSquare);
}
AddResult(ref result, x, yc, radius);
if (MathUtil.IsZero(b1))
{
AddResult(ref result, x, -yc, radius);
}
}
}
}
}
//convert back to original coordinate system by using the inverse
//of the original matrix
if (result != null)
{
trans = trans.Inversed;
for (int l = 0; l < result.Count; l++)
{
result[l].Transform(trans);
}
return(result.ToArray());
}
return(null);
}
public static Circle2d[] CircleLineLine(Circle2d ci, Line2d l1, Line2d l2)
{
// see http://www.arcenciel.co.uk/geometry/ for explanation
List <Circle2d> result = null;
//translate everyting so circle center at origo
double dx = ci.X, dy = ci.Y;
ci = new Circle2d(0, 0, ci.Radius);
l1 = new Line2d(l1.X1 - dx, l1.Y1 - dy, l1.X2 - dx, l1.Y2 - dy);
l2 = new Line2d(l2.X1 - dx, l2.Y1 - dy, l2.X2 - dx, l2.Y2 - dy);
//if first line vertical, swap lines...
if (MathUtil.Equals(l1.X1, l1.X2))
{
var tmp = l1; l1 = l2; l2 = tmp;
}
//if first line still vertical, special case:
if (MathUtil.Equals(l1.X1, l1.X2))
{
double rad = (l1.X1 - l2.X1) / 2.0;
double xcenter = (l1.X1 + l2.X1) / 2.0;
double yc = Math.Sqrt((rad + ci.Radius) * (rad + ci.Radius) - xcenter * xcenter);
AddResult(ref result, xcenter, ci.Y + yc, rad);
AddResult(ref result, xcenter, ci.Y - yc, rad);
}
else
{
//now we know that first line is not vertical, and circle is centered at origo
double a1, b1, c1, a2, b2, c2, u, w, s, a, b, c, xcenter, ycenter, t1, t2, r3;
if (!l1.ToEquation(out a1, out b1, out c1))
{
return(null);
}
if (!l2.ToEquation(out a2, out b2, out c2))
{
return(null);
}
for (int signcase = 0; signcase < 8; signcase++)
{
t1 = ((signcase & 1) == 0) ? -1 : 1;
t2 = ((signcase & 2) == 0) ? -1 : 1;
r3 = ((signcase & 4) == 0) ? -ci.Radius : ci.Radius;
u = (t1 * b2) - (t2 * b1);
w = (b1 * c2) - (b2 * c1);
s = (a1 * b2) - (a2 * b1);
a = (u * u) - (2 * a1 * s * u * t1) + (t1 * t1 * s * s) - (b1 * b1 * s * s);
b = 2.0 * ((u * w) + (c1 * a1 * s * u) - (a1 * s * t1 * w) - (c1 * t1 * s * s) - (r3 * b1 * b1 * s * s));
c = (w * w) + (2 * a1 * s * c1 * w) + (c1 * c1 * s * s) - (b1 * b1 * r3 * r3 * s * s);
double[] roots = RealPolynomial.SolveQuadric(a, b, c);
if (roots != null)
{
foreach (double radius in roots)
{
if (radius < minradius || radius > maxradius)
{
continue;
}
if (!MathUtil.IsZero(s))
{ //non parallel lines, one center per root
xcenter = (radius * u + w) / s;
ycenter = ((-a1 * xcenter) - c1 + (radius * t1)) / b1;
AddResult(ref result, xcenter, ycenter, radius);
}
else //parallel lines, two centers per root
{
a = t1 * t1;
b = 2.0 * a1 * (c1 - (radius * t1));
c = ((radius * t1) - c1) * ((radius * t1) - c1) - (b1 * b2 * (r3 + radius) * (r3 + radius));
double[] roots2 = RealPolynomial.SolveQuadric(a, b, c);
if (roots2 != null)
{
foreach (double x in roots2)
{
ycenter = (-a1 * x - c1 + radius * t1) / b1;
AddResult(ref result, x, ycenter, radius);
}
}
}
}
}
}
}
//translate results back to original position
if (result != null)
{
foreach (Circle2d c in result)
{
c.X += dx;
c.Y += dy;
}
return(result.ToArray());
}
return(null);
}
