add quintic polynomial class

src/decition/decition_brain/decition/adc_math/quartic_polynomail.cpp

@@ -7,8 +7,7 @@ namespace math {
 
 QuarticPolynomial::QuarticPolynomial(const double x0, const double dx0, const double ddx0,
                                      const double dx1, const double ddx1,
-                                     const double t
-                                     )
+                                     const double t)
 {
     t_ = t;
     start_condition_[0] = x0;
@@ -19,21 +18,15 @@ QuarticPolynomial::QuarticPolynomial(const double x0, const double dx0, const do
     computeCoefficients(x0, dx0, ddx0, dx1, ddx1, t);
 }
 
-QuarticPolynomial::QuarticPolynomial(
-    const std::array<double, 3> &start,
-    const std::array<double, 2> &end,
-    const double t)
-    : QuarticPolynomial::QuarticPolynomial(
-        start[0], start[1], start[2],
-        end[0], end[1], t) {}
+QuarticPolynomial::QuarticPolynomial(const std::array<double, 3> &start,
+                                     const std::array<double, 2> &end,
+                                     const double t)
+    : QuarticPolynomial::QuarticPolynomial(start[0], start[1], start[2],
+                                           end[0], end[1], t) {}
 
-void QuarticPolynomial::computeCoefficients(const double x0,
-                                            const double dx0,
-                                            const double ddx0,
-                                            const double dx1,
-                                            const double ddx1,
-                                            const double t
-                                            )
+void QuarticPolynomial::computeCoefficients(const double x0, const double dx0, const double ddx0,
+                                            const double dx1, const double ddx1,
+                                            const double t)
 {
     coef_[0] = x0;
     coef_[1] = dx0;
@@ -53,11 +46,9 @@ double QuarticPolynomial::evaluate(const std::uint32_t order, const double p) co
     switch (order)
     {
     case 0:
-        return (((coef_[4] * p + coef_[3]) * p + coef_[2]) * p + coef_[1]) * p +
-               coef_[0];
+        return (((coef_[4] * p + coef_[3]) * p + coef_[2]) * p + coef_[1]) * p + coef_[0];
     case 1:
-        return ((4.0 * coef_[4] * p + 3.0 * coef_[3]) * p + 2.0 * coef_[2]) * p +
-               coef_[1];
+        return ((4.0 * coef_[4] * p + 3.0 * coef_[3]) * p + 2.0 * coef_[2]) * p + coef_[1];
     case 2:
         return (12.0 * coef_[4] * p + 6.0 * coef_[3]) * p + 2.0 * coef_[2];
     case 3:
@@ -69,7 +60,7 @@ double QuarticPolynomial::evaluate(const std::uint32_t order, const double p) co
     }
 }
 
-double QuarticPolynomial::coef(const std::int32_t order) const
+double QuarticPolynomial::coef(const size_t order) const
 {
     return coef_[order];
 }
@@ -81,7 +72,7 @@ const std::array<double, 5> &QuarticPolynomial::coef() const
 
 std::string QuarticPolynomial::toString() const
 {
-    return absl::StrCat("QuarticPolynomial(", absl::StrJoin(coef_, ", "), t_, ")\n");
+    return absl::StrCat("QuarticPolynomial(", absl::StrJoin(coef_, ", "), ")");
 }
 }
 }

src/decition/decition_brain/decition/adc_math/quartic_polynomial.h

@@ -42,8 +42,7 @@ public:
     */
     QuarticPolynomial(const double x0, const double dx0, const double ddx0,
                       const double dx1, const double ddx1,
-                      const double t
-                      );
+                      const double t);
 
     /**
     * @brief You can also pack the boundary conditions into two arrays.
@@ -62,7 +61,7 @@ public:
     /**
     * @brief Return the coefficient of the k-th term.
     */
-    double coef(const std::int32_t order) const;
+    double coef(const size_t order) const;
 
     /**
     * @brief Return all coefficients as a std::array.
@@ -74,6 +73,13 @@ public:
     */
     std::string toString() const;
 
+    size_t order() const { return 4; }
+
+    /**
+    * @brief Return the length of the fitting interval [0, t]
+    */
+    double paramLength() const { return t_; }
+
 private:
 
     /**
@@ -81,8 +87,7 @@ private:
     */
     void computeCoefficients(const double x0, const double dx0, const double ddx0,
                              const double dx1, const double ddx1,
-                             const double t
-                             );
+                             const double t);
 
     double t_;
     std::array<double, 3> start_condition_;

src/decition/decition_brain/decition/adc_math/quintic_polynomial.cpp

