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

BodyLegIk() private static method

private static 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 ) : double[]
legIndex int
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 double
return double[]
        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;
        }