int LinearMotion::getPose(long time, Vector3D points[FOOTS_COUNT]) {
double k = 1.0*time/m_MaxTime;
if(k < 0.25) {
linearFunc(2.0, -m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k,
points[LEFT_FRONT_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k,
points[LEFT_BACK_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k,
points[RIGTH_MIDLE_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k,
points[LEFT_MIDLE_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k,
points[RIGTH_FRONT_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k,
points[RIGTH_BACK_FOOT_IDX]);
int h = (int)m_Heigth*cos(M_PI*k*2);
points[LEFT_MIDLE_FOOT_IDX].z += h;
points[RIGTH_FRONT_FOOT_IDX].z += h;
points[RIGTH_BACK_FOOT_IDX].z += h;
} else if(k < 0.5) {
linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
points[LEFT_FRONT_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
points[LEFT_BACK_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
points[RIGTH_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
points[LEFT_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
points[RIGTH_FRONT_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
points[RIGTH_BACK_FOOT_IDX]);
int h = (int)m_Heigth*sin(M_PI*(k - 0.25)*2);
points[LEFT_FRONT_FOOT_IDX].z += h;
points[LEFT_BACK_FOOT_IDX].z += h;
points[RIGTH_MIDLE_FOOT_IDX].z += h;
} else if(k < 0.75) {
linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], k - 0.5,
points[LEFT_FRONT_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], k - 0.5,
points[LEFT_BACK_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], k - 0.5,
points[RIGTH_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], k - 0.5,
points[LEFT_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], k - 0.5,
points[RIGTH_FRONT_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], k - 0.5,
points[RIGTH_BACK_FOOT_IDX]);
int h = (int)m_Heigth*cos(M_PI*(k - 0.5)*2);
points[LEFT_FRONT_FOOT_IDX].z += h;
points[LEFT_BACK_FOOT_IDX].z += h;
points[RIGTH_MIDLE_FOOT_IDX].z += h;
} else {
linearFunc(2.0, m_Offset, m_Pose0[LEFT_FRONT_FOOT_IDX], 1 - k,
points[LEFT_FRONT_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[LEFT_BACK_FOOT_IDX], 1 - k,
points[LEFT_BACK_FOOT_IDX]);
linearFunc(2.0, m_Offset, m_Pose0[RIGTH_MIDLE_FOOT_IDX], 1 - k,
points[RIGTH_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[LEFT_MIDLE_FOOT_IDX], 1 - k,
points[LEFT_MIDLE_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_FRONT_FOOT_IDX], 1 - k,
points[RIGTH_FRONT_FOOT_IDX]);
linearFunc(2.0, -m_Offset, m_Pose0[RIGTH_BACK_FOOT_IDX], 1 - k,
points[RIGTH_BACK_FOOT_IDX]);
int h = (int)m_Heigth*sin(M_PI*(k - 0.75)*2);
points[LEFT_MIDLE_FOOT_IDX].z += h;
points[RIGTH_FRONT_FOOT_IDX].z += h;
points[RIGTH_BACK_FOOT_IDX].z += h;
}
return S_OK;
}