Polynomial.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <cmath>
#include <iostream>
#include "util/Eigen.hpp"
 
namespace NAV
{
 
template<typename Scalar>
class Polynomial
{
  public:
    template<typename Derived>
    Polynomial(const Eigen::DenseBase<Derived>& coefficients) // NOLINT(google-explicit-constructor, hicpp-explicit-conversions)
        : _coefficients(coefficients)
    {}
 
    [[nodiscard]] Scalar f(Scalar x) const
    {
        Scalar value = 0;
        auto xpow = static_cast<Scalar>(1.0);
        for (const auto& coeff : _coefficients)
        {
            value += coeff * xpow;
            xpow *= x;
        }
        return value;
    }
 
    [[nodiscard]] Polynomial<Scalar> getDerivative() const
    {
        int degree = _coefficients.size() - 1;
        Eigen::VectorX<Scalar> derivative = Eigen::VectorX<Scalar>::Zero(degree);
        for (int i = 0; i < degree; i++)
        {
            derivative(i) = (i + 1) * _coefficients(i + 1);
        }
        return Polynomial<Scalar>(derivative);
    }
 
    [[nodiscard]] const Eigen::VectorX<Scalar>& coeffs() const
    {
        return _coefficients;
    }
 
    [[nodiscard]] std::string toString(fmt::format_string<double> fmt = "{:.1e}", const char* var = "x") const
    {
        std::string out;
        for (int i = 0; i < _coefficients.rows(); i++)
        {
            bool positive = _coefficients(i) >= 0;
            std::string a = fmt::format(fmt, std::abs(_coefficients(i)));
            if (i == 0)
            {
                out += fmt::format("{}{}", positive ? "" : "-", a);
            }
            else
            {
                out += fmt::format(" {} {}{}{}",
                                   positive ? "+" : "-",
                                   a,
                                   var,
                                   i > 1 ? fmt::format("^{}", i) : "");
            }
        }
        return out;
    }
 
  private:
    Eigen::VectorX<Scalar> _coefficients;
};
 
} // namespace NAV
 
#ifndef DOXYGEN_IGNORE
 
template<typename Scalar>
struct fmt::formatter<NAV::Polynomial<Scalar>> : fmt::formatter<std::string>
{
    template<typename FormatContext>
    auto format(const NAV::Polynomial<Scalar>& poly, FormatContext& ctx) const
    {
        return fmt::formatter<std::string>::format(poly.toString(), ctx);
    }
};
 
#endif
 
template<typename Scalar>
std::ostream& operator<<(std::ostream& os, const NAV::Polynomial<Scalar>& obj)
{
    return os << fmt::format("{}", obj);
}