第十九篇，解析法求解五阶多项式

// x0为初始约束，时间为0；x1为结束约束，时间为t
// coef_为求解结果，定义x=at^5 + bt^4 + ct^3 + dt^2 + et + f，
// 则coef_[0]为f，coef_[5]为a
void ComputeCoefficients_order5(const double x0, const double dx0, const double ddx0, const double x1, const double dx1, const double ddx1, const double t, double* coef_)
{
  if (t <= 0.0)
  {
    return;
  }

  coef_[5] = x0;
  coef_[4] = dx0;
  coef_[3] = ddx0 / 2.0;

  double t2 = t * t;
  double t3 = t * t2;

  // the analytical method is at least 6 times faster than matrix inversion.
  double c0 = (x1 - t2*ddx0/2 - dx0*t - x0) / t3;
  double c1 = (dx1 - ddx0*t - dx0) / t2;
  double c2 = (ddx1 - ddx0) / t;

  coef_[0] = (6*c0 - 3*c1 + 0.5*c2) / t2;
  coef_[1] = (-15*c0 + 7*c1 - c2) / t;
  coef_[2] = 0.5 * (20*c0 - 8*c1 + c2);
}

求解五阶多项式的核心算法往往做矩阵求逆，效率较低。

其实受益于初始时间t=0的设定，这个方程组并不难解，草稿纸上画画就很容易得出解析解的表达式，简单的加减乘除速度很快。

可对比先前发过的文章第十七篇，五阶多项式_qq_404546876的博客-CSDN博客_五阶多项式。