/// <summary>
/// Returns a copy of an array of roots without any double roots with
/// an absolute difference that is smaller than the supplied epsilon.
/// Roots with odd multiplicity will be left as single roots.
/// </summary>
public static double[] WithoutDoubleRoots(
this double[] a, double epsilon)
{
int last = a.Length - 1;
if (last < 4)
{
return(WithoutDoubleRoots4(a, epsilon));
}
var r = new List <double>(last + 1);
int i = 0;
while (i < last)
{
int j = i + 1;
if (Fun.Abs(a[i] - a[j]) < epsilon)
{
i += 2; continue;
}
r.Add(a[i]);
i = j;
}
if (i == last)
{
r.Add(a[i]);
}
return(r.ToArray());
}
/// <summary>
/// Returns the minimal absolute distance between the components of
/// the two matrices.
/// </summary>
public static __ftype__ DistanceMin(__nmtype__ a, __nmtype__ b)
{
return(Fun.Min( /*# n.ForEach(i => { */
Fun.Min( /*# m.ForEach(j => { */
Fun.Abs(b.M__i____j__ - a.M__i____j__) /*#
* }, comma); */) /*# }, comma); */));
}
/// <summary>
/// Computes from a <see cref="V3f"/> point (origin) and
/// a <see cref="V3f"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M44f"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M44f"/>The trafofrom global to local system.</param>
public static void NormalFrame(V3f origin, V3f normal,
out M44f local2global, out M44f global2local
)
{
V3f min;
float x = Fun.Abs(normal.X);
float y = Fun.Abs(normal.Y);
float z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V3f.XAxis;
}
else
{
min = V3f.ZAxis;
}
}
else
{
if (y < z)
{
min = V3f.YAxis;
}
else
{
min = V3f.ZAxis;
}
}
V3f xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V3f yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V3f zVec = normal;
zVec.Normalize();
local2global = new M44f(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M44f mat = new M44f(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M44f.Translation(-origin);
global2local = mat * shift;
}
/// <summary>
/// Computes from a <see cref="V3d"/> point (origin) and
/// a <see cref="V3d"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M44d"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M44d"/>The trafofrom global to local system.</param>
public static void NormalFrame(V3d origin, V3d normal,
out M44d local2global, out M44d global2local
)
{
V3d min;
double x = Fun.Abs(normal.X);
double y = Fun.Abs(normal.Y);
double z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V3d.XAxis;
}
else
{
min = V3d.ZAxis;
}
}
else
{
if (y < z)
{
min = V3d.YAxis;
}
else
{
min = V3d.ZAxis;
}
}
V3d xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V3d yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V3d zVec = normal;
zVec.Normalize();
local2global = new M44d(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M44d mat = new M44d(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M44d.Translation(-origin);
global2local = mat * shift;
}
/// <summary>
/// Computes from a <see cref="V__x3t__"/> point (origin) and
/// a <see cref="V__x3t__"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M4__x4t__"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M4__x4t__"/>The trafofrom global to local system.</param>
public static void NormalFrame(V__x3t__ origin, V__x3t__ normal,
out M4__x4t__ local2global, out M4__x4t__ global2local
)
{
V__x3t__ min;
__ft__ x = Fun.Abs(normal.X);
__ft__ y = Fun.Abs(normal.Y);
__ft__ z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V__x3t__.XAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
else
{
if (y < z)
{
min = V__x3t__.YAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
V__x3t__ xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V__x3t__ yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V__x3t__ zVec = normal;
zVec.Normalize();
local2global = new M4__x4t__(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M4__x4t__ mat = new M4__x4t__(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M4__x4t__.Translation(-origin);
global2local = mat * shift;
}
/// <summary>
/// Computes from a <see cref="V__x3t__"/> normal the transformation matrix
/// from the local coordinate system where the normal is the z-axis to
/// the global coordinate system.
/// </summary>
/// <param name="normal">The normal vector of the new ground plane.</param>
public static M3__x3t__ NormalFrameLocal2Global(V__x3t__ normal)
{
V__x3t__ min;
double x = Fun.Abs(normal.X);
double y = Fun.Abs(normal.Y);
double z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V__x3t__.XAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
else
{
if (y < z)
{
min = V__x3t__.YAxis;
}
else
{
min = V__x3t__.ZAxis;
}
}
var xVec = V__x3t__.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
var yVec = V__x3t__.Cross(normal, xVec);
yVec.Normalize();
var zVec = normal;
zVec.Normalize();
return(new M3__x3t__(xVec.X, yVec.X, zVec.X,
xVec.Y, yVec.Y, zVec.Y,
xVec.Z, yVec.Z, zVec.Z));
}
/// <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);
}
public static float DistMax(this Vector <float> v0, Vector <float> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), 0.0f, Fun.Max);
public static float Dist1(this Vector <float> v0, Vector <float> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), 0.0f, (s, p) => s + p);
public static double DistMax(this Vector <double> v0, Vector <double> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), 0.0, Fun.Max);
public static double Dist1(this Vector <double> v0, Vector <double> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), 0.0, (s, p) => s + p);
static double GetArea(V2d axis0, V2d axis1)
{
return(Fun.Abs(axis0.X * axis1.Y - axis0.Y * axis1.X) * Constant.Pi);
}
/// <summary>
/// Returns the p-distance between two matrices.
/// </summary>
public static __ctype__ Distance(__nmtype__ a, __nmtype__ b, __ctype__ p)
{
return(( /*# n.ForEach(i => { m.ForEach(j => { */
Fun.Abs(b.M__i____j__ - a.M__i____j__).Pow(p) /*# }, add); }, add); */
).Pow(1 / p));
}
/// <summary>
/// Returns the Manhatten (or 1-) distance between two matrices.
/// </summary>
public static __ftype__ Distance1(__nmtype__ a, __nmtype__ b)
{
return/*# n.ForEach(i => { m.ForEach(j => { */
(Fun.Abs(b.M__i____j__ - a.M__i____j__) /*# }, add); }, add); */);
}
/// <summary>
/// Returns the p-norm of the matrix. This is calculated as
/// (|M00|^p + |M01|^p + ... )^(1/p)
/// </summary>
public __ctype__ Norm(__ctype__ p)
{
return(( /*# n.ForEach(i => { m.ForEach(j => { */
Fun.Abs(M__i____j__).Pow(p) /*# }, add); }, add); */
).Pow(1 / p));
}
/// <summary>
/// Returns the rotation of the supplied counter clockwise enumerated
/// convex polygon that results in the minimum area enclosing box.
/// If multiple rotations are within epsilon in their area, the one
/// that is closest to an axis-aligned rotation (0, 90, 180, 270) is
/// returned. O(n).
/// </summary>
public static M22d ComputeMinAreaEnclosingBoxRotation(
this Polygon2d polygon, double epsilon = 1e-6)
{
polygon = polygon.WithoutMultiplePoints(epsilon);
var pc = polygon.PointCount;
if (pc < 2)
{
return(M22d.Identity);
}
var ea = polygon.GetEdgeArray();
ea.Apply(v => v.Normalized);
int i0 = 0, i1 = 0;
int i2 = 0, i3 = 0;
var min = polygon[0]; var max = polygon[0];
for (int pi = 1; pi < pc; pi++)
{
var p = polygon[pi];
if (p.Y < min.Y)
{
i0 = pi; min.Y = p.Y;
}
else if (p.Y > max.Y)
{
i2 = pi; max.Y = p.Y;
}
if (p.X > max.X)
{
i1 = pi; max.X = p.X;
}
else if (p.X < min.X)
{
i3 = pi; min.X = p.X;
}
}
V2d p0 = polygon[i0], e0 = ea[i0], p1 = polygon[i1], e1 = ea[i1];
V2d p2 = polygon[i2], e2 = ea[i2], p3 = polygon[i3], e3 = ea[i3];
int end0 = (i0 + 1) % pc, end1 = (i1 + 1) % pc;
int end2 = (i2 + 1) % pc, end3 = (i3 + 1) % pc;
var dir = V2d.XAxis;
var best = dir;
var bestArea = double.MaxValue;
var bestValue = double.MaxValue;
while (true)
{
var s0 = Fun.FastAtan2(e0.Dot90(dir), e0.Dot(dir));
var s1 = Fun.FastAtan2(e1.Dot180(dir), e1.Dot90(dir));
var s2 = Fun.FastAtan2(e2.Dot270(dir), e2.Dot180(dir));
var s3 = Fun.FastAtan2(e3.Dot(dir), e3.Dot270(dir));
int si, si01, si23; double s01, s23;
if (s0 < s1)
{
s01 = s0; si01 = 0;
}
else
{
s01 = s1; si01 = 1;
}
if (s2 < s3)
{
s23 = s2; si23 = 2;
}
else
{
s23 = s3; si23 = 3;
}
if (s01 < s23)
{
si = si01;
}
else
{
si = si23;
}
if (si == 0)
{
dir = ea[i0];
}
else if (si == 1)
{
dir = ea[i1].Rot270;
}
else if (si == 2)
{
dir = ea[i2].Rot180;
}
else
{
dir = ea[i3].Rot90;
}
double sx = (p2 - p0).Dot90(dir), sy = (p1 - p3).Dot(dir);
double area = sx * sy;
double value = Fun.Min(Fun.Abs(dir.X), Fun.Abs(dir.Y));
if (area < bestArea - epsilon ||
(area < bestArea + epsilon && value < bestValue))
{
bestArea = area; bestValue = value; best = dir;
}
if (si == 0)
{
if (++i0 >= pc)
{
i0 -= pc;
}
if (i0 == end1)
{
break;
}
p0 = polygon[i0]; e0 = ea[i0];
}
else if (si == 1)
{
if (++i1 >= pc)
{
i1 -= pc;
}
if (i1 == end2)
{
break;
}
p1 = polygon[i1]; e1 = ea[i1];
}
else if (si == 2)
{
if (++i2 >= pc)
{
i2 -= pc;
}
if (i2 == end3)
{
break;
}
p2 = polygon[i2]; e2 = ea[i2];
}
else
{
if (++i3 >= pc)
{
i3 -= pc;
}
if (i3 == end0)
{
break;
}
p3 = polygon[i3]; e3 = ea[i3];
}
}
return(new M22d(best.X, best.Y, -best.Y, best.X));
}
public Fraction(long numerator, long denominator)
{
// ensure positive denominator
Numerator = denominator < 0 ? -numerator : numerator;
Denominator = Fun.Abs(denominator);
}
public static __ft__ Dist1(this Vector <__ft__> v0, Vector <__ft__> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), __zero__, (s, p) => s + p);
public static __ft__ DistMax(this Vector <__ft__> v0, Vector <__ft__> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), __zero__, Fun.Max);
public bool HitsCylinder(Cylinder3d cylinder,
double tmin, double tmax,
ref RayHit3d hit)
{
var axisDir = cylinder.Axis.Direction.Normalized;
// Vector Cyl.P0 -> Ray.Origin
var op = Origin - cylinder.P0;
// normal RayDirection - CylinderAxis
var normal = Direction.Cross(axisDir);
var unitNormal = normal.Normalized;
// normal (Vec Cyl.P0 -> Ray.Origin) - CylinderAxis
var normal2 = op.Cross(axisDir);
var t = -normal2.Dot(unitNormal) / normal.Length;
var radius = cylinder.Radius;
if (cylinder.DistanceScale != 0)
{ // cylinder gets bigger, the further away it is
var pnt = GetPointOnRay(t);
var dis = V3d.Distance(pnt, this.Origin);
radius = ((cylinder.Radius / cylinder.DistanceScale) * dis) * 2;
}
// between enitre rays (caps are ignored)
var shortestDistance = Fun.Abs(op.Dot(unitNormal));
if (shortestDistance <= radius)
{
var s = Fun.Abs(Fun.Sqrt(radius.Square() - shortestDistance.Square()) / Direction.Length);
var t1 = t - s; // first hit of Cylinder shell
var t2 = t + s; // second hit of Cylinder shell
if (t1 > tmin && t1 < tmax)
{
tmin = t1;
}
if (t2 < tmax && t2 > tmin)
{
tmax = t2;
}
hit.T = t1;
hit.Point = GetPointOnRay(t1);
// check if found point is outside of Cylinder Caps
var bottomPlane = new Plane3d(cylinder.Circle0.Normal, cylinder.Circle0.Center);
var topPlane = new Plane3d(cylinder.Circle1.Normal, cylinder.Circle1.Center);
var heightBottom = bottomPlane.Height(hit.Point);
var heightTop = topPlane.Height(hit.Point);
// t1 lies outside of caps => find closest cap hit
if (heightBottom > 0 || heightTop > 0)
{
hit.T = tmax;
// intersect with bottom Cylinder Cap
var bottomHit = HitsPlane(bottomPlane, tmin, tmax, ref hit);
// intersect with top Cylinder Cap
var topHit = HitsPlane(topPlane, tmin, tmax, ref hit);
// hit still close enough to cylinder axis?
var distance = cylinder.Axis.Ray3d.GetMinimalDistanceTo(hit.Point);
if (distance <= radius && (bottomHit || topHit))
{
return(true);
}
}
else
{
return(true);
}
}
hit.T = tmax;
hit.Point = V3d.NaN;
return(false);
}
static double GetArea(V2d axis0, V2d axis1) => Fun.Abs(axis0.X * axis1.Y - axis0.Y * axis1.X) * Constant.Pi;