Gazebo Math

API Reference

8.1.0
Polynomial3.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GZ_MATH_POLYNOMIAL3_HH_
18#define GZ_MATH_POLYNOMIAL3_HH_
19
20#include <algorithm>
21#include <cmath>
22#include <limits>
23#include <string>
24#include <utility>
25
26#include <gz/math/Interval.hh>
27#include <gz/math/Vector4.hh>
28#include <gz/math/config.hh>
29
30namespace gz::math
31{
32 // Inline bracket to help doxygen filtering.
33 inline namespace GZ_MATH_VERSION_NAMESPACE {
34 //
41 template <typename T>
43 {
45 public: Polynomial3() = default;
46
49 public: explicit Polynomial3(Vector4<T> _coeffs)
50 : coeffs(std::move(_coeffs))
51 {
52 }
53
56 public: static Polynomial3 Constant(T _value)
57 {
58 return Polynomial3(Vector4<T>(0., 0., 0., _value));
59 }
60
63 public: const Vector4<T> &Coeffs() const { return this->coeffs; }
64
70 public: T Evaluate(const T &_x) const
71 {
73 if (isnan(_x))
74 {
75 return _x;
76 }
77 if (!isfinite(_x))
78 {
79 using std::abs, std::copysign;
80 const T epsilon =
82 if (abs(this->coeffs[0]) >= epsilon)
83 {
84 return _x * copysign(T(1.), this->coeffs[0]);
85 }
86 if (abs(this->coeffs[1]) >= epsilon)
87 {
88 return copysign(_x, this->coeffs[1]);
89 }
90 if (abs(this->coeffs[2]) >= epsilon)
91 {
92 return _x * copysign(T(1.), this->coeffs[2]);
93 }
94 return this->coeffs[3];
95 }
96 const T _x2 = _x * _x;
97 const T _x3 = _x2 * _x;
98
99 return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 +
100 this->coeffs[2] * _x + this->coeffs[3]);
101 }
102
105 public: T operator()(const T &_x) const
106 {
107 return this->Evaluate(_x);
108 }
109
116 public: T Minimum(const Interval<T> &_interval, T &_xMin) const
117 {
118 if (_interval.Empty())
119 {
122 }
123 T yMin;
124 // For open intervals, assume continuity in the limit
125 const T &xLeft = _interval.LeftValue();
126 const T &xRight = _interval.RightValue();
127 const T yLeft = this->Evaluate(xLeft);
128 const T yRight = this->Evaluate(xRight);
129 if (yLeft <= yRight)
130 {
131 yMin = yLeft;
132 _xMin = xLeft;
133 }
134 else
135 {
136 yMin = yRight;
137 _xMin = xRight;
138 }
139 using std::abs, std::sqrt; // enable ADL
140 constexpr T epsilon = std::numeric_limits<T>::epsilon();
141 if (abs(this->coeffs[0]) >= epsilon)
142 {
143 // Polynomial function p(x) is cubic, look
144 // for local minima within the given interval
145
146 // Find local extrema by computing the roots
147 // of p'(x), a quadratic polynomial function
148 const T a = this->coeffs[0] * T(3.);
149 const T b = this->coeffs[1] * T(2.);
150 const T c = this->coeffs[2];
151
152 const T discriminant = b * b - T(4.) * a * c;
153 if (discriminant >= T(0.))
154 {
155 // Roots of p'(x) are real, check local minima
156 const T x = (-b + sqrt(discriminant)) / (T(2.) * a);
157 if (_interval.Contains(x))
158 {
159 const T y = this->Evaluate(x);
160 if (y < yMin)
161 {
162 _xMin = x;
163 yMin = y;
164 }
165 }
166 }
167 }
168 else if (abs(this->coeffs[1]) >= epsilon)
169 {
170 // Polynomial function p(x) is quadratic,
171 // look for global minima if concave
172 const T a = this->coeffs[1];
173 const T b = this->coeffs[2];
174 if (a > T(0.))
175 {
176 const T x = -b / (T(2.) * a);
177 if (_interval.Contains(x))
178 {
179 const T y = this->Evaluate(x);
180 if (y < yMin)
181 {
182 _xMin = x;
183 yMin = y;
184 }
185 }
186 }
187 }
188 return yMin;
189 }
190
195 public: T Minimum(const Interval<T> &_interval) const
196 {
197 T xMin;
198 return this->Minimum(_interval, xMin);
199 }
200
204 public: T Minimum(T &_xMin) const
205 {
206 return this->Minimum(Interval<T>::Unbounded, _xMin);
207 }
208
211 public: T Minimum() const
212 {
213 T xMin;
214 return this->Minimum(Interval<T>::Unbounded, xMin);
215 }
216
220 public: void Print(std::ostream &_out, const std::string &_x = "x") const
221 {
222 constexpr T epsilon =
224 bool streamStarted = false;
225 for (size_t i = 0; i < 4; ++i)
226 {
227 using std::abs; // enable ADL
228 const T magnitude = abs(this->coeffs[i]);
229 const bool sign = this->coeffs[i] < T(0);
230 const int exponent = 3 - static_cast<int>(i);
231 if (magnitude >= epsilon)
232 {
233 if (streamStarted)
234 {
235 if (sign)
236 {
237 _out << " - ";
238 }
239 else
240 {
241 _out << " + ";
242 }
243 }
244 else if (sign)
245 {
246 _out << "-";
247 }
248 if (exponent > 0)
249 {
250 if ((magnitude - T(1)) > epsilon)
251 {
252 _out << magnitude << " ";
253 }
254 _out << _x;
255 if (exponent > 1)
256 {
257 _out << "^" << exponent;
258 }
259 }
260 else
261 {
262 _out << magnitude;
263 }
264 streamStarted = true;
265 }
266 }
267 if (!streamStarted)
268 {
269 _out << this->coeffs[3];
270 }
271 }
272
277 public: friend std::ostream &operator<<(
279 {
280 _p.Print(_out, "x");
281 return _out;
282 }
283
285 private: Vector4<T> coeffs;
286 };
289 } // namespace GZ_MATH_VERSION_NAMESPACE
290} // namespace gz::math
291#endif // GZ_MATH_POLYNOMIAL3_HH_