add new code

src/decition/decition_brain/decision/adc_math/core/include/splines/PolynomialXd.h

@@ -1,130 +0,0 @@
-#ifndef POLYNOMIALXD_H
-#define POLYNOMIALXD_H
-
-#pragma once
-
-#include <vector>
-
-#include "absl/strings/str_cat.h"
-#include "absl/strings/str_join.h"
-
-namespace adtoolbox {
-namespace core {
-
-class PolynomialXd
-{
-public:
-    PolynomialXd() = default;
-    explicit PolynomialXd(const size_t order);
-    explicit PolynomialXd(const std::vector<double> &params);
-    double operator()(const double value) const;
-    double operator[](const size_t index) const;
-    void setParams(const std::vector<double> &params);
-
-    static PolynomialXd derivedFrom(const PolynomialXd &other);
-    static PolynomialXd integratedFrom(const PolynomialXd &other, const double c);
-
-    size_t order() const;
-    const std::vector<double> &params() const;
-    std::string toString() const;
-
-private:
-    std::vector<double> params_;
-};
-
-// ----------------------------------------
-
-PolynomialXd::PolynomialXd(const size_t order)
-    : params_(order + 1, 0.0) {}
-
-// ----------------------------------------
-
-PolynomialXd::PolynomialXd(const std::vector<double> &params)
-    : params_(params) {}
-
-// ----------------------------------------
-
-size_t PolynomialXd::order() const
-{
-    return params_.size() - 1;
-}
-
-// ----------------------------------------
-
-void PolynomialXd::setParams(const std::vector<double> &params)
-{
-    params_ = params;
-}
-
-// ----------------------------------------
-
-const std::vector<double> &PolynomialXd::params() const
-{
-    return params_;
-}
-
-// ----------------------------------------
-
-double PolynomialXd::operator()(const double value) const
-{
-    double result = 0.0;
-    for (auto rit = params_.rbegin(); rit != params_.rend(); ++rit)
-    {
-        result = result * value + (*rit);
-    }
-    return result;
-}
-
-// ----------------------------------------
-
-double PolynomialXd::operator[](const size_t index) const
-{
-    return index >= params_.size() ? 0.0 : params_[index];
-}
-
-// ----------------------------------------
-
-PolynomialXd PolynomialXd::derivedFrom(const PolynomialXd &other)
-{
-    std::vector<double> params;
-    if (other.order() <= 0)
-    {
-        params.clear();
-    }
-    else
-    {
-        params.resize(other.params().size() - 1);
-        for (size_t i = 1; i < other.order() + 1; i++)
-        {
-            params[i - 1] = other[i] * i;
-        }
-    }
-    return PolynomialXd(params);
-}
-
-// ----------------------------------------
-
-PolynomialXd PolynomialXd::integratedFrom(const PolynomialXd &other, const double c)
-{
-    std::vector<double> params;
-    params.resize(other.params().size() + 1);
-    params[0] = c;
-    for (size_t i = 0; i < other.params().size(); i++)
-    {
-        params[i + 1] = other[i] / (i + 1);
-    }
-    return PolynomialXd(params);
-}
-
-// ----------------------------------------
-
-std::string PolynomialXd::toString() const
-{
-    return absl::StrCat("PolynomailXd(", absl::StrJoin(params_, ", "), ")");
-}
-// ----------------------------------------
-// Ends class PolynomialXd
-}
-}
-
-#endif // POLYNOMIALXD_H

src/decition/decition_brain/decision/adc_math/core/include/splines/QuarticPolynomial.h

