[C#/WINFORM] 삼변 측량(trilateration) 하기

TestProject.zip

다운로드

▶ ArcInformation.cs

using System;
using System.Drawing;

namespace TestProject
{
    /// <summary>
    /// 호 정보
    /// </summary>
    public class ArcInformation
    {
        //////////////////////////////////////////////////////////////////////////////////////////////////// Field
        ////////////////////////////////////////////////////////////////////////////////////////// Public

        #region Field

        /// <summary>
        /// 중심 포인트
        /// </summary>
        public PointF CenterPoint;

        /// <summary>
        /// 포인트 1
        /// </summary>
        public PointF Point1;
        
        /// <summary>
        /// 포인트 2
        /// </summary>
        public PointF Point2;
        
        /// <summary>
        /// 사각형
        /// </summary>
        public RectangleF Rectangle;

        /// <summary>
        /// 시작 각도
        /// </summary>
        public float StartAngle;
        
        /// <summary>
        /// 스윕 각도
        /// </summary>
        public float SweepAngle;

        #endregion

        //////////////////////////////////////////////////////////////////////////////////////////////////// Constructor
        ////////////////////////////////////////////////////////////////////////////////////////// Public

        #region 생성자 - ArcInformation(centerPoint, point1, point2)

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="centerPoint">중심 포인트</param>
        /// <param name="point1">포인트 1</param>
        /// <param name="point2">포인트 2</param>
        public ArcInformation(PointF centerPoint, PointF point1, PointF point2)
        {
            CenterPoint = centerPoint;
            Point1      = point1;
            Point2      = point2;

            float deltaX1 = Point1.X - CenterPoint.X;
            float deltaY1 = Point1.Y - CenterPoint.Y;
            float deltaX2 = Point2.X - CenterPoint.X;
            float deltaY2 = Point2.Y - CenterPoint.Y;

            float radius = (float)Math.Sqrt(deltaX1 * deltaX1 + deltaY1 * deltaY1);

            Rectangle = new RectangleF
            (
                CenterPoint.X - radius,
                CenterPoint.Y - radius,
                2 * radius,
                2 * radius
            );

            double angle1 = Math.Atan2(deltaY1, deltaX1);
            double angle2 = Math.Atan2(deltaY2, deltaX2);

            if(angle1 > angle2)
            {
                angle2 += 2 * Math.PI;
            }

            double sweep = angle2 - angle1;

            if(sweep > Math.PI)
            {
                Swap(ref angle1, ref angle2);
                Swap(ref Point1, ref Point2);

                angle2 += 2 * Math.PI;

                sweep = angle2 - angle1;
            }

            StartAngle = (float)(angle1 * 180 / Math.PI);
            SweepAngle = (float)((angle2 - angle1) * 180 / Math.PI);
        }

        #endregion

        //////////////////////////////////////////////////////////////////////////////////////////////////// Method
        ////////////////////////////////////////////////////////////////////////////////////////// Public

        #region 교환하기 - Swap<T>(a, b)

        /// <summary>
        /// 교환하기
        /// </summary>
        /// <typeparam name="T">타입</typeparam>
        /// <param name="a">A</param>
        /// <param name="b">B</param>
        public void Swap<T>(ref T a, ref T b)
        {
            T temp = a;

            a = b;
            b = temp;
        }

        #endregion
        #region 그리기 - Draw(graphics, pen)

        /// <summary>
        /// 그리기
        /// </summary>
        /// <param name="graphics">그래픽스</param>
        /// <param name="pen">펜</param>
        public void Draw(Graphics graphics, Pen pen)
        {
            graphics.DrawArc(pen, Rectangle, StartAngle, SweepAngle);
        }

        #endregion
    }
}

 

728x90

 

▶ MainForm.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Windows.Forms;

namespace TestProject
{
    /// <summary>
    /// 메인 폼
    /// </summary>
    public partial class MainForm : Form
    {
        //////////////////////////////////////////////////////////////////////////////////////////////////// Field
        ////////////////////////////////////////////////////////////////////////////////////////// Private

        #region Field

        /// <summary>
        /// 사각형 리스트
        /// </summary>
        private List<RectangleF> rectangleList = new List<RectangleF>();

        /// <summary>
        /// 삼각형 포인트 배열
        /// </summary>
        private PointF[] trianglePointArray = null;

        /// <summary>
        /// 중심 포인트
        /// </summary>
        private PointF centerPoint;

        /// <summary>
        /// 호 그래픽스 패스
        /// </summary>
        private GraphicsPath arcGraphicsPath = null;

        /// <summary>
        /// 그리기 여부
        /// </summary>
        private bool isDrawing = false;

        /// <summary>
        /// 시작 포인트
        /// </summary>
        private PointF startPoint;
        
        /// <summary>
        /// 종료 포인트
        /// </summary>
        private PointF endPoint;

        /// <summary>
        /// 호 정보 배열
        /// </summary>
        private ArcInformation[] arcInformationArray;

        #endregion

        //////////////////////////////////////////////////////////////////////////////////////////////////// Property
        ////////////////////////////////////////////////////////////////////////////////////////// Public

        //////////////////////////////////////////////////////////////////////////////////////////////////// Constructor
        ////////////////////////////////////////////////////////////////////////////////////////// Public

        #region 생성자 - MainForm()

        /// <summary>
        /// 생성자
        /// </summary>
        public MainForm()
        {
            InitializeComponent();

            this.pictureBox.MouseDown += pictureBox_MouseDown;
            this.pictureBox.MouseMove += pictureBox_MouseMove;
            this.pictureBox.MouseUp   += pictureBox_MouseUp;
            this.pictureBox.Paint     += pictureBox_Paint;
        }

        #endregion

        //////////////////////////////////////////////////////////////////////////////////////////////////// Method
        ////////////////////////////////////////////////////////////////////////////////////////// Private
        //////////////////////////////////////////////////////////////////////////////// Event

        #region 픽처 박스 마우스 DOWN 처리하기 - pictureBox_MouseDown(sender, e)

        /// <summary>
        /// 픽처 박스 마우스 DOWN 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void pictureBox_MouseDown(object sender, MouseEventArgs e)
        {
            if(this.rectangleList.Count == 3)
            {
                this.rectangleList = new List<RectangleF>();

                this.trianglePointArray = null;

                this.arcGraphicsPath = null;
            }

            this.startPoint = e.Location;
            this.endPoint   = e.Location;
            this.isDrawing  = true;
        }

        #endregion
        #region 픽처 박스 마우스 이동시 처리하기 - pictureBox_MouseMove(sender, e)

        /// <summary>
        /// 픽처 박스 마우스 이동시 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void pictureBox_MouseMove(object sender, MouseEventArgs e)
        {
            if(!this.isDrawing)
            {
                return;
            }

            this.endPoint = e.Location;

            this.pictureBox.Refresh();
        }

        #endregion
        #region 픽처 박스 마우스 UP 처리하기 - pictureBox_MouseUp(sender, e)

        /// <summary>
        /// 픽처 박스 마우스 UP 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void pictureBox_MouseUp(object sender, MouseEventArgs e)
        {
            if(!this.isDrawing)
            {
                return;
            }

            this.isDrawing = false;

            if((this.startPoint.X != this.endPoint.X) && (this.startPoint.X != this.endPoint.X))
            {
                this.rectangleList.Add(GetCircleRectangle(this.startPoint, this.endPoint));

                if(this.rectangleList.Count == 3)
                {
                    try
                    {
                        this.arcInformationArray = null;

                        this.trianglePointArray = Trilaterate
                        (
                            this.rectangleList[0],
                            this.rectangleList[1],
                            this.rectangleList[2],
                            out this.arcInformationArray
                        );

                        this.centerPoint = FindTriangleCenterPoint
                        (
                            this.trianglePointArray[0],
                            this.trianglePointArray[1],
                            this.trianglePointArray[2]
                        );

                        this.arcGraphicsPath = GetArcGraphicaPath(this.arcInformationArray);
                    }
                    catch(Exception exception)
                    {
                        MessageBox.Show(exception.Message);

                        this.trianglePointArray = null;

                        this.arcGraphicsPath = null;
                    }
                }
            }

            this.pictureBox.Refresh();
        }

        #endregion
        #region 픽처 박스 페인트시 처리하기 - pictureBox_Paint(sender, e)

        /// <summary>
        /// 픽처 박스 페인트시 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void pictureBox_Paint(object sender, PaintEventArgs e)
        {
            e.Graphics.Clear(this.pictureBox.BackColor);

            e.Graphics.SmoothingMode = SmoothingMode.AntiAlias;

            if(this.arcGraphicsPath != null)
            {
                e.Graphics.FillPath(Brushes.Pink, this.arcGraphicsPath);

                Color[] arcColorArray = { Color.Red, Color.Green, Color.Blue };

                using(Pen pen = new Pen(Color.Black, 3))
                {
                    for(int i = 0; i < 3; i++)
                    {
                        pen.Color = arcColorArray[i];

                        this.arcInformationArray[i].Draw(e.Graphics, pen);
                    }
                }
            }

            if(this.trianglePointArray != null)
            {
                e.Graphics.FillPolygon(Brushes.Pink, this.trianglePointArray);

                foreach(PointF point in this.trianglePointArray)
                {
                    e.Graphics.DrawPoint(Brushes.Black, Pens.Black, point, 3);
                }

                e.Graphics.DrawPoint(Brushes.LightGreen, Pens.Green, this.centerPoint, 3);
            }

            Pen[] penArray = { Pens.Red, Pens.Green, Pens.Blue };

            for(int i = 0; i < this.rectangleList.Count; i++)
            {
                e.Graphics.DrawEllipse(penArray[i], this.rectangleList[i]);
            }

            if(this.isDrawing)
            {
                using(Pen pen = new Pen(Color.Black, 2))
                {
                    RectangleF rectangle = GetCircleRectangle(this.startPoint, this.endPoint);

                    e.Graphics.DrawEllipse(pen, rectangle);

                    pen.Color = Color.White;

                    pen.DashPattern = new float[] { 5, 5 };

                    e.Graphics.DrawEllipse(pen, rectangle);
                }
            }
        }

        #endregion

        //////////////////////////////////////////////////////////////////////////////// Function

        #region 호 그래픽스 패스 구하기 - GetArcGraphicaPath(arcInformationArray)

        /// <summary>
        /// 호 그래픽스 패스 구하기
        /// </summary>
        /// <param name="arcInformationArray">호 정보 배열</param>
        /// <returns>호 그래픽스 패스</returns>
        private GraphicsPath GetArcGraphicaPath(ArcInformation[] arcInformationArray)
        {
            GraphicsPath path = new GraphicsPath();

            foreach(ArcInformation arcInformation in arcInformationArray)
            {
                path.AddArc(arcInformation.Rectangle, arcInformation.StartAngle, arcInformation.SweepAngle);
            }

            return path;
        }

        #endregion
        #region 원 사각형 구하기 - GetRectangle(point1, point2)

        /// <summary>
        /// 원 사각형 구하기
        /// </summary>
        /// <param name="point1">포인트 1</param>
        /// <param name="point2">포인트 2</param>
        /// <returns>원 사각형</returns>
        private RectangleF GetCircleRectangle(PointF point1, PointF point2)
        {
            float deltaX = point2.X - point1.X;
            float deltaY = point2.Y - point1.Y;

            float radius = (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY) / 2f;

            float centerX = (point2.X + point1.X) / 2f;
            float centerY = (point2.Y + point1.Y) / 2f;

            return new RectangleF
            (
                centerX - radius,
                centerY - radius,
                2 * radius,
                2 * radius
            );
        }

        #endregion
        #region 원과 원 사이 교차 포인트 찾기 - FindIntersectionPointBetweenCircleAndCircle(centerPointX1, centerPointY1, radius1,
            centerPointX2, centerPointY2, radius2, intersectionPoint1, intersectionPoint2)

        /// <summary>
        /// 원과 원 사이 교차 포인트 찾기
        /// </summary>
        /// <param name="centerPointX1">중심 포인트 X1</param>
        /// <param name="centerPointY1">중심 포인트 Y1</param>
        /// <param name="radius1">반경 1</param>
        /// <param name="centerPointX2">중심 포인트 X2</param>
        /// <param name="centerPointY2">중심 포인트 Y2</param>
        /// <param name="radius2">반경 2</param>
        /// <param name="intersectionPoint1">교차 포인트 1</param>
        /// <param name="intersectionPoint2">교차 포인트 2</param>
        /// <returns>교차 포인트 수</returns>
        private int FindIntersectionPointBetweenCircleAndCircle
        (
            float centerPointX1,
            float centerPointY1,
            float radius1,
            float centerPointX2,
            float centerPointY2,
            float radius2,
            out PointF intersectionPoint1,
            out PointF intersectionPoint2
        )
        {
            float deltaX = centerPointX1 - centerPointX2;
            float deltaY = centerPointY1 - centerPointY2;

            double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);

            if(distance > radius1 + radius2)
            {
                intersectionPoint1 = new PointF(float.NaN, float.NaN);
                intersectionPoint2 = new PointF(float.NaN, float.NaN);

                return 0;
            }
            else if(distance < Math.Abs(radius1 - radius2))
            {
                intersectionPoint1 = new PointF(float.NaN, float.NaN);
                intersectionPoint2 = new PointF(float.NaN, float.NaN);

                return 0;
            }
            else if((distance == 0) && (radius1 == radius2))
            {
                intersectionPoint1 = new PointF(float.NaN, float.NaN);
                intersectionPoint2 = new PointF(float.NaN, float.NaN);

                return 0;
            }
            else
            {
                double a = (radius1 * radius1 - radius2 * radius2 + distance * distance) / (2 * distance);
                double h = Math.Sqrt(radius1 * radius1 - a * a);

                double centerX3 = centerPointX1 + a * (centerPointX2 - centerPointX1) / distance;
                double centerY3 = centerPointY1 + a * (centerPointY2 - centerPointY1) / distance;

                intersectionPoint1 = new PointF
                (
                    (float)(centerX3 + h * (centerPointY2 - centerPointY1) / distance),
                    (float)(centerY3 - h * (centerPointX2 - centerPointX1) / distance)
                );

                intersectionPoint2 = new PointF
                (
                    (float)(centerX3 - h * (centerPointY2 - centerPointY1) / distance),
                    (float)(centerY3 + h * (centerPointX2 - centerPointX1) / distance)
                );

                if(distance == radius1 + radius2)
                {
                    return 1;
                }

                return 2;
            }
        }

        #endregion
        #region 거리 구하기 - GetDistance(point1, point2)

        /// <summary>
        /// 거리 구하기
        /// </summary>
        /// <param name="point1">포인트 1</param>
        /// <param name="point2">포인트 2</param>
        /// <returns>거리</returns>
        private float GetDistance(PointF point1, PointF point2)
        {
            float deltaX = point1.X - point2.X;
            float deltaY = point1.Y - point2.Y;

            return (float)Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        }

        #endregion
        #region 삼변 측량하기 - Trilaterate(circleRectangle1, circleRectangle2, circleRectangle3, arcInformationArray)

        /// <summary>
        /// 삼변 측량하기
        /// </summary>
        /// <param name="circleRectangle1">원 사각형 1</param>
        /// <param name="circleRectangle2">원 사각형 2</param>
        /// <param name="circleRectangle3">원 사각형 3</param>
        /// <param name="arcInformationArray">호 정보 배열</param>
        /// <returns>포인트 배열</returns>
        private PointF[] Trilaterate
        (
            RectangleF           circleRectangle1,
            RectangleF           circleRectangle2,
            RectangleF           circleRectangle3,
            out ArcInformation[] arcInformationArray
        )
        {
            float centerX1 = circleRectangle1.X + circleRectangle1.Width  / 2f;
            float centerY1 = circleRectangle1.Y + circleRectangle1.Height / 2f;

            float centerX2 = circleRectangle2.X + circleRectangle2.Width  / 2f;
            float centerY2 = circleRectangle2.Y + circleRectangle2.Height / 2f;

            float centerX3 = circleRectangle3.X + circleRectangle3.Width  / 2f;
            float centerY3 = circleRectangle3.Y + circleRectangle3.Height / 2f;

            float radius1 = circleRectangle1.Width / 2f;
            float radius2 = circleRectangle2.Width / 2f;
            float radius3 = circleRectangle3.Width / 2f;

            PointF intersection12A;
            PointF intersection12B;
            PointF intersection23A;
            PointF intersection23B;
            PointF intersection31A;
            PointF intersection31B;

            int result1 = FindIntersectionPointBetweenCircleAndCircle
            (
                centerX1,
                centerY1,
                radius1,
                centerX2,
                centerY2,
                radius2,
                out intersection12A,
                out intersection12B
            );

            if(result1 == 0)
            {
                throw new Exception("circle1 and circle2 do not intersect.");
            }

            int result2 = FindIntersectionPointBetweenCircleAndCircle
            (
                centerX2,
                centerY2,
                radius2,
                centerX3,
                centerY3,
                radius3,
                out intersection23A,
                out intersection23B
            );

            if(result2 == 0)
            {
                throw new Exception("circle2 and circle3 do not intersect.");
            }

            int result3 = FindIntersectionPointBetweenCircleAndCircle
            (
                centerX3,
                centerY3,
                radius3,
                centerX1,
                centerY1,
                radius1,
                out intersection31A,
                out intersection31B
            );

            if(result3 == 0)
            {
                throw new Exception("circle3 and circle1 do not intersect.");
            }

            PointF[] trianglePointArray = new PointF[3];

            PointF center1 = new PointF(centerX1, centerY1);
            PointF center2 = new PointF(centerX2, centerY2);
            PointF center3 = new PointF(centerX3, centerY3);

            if(GetDistance(intersection12A, center3) < GetDistance(intersection12B, center3))
            {
                trianglePointArray[0] = intersection12A;
            }
            else
            {
                trianglePointArray[0] = intersection12B;
            }

            if(GetDistance(intersection23A, center1) < GetDistance(intersection23B, center1))
            {
                trianglePointArray[1] = intersection23A;
            }
            else
            {
                trianglePointArray[1] = intersection23B;
            }

            if(GetDistance(intersection31A, center2) < GetDistance(intersection31B, center2))
            {
                trianglePointArray[2] = intersection31A;
            }
            else
            {
                trianglePointArray[2] = intersection31B;
            }

            ArcInformation arcInformation1 = new ArcInformation
            (
                center1,
                trianglePointArray[0],
                trianglePointArray[2]
            );

            ArcInformation arcInformation2 = new ArcInformation
            (
                center2,
                trianglePointArray[0],
                trianglePointArray[1]
            );

            ArcInformation arcInformation3 = new ArcInformation
            (
                center3,
                trianglePointArray[1],
                trianglePointArray[2]
            );

            arcInformationArray = new ArcInformation[3];

            arcInformationArray[0] = arcInformation1;

            if(arcInformation1.Point2 == arcInformation2.Point1)
            {
                arcInformationArray[1] = arcInformation2;
                arcInformationArray[2] = arcInformation3;
            }
            else
            {
                arcInformationArray[1] = arcInformation3;
                arcInformationArray[2] = arcInformation2;
            }

            return trianglePointArray;
        }

        #endregion
        #region 삼각형 중심 포인트 찾기 - FindTriangleCenterPoint(point1, point2, point3)

        /// <summary>
        /// 삼각형 중심 포인트 찾기
        /// </summary>
        /// <param name="point1">포인트 1</param>
        /// <param name="point2">포인트 2</param>
        /// <param name="point3">포인트 3</param>
        /// <returns>삼각형 중심 포인트</returns>
        private PointF FindTriangleCenterPoint(PointF point1, PointF point2, PointF point3)
        {
            return new PointF
            (
                (point1.X + point2.X + point3.X) / 3f,
                (point1.Y + point2.Y + point3.Y) / 3f
            );
        }

        #endregion
    }
}