File content as of revision 29:7d8b8011a88d:

#define _USE_MATH_DEFINES
#include "math.h"
#include <stdio.h>
#include "stdlib.h"
#include "Walk.h"
 #include "pi.h"
//Orbitは足毎の軌道をあらわす。
const float kGravity = 9.8;

Orbit::Orbit(OrbitPattern pattern)
{
    pattern_ = pattern;
}
float Orbit::GetOneWalkTime()
{
    float time_s;
    switch (pattern_)
    {
    default:
        time_s = TriangleOrbit::GetOneWalkTime();
        break;
    }
    return time_s;
}
int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
    int ret;
    switch (pattern_)
    {
    default:
        ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
        break;
    }
    return ret;
}

Walk::Walk()
{
    for (int i = 0; i < 4; i++)
        offset_multi_[i] = 0;
}
float Walk::calctime_s_;
int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
{
    int is_out = 0;
    for (int i = 0; i < 4; i++)
    {
        float one_walk_time = orbit_[i].GetOneWalkTime();
        phasetime_s_[i] += calctime_s_;
        while (phasetime_s_[i] > one_walk_time)
            phasetime_s_[i] -= one_walk_time;
        if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
        {
            printf("error:leg %d in Cal4LegsPosi\r\n", i);
            is_out = 1;
        }
    }
    return is_out;
}
float Walk::GetOneWalkTime()
{
    return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
};
void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
{
    offset_multi_[0] = offset_multi0;
    offset_multi_[1] = offset_multi1;
    offset_multi_[2] = offset_multi2;
    offset_multi_[3] = offset_multi3;
}
//軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
int Walk::CheckOrbit(OneLeg templateleg)
{
    for (int i = 0; i < 4; i++)
    {
        //軌道が値域の外に出ないか計算で確かめる
        float one_walk_time = GetOneWalkTime();
        int step = one_walk_time / calctime_s_ * 2;
        for (float j = 0; j < one_walk_time; j += step)
        {
            if (orbit_[i].GetOrbit(templateleg, j) == 1)
            {
                printf("error:leg %d, time %f", i, j);
                return 1; //解が出ないときは1を返す
            }
        }
        phasetime_s_[i] = one_walk_time * offset_multi_[i];
    }
    return 0;
}
void Walk::SetOneOrbit(int legnum, Orbit orbit)
{
    orbit_[legnum] = orbit;
}
void Walk::SetAllOrbit(Orbit orbit)
{
    for (int i = 0; i < 4; i++)
        SetOneOrbit(i, orbit);
}
void Walk::SetOneLegStandParam(int legnum,float x_m, float y_m, float time_s)
{
    Orbit triangle(TRIANGLE);
    triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0,0);
    SetOneOrbit(legnum, triangle);
}
void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s) 
{
    for (int i = 0; i < 4; i ++)
        SetOneLegStandParam(i, x_m, y_m, time_s);
}
void Walk::SetOneLegTriangleParam(int legnum, float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
                                                                                                     float stridetime_s, float toptime_s, float buffer_time_s)
{
    Orbit triangle(TRIANGLE);
    triangle.SetTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
                              stridetime_s, toptime_s, buffer_time_s);
    SetOneOrbit(legnum, triangle);
}
void Walk::SetAllLegTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
                                  float stridetime_s, float toptime_s, float buffer_time_s)
{
    for (int i = 0; i < 4; i++)
        SetOneLegTriangleParam(i, start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
                               stridetime_s, toptime_s, buffer_time_s);
}

void TriangleOrbit::SetTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
                                     float stridetime_s, float toptime_s, float buffer_time_s)
{
    reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
    start_x_m_ = start_x_m;
    start_y_m_ = start_y_m;
    stride_m_ = stride_m;
    height_m_ = height_m;               //足上げ幅
    buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
    stridetime_s_ = stridetime_s;
    toptime_s_ = toptime_s;         //頂点に行くまでの時間
    buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.

    //事前に計算しておく
    top_x_m_ = start_x_m_ - height_m * reverse_tanbeta_;
    top_y_m_ = -height_m_ + start_y_m_;
    buffer_x_m_ = start_x_m_ - buffer_height_m_ * reverse_tanbeta_;
    buffer_y_m_ = -buffer_height_m_ + start_y_m_;
}
//足一周の時間
float TriangleOrbit::GetOneWalkTime()
{
    return stridetime_s_ + toptime_s_ + buffer_time_s_;
};
int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
    int ret = 0;
    if (phasetime_s < stridetime_s_)
        ret = StrideLineAccel_(leg, phasetime_s);
    else if (phasetime_s < stridetime_s_ + toptime_s_)
        ret = leg.SetXY_m(top_x_m_, top_y_m_);
    else
        ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
    return ret;
};
int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
{
    float x_m = start_x_m_ - stride_m_ * phasetime_s / stridetime_s_;
    float y_m = start_y_m_;
    return leg.SetXY_m(x_m, y_m);
}

int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
{
    ///////////x,yを計算
    float s0 = start_x_m_;
    float s1 = start_x_m_ - stride_m_;
    float g_h = sqrtf(kGravity / start_y_m_);
    float t = phasetime_s / stridetime_s_;
    float denominator = expf(g_h) - expf(-g_h); //分母

    float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
    float y_m = start_y_m_;
    //x,yを代入
    return leg.SetXY_m(x_m, y_m);
}