Robot.Leg.CalculateNewCoordinatesForRotation C# (CSharp) Метод

        public static void CalculateNewCoordinatesForRotation(double realX, double realZ, double xCenter, double zCenter, double degrees, out double newX, out double newZ)
        {
            double angleXRCT = Math.Atan2(realZ - zCenter, realX - xCenter);
               // double angleXRCT = Math.Atan((realZ - zCenter)/(realX - xCenter));
            newX = Math.Cos(angleXRCT + IK.DegToRad(degrees))* CalculateHypotenuse(realX - xCenter, realZ - zCenter) + xCenter;

            newZ = Math.Sin(angleXRCT + IK.DegToRad(degrees)) * CalculateHypotenuse(realX - xCenter, realZ - zCenter) + zCenter;
        }