Diff: Walk/orbit/triangle/triangle.cpp

Revision:

39:87dcdff27797

Parent:

35:b4e1b8f25cd7

Child:

40:5eb3b3908edd

--- a/Walk/orbit/triangle/triangle.cpp	Mon Mar 04 10:51:46 2019 +0000
+++ b/Walk/orbit/triangle/triangle.cpp	Mon Mar 04 23:41:48 2019 +0000
@@ -4,7 +4,6 @@
 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;
@@ -19,9 +18,11 @@
 }
 void Triangle::CalOtherParam()
 {
-    top_x_m_ = offset_x_m_ + stride_m_ * 0.5 - height_m_ * reverse_tanbeta_;
+    beta_degree_ = 81; //論文よりこれが最適らしい
+    reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI);
+    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_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_;
     buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
 }
 //足一周の時間
@@ -49,13 +50,14 @@
 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 s0 = stride_m_ * 0.5;
+    float s1 = -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;
+    x_m += offset_x_m_;
     float y_m = offset_y_m_;
     //x,yを代入
     return leg.SetXY_m(x_m, y_m);
@@ -82,6 +84,24 @@
     }
     CalOtherParam();
 }
-void Triangle::Copy(const Triangle & origin){
+void Triangle::Copy(const Triangle &origin)
+{
     *this = origin;
+}
+
+void FourPoint::CalOtherParam()
+{
+    beta_degree_ = 60; //論文よりこれが最適らしい
+    reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI);
+    top_x_m_ = offset_x_m_;
+    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_;
+}
+void FourPoint::SetFourPointParam(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)
+{
+    SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
+                     stridetime_s, toptime_s, buffer_time_s);
+    CalOtherParam();
 }
\ No newline at end of file