gz/math/Polynomial3.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef GZ_MATH_POLYNOMIAL3_HH_
#define GZ_MATH_POLYNOMIAL3_HH_
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <utility>
 
#include <gz/math/Interval.hh>
#include <gz/math/Vector4.hh>
#include <gz/math/config.hh>
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  template <typename T>
  class Polynomial3
  {
    public: Polynomial3() = default;
 
    public: explicit Polynomial3(Vector4<T> _coeffs)
    : coeffs(std::move(_coeffs))
    {
    }
 
    public: static Polynomial3 Constant(T _value)
    {
      return Polynomial3(Vector4<T>(0., 0., 0., _value));
    }
 
    public: const Vector4<T> &Coeffs() const { return this->coeffs; }
 
    public: T Evaluate(const T &_x) const
    {
      using std::isnan, std::isfinite;
      if (isnan(_x))
      {
        return _x;
      }
      if (!isfinite(_x))
      {
        using std::abs, std::copysign;
        const T epsilon =
            std::numeric_limits<T>::epsilon();
        if (abs(this->coeffs[0]) >= epsilon)
        {
          return _x * copysign(T(1.), this->coeffs[0]);
        }
        if (abs(this->coeffs[1]) >= epsilon)
        {
          return copysign(_x, this->coeffs[1]);
        }
        if (abs(this->coeffs[2]) >= epsilon)
        {
          return _x * copysign(T(1.), this->coeffs[2]);
        }
        return this->coeffs[3];
      }
      const T _x2 = _x * _x;
      const T _x3 = _x2 * _x;
 
      return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 +
              this->coeffs[2] * _x + this->coeffs[3]);
    }
 
    public: T operator()(const T &_x) const
    {
      return this->Evaluate(_x);
    }
 
    public: T Minimum(const Interval<T> &_interval, T &_xMin) const
    {
      if (_interval.Empty())
      {
        _xMin = std::numeric_limits<T>::quiet_NaN();
        return std::numeric_limits<T>::quiet_NaN();
      }
      T yMin;
      // For open intervals, assume continuity in the limit
      const T &xLeft = _interval.LeftValue();
      const T &xRight = _interval.RightValue();
      const T yLeft = this->Evaluate(xLeft);
      const T yRight = this->Evaluate(xRight);
      if (yLeft <= yRight)
      {
        yMin = yLeft;
        _xMin = xLeft;
      }
      else
      {
        yMin = yRight;
        _xMin = xRight;
      }
      using std::abs, std::sqrt;  // enable ADL
      constexpr T epsilon = std::numeric_limits<T>::epsilon();
      if (abs(this->coeffs[0]) >= epsilon)
      {
        // Polynomial function p(x) is cubic, look
        // for local minima within the given interval
 
        // Find local extrema by computing the roots
        // of p'(x), a quadratic polynomial function
        const T a = this->coeffs[0] * T(3.);
        const T b = this->coeffs[1] * T(2.);
        const T c = this->coeffs[2];
 
        const T discriminant = b * b - T(4.) * a * c;
        if (discriminant >= T(0.))
        {
          // Roots of p'(x) are real, check local minima
          const T x = (-b + sqrt(discriminant)) / (T(2.) * a);
          if (_interval.Contains(x))
          {
            const T y = this->Evaluate(x);
            if (y < yMin)
            {
              _xMin = x;
              yMin = y;
            }
          }
        }
      }
      else if (abs(this->coeffs[1]) >= epsilon)
      {
        // Polynomial function p(x) is quadratic,
        // look for global minima if concave
        const T a = this->coeffs[1];
        const T b = this->coeffs[2];
        if (a > T(0.))
        {
          const T x = -b / (T(2.) * a);
          if (_interval.Contains(x))
          {
            const T y = this->Evaluate(x);
            if (y < yMin)
            {
              _xMin = x;
              yMin = y;
            }
          }
        }
      }
      return yMin;
    }
 
    public: T Minimum(const Interval<T> &_interval) const
    {
      T xMin;
      return this->Minimum(_interval, xMin);
    }
 
    public: T Minimum(T &_xMin) const
    {
      return this->Minimum(Interval<T>::Unbounded, _xMin);
    }
 
    public: T Minimum() const
    {
      T xMin;
      return this->Minimum(Interval<T>::Unbounded, xMin);
    }
 
    public: void Print(std::ostream &_out, const std::string &_x = "x") const
    {
      constexpr T epsilon =
          std::numeric_limits<T>::epsilon();
      bool streamStarted = false;
      for (size_t i = 0; i < 4; ++i)
      {
        using std::abs;  // enable ADL
        const T magnitude = abs(this->coeffs[i]);
        const bool sign = this->coeffs[i] < T(0);
        const int exponent = 3 - static_cast<int>(i);
        if (magnitude >= epsilon)
        {
          if (streamStarted)
          {
            if (sign)
            {
              _out << " - ";
            }
            else
            {
              _out << " + ";
            }
          }
          else if (sign)
          {
            _out << "-";
          }
          if (exponent > 0)
          {
            if ((magnitude - T(1)) > epsilon)
            {
              _out << magnitude << " ";
            }
            _out <<  _x;
            if (exponent > 1)
            {
              _out << "^" << exponent;
            }
          }
          else
          {
            _out << magnitude;
          }
          streamStarted = true;
        }
      }
      if (!streamStarted)
      {
        _out << this->coeffs[3];
      }
    }
 
    public: friend std::ostream &operator<<(
      std::ostream &_out, const gz::math::Polynomial3<T> &_p)
    {
      _p.Print(_out, "x");
      return _out;
    }
 
    private: Vector4<T> coeffs;
  };
  using Polynomial3f = Polynomial3<float>;
  using Polynomial3d = Polynomial3<double>;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_POLYNOMIAL3_HH_