@@ -1,127 +0,0 @@
-#ifndef QUARTICPOLYNOMIAL_H
-#define QUARTICPOLYNOMIAL_H
-
-#pragma once
-
-#include <array>
-#include "absl/strings/str_cat.h"
-#include "absl/strings/str_join.h"
-
-namespace adtoolbox {
-namespace core {
-
-class QuarticPolynomial
-{
-public:
-    QuarticPolynomial() = default;
-    virtual ~QuarticPolynomial() = default;
-
-    QuarticPolynomial(const double x0, const double dx0, const double ddx0,
-                      const double dx1, const double ddx1,
-                      const double t);
-
-    QuarticPolynomial(const std::array<double, 3> &start,
-                      const std::array<double, 2> &end,
-                      const double t);
-
-    double evaluate(const size_t order, const double t) const;
-    size_t order() const;
-    double paramLength() const;
-    double coef(const size_t order) const;
-    std::string toString() const;
-
-private:
-    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_;
-};
-
-// ----------------------------------------
-
-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) {}
-
-double QuarticPolynomial::evaluate(const size_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;
-    }
-}
-
-// ----------------------------------------
-
-size_t QuarticPolynomial::order() const { return 4; }
-
-// ----------------------------------------
-
-double QuarticPolynomial::coef(const size_t order) const { return coef_[order];}
-
-// ----------------------------------------
-
-double QuarticPolynomial::paramLength() const { return t_; }
-
-// ----------------------------------------
-
-std::string QuarticPolynomial::toString() const
-{
-    return absl::StrCat("QuarticPoly(", absl::StrJoin(coef_, ", "), ")");
-}
-
-// ----------------------------------------
-
-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);
-}
-// ----------------------------------------
-// Ends class QuarticPolynomial
-}
-}
-#endif // QUARTICPOLYNOMIAL_H

src/decition/decition_brain/decision/adc_math/core/include/splines/QuinticPolynomial.h

