379 lines
17 KiB
C#
379 lines
17 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
|
|
namespace WeldingCommon.Vision.WeldingObjectGenerator.GeometryLibrary
|
|
{
|
|
using TPolygon = List<TPoint>;
|
|
|
|
public class TPoint : IEquatable<TPoint>
|
|
{
|
|
public int x, y;
|
|
public TPoint() { x = y = 0; }
|
|
public TPoint(int x, int y) { this.x = x; this.y = y; }
|
|
public TPoint(TPoint pt) { this.x = pt.x; this.y = pt.y; }
|
|
|
|
public static bool operator <(TPoint p1, TPoint p2) { return p1.x * p1.x + p2.y * p2.y < p2.x * p2.x + p2.y * p2.y; }
|
|
public static bool operator >(TPoint p1, TPoint p2) { return p1.x * p1.x + p2.y * p2.y > p2.x * p2.x + p2.y * p2.y; }
|
|
public static TPoint operator +(TPoint p1, TPoint p2) { return new TPoint(p1.x + p2.x, p1.y + p2.y); }
|
|
public static TPoint operator -(TPoint p1, TPoint p2) { return new TPoint(p1.x - p2.x, p1.y - p2.y); }
|
|
|
|
// == , != 는 보통은 주소를 비교, equals 는 값을 비교하는것인데, 편의상 == , != 를 값비교로 override 하였음
|
|
public static bool operator !=(TPoint p1, TPoint p2) { return p1.x != p2.x || p1.y != p2.y; }
|
|
public static bool operator ==(TPoint p1, TPoint p2) { return p1.x == p2.x && p1.y == p2.y; }
|
|
|
|
#region Defualt override functions
|
|
// 안써줘도 되지만 명시적으로 만들어둔다.
|
|
public override bool Equals(object obj)
|
|
{
|
|
return Equals(obj as TPoint);
|
|
}
|
|
|
|
public bool Equals(TPoint other)
|
|
{
|
|
return !(other is null) &&
|
|
x == other.x &&
|
|
y == other.y;
|
|
}
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
return HashCode.Combine(x, y);
|
|
}
|
|
#endregion
|
|
}
|
|
|
|
public class DPoint
|
|
{
|
|
public double x, y;
|
|
public DPoint() { x = y = 0; }
|
|
public DPoint(double x, double y) { this.x = x; this.y = y; }
|
|
public DPoint(ref DPoint pt) { this.x = pt.x; this.y = pt.y; }
|
|
|
|
public static bool operator <(DPoint p1, DPoint p2) { return p1.x * p1.x + p2.y * p2.y < p2.x * p2.x + p2.y * p2.y; }
|
|
public static bool operator >(DPoint p1, DPoint p2) { return p1.x * p1.x + p2.y * p2.y > p2.x * p2.x + p2.y * p2.y; }
|
|
public static DPoint operator +(DPoint p1, DPoint p2) { return new DPoint(p1.x + p2.x, p1.y + p2.y); }
|
|
public static DPoint operator -(DPoint p1, DPoint p2) { return new DPoint(p1.x - p2.x, p1.y - p2.y); }
|
|
public static bool operator !=(DPoint p1, DPoint p2) { return p1.x != p2.x || p1.y != p2.y; }
|
|
public static bool operator ==(DPoint p1, DPoint p2) { return p1.x == p2.x && p1.y == p2.y; }
|
|
}
|
|
|
|
public class CoordConverter
|
|
{
|
|
// from ptOrg in rect to prNew in (LB,RB,LT,RT)
|
|
public static bool CoordConversionByBilinearInterpolation(ref Rectangle rect, ref TPoint LB, ref TPoint RB, ref TPoint LT, ref TPoint RT, ref TPoint ptOrg, ref double xNew, ref double yNew)
|
|
{
|
|
// Bilinear interpolation을 수행한다.
|
|
// 길이, 면적 계산하여 interpolation에서 사용 (손지수Y 코드 사용)
|
|
double x = ptOrg.x, y = ptOrg.y;
|
|
double x1 = rect.Left, x2 = rect.Right, y1 = rect.Bottom, y2 = rect.Top;
|
|
TPoint p11 = new TPoint(LB), p12 = new TPoint(LT), p21 = new TPoint(RB), p22 = new TPoint(RT);
|
|
|
|
double xx1 = x - x1; double xx2 = x2 - x;
|
|
double yy1 = y - y1; double yy2 = y2 - y;
|
|
|
|
double a11 = xx1 * yy1; double a21 = xx2 * yy1;
|
|
double a12 = xx1 * yy2; double a22 = xx2 * yy2;
|
|
double A = a11 + a12 + a21 + a22;
|
|
|
|
// x,y 좌표 bilinear interpolation
|
|
double output_x = (p11.x * a22 + p12.x * a21 + p21.x * a12 + p22.x * a11) / A;
|
|
double output_y = (p11.y * a22 + p12.y * a21 + p21.y * a12 + p22.y * a11) / A;
|
|
|
|
xNew = output_x;
|
|
yNew = output_y;
|
|
|
|
return true;
|
|
}
|
|
// from ptOrg in rect to prNew in (LB,RB,LT,RT)
|
|
public static bool CoordConversionByBilinearInterpolation(Rectangle rect, TPoint LB, TPoint RB, TPoint LT, TPoint RT, ref double xOrg, ref double yOrg, ref double xNew, ref double yNew)
|
|
{
|
|
// Bilinear interpolation을 수행한다.
|
|
// 길이, 면적 계산하여 interpolation에서 사용 (손지수Y 코드 사용)
|
|
double x = xOrg, y = yOrg;
|
|
double x1 = rect.Left, x2 = rect.Right, y1 = rect.Bottom, y2 = rect.Top;
|
|
TPoint p11 = new TPoint(LB), p12 = new TPoint(LT), p21 = new TPoint(RB), p22 = new TPoint(RT);
|
|
|
|
double xx1 = x - x1; double xx2 = x2 - x;
|
|
double yy1 = y - y1; double yy2 = y2 - y;
|
|
|
|
double a11 = xx1 * yy1; double a21 = xx2 * yy1;
|
|
double a12 = xx1 * yy2; double a22 = xx2 * yy2;
|
|
double A = a11 + a12 + a21 + a22;
|
|
|
|
// x,y 좌표 bilinear interpolation
|
|
double output_x = (p11.x * a22 + p12.x * a21 + p21.x * a12 + p22.x * a11) / A;
|
|
double output_y = (p11.y * a22 + p12.y * a21 + p21.y * a12 + p22.y * a11) / A;
|
|
|
|
xNew = output_x;
|
|
yNew = output_y;
|
|
|
|
return true;
|
|
}
|
|
static void GetPolygonExtent(ref TPolygon Poly, ref int xMin, ref int yMin, ref int xMax, ref int yMax)
|
|
{
|
|
List<int> vX = new List<int>();
|
|
List<int> vY = new List<int>();
|
|
for (int i = 0; i < Poly.Count; i++) {
|
|
vX.Add(Poly[i].x);
|
|
vY.Add(Poly[i].y);
|
|
}
|
|
vX.Sort();
|
|
vY.Sort();
|
|
xMin = vX.First();
|
|
xMax = vX.Last();
|
|
yMin = vY.First();
|
|
yMax = vY.Last();
|
|
}
|
|
}
|
|
|
|
static class Const
|
|
{
|
|
public const double EPSILON = 0.0001;
|
|
}
|
|
public class Vector2 : IEquatable<Vector2>
|
|
{
|
|
public double x, y;
|
|
public Vector2(double x, double y) { this.x = x; this.y = y; }
|
|
public Vector2(ref Vector2 v) { this.x = v.x; this.y = v.y; }
|
|
public Vector2() { x = 0; y = 0; }
|
|
// public double cross(ref Vector2 v) {return x* v.y - y* v.x; }
|
|
public double cross(Vector2 v) { return x * v.y - y * v.x; }
|
|
double scalar(ref Vector2 v) { return x * v.x + y * v.y; }
|
|
public static Vector2 operator *(Vector2 v, double r) { return new Vector2(v.x * r, v.y * r); }
|
|
public static Vector2 operator -(Vector2 v1, Vector2 v2) { return new Vector2(v1.x - v2.x, v1.y - v2.y); }
|
|
public static Vector2 operator +(Vector2 v1, Vector2 v2) { return new Vector2(v1.x + v2.x, v1.y + v2.y); }
|
|
public static bool operator ==(Vector2 v1, Vector2 v2) { return Math.Abs(v1.x - v2.x) < Const.EPSILON && Math.Abs(v1.y - v2.y) < Const.EPSILON; }
|
|
public static bool operator !=(Vector2 v1, Vector2 v2) { return Math.Abs(v1.x - v2.x) > Const.EPSILON || Math.Abs(v1.y - v2.y) > Const.EPSILON; }
|
|
public static bool operator <(Vector2 v1, Vector2 v2) { return Math.Abs(v1.x - v2.x) < Const.EPSILON ? v1.y < v2.y : v1.x < v2.x; }
|
|
public static bool operator >(Vector2 v1, Vector2 v2) { return Math.Abs(v1.x - v2.x) > Const.EPSILON ? v1.y > v2.y : v1.x > v2.x; }
|
|
public double DistanceTo(ref Vector2 Pt)
|
|
{
|
|
return Math.Sqrt((Pt.x - x) * (Pt.x - x) + (Pt.y - y) * (Pt.y - y));
|
|
}
|
|
public Vector2 Rotate(double theta) // radian
|
|
{
|
|
double x = this.x, y = this.y;
|
|
this.x = Math.Cos(theta) * x - Math.Sin(theta) * y;
|
|
this.y = Math.Sin(theta) * x + Math.Cos(theta) * y;
|
|
return this;
|
|
}
|
|
public double GetAngle() // ±PI radian
|
|
{
|
|
if (this.x == 0)
|
|
return 0;
|
|
return Math.Atan2(this.y, this.x);
|
|
}
|
|
public double Length()
|
|
{
|
|
return Math.Sqrt(this.x * this.x + this.y * this.y);
|
|
}
|
|
public double DotProduct(ref Vector2 Pt)
|
|
{
|
|
return this.x * Pt.x + this.y * Pt.y;
|
|
}
|
|
|
|
#region Defualt override functions
|
|
public override bool Equals(object obj)
|
|
{
|
|
return Equals(obj as Vector2);
|
|
}
|
|
|
|
public bool Equals(Vector2 other)
|
|
{
|
|
return !(other is null) &&
|
|
x == other.x &&
|
|
y == other.y;
|
|
}
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
return HashCode.Combine(x, y);
|
|
}
|
|
#endregion
|
|
}
|
|
|
|
class Geometry
|
|
{
|
|
public Geometry() { }
|
|
public bool lineIntersection(Vector2 a, Vector2 b, Vector2 c, Vector2 d, ref Vector2 x)
|
|
{
|
|
double det = (b - a).cross(d - c);
|
|
if (Math.Abs(det) < Const.EPSILON)
|
|
return false; // 평행
|
|
x = a + (b - a) * ((c - a).cross(d - c) / det);
|
|
return true;
|
|
}
|
|
|
|
public double ccw(Vector2 a, Vector2 b) { return a.cross(b); } // 원점 기준으로 벡터 b가 베터 a 의 반시계 방향인가? 반시계 +, 시계 -, 평행 0 return.
|
|
public double ccw(Vector2 p, Vector2 a, Vector2 b) { return ccw(a - p, b - p); } // 점 p 기준으로 벡터 b가 베터 a 의 반시계 방향인가? 반시계 +, 시계 -, 평행 0 return.
|
|
|
|
// 선분 ab, 선분 bc 가 일직선 상에 놓이지 않으면 false, 놓이면 true, 겹치는 경우라면 교차점 p 를 return
|
|
public bool parallelSegments(Vector2 a, Vector2 b, Vector2 c, Vector2 d, ref Vector2 p)
|
|
{
|
|
//2020.6.11. a,b 가 동일한 점이거나, c,d 가 동일한 점인 경우 false return..
|
|
if (a == b || c == d) return false;
|
|
|
|
|
|
// if (b < a) std::swap(a, b);
|
|
// if (d < c) std::swap(c, d);
|
|
if (b < a) (a, b) = (b, a);
|
|
if (d < c) (c, d) = (d, c);
|
|
|
|
// 한 직선 위에 없거나, 두 선분이 겹치지 않으면 false
|
|
if (ccw(a, b, c) != 0 || b < c || d < a) return false;
|
|
|
|
// 두 선분이 겹치는 경우 true
|
|
if (a < c) p = c;
|
|
else p = a;
|
|
return true;
|
|
}
|
|
|
|
// 원점과 a 의 직사각형과 원점과 b 의 직사각형 사이에 p가 놓이는가
|
|
public bool inBoundingRect(ref Vector2 p, Vector2 a, Vector2 b)
|
|
{
|
|
// if (b < a) std::swap(a, b);
|
|
if (b < a) (a, b) = (b, a);
|
|
return p == a || p == b || (a < p && p < b);
|
|
// return p == a || p == b || (a.x < p.x && p.x < b.x); //2019.5.29.. 위에 식이 버그가 있는데.... 이렇게 고치면 또 버그가 생기고....
|
|
}
|
|
|
|
public bool segmentIntersection(Vector2 a, Vector2 b, Vector2 c, Vector2 d, ref Vector2 p)
|
|
{
|
|
if (!lineIntersection(a, b, c, d, ref p))
|
|
return parallelSegments(a, b, c, d, ref p);
|
|
// return inBoundingRect(p,a,b) && inBoundingRect(p,c,d);
|
|
// return ccw(a,p)*ccw(b,p) <= 0 && ccw(c,p)*ccw(d,p) <= 0; //2019.4.27. 원점기준으로 a,b,p 또는 c,d,p 가 모두 일직선에 온 경우 버그 있음.
|
|
// 2019.5.31. 교점이 선분 내인지 판단 방법 변경
|
|
double AP, BP, CP, DP, AB, CD;
|
|
|
|
AP = (a.x - p.x) * (a.x - p.x) + (a.y - p.y) * (a.y - p.y);
|
|
BP = (b.x - p.x) * (b.x - p.x) + (b.y - p.y) * (b.y - p.y);
|
|
AB = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
|
|
if (AP > AB)
|
|
return false;
|
|
if (BP > AB)
|
|
return false;
|
|
CP = (c.x - p.x) * (c.x - p.x) + (c.y - p.y) * (c.y - p.y);
|
|
DP = (d.x - p.x) * (d.x - p.x) + (d.y - p.y) * (d.y - p.y);
|
|
CD = (c.x - d.x) * (c.x - d.x) + (c.y - d.y) * (c.y - d.y);
|
|
if (CP > CD)
|
|
return false;
|
|
if (DP > CD)
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
//2018.12.27
|
|
public double SegmentDistance(Vector2 A, Vector2 B, Vector2 P)
|
|
{
|
|
double dLineLength = A.DistanceTo(ref B);
|
|
if (dLineLength == 0) return A.DistanceTo(ref P); // A==B
|
|
double Prj = ((P.x - A.x) * (B.x - A.x) + (P.y - A.y) * (B.y - A.y)) / dLineLength; // inner product
|
|
if (Prj < 0) return A.DistanceTo(ref P); // outside of segment
|
|
else if (Prj > dLineLength) return B.DistanceTo(ref P); // outside of segment
|
|
return Math.Abs((P.y - A.y) * (B.x - A.x) - (P.x - A.x) * (B.y - A.y)) / dLineLength; // cross product
|
|
}
|
|
|
|
public bool GetIntersectionPointBWLineAndCircle(Vector2 p1, Vector2 p2, Vector2 c, double r, ref Vector2 Ans1, ref Vector2 Ans2) // p1-p2 를 잇는 직선과 중심이 c이고 반경 r 인 원의 교점 구하기. 2020.5.15. by jtj.
|
|
{
|
|
if (Math.Abs(p1.x - p2.x) < Const.EPSILON) // 기울기 무한대
|
|
{
|
|
if (Math.Abs(c.x - p1.x) > r)
|
|
return false; // 교점 없음.
|
|
|
|
Ans1 = new Vector2(p1.x, c.y + Math.Sqrt(r * r - (p1.x - c.x) * (p1.x - c.x)));
|
|
Ans2 = new Vector2(p1.x, c.y - Math.Sqrt(r * r - (p1.x - c.x) * (p1.x - c.x)));
|
|
return true;
|
|
}
|
|
double slope = (p1.y - p2.y) / (p1.x - p2.x);
|
|
return GetIntersectionPointBWLineAndCircle(p1, slope, c, r, ref Ans1, ref Ans2);
|
|
}
|
|
|
|
public bool GetIntersectionPointBWLineAndCircle(Vector2 p1, double slope, Vector2 c, double r, ref Vector2 Ans1, ref Vector2 Ans2) // p1을 지나 기울기 slope 인 직선과 중심이 c이고 반경 r 인 원의 교점 구하기. 2020.5.15. by jtj.
|
|
{
|
|
if (Math.Abs(Math.PI / 2 - slope) < Const.EPSILON) // 기울기 무한대
|
|
{
|
|
if (Math.Abs(c.x - p1.x) > r)
|
|
return false; // 교점 없음.
|
|
|
|
Ans1 = new Vector2(p1.x, c.y + Math.Sqrt(r * r - (p1.x - c.x) * (p1.x - c.x)));
|
|
Ans2 = new Vector2(p1.x, c.y - Math.Sqrt(r * r - (p1.x - c.x) * (p1.x - c.x)));
|
|
return true;
|
|
}
|
|
double bias = -slope * p1.x + p1.y;
|
|
// 계산 편의를 위해, 원의 중심이 0,0이 되도록 좌표 이동 후 다시 좌표 원복 x' = x - c.x , y' = y - c.y. 기울기는 같고 절편만 바뀜.
|
|
double biasMod = slope * c.x + bias - c.y; //원의 중심을 좌표계 중심으로 했을 때 절편.
|
|
double A, B, C; // x'^2 + y'^2 =r^2 에 y' = slope*x' + biasMod 를 대입하여 x' 에 대한 2차식을 만들었을 때의 계수들. 즉 Ax'^2 + Bx' + C = 0 .
|
|
|
|
A = slope * slope + 1;
|
|
B = 2 * slope * biasMod;
|
|
C = biasMod * biasMod - r * r;
|
|
if (B * B - 4 * A * C < 0)
|
|
return false; // 교점 없음.
|
|
|
|
double Xp1 = (-B + Math.Sqrt(B * B - 4 * A * C)) / 2 / A;
|
|
double Xp2 = (-B - Math.Sqrt(B * B - 4 * A * C)) / 2 / A;
|
|
double Yp1 = slope * Xp1 + biasMod;
|
|
double Yp2 = slope * Xp2 + biasMod;
|
|
|
|
// 좌표계 원복 x',y' -> x,y
|
|
Ans1.x = Xp1 + c.x;
|
|
Ans1.y = Yp1 + c.y;
|
|
Ans2.x = Xp2 + c.x;
|
|
Ans2.y = Yp2 + c.y;
|
|
return true;
|
|
}
|
|
|
|
public bool GetCenterOfCircle(TPoint P1, TPoint P2, TPoint P3, ref TPoint Center)
|
|
{
|
|
if ((P2.x - P1.x) * (P3.y - P2.y) == (P2.y - P1.y) * (P3.x - P2.x))
|
|
return false; // 일직선 상에 세점이 위치함.
|
|
double x, y;
|
|
//x = (P3.y - P2.y) * (P2.x*P2.x - P1.x*P1.x + P2.y*P2.y - P1.y*P1.y) +
|
|
// (P1.y - P2.y) * (P3.x*P3.x - P2.x*P2.x + P3.y*P3.y - P2.y*P2.y);
|
|
//y = (P2.x - P3.x) * (P2.x*P2.x - P1.x*P1.x + P2.y*P2.y - P1.y*P1.y) +
|
|
// (P2.x - P1.x) * (P3.x*P3.x - P2.x*P2.x + P3.y*P3.y - P2.y*P2.y);
|
|
//x /= (2 * ((P2.x - P1.x) * (P3.y - P2.y) - (P2.y - P1.y)*(P3.x - P2.x)));
|
|
//y /= (2 * ((P2.x - P1.x) * (P3.y - P2.y) - (P2.y - P1.y)*(P3.x - P2.x)));
|
|
//Center.x = x+0.5, Center.y = y+0.5;
|
|
// 값이 너무 커서 계산이 틀림.. 1000.0으로 나누고 마지막에 다시 1000.0을 곱함.
|
|
|
|
double x1 = P1.x / 1000.0, y1 = P1.y / 1000.0, x2 = P2.x / 1000.0, y2 = P2.y / 1000.0, x3 = P3.x / 1000.0, y3 = P3.y / 1000.0;
|
|
|
|
x = (y3 - y2) * (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) + (y1 - y2) * (x3 * x3 - x2 * x2 + y3 * y3 - y2 * y2);
|
|
y = (x2 - x3) * (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1) + (x2 - x1) * (x3 * x3 - x2 * x2 + y3 * y3 - y2 * y2);
|
|
x /= (2 * ((x2 - x1) * (y3 - y2) - (y2 - y1) * (x3 - x2)));
|
|
y /= (2 * ((x2 - x1) * (y3 - y2) - (y2 - y1) * (x3 - x2)));
|
|
|
|
Center.x = Convert.ToInt32(x * 1000.0);
|
|
Center.y = Convert.ToInt32(y * 1000.0);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
public bool GetCenterOfCircle(Vector2 P1, Vector2 P2, double R, ref Vector2 C1, ref Vector2 C2) // 원주 상의 두 점과 반지름이 주어질 때 중심 구하기. 두개가 나온다.
|
|
{
|
|
double fdx = (P1.x - P2.x);
|
|
double fdy = (P1.y - P2.y);
|
|
double fxc = (P1.x + P2.x) / 2.0;
|
|
double fyc = (P1.y + P2.y) / 2.0;
|
|
|
|
double fd = Math.Sqrt(fdx * fdx + fdy * fdy); //두점사이의 거리
|
|
double fd2 = fd / 2; //거리의 중간
|
|
double fOffset = Math.Sqrt(R * R - fd2 * fd2);
|
|
|
|
double fplusx = fOffset * fdy / fd;
|
|
double fplusy = fOffset * fdx / fd;
|
|
|
|
C1.x = fxc + fplusx;
|
|
C1.y = fyc - fplusy;
|
|
C2.x = fxc - fplusx;
|
|
C2.y = fyc + fplusy;
|
|
return true;
|
|
}
|
|
}
|
|
}
|