Diff: Walk/orbit/triangle/triangle.cpp

Revision:

35:b4e1b8f25cd7

Child:

39:87dcdff27797

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/orbit/triangle/triangle.cpp	Mon Mar 04 09:54:47 2019 +0000
@@ -0,0 +1,87 @@
+#include "triangle.h"
+#include "pi.h"
+#include <math.h>
+void Triangle::SetTriangleParam(float offset_x_m, float offset_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); //論文よりこれが最適らしい
+    offset_x_m_ = offset_x_m;
+    offset_y_m_ = offset_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; //頂点から一時停止点までの時間.
+
+    //事前に計算しておく
+    CalOtherParam();
+}
+void Triangle::CalOtherParam()
+{
+    top_x_m_ = offset_x_m_ + stride_m_ * 0.5 - height_m_ * reverse_tanbeta_;
+    top_y_m_ = -height_m_ + offset_y_m_;
+    buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_;
+    buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
+}
+//足一周の時間
+float Triangle::GetOneWalkTime()
+{
+    return stridetime_s_ + toptime_s_ + buffer_time_s_;
+};
+int Triangle::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 Triangle::StrideLine_(OneLeg &leg, float phasetime_s)
+{
+    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
+    float y_m = offset_y_m_;
+    return leg.SetXY_m(x_m, y_m);
+}
+int Triangle::StrideLineAccel_(OneLeg &leg, float phasetime_s)
+{
+    ///////////x,yを計算
+    float s0 = stride_m_ * 0.5 + offset_x_m_;
+    float s1 = offset_x_m_ - stride_m_ * 0.5;
+    float g_h = sqrtf(kGravity / offset_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 = offset_y_m_;
+    //x,yを代入
+    return leg.SetXY_m(x_m, y_m);
+}
+void Triangle::ChangeOneParam(TriangleParams param, float val)
+{
+    switch (param)
+    {
+    case OFFSET_X_M:
+        offset_x_m_ = val;
+        break;
+    case OFFSET_Y_M:
+        offset_y_m_ = val;
+        break;
+    case STRIDE_M:
+        stride_m_ = val;
+        break;
+    case HEIGHT_M:
+        height_m_ = val;
+        break;
+    case BUFFER_HEIGHT_M:
+        buffer_height_m_ = val;
+        break;
+    }
+    CalOtherParam();
+}
+void Triangle::Copy(const Triangle & origin){
+    *this = origin;
+}
\ No newline at end of file