490 lines
27 KiB
C#
490 lines
27 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
|
|
namespace WeldingCommon.Vision.WeldingObjectGenerator.PathEditorLibrary
|
|
{
|
|
using TPoint = GeometryLibrary.TPoint;
|
|
using TPolygon = List<GeometryLibrary.TPoint>;
|
|
using Geometry = GeometryLibrary.Geometry;
|
|
using Vector2 = GeometryLibrary.Vector2;
|
|
public class PathItem
|
|
{
|
|
public enum _PathType { Line, Arc }
|
|
public PathItem(_PathType type, Int32 start, Int32 end, UInt32 radi = 0)
|
|
{
|
|
m_Type = type;
|
|
m_startPointIndex = start;
|
|
m_endPointIndex = end;
|
|
m_Radius = radi;
|
|
}
|
|
private _PathType m_Type;
|
|
private Int32 m_startPointIndex; // polygon start point
|
|
private Int32 m_endPointIndex; // polygon end point
|
|
private Int64 m_startX, m_startY; // start point. um.
|
|
private Int64 m_endX, m_endY; // end point. um.
|
|
private UInt32 m_Radius; // for arc type. um unit
|
|
private Int64 m_centerX, m_centerY; // for arc. um
|
|
public _PathType GetType() { return m_Type; }
|
|
public void GetStartEndIndex(out Int32 start, out Int32 end) { start = m_startPointIndex; end = m_startPointIndex; }
|
|
public UInt32 GetRadius() { return m_Radius; }
|
|
public void SetStart(Int64 x, Int64 y) { m_startX = x; m_startY = y; }
|
|
public void SetEnd(Int64 x, Int64 y) { m_endX = x; m_endY = y; }
|
|
public void SetCenter(Int64 x, Int64 y) { m_centerX = x; m_centerY = y; }
|
|
public void GetStart(out Int64 x, out Int64 y) { x = m_startX; y = m_startY; }
|
|
public void GetEnd(out Int64 x, out Int64 y) { x = m_endX; y = m_endY; }
|
|
public void GetCenter(out Int64 x, out Int64 y) { x = m_centerX; y = m_centerY; }
|
|
}
|
|
public class PathEditor
|
|
{
|
|
public PathEditor() { }
|
|
private List<PathItem> m_vAutoPathItem;
|
|
public ref List<PathItem> GetAutoPathItem() { return ref m_vAutoPathItem; }
|
|
public void MergePoints(TPolygon vOrigPts, out TPolygon vDestPts, double mergeDist) // dist unit : um
|
|
{
|
|
TPolygon vMergedPts = new TPolygon();
|
|
vMergedPts.Add(vOrigPts[0]);
|
|
|
|
mergeDist *= mergeDist;
|
|
Int32 Ref = 0;
|
|
for (Int32 i = Ref + 1; i < vOrigPts.Count; i++) {
|
|
TPoint pt1 = new TPoint(), pt2 = new TPoint();
|
|
pt1.x = vOrigPts[Ref].x;
|
|
pt1.y = vOrigPts[Ref].y;
|
|
pt2.x = vOrigPts[i].x;
|
|
pt2.y = vOrigPts[i].y;
|
|
Int64 dist = (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
|
|
if (dist < mergeDist)
|
|
continue;
|
|
vMergedPts.Add(pt2);
|
|
Ref = i;
|
|
}
|
|
|
|
//Merge후 마지막 Index가 add 되지 않았을 때 넣어줘야 함
|
|
if (Ref < vOrigPts.Count - 1)
|
|
vMergedPts.Add(vOrigPts[vOrigPts.Count - 1]);
|
|
|
|
vDestPts = vMergedPts;
|
|
}
|
|
public void RearrangePoints(TPolygon master, out TPolygon vDestPts)
|
|
{
|
|
vDestPts = null;
|
|
if (master.Count < 2) return;
|
|
TPolygon reArr = new TPolygon();
|
|
reArr.Add(master.First());
|
|
reArr.Add(master[1]);
|
|
Int32 curIndex = 1;
|
|
for (Int32 i = curIndex + 1; i < master.Count; i++) {
|
|
TPoint prev = reArr[curIndex - 1];
|
|
TPoint cur = reArr[curIndex];
|
|
TPoint next = master[i];
|
|
TPoint CurToPrev = new TPoint(prev.x - cur.x, prev.y - cur.y);
|
|
TPoint CurToNext = new TPoint(next.x - cur.x, next.y - cur.y);
|
|
double dotProduct = CurToPrev.x * CurToNext.x + CurToPrev.y * CurToNext.y;
|
|
double absCurToPrev = Math.Sqrt((double)CurToPrev.x * CurToPrev.x + (double)CurToPrev.y * CurToPrev.y);
|
|
double absCurToNext = Math.Sqrt((double)CurToNext.x * CurToNext.x + (double)CurToNext.y * CurToNext.y);
|
|
double cosVal = dotProduct / absCurToPrev / absCurToNext;
|
|
if (cosVal >= Math.Cos(Math.PI / 6)) // 30도 보다 예각인 경우는 제외
|
|
{
|
|
continue;
|
|
}
|
|
reArr.Add(next);
|
|
curIndex++;
|
|
}
|
|
//앞서 Merge에서 마지막 Index를 넣어줬기 때문에 여기서 넣어주면 시작,끝 위치가 맞지 않아서 주석함
|
|
//reArr.Add(master.First());
|
|
//reArr.Add(master[1]);
|
|
vDestPts = reArr;
|
|
}
|
|
public void GeneratePathWDegree(TPolygon master, int ObjectN, int end_index)
|
|
{
|
|
if (ObjectN == 0) return;
|
|
m_vAutoPathItem = new List<PathItem>();
|
|
Geometry geo = new Geometry();
|
|
TPoint center = new TPoint();
|
|
TPoint p1, p2;
|
|
double R, min_dist;
|
|
int min_point;
|
|
for (int i = 0; i < ObjectN - 1; i++) {
|
|
int start = (int)((float)end_index * (float)i / (float)ObjectN);
|
|
int end = (int)((float)end_index * (float)(i + 1) / (float)ObjectN);
|
|
|
|
p1 = master[start];
|
|
p2 = master[end];
|
|
|
|
|
|
min_dist = 1e10;
|
|
min_point = 0;
|
|
for (int p = start + 1; p < end - 1; p++) {
|
|
geo.GetCenterOfCircle(p1, master[p], p2, ref center);
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
double dist_sum = 0;
|
|
for (int c = start + 1; c < end - 1; c++) {
|
|
dist_sum += Math.Abs(Math.Sqrt((center.x - master[c].x) / 1000.0 * (center.x - master[c].x) / 1000.0 + (center.y - master[c].y) / 1000.0 * (center.y - master[c].y) / 1000.0) - R);
|
|
}
|
|
if (dist_sum < min_dist) {
|
|
min_dist = dist_sum;
|
|
min_point = p;
|
|
}
|
|
}
|
|
geo.GetCenterOfCircle(p1, master[min_point], p2, ref center);
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
PathItem Item;
|
|
Item = new PathItem(PathItem._PathType.Arc, start, end, Convert.ToUInt32(R * 1000.0));
|
|
Item.SetStart(p1.x, p1.y);
|
|
Item.SetEnd(p2.x, p2.y);
|
|
Item.SetCenter(center.x, center.y);
|
|
m_vAutoPathItem.Add(Item);
|
|
}
|
|
int last_start = (int)((float)end_index * (float)(ObjectN - 1) / (float)ObjectN);
|
|
p1 = master[last_start];
|
|
p2 = master[master.Count - 1];
|
|
min_dist = 1e10;
|
|
min_point = 0;
|
|
for (int p = last_start + 1; p < master.Count - 1; p++) {
|
|
geo.GetCenterOfCircle(p1, master[p], p2, ref center);
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
double dist_sum = 0;
|
|
for (int c = last_start + 1; c < master.Count - 1; c++) {
|
|
dist_sum += Math.Abs(Math.Sqrt((center.x - master[c].x) / 1000.0 * (center.x - master[c].x) / 1000.0 + (center.y - master[c].y) / 1000.0 * (center.y - master[c].y) / 1000.0) - R);
|
|
}
|
|
if (dist_sum < min_dist) {
|
|
min_dist = dist_sum;
|
|
min_point = p;
|
|
}
|
|
}
|
|
geo.GetCenterOfCircle(p1, master[min_point], p2, ref center);
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
|
|
PathItem Item_last = new PathItem(PathItem._PathType.Arc, last_start, master.Count - 1, Convert.ToUInt32(R * 1000.0));
|
|
Item_last.SetStart(p1.x, p1.y);
|
|
Item_last.SetEnd(p2.x, p2.y);
|
|
Item_last.SetCenter(center.x, center.y);
|
|
m_vAutoPathItem.Add(Item_last);
|
|
|
|
}
|
|
public void GenerateAutoPath(TPolygon master, Int32 iStartPointIndex, Int32 iEndPointIndex, UInt32 iMaxArcRadius, UInt32 iSingleLineTolerance, UInt32 iSingleArcTolerance, UInt32 DetectedContourCenterX, UInt32 DetectedContourCenterY)
|
|
{
|
|
if (Math.Abs((int)(iStartPointIndex - iEndPointIndex)) < 2) return; // 3 포이트 이상 되어야 한다.
|
|
|
|
// 어차피 new 를 하는데 clear 할 필요가 있나?
|
|
//if (m_vAutoPathItem != null)
|
|
// m_vAutoPathItem.Clear();
|
|
m_vAutoPathItem = new List<PathItem>();
|
|
Int32 start = iStartPointIndex - 1, end = iEndPointIndex - 1, inc = 1;
|
|
|
|
if (iEndPointIndex < iStartPointIndex) {
|
|
start = iEndPointIndex - 1;
|
|
end = iStartPointIndex - 1;
|
|
inc = -1;
|
|
}
|
|
Geometry geo = new Geometry();
|
|
for (Int32 i = start; i <= end - 2;) {
|
|
TPoint p1 = new TPoint(master[i]);
|
|
TPoint p2 = new TPoint(master[i + 1]);
|
|
TPoint p3 = new TPoint(master[i + 2]);
|
|
TPoint center = new TPoint();
|
|
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) ||
|
|
geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y)) < iSingleLineTolerance)
|
|
// 일직선 상인 경우나 p1,p2 의 직선과 p3의 거리가 공차 이내일 때
|
|
{
|
|
if (end < i + 3) {
|
|
PathItem item3 = new PathItem(PathItem._PathType.Line, i, i + 2);
|
|
item3.SetStart(master[i].x, master[i].y);
|
|
item3.SetEnd(master[i + 2].x, master[i + 2].y);
|
|
m_vAutoPathItem.Add(item3);
|
|
}
|
|
Int32 J;
|
|
for (J = i + 3; J <= end; J++) {
|
|
p3 = master[J];
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) ||
|
|
geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y)) < iSingleLineTolerance)
|
|
continue;
|
|
break;
|
|
}
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, J - 1);
|
|
item.SetStart(master[i].x, master[i].y);
|
|
item.SetEnd(master[J - 1].x, master[J - 1].y);
|
|
m_vAutoPathItem.Add(item);
|
|
i = J - 1;
|
|
continue;
|
|
}
|
|
double R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
double DistToPolygonCenter = Math.Sqrt((center.x - DetectedContourCenterX) / 1000.0 * (center.x - DetectedContourCenterX) / 1000.0 + (center.y - DetectedContourCenterY) / 1000.0 * (center.y - DetectedContourCenterY) / 1000.0);
|
|
|
|
if (R >= iMaxArcRadius || DistToPolygonCenter > R) //일직선 상은 아니지만 Arc 로 처리하기에는 너무 곡률반경이 클 때 2개의 라인으로 처리.
|
|
{
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, i + 1);
|
|
item.SetStart(master[i].x, master[i].y);
|
|
item.SetEnd(master[i + 1].x, master[i + 1].y);
|
|
m_vAutoPathItem.Add(item);
|
|
PathItem Item2 = new PathItem(PathItem._PathType.Line, i + 1, i + 2);
|
|
Item2.SetStart(master[i + 1].x, master[i + 1].y);
|
|
Item2.SetEnd(master[i + 2].x, master[i + 2].y);
|
|
m_vAutoPathItem.Add(Item2);
|
|
i = i + 2;
|
|
continue;
|
|
}
|
|
TPoint p12Center = new TPoint(p1.x / 2 + p2.x / 2, p1.y / 2 + p2.y / 2), p23Center = new TPoint(p2.x / 2 + p3.x / 2, p2.y / 2 + p3.y / 2);
|
|
double distP12CToC =
|
|
Math.Sqrt((p12Center.x - center.x) / 1000.0 * (p12Center.x - center.x) / 1000.0 + (p12Center.y - center.y) / 1000.0 * (p12Center.y - center.y) / 1000.0) * 1000.0;
|
|
double distP23CToC =
|
|
Math.Sqrt((p23Center.x - center.x) / 1000.0 * (p23Center.x - center.x) / 1000.0 + (p23Center.y - center.y) / 1000.0 * (p23Center.y - center.y) / 1000.0) * 1000.0;
|
|
if (Math.Abs(distP12CToC - R * 1000.0) > iSingleArcTolerance || Math.Abs(distP23CToC - R * 1000.0) > iSingleArcTolerance) {
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, i + 1);
|
|
item.SetStart(p1.x, p1.y);
|
|
item.SetEnd(p2.x, p2.y);
|
|
m_vAutoPathItem.Add(item);
|
|
PathItem item2 = new PathItem(PathItem._PathType.Line, i + 1, i + 2);
|
|
item2.SetStart(p2.x, p2.y);
|
|
item2.SetEnd(p3.x, p3.y);
|
|
m_vAutoPathItem.Add(item2);
|
|
i = i + 2;
|
|
continue;
|
|
}
|
|
|
|
// 원호 처리
|
|
Int32 j;
|
|
for (j = i + 3; j <= end; j++) {
|
|
p3 = master[j];
|
|
double distance =
|
|
Math.Abs(R - Math.Sqrt((center.x - p3.x) / 1000.0 * (center.x - p3.x) / 1000.0 + (center.y - p3.y) / 1000.0 * (center.y - p3.y) / 1000.0)) * 1000.0;
|
|
if (distance > iSingleArcTolerance)
|
|
break;
|
|
TPoint new_center = new TPoint();
|
|
geo.GetCenterOfCircle(p1, p2, master[j], ref new_center);
|
|
DistToPolygonCenter = Math.Sqrt((new_center.x - DetectedContourCenterX) / 1000.0 * (new_center.x - DetectedContourCenterX) / 1000.0 + (new_center.y - DetectedContourCenterY) / 1000.0 * (new_center.y - DetectedContourCenterY) / 1000.0);
|
|
if (R >= iMaxArcRadius || DistToPolygonCenter > R) break;
|
|
TPoint prevP3C = new TPoint(p3.x / 2 + master[j - 1].x / 2, p3.y / 2 + master[j - 1].y / 2);
|
|
double distprevP3CToC =
|
|
Math.Sqrt((prevP3C.x - center.x) / 1000.0 * (prevP3C.x - center.x) / 1000.0 + (prevP3C.y - center.y) / 1000.0 * (prevP3C.y - center.y) / 1000.0) * 1000.0;
|
|
|
|
if (Math.Abs(distprevP3CToC - R * 1000.0) > iSingleArcTolerance || Math.Abs(distprevP3CToC - R * 1000.0) > iSingleArcTolerance) {
|
|
break;
|
|
}
|
|
}
|
|
// p3 가 업데이트 되었는데 line 인지 체크하는것을 안하고 있었다. GetCenterOfCircle 가 false return 하면 center 가 쓰레기값이 들어간다.
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) ||
|
|
geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y)) < iSingleLineTolerance)
|
|
// 일직선 상인 경우나 p1,p2 의 직선과 p3의 거리가 공차 이내일 때
|
|
{
|
|
if (end < i + 3) {
|
|
PathItem item3 = new PathItem(PathItem._PathType.Line, i, i + 2);
|
|
item3.SetStart(master[i].x, master[i].y);
|
|
item3.SetEnd(master[i + 2].x, master[i + 2].y);
|
|
m_vAutoPathItem.Add(item3);
|
|
}
|
|
Int32 J;
|
|
for (J = i + 3; J <= end; J++) {
|
|
p3 = master[J];
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) ||
|
|
geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y)) < iSingleLineTolerance)
|
|
continue;
|
|
break;
|
|
}
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, J - 1);
|
|
item.SetStart(master[i].x, master[i].y);
|
|
item.SetEnd(master[J - 1].x, master[J - 1].y);
|
|
m_vAutoPathItem.Add(item);
|
|
i = J - 1;
|
|
continue;
|
|
} else {
|
|
|
|
geo.GetCenterOfCircle(p1, p2, master[j - 1], ref center); // 마지막 점을 지나도록 다시 원을 계산.
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
|
|
PathItem Item = new PathItem(PathItem._PathType.Arc, i, j - 1, Convert.ToUInt32(R * 1000.0));
|
|
Item.SetStart(p1.x, p1.y);
|
|
Item.SetEnd(master[j - 1].x, master[j - 1].y);
|
|
Item.SetCenter(center.x, center.y);
|
|
|
|
m_vAutoPathItem.Add(Item);
|
|
i = j - 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void GenerateAutoPath(TPolygon master, UInt32 iMaxArcRadius, UInt32 iSingleLineTolerance, UInt32 iSingleArcTolerance, UInt32 DetectedContourCenterX, UInt32 DetectedContourCenterY)
|
|
{
|
|
// polygon list 전체에서 한번에 모든 경로 생성
|
|
GenerateAutoPath(master, 1, master.Count, iMaxArcRadius, iSingleLineTolerance, iSingleArcTolerance, DetectedContourCenterX, DetectedContourCenterY);
|
|
}
|
|
public void GenerateAutoSubPath(TPolygon master, UInt32 iMaxArcRadius, double iSingleLineTolerance, double iSingleArcTolerance, int startindex, double length, out int endindex)
|
|
{
|
|
// polygon list 중 start index 부터 unitlength 길이까지만 경로 생성. 마지막에 사용한 polygon 의 index 를 return 한다.
|
|
// 마지막 index 를 return 해야 다음 sub path 를 생성할 수 있기 때문
|
|
m_vAutoPathItem = new List<PathItem>();
|
|
|
|
endindex = 0;
|
|
double lengthsum = 0; // lengthsum 이 length 를 넘으면 생성종료하고 end index 리턴
|
|
|
|
// 라인 길이를 구하기 위하여 2점 거리를 구하는 람다함수
|
|
Func<TPoint, TPoint, double> distance2D =
|
|
(p1, p2) => Math.Sqrt(Math.Pow(p2.x - p1.x, 2) + Math.Pow(p2.y - p1.y, 2));
|
|
|
|
if (Math.Abs((int)(startindex - master.Count)) < 2) return; // 3 포이트 이상 되어야 한다.
|
|
|
|
//m_vAutoPathItem = new List<PathItem>();
|
|
Int32 start = startindex - 1, end = master.Count - 1;
|
|
Geometry geo = new Geometry();
|
|
for (Int32 i = start; i <= end - 2;) {
|
|
TPoint p1 = new TPoint(master[i]);
|
|
TPoint p2 = new TPoint(master[i + 1]);
|
|
TPoint p3 = new TPoint(master[i + 2]);
|
|
TPoint center = new TPoint();
|
|
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) ||
|
|
geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y)) < iSingleLineTolerance)
|
|
// 일직선 상인 경우나 p1,p2 의 직선과 p3의 거리가 공차 이내일 때
|
|
{
|
|
if (end < i + 3) {
|
|
PathItem item3 = new PathItem(PathItem._PathType.Line, i, i + 2);
|
|
item3.SetStart(master[i].x, master[i].y);
|
|
item3.SetEnd(master[i + 2].x, master[i + 2].y);
|
|
m_vAutoPathItem.Add(item3);
|
|
// 여기로 들어오면 전체 for문이 끝난것임. polygon list 의 마지막 인덱스까지 붙인것이다. lengthsum 와 무관하게 종료하면된다.
|
|
// 어차피 for문의 마지막이라서 break 나 return 을 하지 않은것 같다. 명시적으로 다시 써준다.
|
|
endindex = i + 2;
|
|
return;
|
|
}
|
|
// 여기로 들어오면 index 마지막까지는 안간것이다.
|
|
Int32 J;
|
|
double geosegmentdistance = 0;
|
|
for (J = i + 3; J <= end; J++) {
|
|
p3 = master[J];
|
|
geosegmentdistance = geo.SegmentDistance(new Vector2(p1.x, p1.y), new Vector2(p2.x, p2.y), new Vector2(p3.x, p3.y));
|
|
if (!geo.GetCenterOfCircle(p1, p2, p3, ref center) || geosegmentdistance < iSingleLineTolerance)
|
|
continue;
|
|
break;
|
|
}
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, J - 1);
|
|
item.SetStart(master[i].x, master[i].y);
|
|
item.SetEnd(master[J - 1].x, master[J - 1].y);
|
|
m_vAutoPathItem.Add(item);
|
|
i = J - 1;
|
|
endindex = i;
|
|
lengthsum += distance2D(master[i], master[J - 1]);
|
|
if (lengthsum > length)
|
|
return;
|
|
continue;
|
|
}
|
|
double R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
if (R >= iMaxArcRadius) //일직선 상은 아니지만 Arc 로 처리하기에는 너무 곡률반경이 클 때 2개의 라인으로 처리.
|
|
{
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, i + 1);
|
|
item.SetStart(master[i].x, master[i].y);
|
|
item.SetEnd(master[i + 1].x, master[i + 1].y);
|
|
m_vAutoPathItem.Add(item);
|
|
lengthsum += distance2D(master[i], master[i + 1]);
|
|
PathItem Item2 = new PathItem(PathItem._PathType.Line, i + 1, i + 2);
|
|
Item2.SetStart(master[i + 1].x, master[i + 1].y);
|
|
Item2.SetEnd(master[i + 2].x, master[i + 2].y);
|
|
m_vAutoPathItem.Add(Item2);
|
|
lengthsum += distance2D(master[i + 1], master[i + 2]);
|
|
i = i + 2;
|
|
endindex = i;
|
|
if (lengthsum > length)
|
|
return;
|
|
continue;
|
|
}
|
|
TPoint p12Center = new TPoint(p1.x / 2 + p2.x / 2, p1.y / 2 + p2.y / 2), p23Center = new TPoint(p2.x / 2 + p3.x / 2, p2.y / 2 + p3.y / 2);
|
|
double distP12CToC =
|
|
Math.Sqrt((p12Center.x - center.x) / 1000.0 * (p12Center.x - center.x) / 1000.0 + (p12Center.y - center.y) / 1000.0 * (p12Center.y - center.y) / 1000.0) * 1000.0;
|
|
double distP23CToC =
|
|
Math.Sqrt((p23Center.x - center.x) / 1000.0 * (p23Center.x - center.x) / 1000.0 + (p23Center.y - center.y) / 1000.0 * (p23Center.y - center.y) / 1000.0) * 1000.0;
|
|
if (Math.Abs(distP12CToC - R * 1000.0) > iSingleArcTolerance || Math.Abs(distP23CToC - R * 1000.0) > iSingleArcTolerance) {
|
|
PathItem item = new PathItem(PathItem._PathType.Line, i, i + 1);
|
|
item.SetStart(p1.x, p1.y);
|
|
item.SetEnd(p2.x, p2.y);
|
|
m_vAutoPathItem.Add(item);
|
|
lengthsum += distance2D(p1, p2);
|
|
|
|
PathItem item2 = new PathItem(PathItem._PathType.Line, i + 1, i + 2);
|
|
item2.SetStart(p2.x, p2.y);
|
|
item2.SetEnd(p3.x, p3.y);
|
|
m_vAutoPathItem.Add(item2);
|
|
lengthsum += distance2D(p2, p3);
|
|
|
|
i = i + 2;
|
|
endindex = i;
|
|
if (lengthsum > length)
|
|
return;
|
|
continue;
|
|
}
|
|
|
|
// 원호 처리
|
|
Int32 j;
|
|
for (j = i + 3; j <= end; j++) {
|
|
p3 = master[j];
|
|
double distance =
|
|
Math.Abs(R - Math.Sqrt((center.x - p3.x) / 1000.0 * (center.x - p3.x) / 1000.0 + (center.y - p3.y) / 1000.0 * (center.y - p3.y) / 1000.0)) * 1000.0;
|
|
if (distance > iSingleArcTolerance)
|
|
break;
|
|
TPoint prevP3C = new TPoint(p3.x / 2 + master[j - 1].x / 2, p3.y / 2 + master[j - 1].y / 2);
|
|
double distprevP3CToC =
|
|
Math.Sqrt((prevP3C.x - center.x) / 1000.0 * (prevP3C.x - center.x) / 1000.0 + (prevP3C.y - center.y) / 1000.0 * (prevP3C.y - center.y) / 1000.0) * 1000.0;
|
|
if (Math.Abs(distprevP3CToC - R * 1000.0) > iSingleArcTolerance || Math.Abs(distprevP3CToC - R * 1000.0) > iSingleArcTolerance) {
|
|
break;
|
|
}
|
|
}
|
|
geo.GetCenterOfCircle(p1, p2, master[j - 1], ref center); // 마지막 점을 지나도록 다시 원을 계산.
|
|
R = Math.Sqrt((center.x - p1.x) / 1000.0 * (center.x - p1.x) / 1000.0 + (center.y - p1.y) / 1000.0 * (center.y - p1.y) / 1000.0); //int 한계를 넘어가므로 1000.0 으로 나눠서 mm 단위로 변경.
|
|
|
|
PathItem Item = new PathItem(PathItem._PathType.Arc, i, j - 1, Convert.ToUInt32(R * 1000.0));
|
|
Item.SetStart(p1.x, p1.y);
|
|
Item.SetEnd(master[j - 1].x, master[j - 1].y);
|
|
Item.SetCenter(center.x, center.y);
|
|
m_vAutoPathItem.Add(Item);
|
|
lengthsum += GetArcLength(center, p1, master[j - 1]);
|
|
|
|
i = j - 1;
|
|
endindex = i;
|
|
if (lengthsum > length)
|
|
return;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
private static double GetArcLength(TPoint center, TPoint P1, TPoint P2)
|
|
{
|
|
double x0 = center.x;
|
|
double y0 = center.y;
|
|
double x1 = P1.x;
|
|
double y1 = P1.y;
|
|
double x2 = P2.x;
|
|
double y2 = P2.y;
|
|
// 1. 반지름 계산 (C에서 P1 또는 P2까지의 거리)
|
|
double radius = Math.Sqrt(Math.Pow(x1 - x0, 2) + Math.Pow(y1 - y0, 2));
|
|
|
|
// 2. 벡터 CP1과 CP2 계산
|
|
double vectorCP1x = x1 - x0;
|
|
double vectorCP1y = y1 - y0;
|
|
double vectorCP2x = x2 - x0;
|
|
double vectorCP2y = y2 - y0;
|
|
|
|
// 3. 벡터 내적 계산
|
|
double dotProduct = vectorCP1x * vectorCP2x + vectorCP1y * vectorCP2y;
|
|
|
|
// 4. 벡터의 길이 (각 벡터의 크기)
|
|
double lengthCP1 = Math.Sqrt(Math.Pow(vectorCP1x, 2) + Math.Pow(vectorCP1y, 2));
|
|
double lengthCP2 = Math.Sqrt(Math.Pow(vectorCP2x, 2) + Math.Pow(vectorCP2y, 2));
|
|
|
|
// 5. 벡터 사이의 각도 (코사인 법칙 이용)
|
|
double cosTheta = dotProduct / (lengthCP1 * lengthCP2);
|
|
|
|
// 부동소수점 오차를 방지하기 위해 cosTheta의 값을 -1과 1 사이로 제한
|
|
cosTheta = Math.Max(-1.0, Math.Min(1.0, cosTheta));
|
|
|
|
// 각도를 라디안으로 구함
|
|
double theta = Math.Acos(cosTheta);
|
|
|
|
// 6. 원호 길이 계산
|
|
double arcLength = radius * theta;
|
|
|
|
return arcLength;
|
|
}
|
|
}
|
|
}
|