/// <summary>
/// Return real roots of the equation ax^2 + bx + c = 0.
/// Double roots are returned as a pair of identical values.
/// If only one (linear) roots exists, it is stored in the first
/// entry and the second entry is NaN.
/// If no real roots exists, then both entries are NaN.
/// </summary>
public static Pair <double> RealRootsOf(double a, double b, double c)
{
if (Fun.IsTiny(a))
{
if (Fun.IsTiny(b))
{
return(Pair.Create(double.NaN, double.NaN));
}
return(Pair.Create(-c / b, double.NaN));
}
var r = b * b - 4 * a * c;
if (r < 0)
{
return(Pair.Create(double.NaN, double.NaN));
}
if (b < 0) // prevent cancellation
{
double d = -b + Fun.Sqrt(r);
return(Pair.Create(2 * c / d, d / (2 * a)));
}
else
{
double d = -b - Fun.Sqrt(r);
return(Pair.Create(d / (2 * a), 2 * c / d));
}
}
public string ToString(string format)
{
if (!Fun.IsTiny(Real))
{
if (Imag > 0.0f)
{
return Real.ToString(format) + " + i" + Imag.ToString(format);
}
else if (Fun.IsTiny(Imag))
{
return Real.ToString(format);
}
else
{
return Real.ToString(format) + " - i" + System.Math.Abs(Imag).ToString(format);
}
}
else
{
if (Fun.IsTiny(Imag - 1.00)) return "i";
else if (Fun.IsTiny(Imag + 1.00)) return "-i";
if (!Fun.IsTiny(Imag))
{
return Imag.ToString(format) + "i";
}
else return "0";
}
}
/// <summary>
/// Return root of the equation ax + b = 0.
/// Returns NaN if no root exists (a = 0).
/// </summary>
public static double RealRootOf(double a, double b)
{
if (Fun.IsTiny(a))
{
return(double.NaN);
}
return(-b / a);
}
/// <summary>
/// Return real roots of the equation: a x^3 + b x^2 + c x + d = 0.
/// Double and triple solutions are returned as replicated values.
/// Imaginary and non existing solutions are returned as NaNs.
/// </summary>
public static Triple <double> RealRootsOf(
double a, double b, double c, double d)
{
if (Fun.IsTiny(a))
{
var r = RealRootsOf(b, c, d);
return(Triple.Create(r.E0, r.E1, double.NaN));
}
return(RealRootsOfNormed(b / a, c / a, d / a));
}
/// <summary>
/// Return real roots of: a x^4 + b x^3 + c x^2 + d x + e = 0
/// Double and triple solutions are returned as replicated values.
/// Imaginary and non existing solutions are returned as NaNs.
/// </summary>
public static Quad <double> RealRootsOf(
double a, double b, double c, double d, double e)
{
if (Fun.IsTiny(a))
{
var r = RealRootsOf(b, c, d, e);
return(Quad.Create(r.E0, r.E1, r.E2, double.NaN));
}
return(RealRootsOfNormed(b / a, c / a, d / a, e / a));
}
/// <summary>
/// Returns true if the ray hits the other ray before the parameter
/// value contained in the supplied hit. Detailed information about
/// the hit is returned in the supplied hit.
/// </summary>
public bool HitsRay(
Ray3d ray,
double tmin, double tmax,
ref RayHit3d hit
)
{
V3d d = Origin - ray.Origin;
V3d u = Direction;
V3d v = ray.Direction;
V3d n = u.Cross(v);
if (Fun.IsTiny(d.Length))
{
return(true);
}
else if (Fun.IsTiny(u.Cross(v).Length))
{
return(false);
}
else
{
//-t0*u + t1*v + t2*n == d
//M = {-u,v,n}
//M*{t0,t1,t2}T == d
//{t0,t1,t2}T == M^-1 * d
M33d M = new M33d();
M.C0 = -u;
M.C1 = v;
M.C2 = n;
if (M.Invertible)
{
V3d t = M.Inverse * d;
if (Fun.IsTiny(t.Z))
{
ProcessHits(t.X, double.MaxValue, tmin, tmax, ref hit);
return(true);
}
else
{
return(false);
}
}
else
{
return(false);
}
}
}
public V2d Intersect(V2d dirVector)
{
if (Origin.Abs().AllSmaller(Constant <double> .PositiveTinyValue))
{
return(Origin); // Early exit when rays have same origin
}
double cross = Direction.Dot270(dirVector);
if (!Fun.IsTiny(cross)) // Rays not parallel
{
return(Origin + Direction * dirVector.Dot270(Origin) / cross);
}
else // Rays are parallel
{
return(V2d.NaN);
}
}
public V2d Intersect(Ray2d r)
{
V2d a = r.Origin - Origin;
if (a.Abs().AllSmaller(Constant <double> .PositiveTinyValue))
{
return(Origin); // Early exit when rays have same origin
}
double cross = Direction.Dot270(r.Direction);
if (!Fun.IsTiny(cross)) // Rays not parallel
{
return(Origin + Direction * r.Direction.Dot90(a) / cross);
}
else // Rays are parallel
{
return(V2d.NaN);
}
}
/// <summary>
/// Return real roots of: x^4 + c3 x^3 + c2 x^2 + c1 x + c0 = 0.
/// Double and triple solutions are returned as replicated values.
/// Imaginary and non existing solutions are returned as NaNs.
/// </summary>
public static Quad <double> RealRootsOfNormed(
double c3, double c2, double c1, double c0)
{
// eliminate cubic term (x = y - c3/4): x^4 + p x^2 + q x + r = 0
double e = c3 * c3;
double p = -3 / 8.0 * e + c2;
double q = (1 / 8.0 * e - 1 / 2.0 * c2) * c3 + c1;
double r = (1 / 16.0 * c2 - 3 / 256.0 * e) * e - 1 / 4.0 * c3 * c1 + c0;
if (Fun.IsTiny(r)) // ---- no absolute term: y (y^3 + p y + q) = 0
{
return(MergeSortedAndShift(
RealRootsOfDepressed(p, q), 0.0, -1 / 4.0 * c3));
}
// ----------------------- take one root of the resolvent cubic...
double z = OneRealRootOfNormed(
-1 / 2.0 * p, -r, 1 / 2.0 * r * p - 1 / 8.0 * q * q);
// --------------------------- ...to build two quadratic equations
double u = z * z - r;
double v = 2.0 * z - p;
if (u < Constant <double> .PositiveTinyValue) // +tiny instead of 0
{
u = 0.0; // improves unique
}
else // root accuracy by a
{
u = Fun.Sqrt(u); // factor of 10^5!
}
if (v < Constant <double> .PositiveTinyValue) // values greater than
{
v = 0.0; // +tiny == 4 * eps
}
else // do not seem to
{
v = Fun.Sqrt(v); // improve accuraccy!
}
double q1 = q < 0 ? -v : v;
return(MergeSortedAndShift(RealRootsOfNormed(q1, z - u),
RealRootsOfNormed(-q1, z + u),
-1 / 4.0 * c3));
}
/// <summary>
/// Perform a partially pivoting LU factorization of the supplied
/// matrix in-place, i.e A = inv(P) LU, so that any matrix equation
/// A x = b can be solved using LU x = P b. The permutation matrix P
/// is filled into the supplied permutation vector p, and the supplied
/// matrix A is overwritten with its factorization LU, both in the
/// parameter alu. The function returns true if the factorization
/// is successful, otherwise the matrix is singular and false is
/// returned. No size checks are performed.
/// </summary>
public static bool LuFactorize(
this double[] alu, long a0, long ax, long ay, int[] p)
{
long n = p.LongLength;
p.SetByIndex(i => i);
for (long k = 0, ak = a0, a_k = 0; k < n - 1; k++, ak += ay, a_k += ax)
{
long pi = k;
double pivot = alu[ak + a_k], absPivot = Fun.Abs(pivot);
for (long i = k + 1, aik = ak + ay + a_k; i < n; i++, aik += ay)
{
double value = alu[aik], absValue = Fun.Abs(value);
if (absValue > absPivot)
{
pivot = value; absPivot = absValue; pi = i;
}
}
if (Fun.IsTiny(pivot))
{
return(false);
}
if (pi != k)
{
long api = a0 + pi * ay;
Fun.Swap(ref p[pi], ref p[k]);
for (long i = 0, apii = api, aki = ak; i < n; i++, apii += ax, aki += ax)
{
Fun.Swap(ref alu[apii], ref alu[aki]);
}
}
for (long j = k + 1, aj = ak + ay, ajk = aj + a_k; j < n; j++, aj += ay, ajk += ay)
{
double factor = alu[ajk] / pivot; alu[ajk] = factor;
for (long i = k + 1, aji = ajk + ax, aki = ak + a_k + ax; i < n; i++, aji += ax, aki += ax)
{
alu[aji] -= alu[aki] * factor;
}
}
}
return(true);
}
/// <summary>
/// Perform a partially pivoting LU factorization of the supplied
/// matrix, i.e A = inv(P) LU, so that any matrix equation A x = b
/// can be solved using LU x = P b. The permutation matrix P is
/// filled into the supplied permutation vector p, and the supplied
/// matrix A is overwritten with its factorization LU, both in the
/// parameter alu. The function returns true if the factorization
/// is successful, otherwise the matrix is singular and false is
/// returned.
/// </summary>
public static bool LuFactorize(this double[,] alu, int[] p)
{
int n = p.Length;
p.SetByIndex(i => i);
for (int k = 0; k < n - 1; k++)
{
int pi = k;
double pivot = alu[pi, k], absPivot = Fun.Abs(pivot);
for (int i = k + 1; i < n; i++)
{
double value = alu[i, k], absValue = Fun.Abs(value);
if (absValue > absPivot)
{
pivot = value; absPivot = absValue; pi = i;
}
}
if (Fun.IsTiny(pivot))
{
return(false);
}
if (pi != k)
{
Fun.Swap(ref p[pi], ref p[k]);
for (int i = 0; i < n; i++)
{
Fun.Swap(ref alu[pi, i], ref alu[k, i]);
}
}
for (int j = k + 1; j < n; j++)
{
double factor = alu[j, k] / pivot;
alu[j, k] = factor; // construct L
for (int i = k + 1; i < n; i++)
{
alu[j, i] -= alu[k, i] * factor;
}
}
}
return(true);
}
internal static bool Intersects(V2d[] poly, V2d p0, V2d p1, ref List <V2d> known, out bool enter, out int index0, out V2d p)
{
int index = 0;
Line2d l;
int i0 = 0;
int i1 = 1;
for (int i = 1; i <= poly.Length; i++)
{
if (i < poly.Length)
{
i0 = i - 1;
i1 = i;
}
else
{
i0 = poly.Length - 1;
i1 = 0;
}
l = new Line2d(poly[i0], poly[i1]);
if (l.Intersects(new Line2d(p0, p1), 4.0 * double.Epsilon, out p))
{
if (!Fun.IsTiny(known.Closest(p)))
{
enter = (l.LeftValueOfPos(p1) >= -4.0 * double.Epsilon);
index0 = index;
return(true);
}
}
index++;
}
p = V2d.NaN;
index0 = -1;
enter = false;
return(false);
}
public string ToString(string format)
{
if (!Fun.IsTiny(Real))
{
if (Imag > 0.0f)
{
return(Real.ToString(format) + " + i" + Imag.ToString(format));
}
else if (Fun.IsTiny(Imag))
{
return(Real.ToString(format));
}
else
{
return(Real.ToString(format) + " - i" + System.Math.Abs(Imag).ToString(format));
}
}
else
{
if (Fun.IsTiny(Imag - 1.00))
{
return("i");
}
else if (Fun.IsTiny(Imag + 1.00))
{
return("-i");
}
if (!Fun.IsTiny(Imag))
{
return(Imag.ToString(format) + "i");
}
else
{
return("0");
}
}
}
public static bool IsLinearCombinationOf(this V3d x, V3d u, V3d v, out double t0, out double t1)
{
//x == t2*u + t1*v
V3d n = u.Cross(v);
double[,] mat = new double[3, 3]
{
{ u.X, v.X, n.X },
{ u.Y, v.Y, n.Y },
{ u.Z, v.Z, n.Z }
};
double[] result = new double[3] {
x.X, x.Y, x.Z
};
int[] perm = mat.LuFactorize();
V3d t = new V3d(mat.LuSolve(perm, result));
if (Fun.IsTiny(t.Z))
{
t0 = t.X;
t1 = t.Y;
return(true);
}
else
{
t0 = double.NaN;
t1 = double.NaN;
return(false);
}
//x ==
}
// 3-Dimensional
#region V3d - V3d
public static bool IsOrthogonalTo(this V3d u, V3d v) => Fun.IsTiny(u.Dot(v));
// 2-Dimensional
#region V2d - V2d
public static bool IsParallelTo(this V2d u, V2d v)
=> Fun.IsTiny(u.X * v.Y - u.Y * v.X);
// 3-Dimensional
#region V3d - V3d
public static bool IsParallelTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Cross(v).Norm1));
}
// 3-Dimensional
#region V3d - V3d
public static bool IsOrthogonalTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Dot(v)));
}
// 2-Dimensional
#region V2d - V2d
public static bool IsParallelTo(this V2d u, V2d v)
{
return(Fun.IsTiny(u.X * v.Y - u.Y * v.X));
}
// 3-Dimensional
#region V3d - V3d
public static bool IsParallelTo(this V3d u, V3d v)
=> Fun.IsTiny(u.Cross(v).Norm1);
/// <summary>
/// Returns the Line-Segments of line inside the Polygon (CCW ordered).
/// Works with all (convex and non-convex) Polygons
/// </summary>
public static IEnumerable <Line2d> ClipWith(this Line2d line, Polygon2d poly)
{
bool i0, i1;
i0 = poly.Contains(line.P0);
i1 = poly.Contains(line.P1);
List <V2d> resulting = new List <V2d>();
List <bool> enter = new List <bool>();
if (i0)
{
resulting.Add(line.P0);
enter.Add(true);
}
if (i1)
{
resulting.Add(line.P1);
enter.Add(false);
}
V2d p = V2d.NaN;
V2d direction = line.Direction;
foreach (var l in poly.EdgeLines)
{
if (line.Intersects(l, out p))
{
V2d d = l.Direction;
V2d n = new V2d(-d.Y, d.X);
if (!p.IsNaN)
{
bool addflag = true;
bool flag = direction.Dot(n) > 0;
for (int i = 0; i < resulting.Count; i++)
{
if (Fun.IsTiny((resulting[i] - p).Length))
{
if (flag != enter[i])
{
resulting.RemoveAt(i);
enter.RemoveAt(i);
}
addflag = false;
break;
}
}
if (addflag)
{
resulting.Add(p);
enter.Add(flag);
}
}
}
}
V2d dir = line.P1 - line.P0;
resulting = (from r in resulting select r).OrderBy(x => x.Dot(dir)).ToList();
int counter = resulting.Count;
List <Line2d> lines = new List <Line2d>();
for (int i = 0; i < counter - 1; i += 2)
{
lines.Add(new Line2d(resulting[i], resulting[i + 1]));
}
return(lines);
}
