HexapiBackground.IK.InverseKinematics.BodyLegIk C# (CSharp) Method

        private static double[] BodyLegIk(int legIndex,
            double legPosX, double legPosY, double legPosZ,
            double bodyPosX, double bodyPosY, double bodyPosZ,
            double gaitPosX, double gaitPosY, double gaitPosZ, double gaitRotY,
            double offsetX, double offsetZ,
            double bodyRotX, double bodyRotZ, double bodyRotY,
            double coxaAngle)
        {
            var posX = 0D;
            if (legIndex <= 2)
                posX = -legPosX + bodyPosX + gaitPosX;
            else
                posX = legPosX - bodyPosX + gaitPosX;

            var posY = (legPosY + bodyPosY + gaitPosY)*100;
            var posZ = legPosZ + bodyPosZ + gaitPosZ;

            var centerOfBodyToFeetX = (offsetX + posX)*100;
            var centerOfBodyToFeetZ = (offsetZ + posZ)*100;

            double bodyRotYSin, bodyRotYCos, bodyRotZSin, bodyRotZCos, bodyRotXSin, bodyRotXCos;

            GetSinCos(bodyRotY + gaitRotY, out bodyRotYSin, out bodyRotYCos);
            GetSinCos(bodyRotZ, out bodyRotZSin, out bodyRotZCos);
            GetSinCos(bodyRotX, out bodyRotXSin, out bodyRotXCos);

            //Calculation of rotation matrix: 
            var bodyFkPosX = (centerOfBodyToFeetX -
                              (centerOfBodyToFeetX*bodyRotYCos*bodyRotZCos -
                               centerOfBodyToFeetZ*bodyRotZCos*bodyRotYSin +
                               posY*bodyRotZSin))/100;

            var bodyFkPosZ = (centerOfBodyToFeetZ -
                              (centerOfBodyToFeetX*bodyRotXCos*bodyRotYSin +
                               centerOfBodyToFeetX*bodyRotYCos*bodyRotZSin*bodyRotXSin +
                               centerOfBodyToFeetZ*bodyRotYCos*bodyRotXCos -
                               centerOfBodyToFeetZ*bodyRotYSin*bodyRotZSin*bodyRotXSin -
                               posY*bodyRotZCos*bodyRotXSin))/100;

            var bodyFkPosY = (posY -
                              (centerOfBodyToFeetX*bodyRotYSin*bodyRotXSin -
                               centerOfBodyToFeetX*bodyRotYCos*bodyRotXCos*bodyRotZSin +
                               centerOfBodyToFeetZ*bodyRotYCos*bodyRotXSin +
                               centerOfBodyToFeetZ*bodyRotXCos*bodyRotYSin*bodyRotZSin +
                               posY*bodyRotZCos*bodyRotXCos))/100;

            var coxaFemurTibiaAngle = new double[3];

            double feetPosX;
            if (legIndex <= 2)
                feetPosX = legPosX - bodyPosX + bodyFkPosX - gaitPosX;
            else
                feetPosX = legPosX + bodyPosX - bodyFkPosX + gaitPosX;

            var feetPosY = legPosY + bodyPosY - bodyFkPosY + gaitPosY;
            var feetPosZ = legPosZ + bodyPosZ - bodyFkPosZ + gaitPosZ;

            double xyhyp;
            var atan2 = GetATan2(feetPosX, feetPosZ, out xyhyp);

            coxaFemurTibiaAngle[0] = (atan2*180)/_pi1K + coxaAngle;

            var ikFeetPosXz = xyhyp/100;
            var ika14 = GetATan2(feetPosY, ikFeetPosXz - CoxaLengthInMm, out xyhyp);
            var ika24 = GetArcCos(((FemurLengthInMm*FemurLengthInMm - TibiaLengthInMm*TibiaLengthInMm)*TenThousand + xyhyp*xyhyp)/(2*FemurLengthInMm*100*xyhyp/TenThousand));

            coxaFemurTibiaAngle[1] = -(ika14 + ika24)*180/_pi1K + 900;

            coxaFemurTibiaAngle[2] = -(900 - GetArcCos(((FemurLengthInMm*FemurLengthInMm + TibiaLengthInMm*TibiaLengthInMm)*TenThousand - xyhyp*xyhyp)/(2*FemurLengthInMm*TibiaLengthInMm))*180/_pi1K);

            return coxaFemurTibiaAngle;
        }