[C#/WINFORM] 로봇 팔 마우스 추적하기

TestProject.zip

다운로드

▶ 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<PointF> elbowPointList = new List<PointF>();

        /// <summary>
        /// 위쪽 팔 길이
        /// </summary>
        private const int UPPER_ARM_LENGTH = 75;

        /// <summary>
        /// 아래쪽 팔 길이
        /// </summary>
        private const int LOWER_ARM_LENGTH = 50;

        /// <summary>
        /// 손목 길이
        /// </summary>
        private const int WRIST_LENGTH = 20;

        /// <summary>
        /// 손 너비
        /// </summary>
        private const int HAND_WIDTH = 48;

        /// <summary>
        /// 손가락 길이
        /// </summary>
        private const int FINGER_LENGTH = 30;

        #endregion

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

        #region 생성자 - MainForm()

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

            #region 이벤트를 설정한다.

            this.joing1HScrollBar.Scroll    += hScrollBar_Scroll;
            this.joint2HScrollBar.Scroll    += hScrollBar_Scroll;
            this.handHScrollBar.Scroll      += hScrollBar_Scroll;
            this.canvasPictureBox.MouseMove += canvasPictureBox_MouseMove;
            this.canvasPictureBox.Paint     += canvasPictureBox_Paint;

            #endregion
        }

        #endregion

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

        #region 수평 스크롤바 스크롤시 처리하기 - hScrollBar_Scroll(sender, e)

        /// <summary>
        /// 수평 스크롤바 스크롤시 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void hScrollBar_Scroll(object sender, ScrollEventArgs e)
        {
            this.canvasPictureBox.Refresh();
        }

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

        /// <summary>
        /// 캔버스 픽처 박스 마우스 이동시 처리하기
        /// </summary>
        /// <param name="sender">이벤트 발생자</param>
        /// <param name="e">이벤트 인자</param>
        private void canvasPictureBox_MouseMove(object sender, MouseEventArgs e)
        {
            if(e.Button == MouseButtons.Left)
            {
                MoveWrist(e.Location);
            }
        }

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

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

            e.Graphics.ResetTransform();

            foreach(PointF point in this.elbowPointList)
            {
                e.Graphics.DrawLine
                (
                    Pens.Black,
                    point.X - 6,
                    point.Y - 6,
                    point.X + 6,
                    point.Y + 6
                );

                e.Graphics.DrawLine
                (
                    Pens.Black,
                    point.X + 6,
                    point.Y - 6,
                    point.X - 6,
                    point.Y + 6
                );
            }
        }

        #endregion

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

        #region 원과 원의 교차 포인트 찾기 - FindCircleCircleIntersectionPoint(cx1, cy1, radius1, cx2, cy2, radius2, intersectionPoint1, intersectionPoint2)

        /// <summary>
        /// 원과 원의 교차 포인트 찾기
        /// </summary>
        /// <param name="cx1">원 1 중심 X</param>
        /// <param name="cy1">원 1 중심 Y</param>
        /// <param name="radius1">원 1 반지름</param>
        /// <param name="cx2">원 2 중심 X</param>
        /// <param name="cy2">원 2 중심 Y</param>
        /// <param name="radius2">원 2 반지름</param>
        /// <param name="intersectionPoint1">교차 포인트 1</param>
        /// <param name="intersectionPoint2">교차 포인트 2</param>
        /// <returns>교차 포인트 수</returns>
        private int FindCircleCircleIntersectionPoint(float cx1, float cy1, float radius1, float cx2, float cy2, float radius2,
            out PointF intersectionPoint1, out PointF intersectionPoint2)
        {
            float dx = cx1 - cx2;
            float dy = cy1 - cy2;

            double distance = Math.Sqrt(dx * dx + dy * dy);

            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 cx3 = cx1 + a * (cx2 - cx1) / distance;
                double cy3 = cy1 + a * (cy2 - cy1) / distance;

                intersectionPoint1 = new PointF
                (
                    (float)(cx3 + h * (cy2 - cy1) / distance),
                    (float)(cy3 - h * (cx2 - cx1) / distance)
                );

                intersectionPoint2 = new PointF
                (
                    (float)(cx3 - h * (cy2 - cy1) / distance),
                    (float)(cy3 + h * (cx2 - cx1) / distance)
                );

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

                return 2;
            }
        }

        #endregion
        #region 각도 유효 여부 구하기 - IsAngleValid(degree, hScrollBar)

        /// <summary>
        /// 각도 유효 여부 구하기
        /// </summary>
        /// <param name="degree">각도</param>
        /// <param name="hScrollBar">수평 스크롤바</param>
        /// <returns>각도 유혀 여부</returns>
        private bool IsAngleValid(ref int degree, HScrollBar hScrollBar)
        {
            while(degree < hScrollBar.Minimum)
            {
                degree += 360;
            }

            while(degree > hScrollBar.Maximum)
            {
                degree -= 360;
            }

            return degree >= hScrollBar.Minimum;
        }

        #endregion
        #region 손목 움직이기 - MoveWrist(point)

        /// <summary>
        /// 손목 움직이기
        /// </summary>
        /// <param name="point">포인트</param>
        private void MoveWrist(PointF point)
        {
            float centerX = this.canvasPictureBox.ClientSize.Width / 2;
            float centerY = this.canvasPictureBox.ClientSize.Height / 2;

            float cx0 = centerX;
            float cy0 = centerY;

            float cx1 = point.X;
            float cy1 = point.Y;

            this.elbowPointList = new List<PointF>();

            PointF point0;
            PointF point1;

            int pointCount = FindCircleCircleIntersectionPoint
            (
                cx0,
                cy0,
                UPPER_ARM_LENGTH,
                cx1,
                cy1,
                LOWER_ARM_LENGTH,
                out point0,
                out point1
            );

            if(pointCount > 0)
            {
                double angle0 = -Math.Atan2(point0.Y - cy0, point0.X - cx0);

                int degrees0 = (int)(angle0 * 180.0 / Math.PI);

                double angle1 = -Math.Atan2(point0.Y - cy1, point0.X - cx1);

                angle1 = (angle1 - angle0) - Math.PI;

                int degrees1 = (int)(angle1 * 180.0 / Math.PI);

                if(!IsAngleValid(ref degrees0, joing1HScrollBar) || !IsAngleValid(ref degrees1, joint2HScrollBar))
                {
                    angle0 = -Math.Atan2(point1.Y - cy0, point1.X - cx0);

                    degrees0 = (int)(angle0 * 180.0 / Math.PI);

                    angle1 = -Math.Atan2(point1.Y - cy1, point1.X - cx1);

                    angle1 = (angle1 - angle0) - Math.PI;

                    degrees1 = (int)(angle1 * 180.0 / Math.PI);
                }

                if(IsAngleValid(ref degrees0, joing1HScrollBar) && IsAngleValid(ref degrees1, joint2HScrollBar))
                {
                    this.joing1HScrollBar.Value = degrees0;
                    this.joint2HScrollBar.Value = degrees1;

                    this.elbowPointList.Add(point0);

                    if(pointCount > 1)
                    {
                        this.elbowPointList.Add(point1);
                    }
                }
            }

            this.canvasPictureBox.Refresh();
        }

        #endregion
        #region 로봇 팔 그리기 - DrawRobotArm(graphics)

        /// <summary>
        /// 로봇 팔 그리기
        /// </summary>
        /// <param name="graphics">그래픽스</param>
        private void DrawRobotArm(Graphics graphics)
        {
            graphics.SmoothingMode = SmoothingMode.AntiAlias;

            graphics.Clear(this.canvasPictureBox.BackColor);

            float centerX = this.canvasPictureBox.ClientSize.Width  / 2;
            float centerY = this.canvasPictureBox.ClientSize.Height / 2;

            graphics.TranslateTransform(centerX, centerY);

            GraphicsState graphcisState = graphics.Save();

            Rectangle rectangle = new Rectangle(0, -2, 100, 5);

            graphics.RotateTransform(-this.joing1HScrollBar.Value, MatrixOrder.Prepend);

            rectangle.Width = UPPER_ARM_LENGTH;

            graphics.FillRectangle(Brushes.LightBlue, rectangle);

            graphics.DrawRectangle(Pens.Blue, rectangle);

            graphics.TranslateTransform(UPPER_ARM_LENGTH, 0, MatrixOrder.Prepend);

            graphics.RotateTransform(-this.joint2HScrollBar.Value, MatrixOrder.Prepend);

            rectangle.Width = LOWER_ARM_LENGTH;

            graphics.FillRectangle(Brushes.LightBlue, rectangle);

            graphics.DrawRectangle(Pens.Blue, rectangle);

            graphics.TranslateTransform(LOWER_ARM_LENGTH, 0, MatrixOrder.Prepend);

            float wristAngle = 90 + this.joing1HScrollBar.Value + this.joint2HScrollBar.Value;

            graphics.RotateTransform(wristAngle, MatrixOrder.Prepend);

            rectangle.Width = WRIST_LENGTH;

            graphics.FillRectangle(Brushes.LightBlue, rectangle);

            graphics.DrawRectangle(Pens.Blue, rectangle);

            graphics.Restore(graphcisState);

            Rectangle jointRectangle = new Rectangle(-4, -4, 9, 9);

            graphics.FillEllipse(Brushes.Red, jointRectangle);

            graphics.DrawEllipse(Pens.Orange, -UPPER_ARM_LENGTH, -UPPER_ARM_LENGTH, 2 * UPPER_ARM_LENGTH, 2 * UPPER_ARM_LENGTH);

            graphics.RotateTransform(-this.joing1HScrollBar.Value, MatrixOrder.Prepend);

            graphics.TranslateTransform(UPPER_ARM_LENGTH, 0, MatrixOrder.Prepend);

            graphics.FillEllipse(Brushes.Red, jointRectangle);

            graphics.RotateTransform(-this.joint2HScrollBar.Value, MatrixOrder.Prepend);

            graphics.TranslateTransform(LOWER_ARM_LENGTH, 0, MatrixOrder.Prepend);

            graphics.FillEllipse(Brushes.Red, jointRectangle);

            graphics.DrawEllipse(Pens.Purple, -LOWER_ARM_LENGTH, -LOWER_ARM_LENGTH, 2 * LOWER_ARM_LENGTH, 2 * LOWER_ARM_LENGTH);

            graphics.RotateTransform(wristAngle, MatrixOrder.Prepend);

            graphics.TranslateTransform(WRIST_LENGTH, 0, MatrixOrder.Prepend);

            graphics.FillRectangle(Brushes.LightGreen, 0, -HAND_WIDTH / 2, 4, HAND_WIDTH);

            graphics.DrawRectangle(Pens.Green, 0, -HAND_WIDTH / 2, 4, HAND_WIDTH);

            graphics.FillRectangle(Brushes.LightGreen, 4, -this.handHScrollBar.Value - 4, FINGER_LENGTH, 4);

            graphics.DrawRectangle(Pens.Green, 4, -this.handHScrollBar.Value - 4, FINGER_LENGTH, 4);

            graphics.FillRectangle(Brushes.LightGreen, 4, this.handHScrollBar.Value, FINGER_LENGTH, 4);

            graphics.DrawRectangle(Pens.Green, 4, this.handHScrollBar.Value, FINGER_LENGTH, 4);
        }

        #endregion
    }
}