freeline.cpp

00001 #include "freeline.h"
00002 void FreeLines::SetFreeLinesParam(LineParam lineparams[], int point_num)
00003 {
00004     point_num_ = point_num;
00005     for (int i = 0; i < point_num_; i++)
00006         lineparams_[i] = lineparams[i];
00007 }
00008 int FreeLines::GetOrbit(OneLeg &leg, float phasetime_s)
00009 {
00010     //現在の最終到達pointを決定
00011     int next_point;
00012     for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
00013     {
00014         if (phasetime_s < lineparams_[next_point].time_s)
00015             break;
00016     }
00017     float x_m, y_m;
00018     if (lineparams_[next_point].is_point_to_point == 1)
00019     {
00020         x_m = lineparams_[next_point].x_m;
00021         y_m = lineparams_[next_point].y_m;
00022     }
00023     else
00024     {
00025         //x,yを計算
00026         float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
00027         float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
00028         float phase_rate = d_phase / d_time;
00029         x_m = lineparams_[next_point - 1].x_m +
00030               (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
00031         y_m = lineparams_[next_point - 1].y_m +
00032               (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
00033     }
00034     return leg.SetXY_m(x_m, y_m);
00035 }
00036 float FreeLines::GetOneWalkTime() //足一周の時間
00037 {
00038     return lineparams_[point_num_ - 1].time_s;
00039 }
00040 void FreeLines::Copy(const FreeLines &origin)
00041 {
00042     point_num_ = origin.point_num_; //pointの数
00043     for (int i = 0; i < point_num_; i++)
00044         lineparams_[i] = origin.lineparams_[i];
00045 }