File content as of revision 50:36741e8ab197:

#define _USE_MATH_DEFINES
#include "math.h"
#include "OneLeg.h"
#include "pi.h"
#include "stdio.h"
//注：未完成。本来ならradから計算したい。今のままだとradが更新されてもx,yが更新されない
float OneLeg::GetX_m()
{
    return x_m_;
}
float OneLeg::GetY_m()
{
    return y_m_;
}
OneLeg::OneLeg(float between_servo_half_m, float leglength1[2], float leglength2[2])
{
    rad_[0] = 0;
    rad_[1] = M_PI;
    between_servo_half_m_ = between_servo_half_m;
    for (int i = 0; i < 2; i++)
    {
        leglength1_[i] = leglength1[i];
        leglength2_[i] = leglength2[i];
    }
}
int OneLeg::SetXY_m(float x_m, float y_m)
{
    int is_error = 0;
    //対応する角度も同時に計算。
    //処理を軽くするために共通部分は先に計算
    float temp_x[] = {
        x_m + between_servo_half_m_,
        x_m - between_servo_half_m_,
    };
    float temp_y2 = y_m * y_m;
    float temp_L[2];
    for (int i = 0; i < 2; i++)
        temp_L[i] = leglength1_[i] * leglength1_[i] - leglength2_[i] * leglength2_[i];

    float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
    float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
    float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L[0] + r1 * r1) / (2.0f * r1 * leglength1_[0])),
                           atan2(y_m, temp_x[0]) + acos((temp_L[1] + r2 * r2) / (2.0f * r2 * leglength1_[1]))};
    if (isnan(targetTheta[0]) || isnan(targetTheta[1])) //解が出ないときは値を更新しない
    {
        printf("error:(x,y) = (%f,%f) is out of range\r\n", x_m, y_m);
        is_error = 1;
    }
    else //解が出るならrad, x, y を更新する
    {
        for (size_t i = 0; i < 2; i++)
            SetRad(targetTheta[i], i);
        x_m_ = x_m, y_m_ = y_m;
    }
    return is_error;
}
void OneLeg::SetRad(float rad, int servo_num)
{
    rad_[servo_num] = rad;
}
float OneLeg::GetRad(int servo_num)
{
    return rad_[servo_num];
}