@@ -0,0 +1,98 @@
+#include "quintic_polynomial.h"
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_join.h"
+
+namespace iv {
+namespace math {
+
+QuinticPolynomial::QuinticPolynomial(const double x0, const double dx0, const double ddx0,
+                                     const double x1, const double dx1, const double ddx1,
+                                     const double t)
+{
+    t_ = t;
+    start_condition_[0] = x0;
+    start_condition_[1] = dx0;
+    start_condition_[2] = ddx0;
+    end_condition_[0] = x1;
+    end_condition_[1] = dx1;
+    end_condition_[2] = ddx1;
+    computeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, t);
+}
+
+QuinticPolynomial::QuinticPolynomial(const std::array<double, 3> &start,
+                                     const std::array<double, 3> &end,
+                                     const double t)
+    : QuinticPolynomial::QuinticPolynomial(start[0], start[1], start[2],
+                                           end[0], end[1], end[2], t) {}
+
+double QuinticPolynomial::evaluate(std::uint32_t order, const double p) const
+{
+    switch (order)
+    {
+    case 0:
+        return ((((coef_[5] * p + coef_[4]) * p + coef_[3]) * p + coef_[2]) * p + coef_[1]) * p + coef_[0];
+    case 1:
+        return (((5.0 * coef_[5] * p + 4.0 * coef_[4]) * p + 3.0 * coef_[3]) * p + 2.0 * coef_[2]) * p + coef_[1];
+    case 2:
+        return (((20.0 * coef_[5] * p + 12.0 * coef_[4]) * p) + 6.0 * coef_[3]) * p + 2.0 * coef_[2];
+    case 3:
+        return (60.0 * coef_[5] * p + 24.0 * coef_[4]) * p + 6.0 * coef_[3];
+    case 4:
+        return 120.0 * coef_[5] * p + 24.0 * coef_[4];
+    case 5:
+        return 120.0 * coef_[5];
+    default:
+        return 0.0;
+    }
+}
+
+void QuinticPolynomial::fitByBoundaryConditions(const double x0, const double dx0, const double ddx0,
+                                                const double x1, const double dx1, const double ddx1,
+                                                const double t)
+{
+    t_ = t;
+    start_condition_[0] = x0;
+    start_condition_[1] = dx0;
+    start_condition_[2] = ddx0;
+    end_condition_[0] = x1;
+    end_condition_[1] = dx1;
+    end_condition_[2] = ddx1;
+    computeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, t);
+}
+
+double QuinticPolynomial::coef(const size_t order) const
+{
+    return coef_[order];
+}
+
+const std::array<double, 6> &QuinticPolynomial::coef() const
+{
+    return coef_;
+}
+
+void QuinticPolynomial::computeCoefficients(const double x0, const double dx0, const double ddx0,
+                                            const double x1, const double dx1, const double ddx1,
+                                            const double t)
+{
+    coef_[0] = x0;
+    coef_[1] = dx0;
+    coef_[2] = 0.5 * ddx0;
+
+    double t2 = t * t;
+    double t3 = t * t2;
+
+    double c0 = (x1 - 0.5 * t2 * ddx0 - dx0 * t - x0) / t3;
+    double c1 = (dx1 - ddx0 * t - dx0) / t2;
+    double c2 = (ddx1 - ddx0) / t;
+
+    coef_[3] = 0.5 * (20.0 * c0 - 8.0 * c1 + c2);
+    coef_[4] = (-15.0 * c0 + 7.0 * c1 - c2) / t;
+    coef_[5] = (6.0 * c0 - 3.0 * c1 + 0.5 * c2) / t2;
+}
+
+std::string QuinticPolynomial::toString() const
+{
+    return absl::StrCat("QuinticPolynomial(", absl::StrJoin(coef_, ", "), ")");
+}
+}
+}

src/decition/decition_brain/decition/adc_math/quintic_polynomial.h

@@ -0,0 +1,93 @@
+/****************************************
+ * Class:    Quintic Polynomial class
+ * Purpose:  Fitting a quinticc polynomial on interval [0, t] by given
+ *     boundary conditions.
+ * Author:   Zhao Liang
+ * Last Updated:  2021/02/24
+*****************************************
+*
+* Note: The polynomial is defined on interval [0, t] and has form
+*
+*     f(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
+*
+* Boundary conditions are given by:
+* 1. x0=f(0), dx0=f'(0), ddx0=f''(0)
+* 2. x1=f(t), dx1=f'(t), ddx1=f''(t)
+*/
+#ifndef QUINTIC_POLYNOMIAL_H
+#define QUINTIC_POLYNOMIAL_H
+
+#pragma once
+
+#include <array>
+
+namespace iv {
+namespace math {
+
+class QuinticPolynomial
+{
+public:
+    QuinticPolynomial() = default;
+    ~QuinticPolynomial() = default;
+
+    /**
+    * @brief Initialize a quintic polynomial by given boundary conditions.
+    * @param x0: start point at x=0
+    * @param dx0: first order derivative at the start point
+    * @param ddx0: second order derivative at the start point
+    * @param x1: end point at x=t
+    * @param dx1: first order derivative at the end point x=t.
+    * @param ddx1: second order derivative at the end point x=t.
+    * @param t: parameter length.
+    */
+    QuinticPolynomial(const double x0, const double dx0, const double ddx0,
+                      const double x1, const double dx1, const double ddx1,
+                      const double t);
+
+    QuinticPolynomial(const std::array<double, 3> &start,
+                      const std::array<double, 3> &end,
+                      const double t);
+
+    void fitByBoundaryConditions(const double x0, const double dx0, const double ddx0,
+                                 const double x1, const double dx1, const double ddx1,
+                                 const double t);
+
+    /**
+    * @brief Evaluate the n-th derivative of this polynomial at a given point p.
+    * @param order: the order of the derivative to be evaluated.
+    * @param p: the point to be evaluated.
+    */
+    double evaluate(const std::uint32_t order, const double p) const;
+
+    /**
+    * @brief Return the coefficient of the k-th term.
+    */
+    double coef(const size_t order) const;
+
+    /**
+    * @brief Return all coefficients as a std::array.
+    */
+    const std::array<double, 6> &coef() const;
+
+    /**
+    * @brief Return a pretty string representation of this polynomial
+    */
+    std::string toString() const;
+
+    size_t order() const { return 5; }
+    double paramLength() const { return t_; }
+
+private:
+    void computeCoefficients(const double x0, const double dx0, const double ddx0,
+                             const double x1, const double dx1, const double ddx1,
+                             const double t);
+
+    double t_;
+    std::array<double, 3> start_condition_;
+    std::array<double, 3> end_condition_;
+    std::array<double, 6> coef_;
+};
+}
+}
+
+#endif // QUINTIC_POLYNOMIAL_H