Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,774 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace CommonUsage.Mathematics
|
||||
{
|
||||
/// <summary>
|
||||
/// 表示一条线段。
|
||||
/// </summary>
|
||||
public class LineSegment
|
||||
{
|
||||
/// <summary>
|
||||
/// 默认构造函数。所有坐标初始化为0。
|
||||
/// </summary>
|
||||
public LineSegment()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一段2D线段。
|
||||
/// </summary>
|
||||
/// <param name="src">线段起点。</param>
|
||||
/// <param name="dst">线段终点。</param>
|
||||
public LineSegment(Vector2 src, Vector2 dst)
|
||||
{
|
||||
Src = src;
|
||||
Dst = dst;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一段3D线段。
|
||||
/// </summary>
|
||||
/// <param name="src">线段起点。</param>
|
||||
/// <param name="dst">线段终点。</param>
|
||||
public LineSegment(Vector3 src, Vector3 dst)
|
||||
{
|
||||
Src3D = src;
|
||||
Dst3D = dst;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一条线段，2D。
|
||||
/// </summary>
|
||||
/// <param name="x1">线段起点x坐标。</param>
|
||||
/// <param name="y1">线段起点y坐标。</param>
|
||||
/// <param name="x2">线段终点x坐标。</param>
|
||||
/// <param name="y2">线段终点y坐标。</param>
|
||||
public LineSegment(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
Src = new Vector2((float)x1, (float)y1);
|
||||
Dst = new Vector2((float)x2, (float)y2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一条线段，2D。
|
||||
/// </summary>
|
||||
/// <param name="x1">线段起点x坐标。</param>
|
||||
/// <param name="y1">线段起点y坐标。</param>
|
||||
/// <param name="z1">线段起点y坐标。</param>
|
||||
/// <param name="x2">线段终点x坐标。</param>
|
||||
/// <param name="y2">线段终点y坐标。</param>
|
||||
/// <param name="z2">线段终点y坐标。</param>
|
||||
public LineSegment(double x1, double y1, double z1, double x2, double y2, double z2)
|
||||
{
|
||||
Src3D = new Vector3((float)x1, (float)y1, (float)z1);
|
||||
Dst3D = new Vector3((float)x2, (float)y2, (float)z2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段长度，2D。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Length()
|
||||
{
|
||||
return Vector2.Distance(Src, Dst);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段长度，3D。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Length3D()
|
||||
{
|
||||
return Vector3.Distance(Src3D, Dst3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段与x轴正方向夹角度数，角度制。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Angle()
|
||||
{
|
||||
return Math.Atan2(Dst.Y - Src.Y, Dst.X - Src.X) / Math.PI * 180;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回一段方向相反的线段。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public LineSegment Reverse()
|
||||
{
|
||||
return new LineSegment(Dst3D, Src3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段2D起点。
|
||||
/// </summary>
|
||||
public Vector2 Src
|
||||
{
|
||||
get => new(_srcX, _srcY);
|
||||
set
|
||||
{
|
||||
_srcX = value.X;
|
||||
_srcY = value.Y;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段2D终点。
|
||||
/// </summary>
|
||||
public Vector2 Dst
|
||||
{
|
||||
get => new(_dstX, _dstY);
|
||||
set
|
||||
{
|
||||
_dstX = value.X;
|
||||
_dstY = value.Y;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段3D起点。
|
||||
/// </summary>
|
||||
public Vector3 Src3D
|
||||
{
|
||||
get => new(_srcX, _srcY, _srcZ);
|
||||
set
|
||||
{
|
||||
_srcX = value.X;
|
||||
_srcY = value.Y;
|
||||
_srcZ = value.Z;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段3D终点。
|
||||
/// </summary>
|
||||
public Vector3 Dst3D
|
||||
{
|
||||
get => new(_dstX, _dstY, _dstZ);
|
||||
set
|
||||
{
|
||||
_dstX = value.X;
|
||||
_dstY = value.Y;
|
||||
_dstZ = value.Z;
|
||||
}
|
||||
}
|
||||
|
||||
private float _srcX, _srcY, _srcZ, _dstX, _dstY, _dstZ;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user