Gazebo Math

API Reference

7.4.0
gz/math/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
31{
32 namespace math
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 //
43 template <typename T>
45 {
47 public: Polynomial3() = default;
48
51 public: explicit Polynomial3(Vector4<T> _coeffs)
52 : coeffs(std::move(_coeffs))
53 {
54 }
55
58 public: static Polynomial3 Constant(T _value)
59 {
60 return Polynomial3(Vector4<T>(0., 0., 0., _value));
61 }
62
65 public: const Vector4<T> &Coeffs() const { return this->coeffs; }
66
72 public: T Evaluate(const T &_x) const
73 {
75 if (isnan(_x))
76 {
77 return _x;
78 }
79 if (!isfinite(_x))
80 {
81 using std::abs, std::copysign;
82 const T epsilon =
84 if (abs(this->coeffs[0]) >= epsilon)
85 {
86 return _x * copysign(T(1.), this->coeffs[0]);
87 }
88 if (abs(this->coeffs[1]) >= epsilon)
89 {
90 return copysign(_x, this->coeffs[1]);
91 }
92 if (abs(this->coeffs[2]) >= epsilon)
93 {
94 return _x * copysign(T(1.), this->coeffs[2]);
95 }
96 return this->coeffs[3];
97 }
98 const T _x2 = _x * _x;
99 const T _x3 = _x2 * _x;
100
101 return (this->coeffs[0] * _x3 + this->coeffs[1] * _x2 +
102 this->coeffs[2] * _x + this->coeffs[3]);
103 }
104
107 public: T operator()(const T &_x) const
108 {
109 return this->Evaluate(_x);
110 }
111
118 public: T Minimum(const Interval<T> &_interval, T &_xMin) const
119 {
120 if (_interval.Empty())
121 {
124 }
125 T yMin;
126 // For open intervals, assume continuity in the limit
127 const T &xLeft = _interval.LeftValue();
128 const T &xRight = _interval.RightValue();
129 const T yLeft = this->Evaluate(xLeft);
130 const T yRight = this->Evaluate(xRight);
131 if (yLeft <= yRight)
132 {
133 yMin = yLeft;
134 _xMin = xLeft;
135 }
136 else
137 {
138 yMin = yRight;
139 _xMin = xRight;
140 }
141 using std::abs, std::sqrt; // enable ADL
142 constexpr T epsilon = std::numeric_limits<T>::epsilon();
143 if (abs(this->coeffs[0]) >= epsilon)
144 {
145 // Polynomial function p(x) is cubic, look
146 // for local minima within the given interval
147
148 // Find local extrema by computing the roots
149 // of p'(x), a quadratic polynomial function
150 const T a = this->coeffs[0] * T(3.);
151 const T b = this->coeffs[1] * T(2.);
152 const T c = this->coeffs[2];
153
154 const T discriminant = b * b - T(4.) * a * c;
155 if (discriminant >= T(0.))
156 {
157 // Roots of p'(x) are real, check local minima
158 const T x = (-b + sqrt(discriminant)) / (T(2.) * a);
159 if (_interval.Contains(x))
160 {
161 const T y = this->Evaluate(x);
162 if (y < yMin)
163 {
164 _xMin = x;
165 yMin = y;
166 }
167 }
168 }
169 }
170 else if (abs(this->coeffs[1]) >= epsilon)
171 {
172 // Polynomial function p(x) is quadratic,
173 // look for global minima if concave
174 const T a = this->coeffs[1];
175 const T b = this->coeffs[2];
176 if (a > T(0.))
177 {
178 const T x = -b / (T(2.) * a);
179 if (_interval.Contains(x))
180 {
181 const T y = this->Evaluate(x);
182 if (y < yMin)
183 {
184 _xMin = x;
185 yMin = y;
186 }
187 }
188 }
189 }
190 return yMin;
191 }
192
197 public: T Minimum(const Interval<T> &_interval) const
198 {
199 T xMin;
200 return this->Minimum(_interval, xMin);
201 }
202
206 public: T Minimum(T &_xMin) const
207 {
208 return this->Minimum(Interval<T>::Unbounded, _xMin);
209 }
210
213 public: T Minimum() const
214 {
215 T xMin;
216 return this->Minimum(Interval<T>::Unbounded, xMin);
217 }
218
222 public: void Print(std::ostream &_out, const std::string &_x = "x") const
223 {
224 constexpr T epsilon =
226 bool streamStarted = false;
227 for (size_t i = 0; i < 4; ++i)
228 {
229 using std::abs; // enable ADL
230 const T magnitude = abs(this->coeffs[i]);
231 const bool sign = this->coeffs[i] < T(0);
232 const int exponent = 3 - static_cast<int>(i);
233 if (magnitude >= epsilon)
234 {
235 if (streamStarted)
236 {
237 if (sign)
238 {
239 _out << " - ";
240 }
241 else
242 {
243 _out << " + ";
244 }
245 }
246 else if (sign)
247 {
248 _out << "-";
249 }
250 if (exponent > 0)
251 {
252 if ((magnitude - T(1)) > epsilon)
253 {
254 _out << magnitude << " ";
255 }
256 _out << _x;
257 if (exponent > 1)
258 {
259 _out << "^" << exponent;
260 }
261 }
262 else
263 {
264 _out << magnitude;
265 }
266 streamStarted = true;
267 }
268 }
269 if (!streamStarted)
270 {
271 _out << this->coeffs[3];
272 }
273 }
274
279 public: friend std::ostream &operator<<(
280 std::ostream &_out, const gz::math::Polynomial3<T> &_p)
281 {
282 _p.Print(_out, "x");
283 return _out;
284 }
285
287 private: Vector4<T> coeffs;
288 };
291 }
292 }
293}
294
295#endif