triangle.cpp

00001 #include "triangle.h"
00002 #include "pi.h"
00003 #include <math.h>
00004 void Triangle::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
00005                                 float stridetime_s, float toptime_s, float buffer_time_s)
00006 {
00007     offset_x_m_ = offset_x_m;
00008     offset_y_m_ = offset_y_m;
00009     stride_m_ = stride_m;
00010     height_m_ = height_m;               //足上げ幅
00011     buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
00012     stridetime_s_ = stridetime_s;
00013     toptime_s_ = toptime_s;         //頂点に行くまでの時間
00014     buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
00015 
00016     //事前に計算しておく
00017     CalOtherParam();
00018 }
00019 void Triangle::CalOtherParam()
00020 {
00021     beta_degree_ = 81; //論文よりこれが最適らしい
00022     reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI);
00023     top_x_m_ = offset_x_m_ + stride_m_ * 0.5 + height_m_ * reverse_tanbeta_;
00024     top_y_m_ = -height_m_ + offset_y_m_;
00025     buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_;
00026     buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
00027 }
00028 //足一周の時間
00029 float Triangle::GetOneWalkTime()
00030 {
00031     return stridetime_s_ + toptime_s_ + buffer_time_s_;
00032 };
00033 int Triangle::GetOrbit(OneLeg &leg, float phasetime_s)
00034 {
00035     int ret = 0;
00036     if (phasetime_s < stridetime_s_)
00037         ret = StrideLineAccel_(leg, phasetime_s);
00038     else if (phasetime_s < stridetime_s_ + toptime_s_)
00039         ret = leg.SetXY_m(top_x_m_, top_y_m_);
00040     else
00041         ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
00042     return ret;
00043 };
00044 int Triangle::StrideLine_(OneLeg &leg, float phasetime_s)
00045 {
00046     float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
00047     float y_m = offset_y_m_;
00048     return leg.SetXY_m(x_m, y_m);
00049 }
00050 int Triangle::StrideLineAccel_(OneLeg &leg, float phasetime_s)
00051 {
00052     ///////////x,yを計算
00053     float s0 = stride_m_ * 0.5;
00054     float s1 = -stride_m_ * 0.5;
00055     float g_h = sqrtf(kGravity / offset_y_m_);
00056     float t = phasetime_s / stridetime_s_;
00057     float denominator = expf(g_h) - expf(-g_h); //分母
00058 
00059     float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
00060     x_m += offset_x_m_;
00061     float y_m = offset_y_m_;
00062     //x,yを代入
00063     return leg.SetXY_m(x_m, y_m);
00064 }
00065 void Triangle::ChangeOneParam(TriangleParams param, float val)
00066 {
00067     switch (param)
00068     {
00069     case OFFSET_X_M:
00070         offset_x_m_ = val;
00071         break;
00072     case OFFSET_Y_M:
00073         offset_y_m_ = val;
00074         break;
00075     case STRIDE_M:
00076         stride_m_ = val;
00077         break;
00078     case HEIGHT_M:
00079         height_m_ = val;
00080         break;
00081     case BUFFER_HEIGHT_M:
00082         buffer_height_m_ = val;
00083         break;
00084     }
00085     CalOtherParam();
00086 }
00087 void Triangle::Copy(const Triangle &origin)
00088 {
00089     *this = origin;
00090 }
00091 
00092 void FourPoint::CalOtherParam()
00093 {
00094     beta_degree_ = 60; //論文よりこれが最適らしい
00095     reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI);
00096     top_x_m_ = offset_x_m_;
00097     top_y_m_ = -height_m_ + offset_y_m_;
00098     buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_;
00099     buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
00100 }
00101 void FourPoint::SetFourPointParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
00102                                   float stridetime_s, float toptime_s, float buffer_time_s)
00103 {
00104     SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
00105                      stridetime_s, toptime_s, buffer_time_s);
00106     CalOtherParam();
00107 }