add quartic polynomial class

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

@@ -0,0 +1,87 @@
+#include "quartic_polynomial.h"
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_join.h"
+
+namespace iv {
+namespace math {
+
+QuarticPolynomial::QuarticPolynomial(const double x0, const double dx0, const double ddx0,
+                                     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] = dx1;
+    end_condition_[1] = ddx1;
+    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) {}
+
+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;
+    coef_[2] = ddx0 * 0.5;
+
+    double b0 = dx1 - ddx0 * t - dx0;
+    double b1 = ddx1 - ddx0;
+    double t2 = t * t;
+    double t3 = t2 * t;
+
+    coef_[3] = (3 * b0 - b1 * t) / (3 * t2);
+    coef_[4] = (-2 * b0 + b1 * t) / (4 * t3);
+}
+
+double QuarticPolynomial::evaluate(const std::uint32_t order, const double p) const
+{
+    switch (order)
+    {
+    case 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];
+    case 2:
+        return (12.0 * coef_[4] * p + 6.0 * coef_[3]) * p + 2.0 * coef_[2];
+    case 3:
+        return 24.0 * coef_[4] * p + 6.0 * coef_[3];
+    case 4:
+        return 24.0 * coef_[4];
+    default:
+        return 0.0;
+    }
+}
+
+double QuarticPolynomial::coef(const std::int32_t order) const
+{
+    return coef_[order];
+}
+
+const std::array<double, 5> &QuarticPolynomial::coef() const
+{
+    return coef_;
+}
+
+std::string QuarticPolynomial::toString() const
+{
+    return absl::StrCat("QuarticPolynomial(", absl::StrJoin(coef_, ", "), t_, ")\n");
+}
+}
+}

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

@@ -0,0 +1,95 @@
+/****************************************
+ * Class:    Quartic Polynomial class
+ * Purpose:  Fitting a quartic polynomial on interval [0, t] by given
+ *     boundary conditions.
+ * Author:   Zhao Liang
+ * Last Updated:  2021/02/23
+*****************************************
+*
+* 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
+*
+* Boundary conditions are given by:
+* 1. x0=f(0), dx0=f'(0), ddx0=f''(0)
+* 2. dx1=f'(t), ddx1=f''(t)
+*/
+
+#ifndef QUARTIC_POLYNOMIAL_H
+#define QUARTIC_POLYNOMIAL_H
+
+#pragma once
+
+#include <array>
+
+namespace iv {
+namespace math {
+
+class QuarticPolynomial
+{
+public:
+    QuarticPolynomial() = default;
+    ~QuarticPolynomial() = default;
+
+    /**
+    * @brief Initialize a quartic 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 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.
+    */
+    QuarticPolynomial(const double x0, const double dx0, const double ddx0,
+                      const double dx1, const double ddx1,
+                      const double t
+                      );
+
+    /**
+    * @brief You can also pack the boundary conditions into two arrays.
+    */
+    QuarticPolynomial(const std::array<double, 3> &start,
+                      const std::array<double, 2> &end,
+                      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 std::int32_t order) const;
+
+    /**
+    * @brief Return all coefficients as a std::array.
+    */
+    const std::array<double, 5> &coef() const;
+
+    /**
+    * @brief Return a pretty string representation of this polynomial
+    */
+    std::string toString() const;
+
+private:
+
+    /**
+    * @brief Compute the coefficients array.
+    */
+    void computeCoefficients(const double x0, const double dx0, const double ddx0,
+                             const double dx1, const double ddx1,
+                             const double t
+                             );
+
+    double t_;
+    std::array<double, 3> start_condition_;
+    std::array<double, 2> end_condition_;
+    std::array<double, 5> coef_;
+};
+}
+}
+
+#endif // QUARTIC_POLYNOMIAL_H