/// <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));
}
/// <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);
}
public static Trafo3d FromToRH(Axis from, Axis to)
{
var s = Fun.Sign(((int)to) - ((int)from)); // +/- sin(90°)
return(FromTo(from, to, s));
}
public __vt__ GetVector(double alpha)
{
return(Axis0 * Fun.Cos(alpha) + Axis1 * Fun.Sin(alpha));
}
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);
}
/// <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-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));
}
// 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 IsOrthogonalTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Dot(v)));
}
public static __ft__ DistMax(this Vector <__ft__> v0, Vector <__ft__> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Abs(x1 - x0), __zero__, Fun.Max);
static double GetArea(V2d axis0, V2d axis1)
{
return(Fun.Abs(axis0.X * axis1.Y - axis0.Y * axis1.X) * Constant.Pi);
}
public static __ft__ Dist2(this Vector <__ft__> v0, Vector <__ft__> v1) => Fun.Sqrt(v0.Dist2Squared(v1));
public static __ft__ Dist2Squared(this Vector <__ft__> v0, Vector <__ft__> v1) => v0.InnerProduct(v1, (x0, x1) => Fun.Square(x1 - x0), __zero__, (s, p) => s + p);
static double GetArea(V2d axis0, V2d axis1) => Fun.Abs(axis0.X * axis1.Y - axis0.Y * axis1.X) * Constant.Pi;
private static void FastHartleyTransformRaw(
this double[] v, long start, long size)
{
if (size < 2)
{
return;
}
/* ---------------------------------------------------------------
* The transforming part of the function. It assumes that all
* indices are already bit-reversed. For successive evaluation
* of the trigonometric functions the following recurrence is
* used:
*
* a = 2 sin(square(d/2))
* b = sin d
*
* cos(t + d) = cos t - [ a * cos t + b sin t ]
* sin(t + d) = sin t - [ a * sin t - b cos t ]
*
* The algorithm is based on the following recursion:
*
* H[f] = Heven[f]
+ cos(2 PI f / n) Hodd[f]
+ sin(2 PI f / n) Hodd[n-f] f in [0, n-1]
+
+ Heven[n/2 + g] = Heven[g]
+ Hodd [n/2 + g] = Hodd [g] g in [0, n/2-1]
+ --------------------------------------------------------------- */
long end = start + size;
for (long i = start; i < end; i += 2)
{
double h0 = v[i], h1 = v[i + 1];
v[i] = h0 + h1; v[i + 1] = h0 - h1; // f = 0, PI
}
if (size < 4)
{
return;
}
for (long i = start; i < end; i += 4)
{
double h0 = v[i], h2 = v[i + 2];
v[i] = h0 + h2; v[i + 2] = h0 - h2; // f = 0, PI
double h1 = v[i + 1], h3 = v[i + 3];
v[i + 1] = h1 + h3; v[i + 3] = h1 - h3; // f = PI/2, 3 * PI/2
}
if (size < 8)
{
return;
}
for (long i = start; i < end; i += 8)
{
double h0 = v[i], h4 = v[i + 4];
v[i] = h0 + h4; v[i + 4] = h0 - h4; // f = 0, PI
double one = Constant.Sqrt2Half * (v[i + 5] + v[i + 7]);
double two = Constant.Sqrt2Half * (v[i + 7] - v[i + 5]);
double h1 = v[i + 1], h3 = v[i + 3];
v[i + 1] = h1 + one; v[i + 5] = h1 - one;
v[i + 3] = h3 - two; v[i + 7] = h3 + two;
double h2 = v[i + 2], h6 = v[i + 6];
v[i + 2] = h2 + h6; v[i + 6] = h2 - h6; // f = PI/2, 3 * PI/2
}
if (size < 16)
{
return;
}
var sTable = new double[size / 4];
var cTable = new double[size / 4];
for (long len = 8, lenDiv2 = 4, lenMul2 = 16;
len < size;
lenDiv2 = len, len = lenMul2, lenMul2 = 2 * len)
{
double d = Constant.PiTimesTwo / lenMul2;
double a = Fun.Sin(d * 0.5).Square() * 2.0; // init trig. recurrence
double b = Fun.Sin(d);
double ct = 1.0, st = 0.0;
for (long f = 1; f < lenDiv2; f++) // all freqs in the first quadrant
{
double dct = a * ct + b * st, dst = a * st - b * ct; // trig. rec
ct -= dct; st -= dst; // cos (t + d), sin (t + d)
cTable[f] = ct; sTable[f] = st;
}
for (long i0 = start; i0 < end; i0 += lenMul2)
{
long i4 = i0 + len;
double h0 = v[i0], h4 = v[i4];
v[i0] = h0 + h4; v[i4] = h0 - h4; // f = 0, PI
for (long f = 1; f < lenDiv2; f++) // all freqs in the first quadrant
{
long i1 = i0 + f, i3 = i0 + len - f;
long i5 = i1 + len, i7 = i3 + len;
double one = cTable[f] * v[i5] + sTable[f] * v[i7];
double two = cTable[f] * v[i7] - sTable[f] * v[i5];
double h1 = v[i1], h3 = v[i3];
v[i1] = h1 + one; v[i5] = h1 - one; // all four quadrants
v[i3] = h3 - two; v[i7] = h3 + two; //
}
long i2 = i0 + lenDiv2, i6 = i4 + lenDiv2;
double h2 = v[i2], h6 = v[i6];
v[i2] = h2 + h6; v[i6] = h2 - h6; // f = PI/2, 3 * PI/2
}
}
}
// 3-Dimensional
#region V3d - V3d
public static bool IsParallelTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Cross(v).Norm1));
}
/// <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>
/// Perform a QR factorization of the supplied m x n matrix using
/// Householder transofmations, i.e. A = QR, where Q is orthogonal
/// (a product of n-2 Householder-Transformations) and R is a right
/// upper n x n triangular matrix. An array of the diagonal elements
/// of R is returned. WARNING: the supplied matrix A is overwritten
/// with its factorization QR, both in the parameter aqr.
/// </summary>
public static double[] QrFactorize(this double[,] aqr)
{
double[] diag = new double[Fun.Min(aqr.GetLength(0), aqr.GetLength(1))];
aqr.QrFactorize(diag);
return(diag);
}
/// <summary>
/// Returns the Euclidean (or 2-) distance between two matrices.
/// </summary>
public static __ctype__ Distance2(__nmtype__ a, __nmtype__ b)
{
return /*# if (ctype != "double") {*/ ((__ctype__)/*# } */ Fun.Sqrt(/*# n.ForEach(i => { m.ForEach(j => { */
Fun.Square(b.M__i____j__ - a.M__i____j__) /*# }, add); }, add); */));
}
public Fraction(long numerator, long denominator)
{
// ensure positive denominator
Numerator = denominator < 0 ? -numerator : numerator;
Denominator = Fun.Abs(denominator);
}
/// <summary>
/// Transposes this matrix (and returns this).
/// </summary>
public void Transpose()
{
//# for (int r = 1; r < n; r++) { r.ForEach(s => {
Fun.Swap(ref M__r____s__, ref M__s____r__);
//# }); }
}
//# });
#endregion
#region LinCom
//# foreach (var it in Meta.IntegerTypes) { var itn = it.Name;
//# for (int tpc = 4; tpc < 7; tpc+=2) {
//# foreach (var rt in Meta.RealTypes) { var rtn = rt.Name; var rtc = rt.Caps[0];
public static __itn__ LinCom(
/*# tpc.ForEach(i => { */ __itn__ p__i__ /*# }, comma); */, ref Tup__tpc__ <__rtn__> w)
{
return((__itn__)Fun.Clamp(/*# tpc.ForEach(i => { */ p__i__ * w.E__i__ /*# }, add); */, (__rtn__)__itn__.MinValue, (__rtn__)__itn__.MaxValue));
}
public __vt__ GetPoint(double alpha)
{
return(Center + Axis0 * Fun.Cos(alpha) + Axis1 * Fun.Sin(alpha));
}
/// <summary>
/// Compute the Delaunay triangluation of the supplied points. Note that
/// the supplied point array must be by 3 larger than the actual number of
/// points.
/// </summary>
private static void Triangulate2d(V2d[] pa,
out Triangle1i[] ta, out int triangleCount)
{
int vc = pa.Length - 4;
int tcMax = 2 * vc + 2; // sharp upper bound with no degenerates
int ecMax = 6 * vc - 3; // sharp bound: last step replaces all tris
ta = new Triangle1i[tcMax];
var ra = new double[tcMax];
var ca = new V2d[tcMax];
var ea = new Line1i[ecMax];
// -------------------------------------- set up the supertriangle
ta[0] = new Triangle1i(vc, vc + 2, vc + 1);
ra[0] = -1.0;
ta[1] = new Triangle1i(vc, vc + 3, vc + 2);
ra[1] = -1.0;
int tc = 2, cc = 0; // triangle count, complete count
// ------------- superquad vertices at the end of vertex array
var box = new Box2d(pa.Take(vc)).EnlargedBy(0.1);
V2d center = box.Center, size = box.Size;
pa[vc + 0] = box.Min;
pa[vc + 1] = new V2d(box.Min.X, box.Max.Y);
pa[vc + 2] = box.Max;
pa[vc + 3] = new V2d(box.Max.X, box.Min.Y);
// ------------------------------ include the points one at a time
for (int i = 0; i < vc; i++)
{
V2d p = pa[i];
int ec = 0;
/*
* if the point lies inside the circumcircle then the triangle
* is removed and its edges are added to the edge array
*/
for (int ti = cc; ti < tc; ti++)
{
var tr = ta[ti];
double r2 = ra[ti];
if (r2 < 0.0)
{
Triangle2d.ComputeCircumCircleSquared(
pa[tr.I0], pa[tr.I1], pa[tr.I2],
out center, out r2);
ra[ti] = r2; ca[ti] = center;
}
else
{
center = ca[ti];
}
// ---------------- include this if points are sorted by X
if (center.X < p.X && (p.X - center.X).Square() > r2)
{
Fun.Swap(ref ta[ti], ref ta[cc]);
ra[ti] = ra[cc]; ca[ti] = ca[cc];
++cc; continue;
}
if (V2d.DistanceSquared(p, center) <= r2)
{
int nec = ec + 3;
if (nec >= ecMax)
{
ecMax = Fun.Max(nec, (int)(1.1 * (double)ecMax));
Array.Resize(ref ea, ecMax);
}
ea[ec] = tr.Line01; ea[ec + 1] = tr.Line12; ea[ec + 2] = tr.Line20;
--tc; ec = nec;
ta[ti] = ta[tc]; ra[ti] = ra[tc]; ca[ti] = ca[tc];
--ti;
}
}
// ---------------------------------------- tag multiple edges
for (int ei = 0; ei < ec - 1; ei++)
{
for (int ej = ei + 1; ej < ec; ej++)
{
if (ea[ei].I0 == ea[ej].I1 && ea[ei].I1 == ea[ej].I0)
{
ea[ei] = Line1i.Invalid; ea[ej] = Line1i.Invalid;
}
}
}
// ------------------ form new triangles for the current point
for (int ei = 0; ei < ec; ei++)
{
var e = ea[ei];
if (e.I0 < 0 || e.I1 < 0)
{
continue; // skip tagged edges
}
if (tc >= tcMax) // necessary for degenerate cases
{
tcMax = Fun.Max(tcMax + 1, (int)(1.1 * (double)tcMax));
Array.Resize(ref ta, tcMax);
Array.Resize(ref ra, tcMax);
Array.Resize(ref ca, tcMax);
}
ta[tc] = new Triangle1i(e.I0, e.I1, i);
ra[tc++] = -1.0;
}
}
// ------------------ remove triangles with supertriangle vertices
for (int ti = 0; ti < tc; ti++)
{
if (ta[ti].I0 >= vc || ta[ti].I1 >= vc || ta[ti].I2 >= vc)
{
ta[ti--] = ta[--tc];
}
}
triangleCount = tc;
}