File content as of revision 22:787a764271dd:

#include "Walk.h"
#include "math.h"
#include "pi.h"
//Orbitは足毎の軌道をあらわす。

void Orbit::SetStraightParam(float stridetime_s, float risetime_s,
                             float stride_m, float height_m, float ground_m,
                             float ellipse_center_x_m, float ellipse_center_y_m)
{
    stridetime_s_ = stridetime_s;
    risetime_s_ = risetime_s;
    stride_m_ = stride_m;
    height_m_ = height_m;
    ground_m_ = ground_m;
    ellipse_center_x_m_ = ellipse_center_x_m;
    ellipse_center_y_m_ = ellipse_center_y_m;
}

void Orbit::SetStandParam(float ground_m)
{
    stridetime_s_ = 1;
    risetime_s_ = 0;
    stride_m_ = 0;
    height_m_ = 0;
    ground_m_ = ground_m;
    ellipse_center_x_m_ = 0;
    ellipse_center_y_m_ = ground_m;
}

//着地中の動き.直線軌道.等速
OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
{
    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipse_center_x_m_;
    float y_m = ellipse_center_y_m_;
    leg.SetXY_m(x_m, y_m);
    return leg;
}
//空中の動き.半分にきれいに切れる楕円軌道
OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
{
    float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
    float x_m = stride_m_ * 0.5 * cos(rad) + ellipse_center_x_m_;
    float y_m = height_m_ * sin(rad) + ellipse_center_y_m_;
    leg.SetXY_m(x_m, y_m);
    return leg;
}

//空中の動き.完全に半分には切れない楕円軌道
OneLeg Orbit::RiseEllipse2_(OneLeg leg, float phasetime_s)
{
    ///////////x,yを計算.注：計算は正しくないので直す必要がある。
    float theta1=acosf(stride_m_/ellipse_long_m_*0.5);                      //stride の始まりの位置の角度（数学基準）
    float theta2=acosf(-stride_m_/ellipse_long_m_*0.5);                      //stride の終わりの位置の角度（数学基準）
    float omega=(2*M_PI-theta2+theta1)/risetime_s_;                         //楕円軌道における角速度、一定の値とする（適当）
    float rad=(phasetime_s-stridetime_s_)*omega+theta2;                     //角度の基準はtheta1とする（適当） 
    float x_m = ellipse_long_m_* cos(rad) + ellipse_center_x_m_;
    float y_m =ellipse_short_m_ * sin(rad) + ellipse_center_y_m_;
    ///////////
    //x,yを代入
    leg.SetXY_m(x_m, y_m);
    return leg;
}
//楕円軌道
OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
{
    if (phasetime_s < stridetime_s_)
        return StrideLine_(leg, phasetime_s);
    else
        return RiseEllipse_(leg, phasetime_s);
}
//将来的には引数などで軌道の種類を変えて出力できるようにしたい。
OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
{
    return OrbitEllipse_(leg, phasetime_s);
}
float Orbit::GetOneWalkTime()
{
    return stridetime_s_ + risetime_s_;
}

Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
{
    for (int i = 0; i < 4; i++)
    {
        orbit_[i] = orbit[i];
        offsettime_s_[i] = offsettime_s[i];
    }
    cycletime_s_ = cycletime_s;
}
void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
{
    phasetime_s_ += cycletime_s_;
    while (phasetime_s_ > orbit_[0].GetOneWalkTime())
        phasetime_s_ -= orbit_[0].GetOneWalkTime();
    for (int i = 0; i < 4; i++)
    {
        float phasetime_s = phasetime_s_ + offsettime_s_[i];
        while (phasetime_s > orbit_[i].GetOneWalkTime())
            phasetime_s -= orbit_[i].GetOneWalkTime();
        leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
    }
}