gistfile1.txt

float Leg::solveTriangle(float a, float b, float c)
{
    // Calculate the angle between a and b, opposite to c.
    a = std::abs(a);
    b = std::abs(b);
    c = std::abs(c);
    if (a + b < c || a + c < b || b + c < a) {
        return NAN;
    }
    return acosf((a * a + b * b - c * c) / (2 * a * b));
}

std::tuple<float, float, float> Leg::inverseKinematics(float x, float y, float z)
{
    // Calculate angles for knee and ankle, and put them in those variables.
    // Return true on success, and false if x and y are out of range.
    float ankle, knee, hip;
    float f = norm(x, y) - COXA;
    float d = norm(f, z);
    if (d > FEMUR + TIBIA) {
        return std::make_tuple(NAN, NAN, NAN);
    }

    hip = atan2f(y, x);
    knee = solveTriangle(FEMUR, d, TIBIA) - atan2f(-z, f);
    ankle = solveTriangle(FEMUR, TIBIA, d) - PI2;
    return std::make_tuple(hip, knee, ankle);
}