diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8b0c07da2..27d306cbe 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -33,6 +33,9 @@ target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MA add_executable(matrix3_example matrix3_example.cc) target_link_libraries(matrix3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(polynomial3_example polynomial3_example.cc) +target_link_libraries(polynomial3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(pose3_example pose3_example.cc) target_link_libraries(pose3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/polynomial3_example.cc b/examples/polynomial3_example.cc new file mode 100644 index 000000000..f268dbdec --- /dev/null +++ b/examples/polynomial3_example.cc @@ -0,0 +1,59 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include +#include + +int main(int argc, char **argv) +{ + // A default constructed polynomial should have zero coefficients. + std::cout << "A default constructed polynomial is always: " + << ignition::math::Polynomial3d() << std::endl; + + // A constant polynomial only has an independent term. + std::cout << "A constant polynomial only has an independent term: " + << ignition::math::Polynomial3d::Constant(-1.) << std::endl; + + // A cubic polynomial may be incomplete. + const ignition::math::Polynomial3d p( + ignition::math::Vector4d(1., 0., -1., 2.)); + std::cout << "A cubic polynomial may be incomplete: " << p << std::endl; + + // A polynomial can be evaluated. + const double x = 0.5; + std::cout << "Evaluating " << p << " at " << x + << " yields " << p(x) << std::endl; + + // A polynomial can be queried for its minimum in a given interval, + // as well as for its global minimum (which may not always be finite). + const ignition::math::Intervald interval = + ignition::math::Intervald::Closed(-1, 1.); + std::cout << "The minimum of " << p << " in the " << interval + << " interval is " << p.Minimum(interval) << std::endl; + std::cout << "The global minimum of " << p + << " is " << p.Minimum() << std::endl; + + const ignition::math::Polynomial3d q( + ignition::math::Vector4d(0., 1., 2., 1.)); + std::cout << "The minimum of " << q << " in the " << interval + << " interval is " << q.Minimum(interval) << std::endl; + std::cout << "The global minimum of " << q + << " is " << q.Minimum() << std::endl; + +} +//! [complete] diff --git a/include/ignition/math/Polynomial3.hh b/include/ignition/math/Polynomial3.hh new file mode 100644 index 000000000..4112d8f80 --- /dev/null +++ b/include/ignition/math/Polynomial3.hh @@ -0,0 +1,295 @@ +/* + * 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 IGNITION_MATH_POLYNOMIAL3_HH_ +#define IGNITION_MATH_POLYNOMIAL3_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + /// \class Polynomial3 Polynomial3.hh ignition/math/Polynomial3.hh + /// \brief The Polynomial3 class represents a cubic polynomial + /// with real coefficients p(x) = c0 x^3 + c1 x^2 + c2 x + c3. + /// ## Example + /// + /// \snippet examples/polynomial3_example.cc complete + template + class Polynomial3 + { + /// \brief Constructor + public: Polynomial3() = default; + + /// \brief Constructor + /// \param[in] _coeffs coefficients c0 through c3, left to right + public: explicit Polynomial3(Vector4 _coeffs) + : coeffs(std::move(_coeffs)) + { + } + + /// \brief Make a constant polynomial + /// \return a p(x) = `_value` polynomial + public: static Polynomial3 Constant(T _value) + { + return Polynomial3(Vector4(0., 0., 0., _value)); + } + + /// \brief Get the polynomial coefficients + /// \return this polynomial coefficients + public: const Vector4 &Coeffs() const { return this->coeffs; } + + /// \brief Evaluate the polynomial at `_x` + /// For non-finite `_x`, this function + /// computes p(z) as z tends to `_x`. + /// \param[in] _x polynomial argument + /// \return the result of evaluating p(`_x`) + 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::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]); + } + + /// \brief Call operator overload + /// \see Polynomial3::Evaluate() + public: T operator()(const T &_x) const + { + return this->Evaluate(_x); + } + + /// \brief Compute polynomial minimum in an `_interval` + /// \param[in] _interval polynomial argument interval to check + /// \param[out] _xMin polynomial argument that yields minimum, + /// or NaN if the interval is empty + /// \return the polynomial minimum in the given interval, + /// or NaN if the interval is empty + public: T Minimum(const Interval &_interval, T &_xMin) const + { + if (_interval.Empty()) + { + _xMin = std::numeric_limits::quiet_NaN(); + return std::numeric_limits::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::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; + } + + /// \brief Compute polynomial minimum in an `_interval` + /// \param[in] _interval polynomial argument interval to check + /// \return the polynomial minimum in the given interval (may + /// not be finite), or NaN if the interval is empty + public: T Minimum(const Interval &_interval) const + { + T xMin; + return this->Minimum(_interval, xMin); + } + + /// \brief Compute polynomial minimum + /// \param[out] _xMin polynomial argument that yields minimum + /// \return the polynomial minimum (may not be finite) + public: T Minimum(T &_xMin) const + { + return this->Minimum(Interval::Unbounded, _xMin); + } + + /// \brief Compute polynomial minimum + /// \return the polynomial minimum (may not be finite) + public: T Minimum() const + { + T xMin; + return this->Minimum(Interval::Unbounded, xMin); + } + + /// \brief Prints polynomial as p(`_x`) to `_out` stream + /// \param[in] _out Output stream to print to + /// \param[in] _x Argument name to be used + public: void Print(std::ostream &_out, const std::string &_x = "x") const + { + constexpr T epsilon = + std::numeric_limits::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 - 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]; + } + } + + /// \brief Stream insertion operator + /// \param _out output stream + /// \param _p Polynomial3 to output + /// \return the stream + public: friend std::ostream &operator<<( + std::ostream &_out, const ignition::math::Polynomial3 &_p) + { + _p.Print(_out, "x"); + return _out; + } + + /// \brief Polynomial coefficients + private: Vector4 coeffs; + }; + using Polynomial3f = Polynomial3; + using Polynomial3d = Polynomial3; + } + } +} + +#endif diff --git a/src/Polynomial3_TEST.cc b/src/Polynomial3_TEST.cc new file mode 100644 index 000000000..dc5998cfa --- /dev/null +++ b/src/Polynomial3_TEST.cc @@ -0,0 +1,376 @@ +/* + * 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. + * +*/ +#include +#include + +#include "ignition/math/Polynomial3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(Polynomial3Test, DefaultConstructor) +{ + const math::Polynomial3d poly; + EXPECT_EQ(poly.Coeffs(), math::Vector4d::Zero); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Constructor) +{ + const math::Polynomial3d poly(math::Vector4d::One); + EXPECT_EQ(poly.Coeffs(), math::Vector4d::One); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, ConstructionHelpers) +{ + const math::Polynomial3d poly = math::Polynomial3d::Constant(1.); + EXPECT_EQ(poly.Coeffs(), math::Vector4d(0., 0., 0., 1.)); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Evaluate) +{ + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + EXPECT_DOUBLE_EQ(p(-1.), 1.); + EXPECT_DOUBLE_EQ(p(0.), 1.); + EXPECT_DOUBLE_EQ(p(1.), 1.); + EXPECT_DOUBLE_EQ(p(math::INF_D), 1.); + EXPECT_TRUE(std::isnan(p(math::NAN_D))); + } + { + const math::Polynomial3d p(math::Vector4d::One); + EXPECT_DOUBLE_EQ(p(-1.), 0.); + EXPECT_DOUBLE_EQ(p(0.), 1.); + EXPECT_DOUBLE_EQ(p(1.), 4.); + EXPECT_DOUBLE_EQ(p(-math::INF_D), -math::INF_D); + EXPECT_TRUE(std::isnan(p(math::NAN_D))); + } +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Minimum) +{ + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + const math::Intervald xInterval = + math::Intervald::Open(0., 0.); + double xMin = 0.; + EXPECT_TRUE(std::isnan(p.Minimum(xInterval))); + EXPECT_TRUE(std::isnan(p.Minimum(xInterval, xMin))); + EXPECT_TRUE(std::isnan(xMin)); + } + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), 1.); + EXPECT_FALSE(std::isnan(xMin)); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., -1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., -1., 1.)); + double xMin = 0.; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 0.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.75); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.75); + EXPECT_DOUBLE_EQ(xMin, -0.5); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-3., -2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), 0.75); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), 0.75); + EXPECT_DOUBLE_EQ(xMin, -0.5); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -1.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-2., -1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.); + EXPECT_DOUBLE_EQ(xMin, -1.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-2., -1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 15.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 15.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.8873882090776197); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.8873882090776197); + EXPECT_DOUBLE_EQ(xMin, -0.2152504370215302); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-3., -2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 15.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 15.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, 3.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -3.); + EXPECT_DOUBLE_EQ(xMin, -1.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -4., -2., -1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 6.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -16.051047904897441); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -16.051047904897441); + EXPECT_DOUBLE_EQ(xMin, 2.8968052532744766); + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -4., -2., -1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, PolynomialStreaming) +{ + { + std::ostringstream os; + os << math::Polynomial3d(math::Vector4d::Zero); + EXPECT_EQ(os.str(), "0"); + } + { + std::ostringstream os; + os << math::Polynomial3d(math::Vector4d::One); + EXPECT_EQ(os.str(), "x^3 + x^2 + x + 1"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(1., 0., 1., 0.)); + EXPECT_EQ(os.str(), "x^3 + x"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(0., 1., 0., -1.)); + EXPECT_EQ(os.str(), "x^2 - 1"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(-1., 2., -2., 0.)); + EXPECT_EQ(os.str(), "-x^3 + 2 x^2 - 2 x"); + } +}