Diff: Walk/orbit/freeline/freeline.cpp

Revision:

35:b4e1b8f25cd7

Child:

41:38d79b6513c0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/orbit/freeline/freeline.cpp	Mon Mar 04 09:54:47 2019 +0000
@@ -0,0 +1,38 @@
+#include "freeline.h"
+void FreeLines::SetFreeLinesParam(LineParam lineparams[], int point_num)
+{
+    point_num_ = point_num;
+    for (int i = 0; i < point_num_; i++)
+        lineparams_[i] = lineparams[i];
+}
+int FreeLines::GetOrbit(OneLeg &leg, float phasetime_s)
+{
+    ;
+    //現在の最終到達pointを決定
+    int next_point;
+    for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
+    {
+        if (phasetime_s < lineparams_[next_point].time_s)
+            break;
+    }
+    //x,yを計算
+    float x_m, y_m;
+    float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
+    float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
+    float phase_rate = d_phase / d_time;
+    x_m = lineparams_[next_point - 1].x_m +
+          (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
+    y_m = lineparams_[next_point - 1].y_m +
+          (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
+    return leg.SetXY_m(x_m, y_m);
+}
+float FreeLines::GetOneWalkTime() //足一周の時間
+{
+    return lineparams_[point_num_ - 1].time_s;
+}
+void FreeLines::Copy(const FreeLines &origin)
+{
+    point_num_ = origin.point_num_;            //pointの数
+    for (int i = 0; i < point_num_; i++)
+        lineparams_[i] = origin.lineparams_[i];
+}
\ No newline at end of file