基点原地逆时针旋转指定角度，XY需要走的值

一个平台,有XYR3个轴，定义一个基点，想要基点原地逆时针旋转指定角度，XY需要走的值

        /// <summary>
        /// 获取基点相对与原点的角度0-360(逆时针为正)
        /// </summary>
        /// <param name="X">基点X</param>
        /// <param name="Y">基点Y</param>
        /// <param name="OrgX">原点X(旋转中心X)</param>
        /// <param name="OrgY">原点Y(旋转中心Y)</param>
        /// <returns></returns>
        double GetAngle(double X, double Y, double OrgX, double OrgY)
        {
            double _a = X - OrgX; double _b = Y - OrgY;
            double _d = 0;
            if (_a > 0 && _b >= 0) _d = 0;//在第一象限
            if (_a < 0 && _b >= 0) _d = Math.PI;//在第而象限
            if (_a < 0 && _b < 0) _d = Math.PI;//在第三象限
            if (_a > 0 && _b < 0) _d = 2 * Math.PI;//在第四象限
            double b2 = 0;//值3.14159=180°
            if (_a != 0)
            {
                b2 = _d + Math.Atan(_b / _a);
            }
            else
            {
                if (_b > 0) b2 = Math.PI / 2;
                if (_b < 0) b2 = Math.PI / 2 * 3;
            }
            return b2 * 180 / Math.PI;
        }
        /// <summary>
        /// 基点原地逆时针旋转指定角度，XY需要走的值
        /// </summary>
        /// <param name="X">基点X</param>
        /// <param name="Y">基点Y</param>
        /// <param name="OrgX">原点X(旋转中心X)</param>
        /// <param name="OrgY">原点Y(旋转中心Y)</param>
        /// <param name="TargetAngle">基点需要转动的角度</param>
        /// <param name="MoveX">X轴位移量</param>
        /// <param name="MoveY">Y轴位移量</param>
        void GetXY(double X, double Y, double OrgX, double OrgY, double TargetAngle, out double MoveX, out double MoveY)
        {
            double len = Math.Sqrt((X - OrgX) * (X - OrgX) + (Y - OrgY) * (Y - OrgY));
            double d1 = GetAngle(X, Y, OrgX, OrgY);
            MoveX = X - OrgX - len * Math.Cos((TargetAngle + d1) * Math.PI / 180);
            MoveY = Y - OrgY - len * Math.Sin((TargetAngle + d1) * Math.PI / 180);
        }

 

 

        /// <summary>
        /// 三点获取圆心半径
        /// </summary>
        /// <param name="x1">点1x</param>
        /// <param name="y1">点1y</param>
        /// <param name="x2"></param>
        /// <param name="y2"></param>
        /// <param name="x3"></param>
        /// <param name="y3"></param>
        /// <param name="orgX">圆心x</param>
        /// <param name="orgY">圆心y</param>
        /// <param name="r">半径</param>
        void GetOrgR(double x1,double y1,double x2,double y2,double x3,double y3 ,out double orgX,out double orgY,out double r)
        {
            orgX = ((y2 - y1) * (y3 * y3 - y1 * y1 + x3 * x3 - x1 * x1) - (y3 - y1) * (y2 * y2 - y1 * y1 + x2 * x2 - x1 * x1)) / (2.0 * ((x3 - x1) * (y2 - y1) - (x2 - x1) * (y3 - y1)));
            orgY = ((x2 - x1) * (x3 * x3 - x1 * x1 + y3 * y3 - y1 * y1) - (x3 - x1) * (x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1)) / (2.0 * ((y3 - y1) * (x2 - x1) - (y2 - y1) * (x3 - x1)));
            r = Math.Sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));
        }