@@ -1,162 +0,0 @@
-#ifndef QUINTICPOLYNOMIAL_H
-#define QUINTICPOLYNOMIAL_H
-
-#pragma once
-
-#include <array>
-#include "absl/strings/str_cat.h"
-#include "absl/strings/str_join.h"
-
-namespace adtoolbox {
-namespace core {
-
-class QuinticPolynomial
-{
-public:
-    QuinticPolynomial() = default;
-    virtual ~QuinticPolynomial() = default;
-
-    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 fitBoundaryConditions(const double x0, const double dx0, const double ddx0,
-                               const double x1, const double dx1, const double ddx1,
-                               const double t);
-
-    double evaluate(const size_t order, const double t) const;
-    size_t order() const;
-    double paramLength() const;
-    double coef(const size_t order) const;
-    std::string toString() const;
-
-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_;
-};
-
-// ----------------------------------------
-
-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) {}
-
-void QuinticPolynomial::fitBoundaryConditions(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::evaluate(const size_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;
-    }
-}
-
-// ----------------------------------------
-
-size_t QuinticPolynomial::order() const
-{
-    return 5;
-}
-
-// ----------------------------------------
-
-double QuinticPolynomial::coef(const size_t order) const
-{
-    return coef_[order];
-}
-
-// ----------------------------------------
-
-double QuinticPolynomial::paramLength() const
-{
-    return t_;
-}
-
-// ----------------------------------------
-
-std::string QuinticPolynomial::toString() const
-{
-    return absl::StrCat("QuinticPoly(", absl::StrJoin(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;
-}
-// ----------------------------------------
-// Ends class QuinticPolynomial
-}
-}
-#endif // QUINTICPOLYNOMIAL_H

src/decition/decition_brain/decision/adc_math/core/include/splines/Spline1dSegment.h

@@ -1,120 +0,0 @@
-#ifndef SPLINE1DSEGMENT_H
-#define SPLINE1DSEGMENT_H
-
-#pragma once
-
-#include <vector>
-#include <eigen3/Eigen/Core>
-
-#include "core/include/splines/PolynomialXd.h"
-
-namespace adtoolbox {
-namespace core {
-
-class Spline1dSegment
-{
-public:
-    explicit Spline1dSegment(const size_t order);
-    explicit Spline1dSegment(const std::vector<double> &params);
-    ~Spline1dSegment() = default;
-
-    void setParams(const std::vector<double> &params);
-    double operator()(const double x) const;
-    double derivative(const double x) const;
-    double secondOrderDerivative(const double x) const;
-    double thirdOrderDerivative(const double x) const;
-
-    const PolynomialXd &splineFunc() const;
-    const PolynomialXd &derivativeFunc() const;
-    const PolynomialXd &secondOrderDerivativeFunc() const;
-    const PolynomialXd &thirdOrderDerivativeFunc() const;
-
-private:
-    void setSplineFunc(const PolynomialXd &spline_func);
-
-    PolynomialXd f_;
-    PolynomialXd df_;
-    PolynomialXd ddf_;
-    PolynomialXd dddf_;
-};
-
-// ----------------------------------------
-
-Spline1dSegment::Spline1dSegment(const size_t order)
-{
-    setSplineFunc(PolynomialXd(order));
-}
-
-// ----------------------------------------
-
-Spline1dSegment::Spline1dSegment(const std::vector<double> &params)
-{
-    setSplineFunc(PolynomialXd(params));
-}
-
-// ----------------------------------------
-
-void Spline1dSegment::setParams(const std::vector<double> &params)
-{
-    setSplineFunc(PolynomialXd(params));
-}
-
-// ----------------------------------------
-
-void Spline1dSegment::setSplineFunc(const PolynomialXd &spline_func)
-{
-    f_ = spline_func;
-    df_ = PolynomialXd::derivedFrom(f_);
-    ddf_ = PolynomialXd::derivedFrom(df_);
-    dddf_ = PolynomialXd::derivedFrom(ddf_);
-}
-
-// ----------------------------------------
-
-double Spline1dSegment::operator()(const double x) const
-{
-    return f_(x);
-}
-
-double Spline1dSegment::derivative(const double x) const
-{
-    return df_(x);
-}
-
-double Spline1dSegment::secondOrderDerivative(const double x) const
-{
-    return ddf_(x);
-}
-
-double Spline1dSegment::thirdOrderDerivative(const double x) const
-{
-    return dddf_(x);
-}
-
-// ----------------------------------------
-
-const PolynomialXd& Spline1dSegment::splineFunc() const
-{
-    return f_;
-}
-
-const PolynomialXd& Spline1dSegment::derivativeFunc() const
-{
-    return df_;
-}
-
-const PolynomialXd& Spline1dSegment::secondOrderDerivativeFunc() const
-{
-    return ddf_;
-}
-
-const PolynomialXd& Spline1dSegment::thirdOrderDerivativeFunc() const
-{
-    return dddf_;
-}
-// ----------------------------------------
-// Ends class Spline1dSegment
-}
-}
-
-#endif // SPLINE1DSEGMENT_H

src/decition/decition_brain/decision/adc_math/core/include/splines/Spline2dSegment.h

@@ -1,133 +0,0 @@
-#ifndef SPLINE2DSEGMENT_H
-#define SPLINE2DSEGMENT_H
-
-#pragma once
-
-#include <vector>
-#include <eigen3/Eigen/Core>
-
-#include "core/include/splines/PolynomialXd.h"
-
-namespace adtoolbox {
-namespace core {
-
-class Spline2dSegment
-{
-public:
-    explicit Spline2dSegment(const size_t order);
-    Spline2dSegment(const std::vector<double> &x_param,
-                    const std::vector<double> &y_param);
-    ~Spline2dSegment() = default;
-
-    bool setParams(const std::vector<double> &x_param,
-                   const std::vector<double> &y_param);
-
-    std::pair<double, double> operator()(const double t) const;
-    double x(const double t) const;
-    double y(const double t) const;
-    double derivativeX(const double t) const;
-    double derivativeY(const double t) const;
-    double secondOrderDerivativeX(const double t) const;
-    double secondOrderDerivativeY(const double t) const;
-    double thirdOrderDerivativeX(const double t) const;
-    double thirdOrderDerivativeY(const double t) const;
-
-    const PolynomialXd& splineFuncX() const;
-    const PolynomialXd& splineFuncY() const;
-    const PolynomialXd& derivativeFuncX() const;
-    const PolynomialXd& derivativeFuncY() const;
-    const PolynomialXd& secondOrderDerivativeFuncX() const;
-    const PolynomialXd& secondOrderDerivativeFuncY() const;
-    const PolynomialXd& thirdOrderDerivativeFuncX() const;
-    const PolynomialXd& thirdOrderDerivativeFuncY() const;
-
-private:
-    PolynomialXd fx_;
-    PolynomialXd fy_;
-    PolynomialXd dfx_;
-    PolynomialXd dfy_;
-    PolynomialXd ddfx_;
-    PolynomialXd ddfy_;
-    PolynomialXd dddfx_;
-    PolynomialXd dddfy_;
-};
-
-// ----------------------------------------
-
-Spline2dSegment::Spline2dSegment(const size_t order)
-    : fx_(order), fy_(order)
-{
-    dfx_ = PolynomialXd::derivedFrom(fx_);
-    dfy_ = PolynomialXd::derivedFrom(fy_);
-    ddfx_ = PolynomialXd::derivedFrom(dfx_);
-    ddfy_ = PolynomialXd::derivedFrom(dfy_);
-    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
-    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
-}
-
-// ----------------------------------------
-
-Spline2dSegment::Spline2dSegment(const std::vector<double>& x_param,
-                                 const std::vector<double>& y_param)
-    : fx_(x_param), fy_(y_param)
-{
-    dfx_ = PolynomialXd::derivedFrom(fx_);
-    dfy_ = PolynomialXd::derivedFrom(fy_);
-    ddfx_ = PolynomialXd::derivedFrom(dfx_);
-    ddfy_ = PolynomialXd::derivedFrom(dfy_);
-    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
-    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
-}
-
-// ----------------------------------------
-
-bool Spline2dSegment::setParams(const std::vector<double> &x_param,
-                                const std::vector<double> &y_param)
-{
-    if (x_param.size() != y_param.size())
-        return false;
-
-    fx_ = PolynomialXd(x_param);
-    fy_ = PolynomialXd(y_param);
-    dfx_ = PolynomialXd::derivedFrom(fx_);
-    dfy_ = PolynomialXd::derivedFrom(fy_);
-    ddfx_ = PolynomialXd::derivedFrom(dfx_);
-    ddfy_ = PolynomialXd::derivedFrom(dfy_);
-    dddfx_ = PolynomialXd::derivedFrom(ddfx_);
-    dddfy_ = PolynomialXd::derivedFrom(ddfy_);
-    return true;
-}
-
-// ----------------------------------------
-
-std::pair<double, double> Spline2dSegment::operator()(const double t) const
-{
-    return std::make_pair(fx_(t), fy_(t));
-}
-
-// ----------------------------------------
-
-double Spline2dSegment::x(const double t) const { return fx_(t); }
-double Spline2dSegment::y(const double t) const { return fy_(t); }
-double Spline2dSegment::derivativeX(const double t) const { return dfx_(t); }
-double Spline2dSegment::derivativeY(const double t) const { return dfy_(t); }
-double Spline2dSegment::secondOrderDerivativeX(const double t) const { return ddfx_(t); }
-double Spline2dSegment::secondOrderDerivativeY(const double t) const { return ddfy_(t); }
-double Spline2dSegment::thirdOrderDerivativeX(const double t) const { return dddfx_(t); }
-double Spline2dSegment::thirdOrderDerivativeY(const double t) const { return dddfy_(t); }
-
-// ----------------------------------------
-
-const PolynomialXd& Spline2dSegment::splineFuncX() const { return fx_; }
-const PolynomialXd& Spline2dSegment::splineFuncY() const { return fy_; }
-const PolynomialXd& Spline2dSegment::derivativeFuncX() const { return dfx_; }
-const PolynomialXd& Spline2dSegment::derivativeFuncY() const { return dfy_; }
-const PolynomialXd& Spline2dSegment::secondOrderDerivativeFuncX() const { return ddfx_; }
-const PolynomialXd& Spline2dSegment::secondOrderDerivativeFuncY() const { return ddfy_; }
-const PolynomialXd& Spline2dSegment::thirdOrderDerivativeFuncX() const { return dddfx_; }
-const PolynomialXd& Spline2dSegment::thirdOrderDerivativeFuncY() const { return dddfy_; }
-// ----------------------------------------
-// Ends class Spline2dSegment
-}
-}
-#endif // SPLINE2DSEGMENT_H