public override bool HitTest(Ray3 ray, out double t)
{
ray = ray.Transform(InvPosition);
double dx = ray.Direction.X;
double dy = ray.Direction.Y;
double dz = ray.Direction.Z;
double x0 = ray.Position.X;
double y0 = ray.Position.Y;
double z0 = ray.Position.Z;
double a = 1.0; //assume ray direction is normalized dx * dx + dy * dy + dz * dz;
double b = 2.0 * (x0 * dx + y0 * dy + z0 * dz);
double c = x0 * x0 + y0 * y0 + z0 * z0 - rad * rad;
bool found = false;
t = double.MaxValue;
foreach (var tt in RealPolynomial.SolveQuadric(a, b, c))
{
if (tt > 1e-4 && tt < t)
{
t = tt;
found = true;
}
}
return(found);
}
public Point2d[] Perpendicular(Point2d from)
{
double i, j; //i,j is from-point in standard space
Transform2d tr = ToStandardPosition;
tr.Apply(from.X, from.Y, out i, out j, true);
//cubic coefficients gotten from langarnge multiplier minimizing distance from i,j to curve
double[] xs = RealPolynomial.SolveCubic2(-1, 0.0, j - 0.25, 0.25 * i);
if (xs == null)
{
return(null);
}
Point2d[] res = new Point2d[xs.Length];
tr = tr.Inversed;
int respos = 0;
foreach (double x in xs)
{
tr.Apply(x, x * x, out i, out j, true);
res[respos++] = new Point2d(i, j);
}
return(res);
}
public void DIntegralTestx_1to2()
{
// p(x) = x
// ∫_1^2(xdx) == [x^2/2]_{x=1}^2 = (4-1)/2 = 3/2
var p = new RealPolynomial(new double[] { 0, 1 });
double r = p.Integral(1, 2);
Assert.IsTrue(IsSimilar(r, 3.0 / 2.0));
}
public void DIntegralTest2_4to3()
{
// p(x) = 2
// ∫_4^3(2dx) == [2x]_{x=4}^3 = (6-8) = -2
var p = new RealPolynomial(new double[] { 2 });
double r = p.Integral(4, 3);
Assert.IsTrue(IsSimilar(r, -2.0));
}
public void DIntegralTest2_3to4()
{
// p(x) = 2
// ∫_3^4(2dx) == [2x]_{x=3}^4 = (8-6) = 2
var p = new RealPolynomial(new double[] { 2 });
double r = p.Integral(3, 4);
Assert.IsTrue(IsSimilar(r, 2.0));
}
public void DIntegralTestx2_0to1()
{
// p(x) = x^2
// ∫_0^1(x^2dx) == [x^3/3]_{x=0}^1 = 1/3
var p = new RealPolynomial(new double[] { 0, 0, 1 });
double r = p.Integral(0, 1);
Assert.IsTrue(IsSimilar(r, 1.0 / 3.0));
}
public void DIntegralTestx2_1to2()
{
// p(x) = x^2
// ∫_1^2(x^2dx) == [x^3/3]_{x=1}^2 = (8-1)/3 = 7/3
var p = new RealPolynomial(new double[] { 0, 0, 1 });
double r = p.Integral(1, 2);
Assert.IsTrue(IsSimilar(r, 7.0 / 3.0));
}
public void DIntegralTestx3px2_2to5()
{
// p(x) = x^3 + x^2
// ∫_2^5(x^3+x^2)dx == [x^4/4+x^3/3]_{x=2}^5 = 765/4
var p = new RealPolynomial(new double[] { 0, 0, 1, 1 });
double r = p.Integral(2, 5);
Assert.IsTrue(IsSimilar(r, 765.0 / 4.0));
}
public void DIntegralTestx_0to1()
{
// p(x) = x
// ∫_0^1(xdx) == [x^2/2]_{x=0}^1 = 1/2
var p = new RealPolynomial(new double[] { 0, 1 });
double r = p.Integral(0, 1);
Assert.IsTrue(IsSimilar(r, 1.0 / 2.0));
}
public void IIntegralTest2()
{
// p(x) = 2
// ∫(2)dx == [2x] +C
double C = 1.0;
var p = new RealPolynomial(new double[] { 2 });
var pI = new RealPolynomial(new double[] { C, 2 });
var r = p.Integral(C);
Assert.IsTrue(IsSimilar(r, pI));
}
public void IIntegralTestx()
{
// p(x) = x
// ∫xdx == [x^2/2] +C
double C = 1.0;
var p = new RealPolynomial(new double[] { 0, 1 });
var pI = new RealPolynomial(new double[] { C, 0, 1.0 / 2.0 });
var r = p.Integral(C);
Assert.IsTrue(IsSimilar(r, pI));
}
public static double[] CircleLineParametric(Circle2d c, Line2d l)
{
double dx = l.X2 - l.X1;
double dy = l.Y2 - l.Y1;
double px = l.X1;
double py = l.Y1;
double cx = c.Center.X;
double cy = c.Center.Y;
double pa = dx * dx + dy * dy;
double pb = 2.0 * (px * dx - cx * dx + py * dy - cy * dy);
double pc = px * px - 2.0 * px * cx + cx * cx +
py * py - 2.0 * py * cy + cy * cy - c.Radius * c.Radius;
return(RealPolynomial.SolveQuadric(pa, pb, pc));
}
public NTFHzcoeffs(int order)
{
if (order < 2 || 9 < order)
{
throw new ArgumentOutOfRangeException("order");
}
mOrder = order;
{
if (0 == (order & 1))
{
mNumer = new RealPolynomial(new double[] { 1.0 });
}
else
{
mNumer = new RealPolynomial(new double[] { ZeroNth(mOrder / 2).Minus().real, 1 });
}
for (int i = mOrder / 2 - 1; 0 <= i; --i)
{
var coeff = new WWComplex[] { ZeroNth(i).Minus(), WWComplex.Unity() };
var pair = WWPolynomial.MulComplexConjugatePair(new ComplexPolynomial(coeff));
mNumer = WWPolynomial.Mul(mNumer, pair);
}
}
{
if (0 == (order & 1))
{
mDenom = new RealPolynomial(new double[] { 1.0 });
}
else
{
mDenom = new RealPolynomial(new double[] { PoleNth(mOrder / 2).Minus().real, 1 });
}
for (int i = mOrder / 2 - 1; 0 <= i; --i)
{
var coeff = new WWComplex[] { PoleNth(i).Minus(), WWComplex.Unity() };
var pair = WWPolynomial.MulComplexConjugatePair(new ComplexPolynomial(coeff));
mDenom = WWPolynomial.Mul(mDenom, pair);
}
}
}
//
//You can use the following additional attributes as you write your tests:
//
//Use ClassInitialize to run code before running the first test in the class
//[ClassInitialize()]
//public static void MyClassInitialize(TestContext testContext)
//{
//}
//
//Use ClassCleanup to run code after all tests in a class have run
//[ClassCleanup()]
//public static void MyClassCleanup()
//{
//}
//
//Use TestInitialize to run code before running each test
//[TestInitialize()]
//public void MyTestInitialize()
//{
//}
//
//Use TestCleanup to run code after each test has run
//[TestCleanup()]
//public void MyTestCleanup()
//{
//}
//
#endregion
private bool IsSimilar(RealPolynomial a, RealPolynomial b)
{
if (a.Degree != b.Degree)
{
return(false);
}
for (int i = 0; i < a.Degree; ++i)
{
double aC = a.Coeffs()[i];
double bC = b.Coeffs()[i];
if (1e-8 <= Math.Abs(aC - bC))
{
return(false);
}
}
return(true);
}
public Vector2d Normal(Point2d pt)
{
double x = pt.X - center.X;
double y = pt.Y - center.Y;
double a = MajorRadius;
double b = MinorRadius;
double e2 = x;
double e1 = b * b - a * a - y * y - x * x;
double e0 = x * a * a - x * b * b;
double[] roots = RealPolynomial.SolveQuadric(e2, e1, e0);
if (roots == null)
{
return(null);
}
Vector2d norm = new Vector2d(x - roots[0], y).Normalized;
return(norm);
}
public static double[] ParabolaLineParamteric(Parabola2d pab, Line2d lin) //tested ok
{
double x1, y1, x2, y2; //line points in parabola standard position
Transform2d tr = pab.ToStandardPosition; //intersect line with y=x^2 => easier
tr.Apply(lin.X1, lin.Y1, out x1, out y1, true);
tr.Apply(lin.X2, lin.Y2, out x2, out y2, true);
double dx = x2 - x1;
double dy = y2 - y1;
double c2 = -dx * dx;
double c1 = dy - 2 * dx * x1;
double c0 = y1 - x1 * x1;
//y1-x1^2+t*(dy-2*dx*x1)-dx^2*t^2=0
double[] ts = RealPolynomial.SolveQuadric(c2, c1, c0);
return(ts);
}
public override bool HitTest(Ray3 ray, out double t)
{
ray = ray.Transform(InvPosition);
double dx = ray.Direction.X;
double dy = ray.Direction.Y;
double dz = ray.Direction.Z;
double x0 = ray.Position.X;
double y0 = ray.Position.Y;
double z0 = ray.Position.Z;
double a = dx * dx * dx * dx + dy * dy * dy * dy + dz * dz * dz * dz;
double b = 4.0 * dx * dx * dx * x0 + 4.0 * dy * dy * dy * y0 + 4.0 * dz * dz * dz;
double c = 6.0 * dx * dx * x0 * x0 - 5.0 * dx * dx + 6.0 * dy * dy * y0 * y0 - 5.0 * dy * dy + 6.0 * dz * dz * z0 * z0 - 5.0 * dz * dz;
double d = 4.0 * dx * x0 * x0 * x0 - 10.0 * dx * x0 + 4.0 * dy * y0 * y0 * y0 - 10.0 * dy * y0 + 4.0 * dz * z0 * z0 * z0;
double e = x0 * x0 * x0 * x0 - 5.0 * x0 * x0 + y0 * y0 * y0 * y0 - 5.0 * y0 * y0 + z0 * z0 * z0 * z0 - 5.0 * z0 * z0 + 11.8;
RealPolynomial poly = new RealPolynomial(a, b, c, d, e);
//poly /= new RealPolynomial(100000);
//poly.Normalize();
double ev = poly.Eval(1.0);
double[] cof = poly.FindRoots(MathUtil.Epsilon, false);
//double[] cof=RealPolynomial.SolveQuartic(a, b, c, d, e);
bool found = false;
t = double.MaxValue;
foreach (var tt in RealPolynomial.SolveQuadric(a, b, c))
{
if (tt > 1e-4 && tt < t)
{
t = tt;
found = true;
}
}
return(found);
}
/// <summary>
/// Intersects a line with a conic that is in standard position, that is, not rotated and
/// centered at 0,0
/// </summary>
/// <param name="con"></param>
/// <param name="lin"></param>
/// <returns>A list of parameters on line that is intersection points, or null if no intersections</returns>
private static double[] ConicLineParametric(GeneralConic2d con, Line2d lin)
{
//We construct a matrix so that: conic is unrotated (B term=0) and line starts at origo and has length=1.0
//This is to improve stabillity of the equation
double invlen = 1.0 / lin.Length;
if (double.IsInfinity(invlen))
{
return(null); //zero length line does not intersect
}
Transform2d tr = Transform2d.Translate(-lin.X1, -lin.Y1) * Transform2d.Rotate(-con.Rotation) * Transform2d.Scale(invlen);
GeneralConic2d c = new GeneralConic2d(con); //copy for modification
double x1 = lin.X2, y1 = lin.Y2;
c.Transform(tr);
tr.Apply(x1, y1, out x1, out y1, true); //transformed line end
double t2 = y1 * y1 * c.C + x1 * x1 * c.A;
double t1 = y1 * c.E + x1 * c.D;
double t0 = c.F;
double[] ts = RealPolynomial.SolveQuadric(t2, t1, t0);
return(ts);
/*double dx=lin.DX;
* double dy=lin.DY;
* double x0=lin.X1;
* double y0=lin.Y1;
*
* double t2=con.C*dy*dy+con.A*dx*dx;
* double t1=2*con.C*dy*y0+2*con.A*dx*x0+dy*con.E+con.D*dx;
* double t0=con.C*y0*y0+con.E*y0+con.A*x0*x0+con.D*x0+con.F;
*
* return RealPolynomial.SolveQuadric(t2, t1, t0, 1e-9);*/
}
public static double[] HyperbolaLineParametric(Hyperbola2d hyp, Line2d lin)
{
// Computes intersection points with a hyperbola and a line
// solved and created by Robert.P. 2013-02-11
//
// solution is created by transforming system to hyperbola standard position,
// and then solving the system x^2/a^2-y^2/b^2-1=0 , x=x0+t*dx and y=y0+t*dy (note that a is 1 in std. pos.)
Transform2d tr = hyp.ToStandardPosition;
//extract standard spaced line
double x0, y0, x1, y1, dx, dy, b = hyp.Ratio;
tr.Apply(lin.X1, lin.Y1, out x0, out y0, true);
tr.Apply(lin.X2, lin.Y2, out x1, out y1, true);
dx = x1 - x0; dy = y1 - y0;
double t2 = -dy * dy + b * b * dx * dx;
double t1 = -2 * dy * y0 + 2 * b * b * dx * x0;
double t0 = -y0 * y0 + b * b * x0 * x0 - b * b;
return(RealPolynomial.SolveQuadric(t2, t1, t0, 0.0));
}
public static Circle2d[] LinePointPoint(Line2d li, Point2d pt1, Point2d pt2)
{
// max two solutions
// solution created by Robert.P. 2013-02-07
List <Circle2d> res = null;
// transform problem so that we have a vertical line thru origo which simplifies stuff a lot
Transform2d toorg = Transform2d.Translate(-li.X1, -li.Y1) * Transform2d.Rotate(-li.Angle + MathUtil.Deg90);
Transform2d fromorg = toorg.Inversed;
double i, j, k, l;
toorg.Apply(pt1.X, pt1.Y, out i, out j, true);
toorg.Apply(pt2.X, pt2.Y, out k, out l, true);
double y2 = 2 * k - 2 * i;
double y1 = 4 * i * l - 4 * j * k;
double y0 = -2 * i * l * l - 2 * i * k * k + (2 * j * j + 2 * i * i) * k;
double[] ys = RealPolynomial.SolveQuadric(y2, y1, y0);
if (ys == null)
{
return(null); //no solutions
}
foreach (double y in ys)
{
double xx = (y * y - 2 * j * y + j * j + i * i) / (2 * i), yy = y; //TODO: what if vertical line
double rad = Math.Abs(xx);
fromorg.Apply(xx, yy, out xx, out yy, true);
AddResult(ref res, xx, yy, rad);
}
return(MakeResult(res));
}
public static Circle2d[] LineLinePoint(Line2d lin1, Line2d lin2, Point2d pnt)
{
// Compute the 2 circles (or 1 in special cases) that tangents two lines and a point.
// Solving for parameter t on the bisector vector between lines that have the same
// distance to point as the radius for that t (given using radfac computed below)
// Solution by Robert.P. 2013-02-14
List <Circle2d> res = null;
Vector2d v1 = lin1.Direction;
Vector2d v2 = lin2.Direction;
Vector2d midvec;
Point2d linint;
double[] ts;
linint = Intersect2d.LineLineAsPoint(lin1, lin2); //basepoint for midline vector
if (linint == null) //special case for parallel lines
{
//use vector and base point on midpoint of radical line of circles
midvec = v1;
Line2d radlin = new Line2d(lin1.ClosestPoint(pnt), lin2.ClosestPoint(pnt));
linint = radlin.MidPoint;
double r = radlin.Length * 0.5, i = pnt.X, j = pnt.Y, x0 = linint.X, y0 = linint.Y, dx = midvec.X, dy = midvec.Y;
double tp2 = 1; //squared length of midlin => assume normalized
double tp1 = 2 * (dy * y0 + dx * x0 - dy * j - dx * i);
double tp0 = y0 * y0 - 2 * j * y0 - r * r + j * j + i * i + x0 * x0 - 2 * i * x0;
ts = RealPolynomial.SolveQuadric(tp2, tp1, tp0, 0.0);
if (ts == null)
{
return(null);
}
foreach (double t in ts)
{
Point2d center = linint + midvec * t;
AddResult(ref res, center.X, center.Y, r);
}
}
else //normal case with non-parallel lines
{
if (pnt.Location(lin1.Start, lin1.End) == pnt.Location(lin2.Start, lin2.End))
{
v2 = -v2; //select correct bisector out of two (actually 4 but it does not matter if we solve for negative t)
}
midvec = v1.Bisect(v2);
double radfac = Math.Sin(v1.AngleTo(v2) / 2.0); //multiplied with midvector t this gives radius of circle at that t
//solve problem in space where pnt=origo
Point2d org = new Point2d(linint.X - pnt.X, linint.Y - pnt.Y);
double t2 = -radfac * radfac + 1; //// 1 for squared length of midlin => assume normalized
double t1 = 2 * (midvec.Y * org.Y + midvec.X * org.X);
double t0 = org.X * org.X + org.Y * org.Y;
ts = RealPolynomial.SolveQuadric(t2, t1, t0, 0.0);
if (ts == null)
{
return(null);
}
foreach (double t in ts)
{
Point2d center = linint + midvec * t;
AddResult(ref res, center.X, center.Y, Math.Abs(t * radfac));
}
}
return(MakeResult(res));
}
private static double[] GetRealRoots(double c3, double c2, double c1, double c0)
{
RealPolynomial rp = new RealPolynomial(c3, c2, c1, c0);
return(rp.FindRoots(true));
}
public static Point2d[] ConicConic(GeneralConic2d tcon1, GeneralConic2d tcon2)
{
// uses the beautiful solution to find the multiplier λ, so that Conic11+λ*Conic2 is
// a degenerate conic, that is a conic of lines (the 'pencil'). The lines in this conic
// intersects each of the conics in their common intersection points, thus the problem
// has been reduced to a Conic-Line intersection problem.
// This technique is described in book Graphics Gems V, of which we are inspired although
// this code differs a somewhat from the one in the book.
// work in standard space for conic 1, gives a more stable computation and speeds up
// the multiple line-conic intersections later on.
GeneralConic2d con1 = new GeneralConic2d(tcon1);
GeneralConic2d con2 = new GeneralConic2d(tcon2);
//TODO: does not work properly, probably because line extractor does not work correctly in some cases
//convert conic coefficients to their matrix form
double a = con1.A, b = con1.B * 0.5, c = con1.C, d = con1.D * 0.5, e = con1.E * 0.5, f = con1.F;
double A = con2.A, B = con2.B * 0.5, C = con2.C, D = con2.D * 0.5, E = con2.E * 0.5, F = con2.F;
//TODO: since conic 1 is in standard position, thoose can be simplified: b,d,e terms are always zero
double c3 = (A * C - B * B) * F - A * E * E + 2 * B * D * E - C * D * D;
double c2 = (a * C - 2 * b * B + c * A) * F - a * E * E + (2 * b * D + 2 * d * B - 2 * e * A) * E - c * D * D + (2 * e * B - 2 * d * C) * D + f * A * C - f * B * B;
double c1 = (a * c - b * b) * F + (2 * b * d - 2 * a * e) * E + (2 * b * e - 2 * c * d) * D + (a * f - d * d) * C + (2 * d * e - 2 * b * f) * B + (c * f - e * e) * A;
double c0 = (a * c - b * b) * f - a * e * e + 2 * b * d * e - c * d * d;
double[] lambdas2 = RealPolynomial.SolveCubic2(c3, c2, c1, c0); //up to three coefficients that will turn conic sum to degenerate lines
double[] lambdas = GetRealRoots(c3, c2, c1, c0);
if (lambdas == null)
{
return(null); //this can never happen on a 3d degree equation but we check it anyway
}
Point2dSet res = new Point2dSet();
foreach (double lambda in lambdas)
{
GeneralConic2d pencil = new GeneralConic2d(
a + lambda * A,
(b + lambda * B) * 2,
c + lambda * C,
(d + lambda * D) * 2,
(e + lambda * E) * 2,
f + lambda * F);
Line2d[] lines = pencil.ToLines();
if (lines != null)
{
foreach (Line2d lin in lines)
{ //max 2 lines
Point2d[] intpts = ConicLine(con1, lin);
if (intpts == null)
{
continue;
}
//validate each point satisfying the conic equations (they can be out of range for finite conics such as ellipses)
foreach (Point2d pt in intpts)
{
double x = pt.X;
double y = pt.Y;
double err1 = con1.A * x * x + con1.B * x * y + con1.C * y * y + con1.D * x + con1.E * y + con1.F;
if (MathUtil.IsZero(err1, 5.0))
{
double err2 = con2.A * x * x + con2.B * x * y + con2.C * y * y + con2.D * x + con2.E * y + con2.F;
if (MathUtil.IsZero(err2, 5.0))
{
res.Add(pt);
}
}
}
}
}
}
//res.Transform(tr.Inversed);
return(res.ToArray());
}
public static Point2d[] EllipseCircle(Ellipse2d el, Circle2d ci)
{
Transform2d tr = el.ToStandardPosition;
ci = new Circle2d(ci); //dont modify original circle, but this copy
ci.Transform(tr);
double b = el.Ratio, b2 = b * b, b4 = b2 * b2;
double i = ci.Center.X, i2 = i * i, i4 = i2 * i2;
double j = ci.Center.Y, j2 = j * j, j4 = j2 * j2;
double r = ci.Radius, r2 = r * r, r4 = r2 * r2;
double x4 = b4 - 2 * b2 + 1;
double x3 = 4 * b2 * i - 4 * i;
double x2 = b2 * (2 * r2 + 2 * j2 - 2 * i2 + 2) - 2 * r2 + 2 * j2 + 6 * i2 - 2 * b4;
double x1 = 4 * i * r2 - 4 * i * j2 - 4 * i * i * i - 4 * b2 * i;
double x0 = r4 + (-2 * j2 - 2 * i2) * r2 + b2 * (-2 * r2 - 2 * j2 + 2 * i2) + j4 + 2 * i2 * j2 + i4 + b4;
//double[] xs = RealPolynomial.SolveQuartic2(x4, x3, x2, x1, x0, 1e-30);
RealPolynomial rp = new RealPolynomial(x4, x3, x2, x1, x0);
double[] xs = rp.FindRoots(true);
if (xs == null)
{
return(null); //no intersections
}
Point2dSet resultset = new Point2dSet();
foreach (double x in xs)
{
//test the two possible y:s to be solutions for this x
double y = (1 - x * x) * b2;
if (y < 0.0)
{
continue;
}
y = Math.Sqrt(y);
for (int t = 0; t < 2; t++) //test booth y solutions...
{
double err = x * x + y * y / b2 - 1.0; //on ellipse
double err2 = MathUtil.Square(x - i) + MathUtil.Square(y - j) - r2; //on circle
if (MathUtil.IsZero(err, 1e-7) && MathUtil.IsZero(err2, MathUtil.Epsilon))
{
resultset.Add(new Point2d(x, y));
}
y = -y; // ...by inverting y in second turn
}
}
if (resultset.Count == 0)
{
return(null);
}
resultset.Transform(tr.Inversed); //back to original position
return(resultset.ToArray());
}
/// <summary>
/// Returns all the perpendicular points on the ellipse from a given point 'from'
/// </summary>
public Point2d[] Perpendicular(Point2d from)
{
// Solved by Robert.P. in december 2012
// Note on solutions:
// Quartic coefficients gotten from applying lagrange multiplier to minimize (x-i)^2+(y-j)^2
// with x^2/a^2+y^2/b^2-1=0 as constraint (a=1 because we work in standard position).
// This gives a system of three equations F_x,F_y,F_lambda, which were solved with
// resultant theory using 'eliminate' in maxima
//work in standard position, retranslate solutions last
Transform2d tostd = ToStandardPosition;
from = from.GetTransformed(tostd);
double b = sigratio, b2 = b * b, b4 = b2 * b2;
double i = from.X, i2 = i * i;
double j = from.Y, j2 = j * j;
double x4 = b4 - 2 * b2 + 1;
double x3 = 2 * b2 * i - 2 * i;
double x2 = b2 * j2 + i2 - b4 + 2 * b2 - 1;
double x1 = 2 * i - 2 * b2 * i;
double x0 = -i2;
double[] sols = RealPolynomial.SolveQuartic2(x4, x3, x2, x1, x0, 1e-16);
if (sols == null)
{
return(null);
}
Point2dSet respts = new Point2dSet();
foreach (double x in sols)
{
double y = (1 - x * x) * b2;
if (y < 0.0)
{
continue;
}
y = Math.Sqrt(y);
for (int l = 0; l < 2; l++)
{
//both posetive and negative y:s can be solutions. Check with each possible
//point that its perpendicular to ellipse (subtracting the inverse ellipse slope (=normal slope) with the slope from 'from' point)
double err;
err = y * (from.X - x) - x * b2 * (from.Y - y);
if (Math.Abs(err) < 1e-6)
{
respts.Add(new Point2d(x, y));
}
y = -y; //test negative solution as well
}
}
respts.Transform(tostd.Inversed);
return(respts.ToArray());
}
public static Circle2d[] CircleCircleCircle(Circle2d ci1, Circle2d ci2, Circle2d ci3)
{
// see http://www.arcenciel.co.uk/geometry/ for explanation
List <Circle2d> result = null;
double r1, r2, r3, a, b, c, t, A, B, C;
double fRadius, xc, yc, distc1c2;
double[] roots;
// if all circles concentric, there are no solutions
distc1c2 = ci1.Center.Distance(ci2.Center);
if (MathUtil.IsZero(distc1c2) && MathUtil.IsZero(ci2.Center.Distance(ci3.Center)))
{
return(null);
}
// make sure first 2 circles are not concentric
// if so swap ci2,ci3
if (MathUtil.IsZero(distc1c2))
{
var tmp = ci2; ci2 = ci3; ci3 = ci2;
}
// transform input so that ci1 is at origo and ci2 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);
ci3 = new Circle2d(ci3);
ci3.Transform(trans);
// Negate the radii to get all combinations
for (int iCase = 0; iCase < 8; ++iCase)
{
r1 = ((iCase & 1) == 0) ? ci1.Radius : -ci1.Radius;
r2 = ((iCase & 2) == 0) ? ci2.Radius : -ci2.Radius;
r3 = ((iCase & 4) == 0) ? ci3.Radius : -ci3.Radius;
// special case where radii of first 2 circles are equal
if (MathUtil.Equals(r1, r2))
{
// Calculate x-cordinate of centre
xc = ci2.X / 2.0;
// if all radii are equal, there will be only one solution
if (MathUtil.Equals(r1, r3))
{
if (MathUtil.IsZero(ci3.Y))
{
continue;
}
// get y-coordinate of centre
yc = (ci3.X * ci3.X - 2.0 * xc * ci3.X + ci3.Y * ci3.Y) / (ci3.Y + ci3.Y);
// compute radius
A = 1;
B = 2 * r1;
C = r1 * r1 - xc * xc - yc * yc;
roots = RealPolynomial.SolveQuadric(A, B, C);
if (roots.Length > 0)
{
fRadius = roots[0];
if (fRadius <= 0.0)
{ //then try other root
if (roots.Length > 1)
{
fRadius = roots[1];
if (fRadius <= 0.0)
{
continue; //no posetive roots
}
}
}
AddResult(ref result, xc, yc, fRadius);
}
}
else
{
// compute constants
double k = r1 * r1 - r3 * r3 + ci3.X * ci3.X + ci3.Y * ci3.Y - 2 * xc * ci3.X;
A = 4 * ((r1 - r3) * (r1 - r3) - ci3.Y * ci3.Y);
B = 4 * (k * (r1 - r3) - 2 * ci3.Y * ci3.Y * r1);
C = 4 * xc * xc * ci3.Y * ci3.Y + k * k - 4 * ci3.Y * ci3.Y * r1 * r1;
if (!MathUtil.IsZero(A))
{
roots = RealPolynomial.SolveQuadric(A, B, C);
foreach (double radius in roots)
{
yc = (2 * radius * (r1 - r3) + k) / (2 * ci3.Y);
AddResult(ref result, xc, yc, radius);
}
}
}
continue;
} //end special case of r1==r2
// Get constants
a = 2 * (ci2.X * (r3 - r1) - ci3.X * (r2 - r1));
b = 2 * ci3.Y * (r1 - r2);
c = (r2 - r1) * (ci3.X * ci3.X + ci3.Y * ci3.Y - (r3 - r1) * (r3 - r1)) - (r3 - r1) * (ci2.X * ci2.X - (r2 - r1) * (r2 - r1));
t = (ci2.X * ci2.X + r1 * r1 - r2 * r2) / 2.0;
A = (r1 - r2) * (r1 - r2) * (a * a + b * b) - (ci2.X * ci2.X * b * b);
B = 2 * (t * (r1 - r2) * (a * a + b * b) + a * c * ci2.X * (r1 - r2) - (r1 * ci2.X * ci2.X * b * b));
C = t * t * (a * a + b * b) + (2 * a * c * ci2.X * t) + (c * c * ci2.X * ci2.X) - (r1 * r1 * ci2.X * ci2.X * b * b);
// Calculate radius
roots = RealPolynomial.SolveQuadric(A, B, C);
if (roots == null)
{
continue;
}
foreach (double radius in roots)
{
if (radius < minradius || radius > maxradius)
{
continue;
}
// get x coordinate of centre (x2 may not be zero)
xc = (radius * (r1 - r2) + t) / ci2.X;
// get y coordinate of centre. b should never be 0, as
// r1=r2 is special case and y3 may not be zero
yc = (-a * xc - c) / b;
AddResult(ref result, xc, 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[] 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);
}
public override bool HitTest(Ray3 ray, out double t)
{
ray = ray.Transform(InvPosition);
double x0 = ray.Position.X;
double y0 = ray.Position.Y;
double z0 = ray.Position.Z;
double dx = ray.Direction.X;
double dy = ray.Direction.Y;
double dz = ray.Direction.Z;
double A = x0 * x0 + y0 * y0 + z0 * z0;
double B = 2 * (x0 * dx + y0 * dy + z0 * dz);
double R2 = R * R;
double r2 = r * r;
double t4 = A * A;
double t3 = 2 * A * B;
double t2 = 2 * A * R2 + B * B + 2 * A * A - 2 * r2 * A;
double t1 = 2 * B * (R2 + A - r2);
double t0 = (2 * A - 2 * r2) * R2 + A * A - 2 * r2 * A + r2 * r2;
t2 -= 4 * R2 * (dx * dx + dy * dy);
t1 -= 8 * R2 * (dx * x0 + dy * y0);
t0 -= 4 * R2 * (x0 * x0 + y0 * y0);
RealPolynomial rp = new RealPolynomial(t4, t3, 2, t1, t0);
//rp.Normalize();
double[] roots = rp.FindRoots();
/*
*
* Vector3 p = ray.Position.ToVector();
* Vector3 d = ray.Direction;
*
* double alpha = d.Dot(d);
* double beta = 2.0 * p.Dot(d);
* double gamma = p.Dot(p) - tubeRadius*tubeRadius - centralRadius*centralRadius;
*
* // quatric coefficients
* double a4 = alpha*alpha;
* double a3 = 2.0 * alpha * beta;
* double a2 = beta*beta + (2.0 * alpha * gamma) + (4.0 * centralRadius*centralRadius * d.Z*d.Z);
* double a1 = (2.0 * beta * gamma) + (8 * centralRadius*centralRadius * p.Z * d.Z);
* double a0 = gamma*gamma + (4.0 * centralRadius*centralRadius * p.Z*p.Z) - (4.0 * centralRadius*centralRadius * tubeRadius*tubeRadius);
*
* // solve polynomial
* double[] roots = RealPolynomial.SolveQuartic(a4, a3, a2, a1, a0);
*/
bool found = false;
t = double.MaxValue;
foreach (var tt in roots)
{
if (tt > 1e-4 && tt < t)
{
t = tt;
found = true;
}
}
return(found);
}
/// <summary>
/// Gets the line geometries, one or two lines. The type of the conic has to be
/// Intersecting lines (2 results), Paralell lines (2 results) or Coincident lienes (1 result),
/// otherwise the result is null.
/// </summary>
/// <returns></returns>
public Line2d[] ToLines()
{
// Inspired by line extraction conmat.c from book Graphics Gems V
double xx, yy;
ConicType type = Type;
Line2d tmplin;
Transform2d tr;
GeneralConic2d cpy;
List <Line2d> res = null;
double de = B * B * 0.25 - A * C;
if (MathUtil.IsZero(A) && MathUtil.IsZero(B) && MathUtil.IsZero(C))
{ //single line
// compute endpoints of the line, avoiding division by zero
res = new List <Line2d>();
if (Math.Abs(d) > Math.Abs(e))
{
res.Add(new Line2d(-f / (d), 0.0, -(e + f) / (d), 1.0));
}
else
{
res.Add(new Line2d(0.0, -f / (e), 1.0, -(d + f) / (e)));
}
}
else
{ // two lines
cpy = new GeneralConic2d(this);
double a = cpy.a, b = cpy.b * 0.5, c = cpy.c, d = cpy.d * 0.5, e = cpy.e * 0.5, f = cpy.f; //get matrix coefficient
// use the expression for phi that takes atan of the smallest argument
double phi = (Math.Abs(b + b) < Math.Abs(a - c) ?
Math.Atan((b + b) / (a - c)) :
MathUtil.Deg360 - Math.Atan((a - c) / (b + b))) / 2.0;
//phi = cpy.Rotation;
if (MathUtil.IsZero(de))
{ //parallel lines
tr = Transform2d.Rotate(-phi);
cpy.Transform(tr);
a = cpy.A; b = cpy.B * 0.5; c = cpy.c; d = cpy.d * 0.5; e = cpy.e * 0.5; f = cpy.f; //get matrix coefficient
if (Math.Abs(c) < Math.Abs(a)) // vertical
{
double[] xs = RealPolynomial.SolveQuadric(a, d, f);
if (xs != null)
{
res = new List <Line2d>();
foreach (double x in xs)
{
tmplin = new Line2d(x, -1, x, 1);
tmplin.Transform(tr.Inversed); //back to original spacxe
res.Add(tmplin);
}
}
}
else //horizontal
{
double[] ys = RealPolynomial.SolveQuadric(c, e, f, 0.0);
if (ys != null)
{
res = new List <Line2d>();
foreach (double y in ys)
{
tmplin = new Line2d(-1, y, 1, y);
tmplin.Transform(tr.Inversed);
res.Add(tmplin);
}
}
}
} //end parallel lines case
else
{ //crossing lines
Point2d center = Center;
double rot = this.Rotation;
tr = Transform2d.Translate(-center.X, -center.Y) * Transform2d.Rotate(-rot);
cpy.Transform(tr);
a = cpy.A; b = cpy.B * 0.5; c = cpy.c; d = cpy.c * 0.5; e = cpy.e * 0.5; f = cpy.f;
res = new List <Line2d>();
xx = Math.Sqrt(Math.Abs(1.0 / a));
yy = Math.Sqrt(Math.Abs(1.0 / c));
tr = tr.Inversed;
tmplin = new Line2d(-xx, -yy, xx, yy);
tmplin.Transform(tr);
res.Add(tmplin);
tmplin = new Line2d(new Line2d(-xx, yy, xx, -yy));
tmplin.Transform(tr);
res.Add(tmplin);
} //end crossing lines case
} //end two lines
if (res == null || res.Count == 0)
{
return(null);
}
return(res.ToArray());
}
