File content as of revision 50:36741e8ab197:

#include "OverCome.h"
#include "change_walk.h"
#include "math.h"
OverCome::OverCome(float start_x_m[4], float start_y_m[4],
                   float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4],
                   OneLeg legs[4], float raise_offset_x_m[4], float d_time, float d_time_slow) : walk(legs)
{
    for (int i = 0; i < 4; i++)
    {
        start_x_m_[i] = start_x_m[i]; //足のスタートx
        start_y_m_[i] = start_y_m[i]; //足のスタートy
        goal_y_m_[i] = goal_y_m[i];   //目標地点までのy
        gravity_dist_[i] = gravity_dist[i];
        height_m_[i] = height_m[i];
        raise_offset_x_m_[i] = raise_offset_x_m[i];
    }
    d_x_m_ = d_x_m; //目標地点までのx

    d_time_ = d_time;//0 .2; //各動きの時間
    d_time_slow_ = d_time_slow;// 0.3;

    next_point_ = 0; //次のparamのindex
    GetLine();
    for (int i = 0; i < 4; i++)
        SetOneLegFreeLinesParam(walk, i, legs_[i], next_point_);
}

void OverCome::Rise(int legnum)
{
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
        legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
    }
    legs_[legnum][next_point_].x_m += raise_offset_x_m_[legnum];
    legs_[legnum][next_point_].y_m -= height_m_[legnum];
    next_point_++;
}
void OverCome::Forward(int legnum)
{
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
        legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
    }
    legs_[legnum][next_point_].x_m += d_x_m_ - raise_offset_x_m_[legnum];
    next_point_++;
}
void OverCome::Land(int legnum)
{
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり着地するために直線軌道を維持
    }
    legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
    next_point_++;
}
void OverCome::Step(int legnum)
{
    Rise(legnum);
    Forward(legnum);
    Land(legnum);
}
//重心移動、param direct:動く正負。+-1を入れる
void OverCome::GravityMove(int legnum)
{
    if (fabs(gravity_dist_[legnum]) < 0.01)
        return;
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
    }
    ++next_point_;
}
void OverCome::StartPoint()
{
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = 0;
        legs_[i][next_point_].x_m = start_x_m_[i];
        legs_[i][next_point_].y_m = start_y_m_[i];
    }
    ++next_point_;
}
void OverCome::GoalPoint()
{
    for (int i = 0; i < 4; i++)
    {
        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
        legs_[i][next_point_].x_m = start_x_m_[i];
        legs_[i][next_point_].y_m = goal_y_m_[i];
        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
    }
    ++next_point_;
}
void OverCome::GetLine()
{
    StartPoint(); //スタート時点
    GravityMove(RIGHT_F);
    Step(RIGHT_F);
    GravityMove(LEFT_B);
    Step(LEFT_B);
    GravityMove(LEFT_F);
    Step(LEFT_F);
    GravityMove(RIGHT_B);
    Step(RIGHT_B);
    // GoalPoint();
}