775 lines
27 KiB
C#
775 lines
27 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Numerics;
|
|
using System.Reflection;
|
|
using System.Runtime.CompilerServices;
|
|
using System.Runtime.InteropServices;
|
|
|
|
namespace CommonUsage.Mathematics
|
|
{
|
|
|
|
using T3 = Tuple<float, float, float>;
|
|
using D3 = Tuple<double, double, double>;
|
|
|
|
public class CommonMath
|
|
{
|
|
public class PrimeEnumerator<T>
|
|
{
|
|
public PrimeEnumerator(List<T> items, Func<T, bool> process)
|
|
{
|
|
_n = items.Count;
|
|
_items = items;
|
|
_process = process;
|
|
|
|
foreach (var pNum in _primes)
|
|
{
|
|
if (_n % pNum != 0)
|
|
{
|
|
_a = pNum;
|
|
_b = 11;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Enumerate()
|
|
{
|
|
using (var enumerator = Get().GetEnumerator())
|
|
{
|
|
while (enumerator.MoveNext()) { }
|
|
}
|
|
}
|
|
|
|
private readonly int _n, _a, _b;
|
|
private readonly int[] _primes = new[] { 29, 23, 19, 17, 13 };
|
|
private List<T> _items;
|
|
private readonly Func<T, bool> _process;
|
|
|
|
private IEnumerable<bool> Get()
|
|
{
|
|
for (var i = 0; i < _n; ++i)
|
|
{
|
|
var id = (i * _a + _b) % _n;
|
|
yield return _process(_items[id]);
|
|
}
|
|
}
|
|
}
|
|
|
|
private static IEnumerable<IEnumerable<T>> GetPermutationsInternal<T>(IEnumerable<T> list, int length)
|
|
{
|
|
if (length == 1) return list.Select(t => new T[] { t });
|
|
|
|
return GetPermutationsInternal(list, length - 1)
|
|
.SelectMany(t => list.Where(e => !t.Contains(e)),
|
|
(t1, t2) => t1.Concat(new T[] { t2 }));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 得到一组数据的所有排列。
|
|
/// </summary>
|
|
/// <typeparam name="T">元素数据类型</typeparam>
|
|
/// <param name="list">所有待选元素</param>
|
|
/// <param name="selectNum">所选出的元素数量</param>
|
|
/// <returns></returns>
|
|
public static List<List<T>> GetPermutations<T>(List<T> list, int selectNum)
|
|
{
|
|
return GetPermutationsInternal(list, selectNum).Select(ll => ll.ToList()).ToList();
|
|
}
|
|
|
|
public static (float bias, Vector2 hPnt, float d) Project2DLine(Vector2 pnt, Vector2 segSt,
|
|
Vector2 segEnd)
|
|
{
|
|
var dir = Vector2.Normalize(segEnd - segSt);
|
|
var fd = Vector2.Dot(pnt - segSt, dir);
|
|
var hPnt = segSt + fd * dir;
|
|
var bias = dir.X * (pnt.Y-segSt.Y) - (pnt.X-segSt.X) * dir.Y;
|
|
return (bias, hPnt, fd);
|
|
}
|
|
|
|
public static (float bias, Vector2 hPnt, float fd) Project2DLine(Vector2 pnt, Vector2 segSt, float tangent)
|
|
{
|
|
var dir = new Vector2((float)System.Math.Cos(tangent / 180 * System.Math.PI), (float)System.Math.Sin(tangent / 180 * System.Math.PI));
|
|
var fd = Vector2.Dot(pnt - segSt, dir);
|
|
var hPnt = segSt + fd * dir;
|
|
var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
|
|
return (bias, hPnt, fd);
|
|
}
|
|
|
|
public class LineEqu
|
|
{
|
|
public double A, B, C, ln, dAB;
|
|
public double px1, px2, py1, py2;
|
|
public float midX;
|
|
public float midY;
|
|
}
|
|
// Fit line with PCA.
|
|
public LineEqu CalcLine(IEnumerable<Vector2> tls)
|
|
{
|
|
var lidarPoint2Ds = tls as Vector2[] ?? tls.ToArray();
|
|
float fx = lidarPoint2Ds.Average(f => f.X);
|
|
float fy = lidarPoint2Ds.Average(f => f.Y);
|
|
float fxx = lidarPoint2Ds.Average(f => f.X * f.X);
|
|
float fxy = lidarPoint2Ds.Average(f => f.X * f.Y);
|
|
float fyy = lidarPoint2Ds.Average(f => f.Y * f.Y);
|
|
float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy;
|
|
double sqt = System.Math.Sqrt((a - c) * (a - c) + 4 * b * b);
|
|
double l1 = a + c + sqt;
|
|
double l2 = a + c - sqt;
|
|
double dx, dy;
|
|
if (System.Math.Abs(a - l1 / 2) > System.Math.Abs(c - l1 / 2))
|
|
{
|
|
dy = l1 / 2 - a; dx = b;
|
|
}
|
|
else
|
|
{
|
|
dx = l1 / 2 - c; dy = b;
|
|
}
|
|
double norm = System.Math.Sqrt(dx * dx + dy * dy);
|
|
dx /= norm; dy /= norm;
|
|
double A = dy, B = -dx, C = dx * fy - dy * fx;
|
|
double dAB = System.Math.Sqrt(A * A + B * B);
|
|
|
|
return new CommonMath.LineEqu
|
|
{
|
|
A = A,
|
|
B = B,
|
|
C = C,
|
|
ln = lidarPoint2Ds.Average(p => System.Math.Abs(p.X * A + p.Y * B + C) / dAB),
|
|
midX = fx,
|
|
midY = fy
|
|
};
|
|
}
|
|
|
|
public static double QuadInterp3(double[] confsF)
|
|
{
|
|
if (confsF[0] > confsF[1] && confsF[0] > confsF[2])
|
|
{
|
|
//printf("left overflow...\n");
|
|
return -1;
|
|
}
|
|
|
|
if (confsF[1] > confsF[0] && confsF[1] > confsF[2])
|
|
{
|
|
return (-(confsF[2] - confsF[0]) / 2.0f / (confsF[0] + confsF[2] - 2.0f * confsF[1] + 0.0001f));
|
|
}
|
|
if (confsF[2] > confsF[0] && confsF[2] > confsF[1])
|
|
{
|
|
//printf("right overflow...\n");
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
public static double cross(PointF O, PointF A, PointF B)
|
|
{
|
|
return (A.X - O.X) * (B.Y - O.Y) - (A.Y - O.Y) * (B.X - O.X);
|
|
}
|
|
|
|
public static List<PointF> GetConvexHull(List<PointF> points)
|
|
{
|
|
if (points == null)
|
|
return null;
|
|
|
|
if (points.Count() <= 1)
|
|
return points;
|
|
|
|
int n = points.Count(), k = 0;
|
|
List<PointF> H = new List<PointF>(new PointF[2 * n]);
|
|
|
|
points.Sort((a, b) =>
|
|
a.X == b.X ? a.Y.CompareTo(b.Y) : a.X.CompareTo(b.X));
|
|
|
|
// Build lower hull
|
|
for (int i = 0; i < n; ++i)
|
|
{
|
|
while (k >= 2 && cross(H[k - 2], H[k - 1], points[i]) <= 0)
|
|
k--;
|
|
H[k++] = points[i];
|
|
}
|
|
|
|
// Build upper hull
|
|
for (int i = n - 2, t = k + 1; i >= 0; i--)
|
|
{
|
|
while (k >= t && cross(H[k - 2], H[k - 1], points[i]) <= 0)
|
|
k--;
|
|
H[k++] = points[i];
|
|
}
|
|
|
|
return H.Take(k - 1).ToList();
|
|
}
|
|
|
|
public static bool IsPointInPolygon4(PointF[] polygon, PointF testPoint)
|
|
{
|
|
// ray casting odd even test.
|
|
bool result = false;
|
|
int j = polygon.Count() - 1;
|
|
for (int i = 0; i < polygon.Count(); i++)
|
|
{
|
|
if (polygon[i].Y < testPoint.Y && polygon[j].Y >= testPoint.Y ||
|
|
polygon[j].Y < testPoint.Y && polygon[i].Y >= testPoint.Y)
|
|
{
|
|
if (polygon[i].X + (testPoint.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) *
|
|
(polygon[j].X - polygon[i].X) < testPoint.X)
|
|
{
|
|
result = !result;
|
|
}
|
|
}
|
|
|
|
j = i;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
public static double Exp(double val)
|
|
{
|
|
if (val < -20) return 0.0000001;
|
|
if (val > 20) return 99999999999999;
|
|
long tmp = (long)(1512775 * val + 1072632447);
|
|
return BitConverter.Int64BitsToDouble(tmp << 32);
|
|
}
|
|
public static double gaussmf(double x, double sig, double c)
|
|
{
|
|
return Exp(-(x - c) * (x - c) / (2 * sig * sig));
|
|
}
|
|
|
|
public static float Exp(float x)
|
|
{
|
|
if (x < -10) return 0;
|
|
if (x > 10) return 99999999999999;
|
|
x = 1.0f + x / 64f;
|
|
x *= x;
|
|
x *= x;
|
|
x *= x;
|
|
x *= x;
|
|
x *= x;
|
|
x *= x;
|
|
return x;
|
|
}
|
|
public static float gaussmf(float x, float sig, float c)
|
|
{
|
|
return Exp(-(x - c) * (x - c) / (2 * sig * sig));
|
|
}
|
|
|
|
public static D3 Transform2D(D3 src, D3 t)
|
|
{
|
|
var rth = src.Item3 / 180.0 * System.Math.PI;
|
|
var p1dtx = (src.Item1 + System.Math.Cos(rth) * t.Item1 -
|
|
System.Math.Sin(rth) * t.Item2);
|
|
var p1dty = (src.Item2 + System.Math.Sin(rth) * t.Item1 +
|
|
System.Math.Cos(rth) * t.Item2);
|
|
var p1dtth = src.Item3 + t.Item3;
|
|
return Tuple.Create(p1dtx, p1dty, p1dtth);
|
|
}
|
|
|
|
public struct LngLatToXY
|
|
{
|
|
public double scale;
|
|
public double rad;
|
|
public double biasX, biasY;
|
|
}
|
|
|
|
public static LngLatToXY GetTransformLngLatToXY(Vector2 lnglat1, Vector2 xy1, Vector2 lnglat2, Vector2 xy2)
|
|
{
|
|
var scale = (xy1 - xy2).Length() / (lnglat1 - lnglat2).Length();
|
|
var dxy = (xy1 - xy2);
|
|
var dlnglat = lnglat1 - lnglat2;
|
|
var rad = System.Math.Atan2(dxy.X, dxy.Y) - System.Math.Atan2(dlnglat.X, dlnglat.Y);
|
|
var intm = lnglat1 * scale;
|
|
var biasX = xy1.X - (intm.X * System.Math.Cos(rad) - intm.Y * System.Math.Sin(rad));
|
|
var biasY = xy1.Y - (intm.X * System.Math.Sin(rad) + intm.Y * System.Math.Cos(rad));
|
|
return new LngLatToXY {rad = rad, biasX = biasX, biasY = biasY, scale = scale};
|
|
}
|
|
|
|
public Vector2 TransformLngLatToXY(Vector2 lnglat, LngLatToXY t)
|
|
{
|
|
var intm = lnglat * (float) t.scale;
|
|
return new Vector2((float) (intm.X * System.Math.Cos(t.rad) - intm.Y * System.Math.Sin(t.rad) + t.biasX),
|
|
(float) (intm.X * System.Math.Sin(t.rad) + intm.Y * System.Math.Cos(t.rad) + t.biasY));
|
|
}
|
|
|
|
public static D3 ReverseTransform(D3 dest, D3 t)
|
|
{
|
|
var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
|
|
var nxT = (dest.Item1 - System.Math.Cos(rth) * t.Item1 +
|
|
System.Math.Sin(rth) * t.Item2);
|
|
var nyT = (dest.Item2 - System.Math.Sin(rth) * t.Item1 -
|
|
System.Math.Cos(rth) * t.Item2);
|
|
var pth = dest.Item3 - t.Item3;
|
|
return Tuple.Create(nxT, nyT, pth);
|
|
}
|
|
|
|
public static D3 SolveTransform2D(D3 src, D3 dest)
|
|
{
|
|
var th = dest.Item3 - src.Item3;
|
|
th = (th - System.Math.Round((th) / 360.0f) * 360);
|
|
var rth = src.Item3 / 180.0 * System.Math.PI;
|
|
var x = ((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
|
|
(dest.Item2 - src.Item2) * System.Math.Sin(rth));
|
|
var y = (-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
|
|
(dest.Item2 - src.Item2) * System.Math.Cos(rth));
|
|
return Tuple.Create(x, y, th);
|
|
}
|
|
|
|
public static T3 Transform2D(T3 src, T3 t)
|
|
{
|
|
var rth = src.Item3 / 180.0 * System.Math.PI;
|
|
var p1dtx = (float)(src.Item1 + System.Math.Cos(rth) * t.Item1 -
|
|
System.Math.Sin(rth) * t.Item2);
|
|
var p1dty = (float)(src.Item2 + System.Math.Sin(rth) * t.Item1 +
|
|
System.Math.Cos(rth) * t.Item2);
|
|
var p1dtth = src.Item3 + t.Item3;
|
|
return Tuple.Create(p1dtx, p1dty, p1dtth);
|
|
}
|
|
|
|
public static Vector2 Transform2D(Vector3 src, Vector3 t)
|
|
{
|
|
var tup = Transform2D(Tuple.Create(src.X, src.Y, src.Z), Tuple.Create(t.X, t.Y, t.Z));
|
|
return new Vector2(tup.Item1, tup.Item2);
|
|
}
|
|
|
|
public static Vector2 Transform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
|
|
{
|
|
var tup = Transform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
|
|
return new Vector2(tup.Item1, tup.Item2);
|
|
}
|
|
|
|
public static T3 ReverseTransform(T3 dest, T3 t)
|
|
{
|
|
var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
|
|
var nxT = (float)(dest.Item1 - System.Math.Cos(rth) * t.Item1 +
|
|
System.Math.Sin(rth) * t.Item2);
|
|
var nyT = (float)(dest.Item2 - System.Math.Sin(rth) * t.Item1 -
|
|
System.Math.Cos(rth) * t.Item2);
|
|
var pth = dest.Item3 - t.Item3;
|
|
return Tuple.Create(nxT, nyT, pth);
|
|
}
|
|
|
|
public static T3 SolveTransform2D(T3 src, T3 dest)
|
|
{
|
|
var th = dest.Item3 - src.Item3;
|
|
th = (float)(th - System.Math.Round((th) / 360.0f) * 360);
|
|
var rth = src.Item3 / 180.0 * System.Math.PI;
|
|
var x = (float)((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
|
|
(dest.Item2 - src.Item2) * System.Math.Sin(rth));
|
|
var y = (float)(-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
|
|
(dest.Item2 - src.Item2) * System.Math.Cos(rth));
|
|
return Tuple.Create(x, y, th);
|
|
}
|
|
|
|
public static Vector2 SolveTransform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
|
|
{
|
|
var tup = SolveTransform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
|
|
return new Vector2(tup.Item1, tup.Item2);
|
|
}
|
|
|
|
public static double dist(double x1, double y1, double x2, double y2)
|
|
{
|
|
return System.Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
private struct FloatIntUnion
|
|
{
|
|
[FieldOffset(0)] public float f;
|
|
|
|
[FieldOffset(0)] public int tmp;
|
|
}
|
|
|
|
public static float Sqrt(float z)
|
|
{
|
|
FloatIntUnion u;
|
|
u.tmp = 0;
|
|
u.f = z;
|
|
u.tmp -= 1 << 23; /* Subtract 2^m. */
|
|
u.tmp >>= 1; /* Divide by 2. */
|
|
u.tmp += 1 << 29; /* Add ((b + 1) / 2) * 2^m. */
|
|
return u.f;
|
|
}
|
|
|
|
public static float dist2(float x1, float y1, float x2, float y2)
|
|
{
|
|
return ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
|
|
}
|
|
|
|
|
|
public static float d2(float x1, float y1, float x2, float y2)
|
|
{
|
|
return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
|
|
}
|
|
|
|
public static float ThAverage(List<float> angles)
|
|
{
|
|
var anchor = angles[0];
|
|
var diff = 0f;
|
|
foreach (var angle in angles)
|
|
diff += ThDiff(angle, anchor);
|
|
return RoundTh(anchor + diff / angles.Count);
|
|
}
|
|
|
|
public static float ThDiff(float th1, float th2)
|
|
{
|
|
return (float)(th1 - th2 -
|
|
System.Math.Round((th1 - th2) / 360.0f) * 360);
|
|
}
|
|
|
|
public static double ThDiff(double th1, double th2)
|
|
{
|
|
return th1 - th2 -
|
|
System.Math.Round((th1 - th2) / 360.0f) * 360;
|
|
}
|
|
|
|
public static double refine(double x)
|
|
{
|
|
if (x < 1 && x > -1) return x;
|
|
if (x > 1)
|
|
return (2 / (1 + System.Math.Exp(-((x - 1) * 2))));
|
|
return (2 / (1 + System.Math.Exp(-((x + 1) * 2)))) - 2;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 求点p到两点式直线p1p2的距离
|
|
/// </summary>
|
|
/// <param name="x">点p的x坐标</param>
|
|
/// <param name="y">点p的y坐标</param>
|
|
/// <param name="x1">直线点p1的x坐标</param>
|
|
/// <param name="y1">直线点p1的y坐标</param>
|
|
/// <param name="x2">直线点p2的x坐标</param>
|
|
/// <param name="y2">直线点p2的y坐标</param>
|
|
/// <returns></returns>
|
|
public static double Point2LineDist(double x, double y, double x1, double y1, double x2, double y2)
|
|
{
|
|
double a1 = -(y1 - y2) / 10;
|
|
double b1 = (x1 - x2) / 10;
|
|
double c1 = (x1 * (y1 - y2) - y1 * (x1 - x2)) / 10;
|
|
return System.Math.Abs(a1 * x + b1 * y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
|
|
}
|
|
|
|
public static double Point2LineDist(Vector2 p, LineSegment ll)
|
|
{
|
|
double a1 = -(ll.Src.Y - ll.Dst.Y) / 10;
|
|
double b1 = (ll.Src.X - ll.Dst.X) / 10;
|
|
double c1 = (ll.Src.X * (ll.Src.Y - ll.Dst.Y) - ll.Src.Y * (ll.Src.X - ll.Dst.X)) / 10;
|
|
return System.Math.Abs(a1 * p.X + b1 * p.Y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 两条两点式直线间的夹角
|
|
/// </summary>
|
|
/// <param name="x1"></param>
|
|
/// <param name="y1"></param>
|
|
/// <param name="x2"></param>
|
|
/// <param name="y2"></param>
|
|
/// <param name="x3"></param>
|
|
/// <param name="y3"></param>
|
|
/// <param name="x4"></param>
|
|
/// <param name="y4"></param>
|
|
/// <returns>角度制</returns>
|
|
public static double AngleBetweenLines(double x1, double y1, double x2, double y2, double x3, double y3,
|
|
double x4, double y4)
|
|
{
|
|
var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
|
|
var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
|
|
return System.Math.Acos(System.Math.Abs(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length())) / System.Math.PI * 180;
|
|
}
|
|
|
|
public static double AngleBetweenLines(LineSegment ls1, LineSegment ls2)
|
|
{
|
|
return AngleBetweenLines(ls1.Src.X, ls1.Src.Y, ls1.Dst.X, ls1.Dst.Y, ls2.Src.X, ls2.Src.Y, ls2.Dst.X,
|
|
ls2.Dst.Y);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 两向量间夹角
|
|
/// </summary>
|
|
/// <param name="x1"></param>
|
|
/// <param name="y1"></param>
|
|
/// <param name="x2"></param>
|
|
/// <param name="y2"></param>
|
|
/// <param name="x3"></param>
|
|
/// <param name="y3"></param>
|
|
/// <param name="x4"></param>
|
|
/// <param name="y4"></param>
|
|
/// <returns>角度制</returns>
|
|
public static double AngleBetweenVectors(double x1, double y1, double x2, double y2, double x3, double y3,
|
|
double x4, double y4)
|
|
{
|
|
var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
|
|
var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
|
|
return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 两向量间夹角.
|
|
/// </summary>
|
|
/// <param name="vec1"></param>
|
|
/// <param name="vec2"></param>
|
|
/// <returns>角度制</returns>
|
|
public static double AngleBetweenVectors(Vector2 vec1, Vector2 vec2)
|
|
{
|
|
return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
|
|
}
|
|
|
|
public static double AngleBetweenVectors(Vector3 vector1, Vector3 vector2)
|
|
{
|
|
float dotProduct = Vector3.Dot(vector1, vector2);
|
|
float magnitude1 = vector1.Length();
|
|
float magnitude2 = vector2.Length();
|
|
float cosine = dotProduct / (magnitude1 * magnitude2);
|
|
|
|
return System.Math.Acos(cosine) / System.Math.PI * 180;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 求点到直线的垂足
|
|
/// </summary>
|
|
/// <param name="x"></param>
|
|
/// <param name="y"></param>
|
|
/// <param name="x1"></param>
|
|
/// <param name="y1"></param>
|
|
/// <param name="x2"></param>
|
|
/// <param name="y2"></param>
|
|
/// <returns></returns>
|
|
public static (double, double) PerpendicularPoint(double x, double y, double x1, double y1, double x2, double y2)
|
|
{
|
|
double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
|
|
var u = ((x - x1) * lx + (y - y1) * ly) / dAB;
|
|
return new(x1 + u * lx, y1 + u * ly);
|
|
}
|
|
|
|
public static Vector2 PerpendicularPoint(Vector2 p, LineSegment ls)
|
|
{
|
|
double lx = ls.Dst.X - ls.Src.X, ly = ls.Dst.Y - ls.Src.Y, dAB = lx * lx + ly * ly;
|
|
var u = ((p.X - ls.Src.X) * lx + (p.Y - ls.Src.Y) * ly) / dAB;
|
|
return new Vector2((float)(ls.Src.X + u * lx), (float)(ls.Src.Y + u * ly));
|
|
}
|
|
|
|
public static double PerpendicularPosition(double x, double y, double x1, double y1, double x2, double y2)
|
|
{
|
|
double lx = x2 - x1, ly = y2 - y1;
|
|
var dAB = CommonMath.Sqrt((float)(lx * lx + ly * ly));
|
|
lx /= dAB;
|
|
ly /= dAB;
|
|
return (x - x1) * lx + (y - y1) * ly;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// 最小二乘法拟合直线，得到两点式。
|
|
/// </summary>
|
|
/// <param name="pts">待拟合的点集，应至少有2个点。</param>
|
|
/// <param name="maxDist2Line">检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。</param>
|
|
/// <returns>返回两点式的两个端点坐标。若坐标为全0，则拟合失败。</returns>
|
|
public static (bool, Vector2, Vector2) FitLineSegment(List<Vector2> pts, double maxDist2Line = -1)
|
|
{
|
|
if (pts.Count < 2)
|
|
{
|
|
Console.WriteLine($"Points too Few! {pts.Count}! Cannot perform line fitting!",
|
|
MethodBase.GetCurrentMethod()?.Name ?? "FitLine");
|
|
return (false, Vector2.Zero, Vector2.Zero);
|
|
};
|
|
|
|
// y = kx + b
|
|
double A = 0, B = 0, C = 0, D = 0;
|
|
foreach (var p in pts)
|
|
{
|
|
A += p.X * p.X;
|
|
B += p.X;
|
|
C += p.X * p.Y;
|
|
D += p.Y;
|
|
}
|
|
|
|
var tmp = A * pts.Count - B * B;
|
|
var k = (C * pts.Count - B * D) / tmp;
|
|
var b = (A * D - C * B) / tmp;
|
|
|
|
double x1 = 0,
|
|
y1 = k * x1 + b,
|
|
x2 = 1000,
|
|
y2 = k * x2 + b;
|
|
|
|
double CalcDist(ref bool fail, ref Vector2 endP, ref Vector2 endQ)
|
|
{
|
|
double distSum = 0;
|
|
double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
|
|
double minU = double.MaxValue, maxU = double.MinValue;
|
|
|
|
foreach (var p in pts)
|
|
{
|
|
var u = ((p.X - x1) * lx + (p.Y - y1) * ly) / dAB;
|
|
var perp = new Vector2((float)(x1 + u * lx), (float)(y1 + u * ly));
|
|
if (u < minU)
|
|
{
|
|
endP = perp;
|
|
minU = u;
|
|
}
|
|
if (u > maxU)
|
|
{
|
|
endQ = perp;
|
|
maxU = u;
|
|
}
|
|
|
|
var curDist = dist(perp.X, perp.Y, p.X, p.Y);
|
|
if (maxDist2Line > -1 && curDist > maxDist2Line) fail = true;
|
|
distSum += curDist;
|
|
}
|
|
|
|
return distSum;
|
|
}
|
|
|
|
var kbFail = false;
|
|
Vector2 endP1 = new Vector2(), endQ1 = new Vector2();
|
|
double kbDist = CalcDist(ref kbFail, ref endP1, ref endQ1);
|
|
|
|
// x = my + n
|
|
A = 0;
|
|
B = 0;
|
|
C = 0;
|
|
D = 0;
|
|
foreach (var p in pts)
|
|
{
|
|
A += p.X * p.Y;
|
|
B += p.Y * p.Y;
|
|
C += p.Y;
|
|
D += p.X;
|
|
}
|
|
|
|
tmp = C * C - B * pts.Count;
|
|
var m = (C * D - A * pts.Count) / tmp;
|
|
var n = (A * C - B * D) / tmp;
|
|
y1 = 0;
|
|
x1 = m * y1 + n;
|
|
y2 = 1000;
|
|
x2 = m * y2 + n;
|
|
|
|
var mnFail = false;
|
|
Vector2 endP2 = new Vector2(), endQ2 = new Vector2();
|
|
double mnDist = CalcDist(ref mnFail, ref endP2, ref endQ2);
|
|
|
|
Vector2 endP = endP1, endQ = endQ1;
|
|
if (mnDist < kbDist)
|
|
{
|
|
if (mnFail) return (false, new Vector2(), new Vector2());
|
|
endP = endP2;
|
|
endQ = endQ2;
|
|
}
|
|
else if (kbFail) return (false, new Vector2(), new Vector2());
|
|
|
|
return (true, endP, endQ);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static int toId(int x, int y, int z)
|
|
{
|
|
return (x * 1140671485 + 12820163) ^ (y * 134775813 + 1) ^ (z * 1103515245 + 12345);
|
|
}
|
|
|
|
public class Clustering<T>
|
|
{
|
|
public int numIteration = 3;
|
|
public int itemNumThreshold = 10;
|
|
|
|
public Func<T, T, bool> inRange;
|
|
public Func<List<T>, T> average;
|
|
private readonly List<T> _inputData;
|
|
private Dictionary<T, List<T>> _clusters = new Dictionary<T, List<T>>();
|
|
|
|
public Clustering(List<T> data, Func<T, T, bool> inRange, Func<List<T>, T> average)
|
|
{
|
|
_inputData = data;
|
|
this.inRange = inRange;
|
|
this.average = average;
|
|
}
|
|
|
|
public Dictionary<T, List<T>> GetClusters()
|
|
{
|
|
var tmp = new List<(T center, List<T> items)>();
|
|
|
|
for (var iter = 0; iter < numIteration; iter++)
|
|
{
|
|
tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
|
|
.Select(cluster => (average(cluster.items), new List<T>())).ToList();
|
|
|
|
foreach (var data in _inputData)
|
|
{
|
|
var added = false;
|
|
foreach (var cluster in tmp)
|
|
{
|
|
if (inRange(cluster.center, data))
|
|
{
|
|
cluster.items.Add(data);
|
|
added = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!added)
|
|
tmp.Add((data, new List<T>() { data }));
|
|
}
|
|
}
|
|
|
|
_clusters = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
|
|
.ToDictionary(cluster => cluster.center, cluster => cluster.items);
|
|
return _clusters;
|
|
}
|
|
}
|
|
|
|
public static (bool, Vector2) TwoLinesIntersection(Vector2 A, Vector2 B, Vector2 C, Vector2 D)
|
|
{
|
|
// Line AB represented as a1x + b1y = c1
|
|
double a1 = B.Y - A.Y;
|
|
double b1 = A.X - B.X;
|
|
double c1 = a1 * (A.X) + b1 * (A.Y);
|
|
|
|
// Line CD represented as a2x + b2y = c2
|
|
double a2 = D.Y - C.Y;
|
|
double b2 = C.X - D.X;
|
|
double c2 = a2 * (C.X) + b2 * (C.Y);
|
|
|
|
double determinant = a1 * b2 - a2 * b1;
|
|
|
|
if (determinant == 0)
|
|
{
|
|
// The lines are parallel. This is simplified
|
|
// by returning a pair of FLT_MAX
|
|
return new(false, new Vector2());
|
|
}
|
|
else
|
|
{
|
|
double x = (b2 * c1 - b1 * c2) / determinant;
|
|
double y = (a1 * c2 - a2 * c1) / determinant;
|
|
return (true, new Vector2((float)x, (float)y));
|
|
}
|
|
}
|
|
|
|
public static bool IsAtLeft(Vector2 anchor, Vector2 dest, Vector2 p)
|
|
{
|
|
var v1 = new Vector3(anchor - p, 0);
|
|
var v2 = new Vector3(dest - p, 0);
|
|
return Vector3.Cross(v1, v2).Z > 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度转化至-180到180度的范围内。
|
|
/// </summary>
|
|
/// <param name="th"></param>
|
|
/// <returns></returns>
|
|
public static double RoundTh(double th)
|
|
{
|
|
return th - System.Math.Round(th / 360) * 360;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度转化至-180到180度的范围内。
|
|
/// </summary>
|
|
/// <param name="th"></param>
|
|
/// <returns></returns>
|
|
public static float RoundTh(float th)
|
|
{
|
|
return th - (float)System.Math.Round(th / 360f) * 360f;
|
|
}
|
|
}
|
|
}
|