PhysicsBasedAnimationToolkit 0.0.10
Cross-platform C++20 library of algorithms and data structures commonly used in computer graphics research on physically-based simulation.
Loading...
Searching...
No Matches
Roots.h
Go to the documentation of this file.
1#ifndef PBAT_MATH_POLYNOMIAL_ROOTS_H
2#define PBAT_MATH_POLYNOMIAL_ROOTS_H
3
20
21#include "pbat/warning/Push.h"
22#include "pbat/warning/OldStyleCast.h"
23#include "pbat/warning/SignConversion.h"
24
25//-------------------------------------------------------------------------------
26
27#include <cassert>
28#include <cmath>
29#include <cstdint>
30#include <cstdlib>
31#include <cstring>
32#include <limits>
33#include <type_traits>
34
35#if !defined(CY_NO_INTRIN_H) && !defined(CY_NO_EMMINTRIN_H) && !defined(CY_NO_IMMINTRIN_H)
36 #include <immintrin.h>
37#endif
38
39namespace pbat::math::polynomial::detail {
40
41// clang-format off
42
43// Copyright (c) 2016, Cem Yuksel <cem@cemyuksel.com>
44// All rights reserved.
45//
46// Permission is hereby granted, free of charge, to any person obtaining a copy
47// of this software and associated documentation files (the "Software"), to deal
48// in the Software without restriction, including without limitation the rights
49// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
50// copies of the Software, and to permit persons to whom the Software is
51// furnished to do so, subject to the following conditions:
52//
53// The above copyright notice and this permission notice shall be included in all
54// copies or substantial portions of the Software.
55//
56// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
57// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
58// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
59// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
60// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
61// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
62// SOFTWARE.
63//
64//-------------------------------------------------------------------------------
65
66#ifndef _CY_CORE_H_INCLUDED_
67#define _CY_CORE_H_INCLUDED_
68
69//-------------------------------------------------------------------------------
70
71#ifndef _CY_CORE_MEMCPY_LIMIT
72 #define _CY_CORE_MEMCPY_LIMIT 256
73#endif
74
75//-------------------------------------------------------------------------------
76namespace cy {
77//-------------------------------------------------------------------------------
79// Compiler compatibility
81
82#if defined(__INTEL_COMPILER)
83 #define _CY_COMPILER_INTEL __INTEL_COMPILER
84 #define _CY_COMPILER_VER_MEETS(msc, gcc, clang, intel) _CY_COMPILER_INTEL >= intel
85 #define _CY_COMPILER_VER_BELOW(msc, gcc, clang, intel) _CY_COMPILER_INTEL < intel
86#elif defined(__clang__)
87 #define _CY_COMPILER_CLANG \
88 (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__)
89 #define _CY_COMPILER_VER_MEETS(msc, gcc, clang, intel) _CY_COMPILER_CLANG >= clang
90 #define _CY_COMPILER_VER_BELOW(msc, gcc, clang, intel) _CY_COMPILER_CLANG < clang
91#elif defined(_MSC_VER)
92 #define _CY_COMPILER_MSC _MSC_VER
93 #define _CY_COMPILER_VER_MEETS(msc, gcc, clang, intel) _CY_COMPILER_MSC >= msc
94 #define _CY_COMPILER_VER_BELOW(msc, gcc, clang, intel) _CY_COMPILER_MSC < msc
95#elif __GNUC__
96 #define _CY_COMPILER_GCC (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
97 #define _CY_COMPILER_VER_MEETS(msc, gcc, clang, intel) _CY_COMPILER_GCC >= gcc
98 #define _CY_COMPILER_VER_BELOW(msc, gcc, clang, intel) _CY_COMPILER_GCC < gcc
99#else
100 #define _CY_COMPILER_UNKNOWN
101 #define _CY_COMPILER_VER_MEETS(msc, gcc, clang, intel) false
102 #define _CY_COMPILER_VER_BELOW(msc, gcc, clang, intel) false
103#endif
104
105// constexpr
106#ifndef __cpp_constexpr
107 #if _CY_COMPILER_VER_MEETS(1900, 40600, 30100, 1310)
108 #define __cpp_constexpr
109 #else
110 #define constexpr
111 #endif
112#endif
113
114// nullptr
115#if _CY_COMPILER_VER_BELOW(1600, 40600, 20900, 1210)
116class _cy_nullptr_t
117{
118 public:
119 template <class T>
120 operator T*() const
121 {
122 return 0;
123 }
124 template <class C, class T>
125 operator T C::*() const
126 {
127 return 0;
128 }
129
130 private:
131 void operator&() const {}
132};
133static _cy_nullptr_t nullptr;
134#endif
135
136// template aliases
137#define _CY_TEMPLATE_ALIAS_UNPACK(...) __VA_ARGS__
138#if _CY_COMPILER_VER_BELOW(1800, 40700, 30000, 1210)
139 #define _CY_TEMPLATE_ALIAS(template_name, template_equivalent) \
140 class template_name : public _CY_TEMPLATE_ALIAS_UNPACK template_equivalent \
141 { \
142 }
143#else
144 #define _CY_TEMPLATE_ALIAS(template_name, template_equivalent) \
145 using template_name = _CY_TEMPLATE_ALIAS_UNPACK template_equivalent
146#endif
147
148// std::is_trivially_copyable
149#if _CY_COMPILER_VER_MEETS(1700, 50000, 30400, 1300)
150 #define _cy_std_is_trivially_copyable 1
151#endif
152
153// restrict
154#if defined(__INTEL_COMPILER)
155// # define restrict restrict
156#elif defined(__clang__)
157 #define restrict __restrict__
158#elif defined(_MSC_VER)
159 #define restrict __restrict
160#elif __GNUC__
161 #define restrict __restrict__
162#else
163 #define restrict
164#endif
165
166// alignment
167#if _CY_COMPILER_VER_BELOW(1900, 40800, 30000, 1500)
168 #if defined(_MSC_VER)
169 #define alignas(alignment_size) __declspec(align(alignment_size))
170 #else
171 #define alignas(alignment_size) __attribute__((aligned(alignment_size)))
172 #endif
173#endif
174
175// final, override
176#if _CY_COMPILER_VER_BELOW(1700, 40700, 20900, 1210)
177 #define override
178 #if defined(_MSC_VER)
179 #define final sealed
180 #else
181 #define final
182 #endif
183#endif
184
185// static_assert
186#if _CY_COMPILER_VER_BELOW(1900, 60000, 20500, 1800)
187 #define static_assert(condition, message) assert(condition&& message)
188#endif
189
190// unrestricted unions
191#ifndef __cpp_unrestricted_unions
192 #if _CY_COMPILER_VER_MEETS(1900, 40600, 30000, 1400)
193 #define __cpp_unrestricted_unions
194 #endif
195#endif
196
197// nodiscard
198// #if _CY_COMPILER_VER_MEETS(1901,40800,30000,1500)
199#if (__cplusplus >= 201703L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
200 #define CY_NODISCARD [[nodiscard]]
201#else
202 #define CY_NODISCARD
203#endif
204
205// default and deleted class member functions
206#if _CY_COMPILER_VER_MEETS(1800, 40400, 30000, 1200)
207 #define CY_CLASS_FUNCTION_DEFAULT = default;
208 #define CY_CLASS_FUNCTION_DELETE = delete;
209#else
210 #define CY_CLASS_FUNCTION_DEFAULT \
211 { \
212 }
213 #define CY_CLASS_FUNCTION_DELETE \
214 { \
215 static_assert(false, "Calling deleted method."); \
216 }
217#endif
218
219// switch statements where default cannot be reached
220#if defined(__INTEL_COMPILER) || defined(__clang__) || defined(__GNUC__)
221 #define nodefault \
222 default: __builtin_unreachable()
223#elif defined(_MSC_VER)
224 #define nodefault \
225 default: __assume(0)
226#else
227 #define nodefault default:
228#endif
229
231// Auto Vectorization
233
234#ifdef _MSC_VER
235 #if _MSC_VER >= 1700
236 #define _CY_IVDEP __pragma(loop(ivdep))
237 #endif
238#elif defined __GNUC__
239 #if _CY_GCC_VER >= 40900
240 #define _CY_IVDEP _Pragma("GCC ivdep");
241 #endif
242#elif defined __clang__
243 #if _CY_CLANG_VER >= 30500
244 #define _CY_IVDEP _Pragma("clang loop vectorize(enable) interleave(enable)");
245 #endif
246#else
247 // # define _CY_IVDEP _Pragma("ivdep");
248 #define _CY_IVDEP
249#endif
250
251#ifndef _CY_IVDEP
252 #define _CY_IVDEP
253#endif
254
255#define _CY_IVDEP_FOR _CY_IVDEP for
256
258// Disabling MSVC's non-standard depreciation warnings
260
261#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
262 #define _CY_CRT_SECURE_NO_WARNINGS __pragma(warning(push)) __pragma(warning(disable : 4996))
263 #define _CY_CRT_SECURE_RESUME_WARNINGS __pragma(warning(pop))
264#else
265 #define _CY_CRT_SECURE_NO_WARNINGS
266 #define _CY_CRT_SECURE_RESUME_WARNINGS
267#endif
268
270// Math functions
272
274
275template <typename T>
276CY_NODISCARD inline T Max(T v1, T v2)
277{
278 return v1 >= v2 ? v1 : v2;
279}
280template <typename T>
281CY_NODISCARD inline T Min(T v1, T v2)
282{
283 return v1 <= v2 ? v1 : v2;
284}
285template <typename T>
286CY_NODISCARD inline T Max(T v1, T v2, T v3)
287{
288 return Max(Max(v1, v2), v3);
289}
290template <typename T>
291CY_NODISCARD inline T Min(T v1, T v2, T v3)
292{
293 return Min(Min(v1, v2), v3);
294}
295template <typename T>
296CY_NODISCARD inline T Max(T v1, T v2, T v3, T const v4)
297{
298 return Max(Max(v1, v2), Max(v3, v4));
299}
300template <typename T>
301CY_NODISCARD inline T Min(T v1, T v2, T v3, T const v4)
302{
303 return Min(Min(v1, v2), Min(v3, v4));
304}
305template <typename T>
306CY_NODISCARD inline T Clamp(T v, T minVal = T(0), T maxVal = T(1))
307{
308 return Min(maxVal, Max(minVal, v));
309}
310
311template <typename T>
312CY_NODISCARD inline T ACosSafe(T v)
313{
314 return (T)std::acos(Clamp(v, T(-1), T(1)));
315}
316template <typename T>
317CY_NODISCARD inline T ASinSafe(T v)
318{
319 return (T)std::asin(Clamp(v, T(-1), T(1)));
320}
321template <typename T>
322CY_NODISCARD inline T Sqrt(T v)
323{
324 return (T)std::sqrt(v);
325}
326template <typename T>
327CY_NODISCARD inline T SqrtSafe(T v)
328{
329 return (T)std::sqrt(Max(v, T(0)));
330}
331
332#ifdef _INCLUDED_IMM
333template <>
334[[maybe_unused]] CY_NODISCARD inline float Sqrt<float>(float v)
335{
336 return _mm_cvtss_f32(_mm_sqrt_ss(_mm_set_ps1(v)));
337}
338template <>
339[[maybe_unused]] CY_NODISCARD inline float SqrtSafe<float>(float v)
340{
341 return _mm_cvtss_f32(_mm_sqrt_ss(_mm_set_ps1(Max(v, 0.0f))));
342}
343template <>
344[[maybe_unused]] CY_NODISCARD inline double Sqrt<double>(double v)
345{
346 __m128d t = _mm_set1_pd(v);
347 return _mm_cvtsd_f64(_mm_sqrt_sd(t, t));
348}
349template <>
350[[maybe_unused]] CY_NODISCARD inline double SqrtSafe<double>(double v)
351{
352 __m128d t = _mm_set1_pd(Max(v, 0.0));
353 return _mm_cvtsd_f64(_mm_sqrt_sd(t, t));
354}
355#endif
356
357template <typename T>
358constexpr inline T Pi()
359{
360 return T(3.141592653589793238462643383279502884197169);
361}
362
363template <typename T>
364CY_NODISCARD inline bool IsFinite(T v)
365{
366 return std::numeric_limits<T>::is_integer || std::isfinite(v);
367}
368
370// Memory Operations
372
373template <typename T>
374inline void MemCopy(T* restrict dest, T const* restrict src, size_t count)
375{
376#ifdef _cy_std_is_trivially_copyable
377 if (std::is_trivially_copyable<T>())
378 {
379 memcpy(dest, src, (count) * sizeof(T));
380 }
381 else
382#endif
383 for (size_t i = 0; i < count; ++i)
384 dest[i] = src[i];
385}
386
387template <typename T, typename S>
388inline void MemConvert(T* restrict dest, S const* restrict src, size_t count)
389{
390 for (size_t i = 0; i < count; ++i)
391 dest[i] = reinterpret_cast<T>(src[i]);
392}
393
394template <typename T>
395inline void MemClear(T* dest, size_t count)
396{
397 memset(dest, 0, count * sizeof(T));
398}
399
400template <typename T>
401inline void SwapBytes(T& v1, T& v2)
402{
403 char t[sizeof(T)];
404 memcpy(&t, &v1, sizeof(T));
405 memcpy(&v1, &v2, sizeof(T));
406 memcpy(&v2, &t, sizeof(T));
407}
408template <typename T>
409inline void Swap(T& v1, T& v2)
410{
411 if (std::is_trivially_copyable<T>::value)
412 {
413 T t = v1;
414 v1 = v2;
415 v2 = t;
416 }
417 else
418 SwapBytes(v1, v2);
419}
420
422// Sorting functions
424
425template <bool ascending, typename T>
426inline void Sort2(T& r0, T& r1, T const& v0, T const& v1)
427{
428 if (ascending)
429 {
430 r0 = Min(v0, v1);
431 r1 = Max(v0, v1);
432 }
433 else
434 {
435 r0 = Max(v0, v1);
436 r1 = Min(v0, v1);
437 }
438}
439
440template <bool ascending, typename T>
441inline void Sort2(T r[2], T const v[2])
442{
443 r[1 - ascending] = Min(v[0], v[1]);
444 r[ascending] = Max(v[0], v[1]);
445}
446
447template <bool ascending, typename T>
448void Sort3(T& r0, T& r1, T& r2, T const& v0, T const& v1, T const& v2)
449{
450 T n01 = Min(v0, v1);
451 T x01 = Max(v0, v1);
452 T n2x01 = Min(v2, x01);
453 r1 = Max(n01, n2x01);
454 if (ascending)
455 {
456 r0 = Min(n2x01, n01);
457 r2 = Max(x01, v2);
458 }
459 else
460 {
461 r0 = Max(x01, v2);
462 r2 = Min(n2x01, n01);
463 }
464}
465
466template <bool ascending, typename T>
467void Sort3(T r[3], T const v[3])
468{
469 T n01 = Min(v[0], v[1]);
470 T x01 = Max(v[0], v[1]);
471 T n2x01 = Min(v[2], x01);
472 T r0 = Min(n2x01, n01);
473 T r1 = Max(n01, n2x01);
474 T r2 = Max(x01, v[2]);
475 if (ascending)
476 {
477 r[0] = r0;
478 r[1] = r1;
479 r[2] = r2;
480 }
481 else
482 {
483 r[0] = r2;
484 r[1] = r1;
485 r[2] = r0;
486 }
487}
488
489template <bool ascending, typename T>
490inline void Sort4(T& r0, T& r1, T& r2, T& r3, T const& v0, T const& v1, T const& v2, T const& v3)
491{
492 T n01 = Min(v0, v1);
493 T x01 = Max(v0, v1);
494 T n23 = Min(v2, v3);
495 T x23 = Max(v2, v3);
496 T x02 = Max(n23, n01);
497 T n13 = Min(x01, x23);
498 if (ascending)
499 {
500 r0 = Min(n01, n23);
501 r1 = Min(x02, n13);
502 r2 = Max(n13, x02);
503 r3 = Max(x23, x01);
504 }
505 else
506 {
507 r0 = Max(x23, x01);
508 r1 = Max(n13, x02);
509 r2 = Min(x02, n13);
510 r3 = Min(n01, n23);
511 }
512}
513
514template <bool ascending, typename T>
515inline void Sort4(T r[4], T const v[4])
516{
517 T n01 = Min(v[0], v[1]);
518 T x01 = Max(v[0], v[1]);
519 T n23 = Min(v[2], v[3]);
520 T x23 = Max(v[2], v[3]);
521 T x02 = Max(n23, n01);
522 T n13 = Min(x01, x23);
523 T r0 = Min(n01, n23);
524 T r1 = Min(x02, n13);
525 T r2 = Max(n13, x02);
526 T r3 = Max(x23, x01);
527 if (ascending)
528 {
529 r[0] = r0;
530 r[1] = r1;
531 r[2] = r2;
532 r[3] = r3;
533 }
534 else
535 {
536 r[0] = r3;
537 r[1] = r2;
538 r[2] = r1;
539 r[3] = r0;
540 }
541}
542
544
545//-------------------------------------------------------------------------------
546} // namespace cy
547//-------------------------------------------------------------------------------
548
549#endif // _CY_CORE_H_INCLUDED_
550
551// Copyright (c) 2022, Cem Yuksel <cem@cemyuksel.com>
552// All rights reserved.
553//
554// Permission is hereby granted, free of charge, to any person obtaining a copy
555// of this software and associated documentation files (the "Software"), to deal
556// in the Software without restriction, including without limitation the rights
557// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
558// copies of the Software, and to permit persons to whom the Software is
559// furnished to do so, subject to the following conditions:
560//
561// The above copyright notice and this permission notice shall be included in all
562// copies or substantial portions of the Software.
563//
564// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
565// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
566// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
567// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
568// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
569// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
570// SOFTWARE.
571//
572//-------------------------------------------------------------------------------
573
574#ifndef _CY_POLYNOMIAL_H_INCLUDED_
575#define _CY_POLYNOMIAL_H_INCLUDED_
576
577//-------------------------------------------------------------------------------
578
579//-------------------------------------------------------------------------------
580namespace cy {
581//-------------------------------------------------------------------------------
583//
590//-------------------------------------------------------------------------------
591
595template <int N, typename ftype>
596inline ftype PolynomialEval(ftype const coef[N + 1], ftype x)
597{
598 ftype r = coef[N];
599 for (int i = N - 1; i >= 0; --i)
600 r = r * x + coef[i];
601 return r;
602}
603
604//-------------------------------------------------------------------------------
605
612template <int N, typename ftype>
613inline ftype PolynomialEvalWithDeriv(ftype& derivativeValue, ftype const coef[N + 1], ftype x)
614{
615 if constexpr (N < 1)
616 {
617 derivativeValue = 0;
618 return coef[0];
619 }
620 else
621 {
622 ftype p = coef[N] * x + coef[N - 1];
623 ftype dp = coef[N];
624 for (int i = N - 2; i >= 0; --i)
625 {
626 dp = dp * x + p;
627 p = p * x + coef[i];
628 }
629 derivativeValue = dp;
630 return p;
631 }
632}
633
634//-------------------------------------------------------------------------------
635
640template <int N, typename ftype>
641inline void PolynomialDerivative(ftype deriv[N], ftype const coef[N + 1])
642{
643 deriv[0] = coef[1];
644 for (int i = 2; i <= N; ++i)
645 deriv[i - 1] = i * coef[i];
646}
647
648//-------------------------------------------------------------------------------
649
661template <int N, typename ftype>
662inline void PolynomialDeflate(ftype defPoly[N], ftype const coef[N + 1], ftype root)
663{
664 defPoly[N - 1] = coef[N];
665 for (int i = N - 1; i > 0; --i)
666 defPoly[i - 1] = coef[i] + root * defPoly[i];
667}
668
669//-------------------------------------------------------------------------------
670
681template <int N, typename ftype>
682inline void PolynomialInflate(ftype infPoly[N + 2], ftype const coef[N + 1], ftype root)
683{
684 infPoly[N + 1] = coef[N];
685 for (int i = N - 1; i >= 0; --i)
686 infPoly[i + 1] = coef[i] - root * infPoly[i + 1];
687 infPoly[0] = -root * coef[0];
688}
689
690//-------------------------------------------------------------------------------
713//-------------------------------------------------------------------------------
714
715template <typename ftype>
716constexpr ftype PolynomialDefaultError()
717{
718 return ftype(0);
719}
720template <>
721[[maybe_unused]] constexpr float PolynomialDefaultError<float>()
722{
723 return 6e-4f;
724}
725template <>
726[[maybe_unused]] constexpr double PolynomialDefaultError<double>()
727{
728 return 6e-7;
729}
730
731//-------------------------------------------------------------------------------
732
733class RootFinderNewton;
734
735#define _CY_POLY_TEMPLATE_N \
736 template < \
737 int N, \
738 typename ftype, \
739 bool boundError = false, \
740 typename RootFinder = RootFinderNewton> \
741 CY_NODISCARD inline
742#define _CY_POLY_TEMPLATE_R \
743 template <typename ftype, bool boundError = false, typename RootFinder = RootFinderNewton> \
744 CY_NODISCARD inline
745#define _CY_POLY_TEMPLATE_A \
746 template <typename ftype> \
747 CY_NODISCARD inline
748#define _CY_POLY_TEMPLATE_D \
749 template <typename ftype> \
750 inline
751
752#define _CY_POLY_TEMPLATE_NC \
753 template < \
754 int N, \
755 typename ftype, \
756 bool boundError = false, \
757 typename RootFinder = RootFinderNewton, \
758 typename RootCallback> \
759 inline
760#define _CY_POLY_TEMPLATE_RC \
761 template < \
762 typename ftype, \
763 bool boundError = false, \
764 typename RootFinder = RootFinderNewton, \
765 typename RootCallback> \
766 inline
767#define _CY_POLY_TEMPLATE_AC \
768 template <typename ftype, typename RootCallback> \
769 inline
770
771//-------------------------------------------------------------------------------
772
775_CY_POLY_TEMPLATE_N int PolynomialRoots(
776 ftype roots[N],
777 ftype const coef[N + 1],
778 ftype xMin,
779 ftype xMax,
780 ftype xError = PolynomialDefaultError<ftype>());
781_CY_POLY_TEMPLATE_R int CubicRoots(
782 ftype roots[3],
783 ftype const coef[4],
784 ftype xMin,
785 ftype xMax,
786 ftype xError = PolynomialDefaultError<ftype>());
787_CY_POLY_TEMPLATE_A int QuadraticRoots(ftype roots[2], ftype const coef[3], ftype xMin, ftype xMax);
788_CY_POLY_TEMPLATE_D int LinearRoot(ftype& root, ftype const coef[2], ftype xMin, ftype xMax);
789
790//-------------------------------------------------------------------------------
791
793_CY_POLY_TEMPLATE_N int PolynomialRoots(
794 ftype roots[N],
795 ftype const coef[N + 1],
796 ftype xError = PolynomialDefaultError<ftype>());
797_CY_POLY_TEMPLATE_R int
798CubicRoots(ftype roots[3], ftype const coef[4], ftype xError = PolynomialDefaultError<ftype>());
799_CY_POLY_TEMPLATE_A int QuadraticRoots(ftype roots[2], ftype const coef[3]);
800_CY_POLY_TEMPLATE_D int LinearRoot(ftype& root, ftype const coef[2]);
801
802//-------------------------------------------------------------------------------
803
806_CY_POLY_TEMPLATE_N bool PolynomialFirstRoot(
807 ftype& root,
808 ftype const coef[N + 1],
809 ftype xMin,
810 ftype xMax,
811 ftype xError = PolynomialDefaultError<ftype>());
812_CY_POLY_TEMPLATE_R bool CubicFirstRoot(
813 ftype& root,
814 ftype const coef[4],
815 ftype xMin,
816 ftype xMax,
817 ftype xError = PolynomialDefaultError<ftype>());
818_CY_POLY_TEMPLATE_A bool
819QuadraticFirstRoot(ftype& root, ftype const coef[3], ftype xMin, ftype xMax);
820
821//-------------------------------------------------------------------------------
822
824_CY_POLY_TEMPLATE_N bool PolynomialFirstRoot(
825 ftype& root,
826 ftype const coef[N + 1],
827 ftype xError = PolynomialDefaultError<ftype>());
828_CY_POLY_TEMPLATE_R bool
829CubicFirstRoot(ftype& root, ftype const coef[4], ftype xError = PolynomialDefaultError<ftype>());
830_CY_POLY_TEMPLATE_A bool QuadraticFirstRoot(ftype& root, ftype const coef[3]);
831
832//-------------------------------------------------------------------------------
833
835_CY_POLY_TEMPLATE_N bool PolynomialHasRoot(
836 ftype const coef[N + 1],
837 ftype xMin,
838 ftype xMax,
839 ftype xError = PolynomialDefaultError<ftype>());
840_CY_POLY_TEMPLATE_A bool CubicHasRoot(ftype const coef[4], ftype xMin, ftype xMax);
841_CY_POLY_TEMPLATE_A bool QuadraticHasRoot(ftype const coef[3], ftype xMin, ftype xMax);
842
843//-------------------------------------------------------------------------------
844
846_CY_POLY_TEMPLATE_N bool
847PolynomialHasRoot(ftype const coef[N + 1], ftype xError = PolynomialDefaultError<ftype>());
848_CY_POLY_TEMPLATE_A bool CubicHasRoot(ftype const coef[4])
849{
850 return true;
851}
852_CY_POLY_TEMPLATE_A bool QuadraticHasRoot(ftype const coef[3])
853{
854 return coef[1] * coef[1] - ftype(4) * coef[0] * coef[2] >= 0;
855}
856
857//-------------------------------------------------------------------------------
858
867_CY_POLY_TEMPLATE_NC bool PolynomialForEachRoot(
868 RootCallback callback,
869 ftype const coef[N + 1],
870 ftype xMin,
871 ftype xMax,
872 ftype xError = PolynomialDefaultError<ftype>());
873_CY_POLY_TEMPLATE_RC bool CubicForEachRoot(
874 RootCallback callback,
875 ftype const coef[4],
876 ftype xMin,
877 ftype xMax,
878 ftype xError = PolynomialDefaultError<ftype>());
879_CY_POLY_TEMPLATE_AC bool
880QuadraticForEachRoot(RootCallback callback, ftype const coef[3], ftype xMin, ftype xMax);
881
882//-------------------------------------------------------------------------------
883
892_CY_POLY_TEMPLATE_NC bool PolynomialForEachRoot(
893 RootCallback callback,
894 ftype const coef[N + 1],
895 ftype xError = PolynomialDefaultError<ftype>());
896_CY_POLY_TEMPLATE_RC bool CubicForEachRoot(
897 RootCallback callback,
898 ftype const coef[4],
899 ftype xError = PolynomialDefaultError<ftype>());
900_CY_POLY_TEMPLATE_AC bool QuadraticForEachRoot(RootCallback callback, ftype const coef[3]);
901
902//-------------------------------------------------------------------------------
904//-------------------------------------------------------------------------------
905
906#define _CY_POLY_TEMPLATE_B \
907 template <bool boundError = false> \
908 CY_NODISCARD
909#define _CY_POLY_TEMPLATE_BC template <bool boundError = false, typename RootCallback>
910
911//-------------------------------------------------------------------------------
920template <typename ftype, int N>
921class Polynomial
922{
923 public:
924 ftype coef[N + 1];
925
926 CY_NODISCARD ftype const& operator[](int i) const
927 {
928 return coef[i];
929 }
930 CY_NODISCARD ftype& operator[](int i)
931 {
932 return coef[i];
933 }
934 CY_NODISCARD ftype operator()(ftype x) const
935 {
936 return Eval(x);
937 }
938
939 CY_NODISCARD ftype Eval(ftype x) const
940 {
941 return PolynomialEval<N, ftype>(coef, x);
942 }
943 CY_NODISCARD ftype EvalWithDeriv(ftype& deriv, ftype x) const
944 {
945 return PolynomialEvalWithDeriv<N, ftype>(deriv, coef, x);
946 }
947
948 CY_NODISCARD Polynomial<ftype, N> operator+(Polynomial<ftype, N> const& p) const
949 {
950 Polynomial<ftype, N> r;
951 for (int i = 0; i <= N; ++i)
952 r[i] = coef[i] + p[i];
953 return r;
954 }
955 CY_NODISCARD Polynomial<ftype, N> operator-(Polynomial<ftype, N> const& p) const
956 {
957 Polynomial<ftype, N> r;
958 for (int i = 0; i <= N; ++i)
959 r[i] = coef[i] - p[i];
960 return r;
961 }
962 CY_NODISCARD Polynomial<ftype, N> operator*(ftype s) const
963 {
964 Polynomial<ftype, N> r;
965 for (int i = 0; i <= N; ++i)
966 r[i] = coef[i] * s;
967 return r;
968 }
969 void operator+=(Polynomial<ftype, N> const& p)
970 {
971 for (int i = 0; i <= N; ++i)
972 coef[i] += p[i];
973 }
974 void operator-=(Polynomial<ftype, N> const& p)
975 {
976 for (int i = 0; i <= N; ++i)
977 coef[i] -= p[i];
978 }
979 void operator*=(ftype s)
980 {
981 for (int i = 0; i <= N; ++i)
982 coef[i] *= s;
983 }
984
986 template <int M>
987 Polynomial<ftype, N + M> operator*(Polynomial<ftype, M> const& p) const
988 {
989 Polynomial<ftype, N + M> r;
990 for (int i = 0; i <= N + M; ++i)
991 r[i] = ftype(0);
992 for (int i = 0; i <= N; ++i)
993 {
994 for (int j = 0; j <= M; ++j)
995 {
996 r[i + j] += coef[i] * p[j];
997 }
998 }
999 return r;
1000 }
1001
1003 CY_NODISCARD Polynomial<ftype, 2 * N> Squared() const
1004 {
1005 Polynomial<ftype, 2 * N> r;
1006 for (int i = 0; i <= 2 * N; ++i)
1007 r[i] = ftype(0);
1008 for (int i = 0; i <= N; ++i)
1009 {
1010 r[2 * i] += coef[i] * coef[i];
1011 for (int j = i + 1; j <= N; ++j)
1012 {
1013 r[i + j] += 2 * coef[i] * coef[j];
1014 }
1015 }
1016 return r;
1017 }
1018
1019 CY_NODISCARD Polynomial<ftype, N - 1> Derivative() const
1020 {
1021 Polynomial<ftype, N - 1> d;
1022 PolynomialDerivative<N, ftype>(d.coef, coef);
1023 return d;
1024 }
1025 CY_NODISCARD Polynomial<ftype, N - 1> Deflate(ftype root) const
1026 {
1027 Polynomial<ftype, N - 1> p;
1028 PolynomialDeflate<N, ftype>(p.coef, coef, root);
1029 return p;
1030 }
1031 CY_NODISCARD Polynomial<ftype, N + 1> Inflate(ftype root) const
1032 {
1033 Polynomial<ftype, N + 1> p;
1034 PolynomialInflate<N, ftype>(p.coef, coef, root);
1035 return p;
1036 }
1037
1038 _CY_POLY_TEMPLATE_B int Roots(ftype roots[N], ftype xError = DefaultError()) const
1039 {
1040 return PolynomialRoots<N, ftype, boundError>(roots, coef, xError);
1041 }
1042 _CY_POLY_TEMPLATE_B bool FirstRoot(ftype& root, ftype xError = DefaultError()) const
1043 {
1044 return PolynomialFirstRoot<N, ftype, boundError>(root, coef, xError);
1045 }
1046 _CY_POLY_TEMPLATE_B bool HasRoot(ftype xError = DefaultError()) const
1047 {
1048 return PolynomialHasRoot<N, ftype, boundError>(coef, xError);
1049 }
1050 _CY_POLY_TEMPLATE_BC void ForEachRoot(RootCallback c, ftype xError = DefaultError()) const
1051 {
1052 return PolynomialForEachRoot<N, ftype, boundError>(c, coef, xError);
1053 }
1054
1055 _CY_POLY_TEMPLATE_B int
1056 Roots(ftype roots[N], ftype xMin, ftype xMax, ftype xError = DefaultError()) const
1057 {
1058 return PolynomialRoots<N, ftype, boundError>(roots, coef, xMin, xMax, xError);
1059 }
1061 _CY_POLY_TEMPLATE_B bool
1062 FirstRoot(ftype& root, ftype xMin, ftype xMax, ftype xError = DefaultError()) const
1063 {
1064 return PolynomialFirstRoot<N, ftype, boundError>(root, coef, xMin, xMax, xError);
1065 }
1067 _CY_POLY_TEMPLATE_B bool HasRoot(ftype xMin, ftype xMax, ftype xError = DefaultError()) const
1068 {
1069 return PolynomialHasRoot<N, ftype, boundError>(coef, xMin, xMax, xError);
1070 }
1071 _CY_POLY_TEMPLATE_BC void
1072 ForEachRoot(RootCallback c, ftype xMin, ftype xMax, ftype xError = DefaultError()) const
1073 {
1074 return PolynomialForEachRoot<N, ftype, boundError>(c, coef, xMin, xMax, xError);
1075 }
1077
1078 CY_NODISCARD bool IsFinite() const
1079 {
1080 for (int i = 0; i <= N; ++i)
1081 if (!cy::IsFinite(coef[i]))
1082 return false;
1083 return true;
1084 }
1085
1086 protected:
1087 static constexpr ftype DefaultError()
1088 {
1089 return PolynomialDefaultError<ftype>();
1090 }
1091};
1092
1093//-------------------------------------------------------------------------------
1097//-------------------------------------------------------------------------------
1098
1099template <typename T, typename S>
1100inline T MultSign(T v, S sign)
1101{
1102 return v * (sign < 0 ? T(-1) : T(1));
1103}
1104template <typename T, typename S>
1105inline bool IsDifferentSign(T a, S b)
1106{
1107 return (a < 0) != (b < 0);
1108}
1109
1110//-------------------------------------------------------------------------------
1114//-------------------------------------------------------------------------------
1115
1123class RootFinderNewton
1124{
1125 public:
1126 template <int N, typename ftype, bool boundError = false>
1127 static inline ftype FindClosed(
1128 ftype const coef[N + 1],
1129 ftype const deriv[N],
1130 ftype x0,
1131 ftype x1,
1132 ftype y0,
1133 ftype y1,
1134 ftype xError);
1136 template <int N, typename ftype, bool boundError = false>
1137 static inline ftype FindOpen(
1138 ftype const coef[N + 1],
1139 ftype const deriv[N],
1140 ftype xError);
1141 template <int N, typename ftype, bool boundError = false>
1142 static inline ftype FindOpenMin(
1143 ftype const coef[N + 1],
1144 ftype const deriv[N],
1145 ftype x1,
1146 ftype y1,
1147 ftype xError);
1149 template <int N, typename ftype, bool boundError = false>
1150 static inline ftype FindOpenMax(
1151 ftype const coef[N + 1],
1152 ftype const deriv[N],
1153 ftype x0,
1154 ftype y0,
1155 ftype xError);
1157 protected:
1158 template <int N, typename ftype, bool boundError, bool openMin>
1159 static inline ftype FindOpen(
1160 ftype const coef[N + 1],
1161 ftype const deriv[N],
1162 ftype xs,
1163 ftype ys,
1164 ftype xr,
1165 ftype xError);
1166};
1167
1168//-------------------------------------------------------------------------------
1169
1178template <int N, typename ftype, bool boundError>
1179inline ftype RootFinderNewton::FindClosed(
1180 ftype const coef[N + 1],
1181 ftype const deriv[N],
1182 ftype x0,
1183 ftype x1,
1184 ftype y0,
1185 [[maybe_unused]] ftype y1,
1186 ftype xError)
1187{
1188 ftype ep2 = 2 * xError;
1189 ftype xr = (x0 + x1) / 2; // mid point
1190 if (x1 - x0 <= ep2)
1191 return xr;
1192
1193 if constexpr (N <= 3)
1194 {
1195 ftype xr0 = xr;
1196 for (int safetyCounter = 0; safetyCounter < 16; ++safetyCounter)
1197 {
1198 ftype xn =
1199 xr - PolynomialEval<N, ftype>(coef, xr) / PolynomialEval<2, ftype>(deriv, xr);
1200 xn = Clamp(xn, x0, x1);
1201 if (std::abs(xr - xn) <= xError)
1202 return xn;
1203 xr = xn;
1204 }
1205 if (!IsFinite(xr))
1206 xr = xr0;
1207 }
1208
1209 ftype yr = PolynomialEval<N, ftype>(coef, xr);
1210 ftype xb0 = x0;
1211 ftype xb1 = x1;
1212
1213 while (true)
1214 {
1215 int side = IsDifferentSign(y0, yr);
1216 if (side)
1217 xb1 = xr;
1218 else
1219 xb0 = xr;
1220 ftype dy = PolynomialEval<N - 1, ftype>(deriv, xr);
1221 ftype dx = yr / dy;
1222 ftype xn = xr - dx;
1223 if (xn > xb0 && xn < xb1)
1224 { // valid Newton step
1225 ftype stepsize = std::abs(xr - xn);
1226 xr = xn;
1227 if (stepsize > xError)
1228 {
1229 yr = PolynomialEval<N, ftype>(coef, xr);
1230 }
1231 else
1232 {
1233 if constexpr (boundError)
1234 {
1235 ftype xs;
1236 if (xError == 0)
1237 {
1238 xs = std::nextafter(side ? xb1 : xb0, side ? xb0 : xb1);
1239 }
1240 else
1241 {
1242 xs = xn - MultSign(xError, side - 1);
1243 if (xs == xn)
1244 xs = std::nextafter(side ? xb1 : xb0, side ? xb0 : xb1);
1245 }
1246 ftype ys = PolynomialEval<N, ftype>(coef, xs);
1247 int s = IsDifferentSign(y0, ys);
1248 if (side != s)
1249 return xn;
1250 xr = xs;
1251 yr = ys;
1252 }
1253 else
1254 break;
1255 }
1256 }
1257 else
1258 { // Newton step failed
1259 xr = (xb0 + xb1) / 2;
1260 if (xr == xb0 || xr == xb1 || xb1 - xb0 <= ep2)
1261 {
1262 if constexpr (boundError)
1263 {
1264 if (xError == 0)
1265 {
1266 ftype xm = side ? xb0 : xb1;
1267 ftype ym = PolynomialEval<N, ftype>(coef, xm);
1268 if (std::abs(ym) < std::abs(yr))
1269 xr = xm;
1270 }
1271 }
1272 break;
1273 }
1274 yr = PolynomialEval<N, ftype>(coef, xr);
1275 }
1276 }
1277 return xr;
1278}
1279
1280//-------------------------------------------------------------------------------
1281
1287template <int N, typename ftype, bool boundError>
1288inline ftype RootFinderNewton::FindOpen(ftype const coef[N + 1], ftype const deriv[N], ftype xError)
1289{
1290 static_assert(
1291 (N & 1) == 1,
1292 "RootFinderNewton::FindOpen only works for polynomials with odd degrees.");
1293 const ftype xr = 0;
1294 const ftype yr = coef[0]; // PolynomialEval<N,ftype>( coef, xr );
1295 if (IsDifferentSign(coef[N], yr))
1296 {
1297 return FindOpenMax<N, ftype, boundError>(coef, deriv, xr, yr, xError);
1298 }
1299 else
1300 {
1301 return FindOpenMin<N, ftype, boundError>(coef, deriv, xr, yr, xError);
1302 }
1303}
1304
1305//-------------------------------------------------------------------------------
1306
1308template <int N, typename ftype, bool boundError>
1309inline ftype RootFinderNewton::FindOpenMin(
1310 ftype const coef[N + 1],
1311 ftype const deriv[N],
1312 ftype x1,
1313 ftype y1,
1314 ftype xError)
1315{
1316 return FindOpen<N, ftype, boundError, true>(coef, deriv, x1, y1, x1 - ftype(1), xError);
1317}
1318
1319//-------------------------------------------------------------------------------
1320
1322template <int N, typename ftype, bool boundError>
1323inline ftype RootFinderNewton::FindOpenMax(
1324 ftype const coef[N + 1],
1325 ftype const deriv[N],
1326 ftype x0,
1327 ftype y0,
1328 ftype xError)
1329{
1330 return FindOpen<N, ftype, boundError, false>(coef, deriv, x0, y0, x0 + ftype(1), xError);
1331}
1332
1333//-------------------------------------------------------------------------------
1334
1336template <int N, typename ftype, bool boundError, bool openMin>
1337inline ftype RootFinderNewton::FindOpen(
1338 ftype const coef[N + 1],
1339 ftype const deriv[N],
1340 ftype xm,
1341 ftype ym,
1342 ftype xr,
1343 ftype xError)
1344{
1345 ftype delta = ftype(1);
1346 ftype yr = PolynomialEval<N, ftype>(coef, xr);
1347
1348 bool otherside = IsDifferentSign(ym, yr);
1349
1350 while (yr != 0)
1351 {
1352 if (otherside)
1353 {
1354 if constexpr (openMin)
1355 {
1356 return FindClosed<N, ftype, boundError>(coef, deriv, xr, xm, yr, ym, xError);
1357 }
1358 else
1359 {
1360 return FindClosed<N, ftype, boundError>(coef, deriv, xm, xr, ym, yr, xError);
1361 }
1362 }
1363 else
1364 {
1365 open_interval:
1366 xm = xr;
1367 ym = yr;
1368 ftype dy = PolynomialEval<N - 1, ftype>(deriv, xr);
1369 ftype dx = yr / dy;
1370 ftype xn = xr - dx;
1371 ftype dif = openMin ? xr - xn : xn - xr;
1372 if (dif <= 0 && std::isfinite(xn))
1373 { // valid Newton step
1374 xr = xn;
1375 if (dif <= xError)
1376 { // we might have converged
1377 if (xr == xm)
1378 break;
1379 ftype xs = xn - MultSign(xError, -ftype(openMin));
1380 ftype ys = PolynomialEval<N, ftype>(coef, xs);
1381 bool s = IsDifferentSign(ym, ys);
1382 if (s)
1383 break;
1384 xr = xs;
1385 yr = ys;
1386 goto open_interval;
1387 }
1388 }
1389 else
1390 { // Newton step failed
1391 xr = openMin ? xr - delta : xr + delta;
1392 delta *= 2;
1393 }
1394 yr = PolynomialEval<N, ftype>(coef, xr);
1395 otherside = IsDifferentSign(ym, yr);
1396 }
1397 }
1398 return xr;
1399}
1400
1401//-------------------------------------------------------------------------------
1403// Implementations of the root finding functions declared above
1405//-------------------------------------------------------------------------------
1406
1407//-------------------------------------------------------------------------------
1408// Linear
1409//-------------------------------------------------------------------------------
1410
1411template <typename ftype>
1412inline int LinearRoot(ftype& root, ftype const coef[2], ftype x0, ftype x1)
1413{
1414 if (coef[1] != ftype(0))
1415 {
1416 ftype r = -coef[0] / coef[1];
1417 root = r;
1418 return (r >= x0 && r <= x1);
1419 }
1420 else
1421 {
1422 root = (x0 + x1) / 2;
1423 return coef[0] == 0;
1424 }
1425}
1426
1427template <typename ftype>
1428inline int LinearRoot(ftype& root, ftype const coef[2])
1429{
1430 root = -coef[0] / coef[1];
1431 return coef[1] != 0;
1432}
1433
1434//-------------------------------------------------------------------------------
1435// Quadratics
1436//-------------------------------------------------------------------------------
1437
1438template <typename ftype>
1439inline int QuadraticRoots(ftype roots[2], ftype const coef[3])
1440{
1441 const ftype c = coef[0];
1442 const ftype b = coef[1];
1443 const ftype a = coef[2];
1444 const ftype delta = b * b - 4 * a * c;
1445 if (delta > 0)
1446 {
1447 const ftype d = Sqrt(delta);
1448 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1449 const ftype rv0 = q / a;
1450 const ftype rv1 = c / q;
1451 roots[0] = Min(rv0, rv1);
1452 roots[1] = Max(rv0, rv1);
1453 return 2;
1454 }
1455 else if (delta < 0)
1456 return 0;
1457 roots[0] = ftype(-0.5) * b / a;
1458 return a != 0;
1459}
1460
1461template <typename ftype>
1462inline int QuadraticRoots(ftype roots[2], ftype const coef[3], ftype x0, ftype x1)
1463{
1464 const ftype c = coef[0];
1465 const ftype b = coef[1];
1466 const ftype a = coef[2];
1467 const ftype delta = b * b - 4 * a * c;
1468 if (delta > 0)
1469 {
1470 const ftype d = Sqrt(delta);
1471 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1472 const ftype rv0 = q / a;
1473 const ftype rv1 = c / q;
1474 const ftype r0 = Min(rv0, rv1);
1475 const ftype r1 = Max(rv0, rv1);
1476 int r0i = (r0 >= x0) & (r0 <= x1);
1477 int r1i = (r1 >= x0) & (r1 <= x1);
1478 roots[0] = r0;
1479 roots[r0i] = r1;
1480 return r0i + r1i;
1481 }
1482 else if (delta < 0)
1483 return 0;
1484 const ftype r0 = ftype(-0.5) * b / a;
1485 roots[0] = r0;
1486 return (r0 >= x0) & (r0 <= x1);
1487}
1488
1489//-------------------------------------------------------------------------------
1490#ifdef _INCLUDED_IMM
1491
1492template <typename RootCallback>
1493inline int QuadraticRoots([[maybe_unused]] float roots[2], float const* coef, RootCallback callback)
1494{
1495 //__m128 _0abc = _mm_set_ps( 0.0f, coef[2], coef[1], coef[0] );
1496 __m128 _0abc = _mm_load_ps(coef);
1497 __m128 _02a2b2c = _mm_add_ps(_0abc, _0abc);
1498 __m128 _2a2c_bb = _mm_shuffle_ps(_0abc, _02a2b2c, _MM_SHUFFLE(2, 0, 1, 1));
1499 __m128 _2c2a_bb = _mm_shuffle_ps(_0abc, _02a2b2c, _MM_SHUFFLE(0, 2, 1, 1));
1500 __m128 _4ac_b2 = _mm_mul_ps(_2a2c_bb, _2c2a_bb);
1501 __m128 _4ac = _mm_shuffle_ps(_4ac_b2, _4ac_b2, _MM_SHUFFLE(2, 2, 2, 2));
1502 if (_mm_comigt_ss(_4ac_b2, _4ac))
1503 {
1504 __m128 delta = _mm_sub_ps(_4ac_b2, _4ac);
1505 __m128 sqrtd = _mm_sqrt_ss(delta);
1506 __m128 signb = _mm_set_ps(-0.0f, -0.0f, -0.0f, -0.0f);
1507 __m128 db = _mm_xor_ps(sqrtd, _mm_and_ps(_2a2c_bb, signb));
1508 __m128 b_db = _mm_add_ss(_2a2c_bb, db);
1509 __m128 _2q = _mm_xor_ps(b_db, signb);
1510 __m128 _2c_2q = _mm_shuffle_ps(_2q, _02a2b2c, _MM_SHUFFLE(0, 0, 0, 0));
1511 __m128 _2q_2a = _mm_shuffle_ps(_02a2b2c, _2q, _MM_SHUFFLE(0, 0, 2, 2));
1512 __m128 rv = _mm_div_ps(_2c_2q, _2q_2a);
1513 __m128 r0 = _mm_min_ps(rv, _mm_shuffle_ps(rv, rv, _MM_SHUFFLE(3, 2, 1, 2)));
1514 __m128 r = _mm_max_ps(r0, _mm_shuffle_ps(r0, r0, _MM_SHUFFLE(3, 2, 2, 0)));
1515 return callback(r);
1516 }
1517 else if (_mm_comilt_ss(_4ac_b2, _4ac))
1518 return 0;
1519 __m128 r = _mm_div_ps(_2a2c_bb, _mm_shuffle_ps(_2a2c_bb, _2a2c_bb, _MM_SHUFFLE(1, 0, 3, 3)));
1520 return callback(r) * (coef[2] != 0);
1521}
1522
1523template <>
1524[[maybe_unused]] inline int QuadraticRoots<float>(float roots[2], float const* coef)
1525{
1526 return QuadraticRoots(roots, coef, [&](__m128 r) {
1527 roots[0] = _mm_cvtss_f32(r);
1528 roots[1] = _mm_cvtss_f32(_mm_shuffle_ps(r, r, _MM_SHUFFLE(3, 2, 0, 1)));
1529 return 2;
1530 });
1531}
1532
1533template <>
1534[[maybe_unused]] inline int QuadraticRoots<float>(float roots[2], float const* coef, float x0, float x1)
1535{
1536 return QuadraticRoots(roots, coef, [&](__m128 r) {
1537 __m128 range = _mm_set_ps(x1, x1, x0, x0);
1538 __m128 minT = _mm_cmpge_ps(r, range);
1539 __m128 maxT = _mm_cmple_ps(r, _mm_shuffle_ps(range, range, _MM_SHUFFLE(3, 2, 2, 2)));
1540 __m128 valid =
1541 _mm_and_ps(minT, _mm_shuffle_ps(maxT, _mm_setzero_ps(), _MM_SHUFFLE(3, 2, 1, 0)));
1542 __m128 rr = _mm_blendv_ps(_mm_shuffle_ps(r, r, _MM_SHUFFLE(3, 2, 0, 1)), r, valid);
1543 roots[0] = _mm_cvtss_f32(rr);
1544 roots[1] = _mm_cvtss_f32(_mm_shuffle_ps(rr, rr, _MM_SHUFFLE(3, 2, 0, 1)));
1545 return _mm_popcnt_u32(_mm_movemask_ps(valid));
1546 });
1547}
1548
1549#endif // _INCLUDED_IMM
1550//-------------------------------------------------------------------------------
1551
1552template <typename ftype>
1553inline bool QuadraticFirstRoot(ftype& root, ftype const coef[3], ftype x0, ftype x1)
1554{
1555 const ftype c = coef[0];
1556 const ftype b = coef[1];
1557 const ftype a = coef[2];
1558 const ftype delta = b * b - 4 * a * c;
1559 if (delta >= 0)
1560 {
1561 const ftype d = Sqrt(delta);
1562 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1563 const ftype rv0 = q / a;
1564 const ftype rv1 = c / q;
1565 const ftype r0 = Min(rv0, rv1);
1566 if (r0 >= x0)
1567 {
1568 root = r0;
1569 return r0 <= x1;
1570 }
1571 else
1572 {
1573 const ftype r1 = Max(rv0, rv1);
1574 root = r1;
1575 return (r1 >= x0) & (r1 <= x1);
1576 }
1577 }
1578 return false;
1579}
1580
1581//-------------------------------------------------------------------------------
1582
1583template <typename ftype>
1584inline bool QuadraticFirstRoot(ftype& root, ftype const coef[3])
1585{
1586 const ftype c = coef[0];
1587 const ftype b = coef[1];
1588 const ftype a = coef[2];
1589 const ftype delta = b * b - 4 * a * c;
1590 if (delta >= 0)
1591 {
1592 const ftype d = Sqrt(delta);
1593 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1594 const ftype rv0 = q / a;
1595 const ftype rv1 = c / q;
1596 root = Min(rv0, rv1);
1597 return true;
1598 }
1599 return false;
1600}
1601
1602//-------------------------------------------------------------------------------
1603
1604template <typename ftype>
1605inline bool QuadraticHasRoot(ftype const coef[3], ftype x0, ftype x1)
1606{
1607 const ftype c = coef[0];
1608 const ftype b = coef[1];
1609 const ftype a = coef[2];
1610 const ftype delta = b * b - 4 * a * c;
1611 if (delta >= 0)
1612 {
1613 const ftype d = Sqrt(delta);
1614 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1615 const ftype rv0 = q / a;
1616 const ftype rv1 = c / q;
1617 const ftype r0 = Min(rv0, rv1);
1618 const ftype r1 = Max(rv0, rv1);
1619 if (r0 >= x0 && r0 <= x1)
1620 return true;
1621 if (r1 >= x0 && r1 <= x1)
1622 return true;
1623 }
1624 return false;
1625}
1626
1627//-------------------------------------------------------------------------------
1628
1629template <typename ftype, typename RootCallback>
1630inline bool QuadraticForEachRoot(RootCallback callback, ftype const coef[3], ftype x0, ftype x1)
1631{
1632 const ftype c = coef[0];
1633 const ftype b = coef[1];
1634 const ftype a = coef[2];
1635 const ftype delta = b * b - 4 * a * c;
1636 if (delta >= 0)
1637 {
1638 const ftype d = Sqrt(delta);
1639 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1640 const ftype rv0 = q / a;
1641 const ftype rv1 = c / q;
1642 const ftype r0 = Min(rv0, rv1);
1643 const ftype r1 = Max(rv0, rv1);
1644 if (r0 >= x0 && r0 <= x1)
1645 if (callback(r0))
1646 return true;
1647 if (r1 >= x0 && r1 <= x1)
1648 return callback(r1);
1649 }
1650 return false;
1651}
1652
1653//-------------------------------------------------------------------------------
1654
1655template <typename ftype, typename RootCallback>
1656inline bool QuadraticForEachRoot(RootCallback callback, ftype const coef[3])
1657{
1658 const ftype c = coef[0];
1659 const ftype b = coef[1];
1660 const ftype a = coef[2];
1661 const ftype delta = b * b - 4 * a * c;
1662 if (delta >= 0)
1663 {
1664 const ftype d = Sqrt(delta);
1665 const ftype q = ftype(-0.5) * (b + MultSign(d, b));
1666 const ftype rv0 = q / a;
1667 const ftype rv1 = c / q;
1668 const ftype r0 = Min(rv0, rv1);
1669 const ftype r1 = Max(rv0, rv1);
1670 if (callback(r0))
1671 return true;
1672 return callback(r1);
1673 }
1674 return false;
1675}
1676
1677//-------------------------------------------------------------------------------
1678// Cubics
1679//-------------------------------------------------------------------------------
1680
1681template <typename ftype, bool boundError, typename RootFinder>
1682inline int CubicRoots(ftype roots[3], ftype const coef[4], ftype x0, ftype x1, ftype xError)
1683{
1684 const ftype y0 = PolynomialEval<3, ftype>(coef, x0);
1685 const ftype y1 = PolynomialEval<3, ftype>(coef, x1);
1686
1687 const ftype a = coef[3] * 3;
1688 const ftype b_2 = coef[2];
1689 const ftype c = coef[1];
1690
1691 const ftype deriv[4] = {c, 2 * b_2, a, 0};
1692
1693 const ftype delta_4 = b_2 * b_2 - a * c;
1694
1695 if (delta_4 > 0)
1696 {
1697 const ftype d_2 = Sqrt(delta_4);
1698 const ftype q = -(b_2 + MultSign(d_2, b_2));
1699 ftype rv0 = q / a;
1700 ftype rv1 = c / q;
1701 const ftype xa = Min(rv0, rv1);
1702 const ftype xb = Max(rv0, rv1);
1703
1704 if (IsDifferentSign(y0, y1))
1705 {
1706 if (xa >= x1 || xb <= x0 || (xa <= x0 && xb >= x1))
1707 { // first, last, or middle interval only
1708 roots[0] = RootFinder::template FindClosed<3, ftype, boundError>(
1709 coef,
1710 deriv,
1711 x0,
1712 x1,
1713 y0,
1714 y1,
1715 xError);
1716 return 1;
1717 }
1718 }
1719 else
1720 {
1721 if ((xa >= x1 || xb <= x0) || (xa <= x0 && xb >= x1))
1722 return 0;
1723 }
1724
1725 int numRoots = 0;
1726 if (xa > x0)
1727 {
1728 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
1729 if (IsDifferentSign(y0, ya))
1730 {
1731 roots[0] = RootFinder::template FindClosed<3, ftype, boundError>(
1732 coef,
1733 deriv,
1734 x0,
1735 xa,
1736 y0,
1737 ya,
1738 xError); // first interval
1739 if constexpr (!boundError)
1740 {
1741 if (IsDifferentSign(ya, y1) ||
1742 (xb < x1 && IsDifferentSign(ya, PolynomialEval<3, ftype>(coef, xb))))
1743 {
1744 ftype defPoly[4];
1745 PolynomialDeflate<3>(defPoly, coef, roots[0]);
1746 return QuadraticRoots(roots + 1, defPoly, xa, x1) + 1;
1747 }
1748 else
1749 return 1;
1750 }
1751 else
1752 numRoots++;
1753 }
1754 if (xb < x1)
1755 {
1756 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
1757 if (IsDifferentSign(ya, yb))
1758 {
1759 roots[!boundError ? 0 : numRoots++] =
1760 RootFinder::template FindClosed<3, ftype, boundError>(
1761 coef,
1762 deriv,
1763 xa,
1764 xb,
1765 ya,
1766 yb,
1767 xError);
1768 if constexpr (!boundError)
1769 {
1770 if (IsDifferentSign(yb, y1))
1771 {
1772 ftype defPoly[4];
1773 PolynomialDeflate<3>(defPoly, coef, roots[0]);
1774 return QuadraticRoots(roots + 1, defPoly, xb, x1) + 1;
1775 }
1776 else
1777 return 1;
1778 }
1779 }
1780 if (IsDifferentSign(yb, y1))
1781 {
1782 roots[!boundError ? 0 : numRoots++] =
1783 RootFinder::template FindClosed<3, ftype, boundError>(
1784 coef,
1785 deriv,
1786 xb,
1787 x1,
1788 yb,
1789 y1,
1790 xError); // last interval
1791 if constexpr (!boundError)
1792 return 1;
1793 }
1794 }
1795 else
1796 {
1797 if (IsDifferentSign(ya, y1))
1798 {
1799 roots[!boundError ? 0 : numRoots++] =
1800 RootFinder::template FindClosed<3, ftype, boundError>(
1801 coef,
1802 deriv,
1803 xa,
1804 x1,
1805 ya,
1806 y1,
1807 xError);
1808 if (!boundError)
1809 return 1;
1810 }
1811 }
1812 }
1813 else
1814 {
1815 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
1816 if (IsDifferentSign(y0, yb))
1817 {
1818 roots[0] = RootFinder::template FindClosed<3, ftype, boundError>(
1819 coef,
1820 deriv,
1821 x0,
1822 xb,
1823 y0,
1824 yb,
1825 xError);
1826 if constexpr (!boundError)
1827 {
1828 if (IsDifferentSign(yb, y1))
1829 {
1830 ftype defPoly[4];
1831 PolynomialDeflate<3>(defPoly, coef, roots[0]);
1832 return QuadraticRoots(roots + 1, defPoly, xb, x1) + 1;
1833 }
1834 else
1835 return 1;
1836 }
1837 else
1838 numRoots++;
1839 }
1840 if (IsDifferentSign(yb, y1))
1841 {
1842 roots[!boundError ? 0 : numRoots++] =
1843 RootFinder::template FindClosed<3, ftype, boundError>(
1844 coef,
1845 deriv,
1846 xb,
1847 x1,
1848 yb,
1849 y1,
1850 xError); // last interval
1851 if constexpr (!boundError)
1852 return 1;
1853 }
1854 }
1855 return numRoots;
1856 }
1857 else
1858 {
1859 if (IsDifferentSign(y0, y1))
1860 {
1861 roots[0] = RootFinder::template FindClosed<3, ftype, boundError>(
1862 coef,
1863 deriv,
1864 x0,
1865 x1,
1866 y0,
1867 y1,
1868 xError);
1869 return 1;
1870 }
1871 return 0;
1872 }
1873}
1874
1875//-------------------------------------------------------------------------------
1876
1877template <typename ftype, bool boundError, typename RootFinder>
1878inline int CubicRoots(ftype roots[3], ftype const coef[4], ftype xError)
1879{
1880 if (coef[3] != 0)
1881 {
1882 const ftype a = coef[3] * 3;
1883 const ftype b_2 = coef[2];
1884 const ftype c = coef[1];
1885
1886 const ftype deriv[4] = {c, 2 * b_2, a, 0};
1887
1888 const ftype delta_4 = b_2 * b_2 - a * c;
1889
1890 if (delta_4 > 0)
1891 {
1892 const ftype d_2 = Sqrt(delta_4);
1893 const ftype q = -(b_2 + MultSign(d_2, b_2));
1894 const ftype rv0 = q / a;
1895 const ftype rv1 = c / q;
1896 const ftype xa = Min(rv0, rv1);
1897 const ftype xb = Max(rv0, rv1);
1898
1899 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
1900 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
1901
1902 if (!IsDifferentSign(coef[3], ya))
1903 {
1904 roots[0] = RootFinder::template FindOpenMin<3, ftype, boundError>(
1905 coef,
1906 deriv,
1907 xa,
1908 ya,
1909 xError);
1910 if constexpr (!boundError)
1911 {
1912 if (IsDifferentSign(ya, yb))
1913 {
1914 ftype defPoly[4];
1915 PolynomialDeflate<3>(defPoly, coef, roots[0]);
1916 return QuadraticRoots(roots + 1, defPoly) + 1;
1917 }
1918 }
1919 else
1920 {
1921 if (IsDifferentSign(ya, yb))
1922 {
1923 roots[1] = RootFinder::template FindClosed<3, ftype, boundError>(
1924 coef,
1925 deriv,
1926 xa,
1927 xb,
1928 ya,
1929 yb,
1930 xError);
1931 roots[2] = RootFinder::template FindOpenMax<3, ftype, boundError>(
1932 coef,
1933 deriv,
1934 xb,
1935 yb,
1936 xError);
1937 return 3;
1938 }
1939 }
1940 }
1941 else
1942 {
1943 roots[0] = RootFinder::template FindOpenMax<3, ftype, boundError>(
1944 coef,
1945 deriv,
1946 xb,
1947 yb,
1948 xError);
1949 }
1950 return 1;
1951 }
1952 else
1953 {
1954 ftype x_inf = -b_2 / a;
1955 ftype y_inf = PolynomialEval<3, ftype>(coef, x_inf);
1956 if (IsDifferentSign(coef[3], y_inf))
1957 {
1958 roots[0] = RootFinder::template FindOpenMax<3, ftype, boundError>(
1959 coef,
1960 deriv,
1961 x_inf,
1962 y_inf,
1963 xError);
1964 }
1965 else
1966 {
1967 roots[0] = RootFinder::template FindOpenMin<3, ftype, boundError>(
1968 coef,
1969 deriv,
1970 x_inf,
1971 y_inf,
1972 xError);
1973 }
1974 return 1;
1975 }
1976 }
1977 else
1978 return QuadraticRoots<ftype>(roots, coef);
1979}
1980
1981//-------------------------------------------------------------------------------
1982
1983template <typename ftype, bool boundError, typename RootFinder>
1984inline bool CubicFirstRoot(ftype& root, ftype const coef[4], ftype x0, ftype x1, ftype xError)
1985{
1986 const ftype y0 = PolynomialEval<3, ftype>(coef, x0);
1987 const ftype y1 = PolynomialEval<3, ftype>(coef, x1);
1988
1989 const ftype a = coef[3] * 3;
1990 const ftype b_2 = coef[2];
1991 const ftype c = coef[1];
1992
1993 const ftype deriv[4] = {c, 2 * b_2, a, 0};
1994
1995 const ftype delta_4 = b_2 * b_2 - a * c;
1996
1997 if (delta_4 > 0)
1998 {
1999 const ftype d_2 = Sqrt(delta_4);
2000 const ftype q = -(b_2 + MultSign(d_2, b_2));
2001 const ftype rv0 = q / a;
2002 const ftype rv1 = c / q;
2003 const ftype xa = Min(rv0, rv1);
2004 const ftype xb = Max(rv0, rv1);
2005
2006 if (IsDifferentSign(y0, y1))
2007 {
2008 if (xa >= x1 || xb <= x0 || (xa <= x0 && xb >= x1))
2009 { // first, last, or middle interval only
2010 root = RootFinder::template FindClosed<3, ftype, boundError>(
2011 coef,
2012 deriv,
2013 x0,
2014 x1,
2015 y0,
2016 y1,
2017 xError); // first/last interval
2018 return true;
2019 }
2020 }
2021 else
2022 {
2023 if ((xa >= x1 || xb <= x0) || (xa <= x0 && xb >= x1))
2024 return false;
2025 }
2026
2027 if (xa > x0)
2028 {
2029 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
2030 if (IsDifferentSign(y0, ya))
2031 {
2032 root = RootFinder::template FindClosed<3, ftype, boundError>(
2033 coef,
2034 deriv,
2035 x0,
2036 xa,
2037 y0,
2038 ya,
2039 xError); // first interval
2040 return true;
2041 }
2042 if (xb < x1)
2043 {
2044 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2045 if (IsDifferentSign(ya, yb))
2046 {
2047 root = RootFinder::template FindClosed<3, ftype, boundError>(
2048 coef,
2049 deriv,
2050 xa,
2051 xb,
2052 ya,
2053 yb,
2054 xError);
2055 return true;
2056 }
2057 if (IsDifferentSign(yb, y1))
2058 {
2059 root = RootFinder::template FindClosed<3, ftype, boundError>(
2060 coef,
2061 deriv,
2062 xb,
2063 x1,
2064 yb,
2065 y1,
2066 xError); // last interval
2067 return true;
2068 }
2069 }
2070 else
2071 {
2072 if (IsDifferentSign(ya, y1))
2073 {
2074 root = RootFinder::template FindClosed<3, ftype, boundError>(
2075 coef,
2076 deriv,
2077 xa,
2078 x1,
2079 ya,
2080 y1,
2081 xError);
2082 return true;
2083 }
2084 }
2085 }
2086 else
2087 {
2088 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2089 if (IsDifferentSign(y0, yb))
2090 {
2091 root = RootFinder::template FindClosed<3, ftype, boundError>(
2092 coef,
2093 deriv,
2094 x0,
2095 xb,
2096 y0,
2097 yb,
2098 xError);
2099 return true;
2100 }
2101 if (IsDifferentSign(yb, y1))
2102 {
2103 root = RootFinder::template FindClosed<3, ftype, boundError>(
2104 coef,
2105 deriv,
2106 xb,
2107 x1,
2108 yb,
2109 y1,
2110 xError); // last interval
2111 return true;
2112 }
2113 }
2114 }
2115 else
2116 {
2117 if (IsDifferentSign(y0, y1))
2118 {
2119 root = RootFinder::template FindClosed<3, ftype, boundError>(
2120 coef,
2121 deriv,
2122 x0,
2123 x1,
2124 y0,
2125 y1,
2126 xError);
2127 return true;
2128 }
2129 }
2130 return false;
2131}
2132
2133//-------------------------------------------------------------------------------
2134
2135template <typename ftype, bool boundError, typename RootFinder>
2136inline bool CubicFirstRoot(ftype& root, ftype const coef[4], ftype xError)
2137{
2138 if (coef[3] != 0)
2139 {
2140 const ftype a = coef[3] * 3;
2141 const ftype b_2 = coef[2];
2142 const ftype c = coef[1];
2143
2144 const ftype deriv[4] = {c, 2 * b_2, a, 0};
2145
2146 const ftype delta_4 = b_2 * b_2 - a * c;
2147
2148 if (delta_4 > 0)
2149 {
2150 const ftype d_2 = Sqrt(delta_4);
2151 const ftype q = -(b_2 + MultSign(d_2, b_2));
2152 const ftype rv0 = q / a;
2153 const ftype rv1 = c / q;
2154 const ftype xa = Min(rv0, rv1);
2155 const ftype xb = Max(rv0, rv1);
2156
2157 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
2158 if (!IsDifferentSign(coef[3], ya))
2159 {
2160 root = RootFinder::template FindOpenMin<3, ftype, boundError>(
2161 coef,
2162 deriv,
2163 xa,
2164 ya,
2165 xError);
2166 }
2167 else
2168 {
2169 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2170 root = RootFinder::template FindOpenMax<3, ftype, boundError>(
2171 coef,
2172 deriv,
2173 xb,
2174 yb,
2175 xError);
2176 }
2177 }
2178 else
2179 {
2180 ftype x_inf = -b_2 / a;
2181 ftype y_inf = PolynomialEval<3, ftype>(coef, x_inf);
2182 if (IsDifferentSign(coef[3], y_inf))
2183 {
2184 root = RootFinder::template FindOpenMax<3, ftype, boundError>(
2185 coef,
2186 deriv,
2187 x_inf,
2188 y_inf,
2189 xError);
2190 }
2191 else
2192 {
2193 root = RootFinder::template FindOpenMin<3, ftype, boundError>(
2194 coef,
2195 deriv,
2196 x_inf,
2197 y_inf,
2198 xError);
2199 }
2200 }
2201 return true;
2202 }
2203 else
2204 return QuadraticFirstRoot<ftype>(root, coef);
2205}
2206
2207//-------------------------------------------------------------------------------
2208
2209template <typename ftype>
2210inline bool CubicHasRoot(ftype const coef[4], ftype x0, ftype x1)
2211{
2212 const ftype y0 = PolynomialEval<3, ftype>(coef, x0);
2213 const ftype y1 = PolynomialEval<3, ftype>(coef, x1);
2214 if (IsDifferentSign(y0, y1))
2215 return true;
2216
2217 const ftype a = coef[3] * 3;
2218 const ftype b_2 = coef[2];
2219 const ftype c = coef[1];
2220
2221 const ftype delta_4 = b_2 * b_2 - a * c;
2222
2223 if (delta_4 > 0)
2224 {
2225 const ftype d_2 = Sqrt(delta_4);
2226 const ftype q = -(b_2 + MultSign(d_2, b_2));
2227 const ftype rv0 = q / a;
2228 const ftype rv1 = c / q;
2229 const ftype xa = Min(rv0, rv1);
2230 const ftype xb = Max(rv0, rv1);
2231
2232 if ((xa >= x1 || xb <= x0) || (xa <= x0 && xb >= x1))
2233 return false;
2234
2235 if (xa > x0)
2236 {
2237 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
2238 if (IsDifferentSign(y0, ya))
2239 return true;
2240 if (xb < x1)
2241 {
2242 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2243 if (IsDifferentSign(y0, yb))
2244 return true;
2245 }
2246 }
2247 else
2248 {
2249 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2250 if (IsDifferentSign(y0, yb))
2251 return true;
2252 }
2253 }
2254 return false;
2255}
2256
2257//-------------------------------------------------------------------------------
2258
2259template <typename ftype, bool boundError, typename RootFinder, typename RootCallback>
2260inline bool
2261CubicForEachRoot(RootCallback callback, ftype const coef[4], ftype x0, ftype x1, ftype xError)
2262{
2263 const ftype y0 = PolynomialEval<3, ftype>(coef, x0);
2264 const ftype y1 = PolynomialEval<3, ftype>(coef, x1);
2265
2266 const ftype a = coef[3] * 3;
2267 const ftype b_2 = coef[2];
2268 const ftype c = coef[1];
2269
2270 const ftype deriv[4] = {c, 2 * b_2, a, 0};
2271
2272 const ftype delta_4 = b_2 * b_2 - a * c;
2273
2274 if (delta_4 > 0)
2275 {
2276 const ftype d_2 = Sqrt(delta_4);
2277 const ftype t = -(b_2 + MultSign(d_2, b_2));
2278 const ftype rv0 = t / a;
2279 const ftype rv1 = c / t;
2280 const ftype xa = Min(rv0, rv1);
2281 const ftype xb = Max(rv0, rv1);
2282
2283 if (xa >= x1 || xb <= x0)
2284 {
2285 if (IsDifferentSign(y0, y1))
2286 {
2287 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2288 coef,
2289 deriv,
2290 x0,
2291 x1,
2292 y0,
2293 y1,
2294 xError)))
2295 return true; // first/last interval
2296 }
2297 }
2298 else if (xa <= x0 && xb >= x1)
2299 {
2300 if (IsDifferentSign(y0, y1))
2301 {
2302 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2303 coef,
2304 deriv,
2305 x0,
2306 x1,
2307 y0,
2308 y1,
2309 xError)))
2310 return true;
2311 }
2312 }
2313 else if (xa > x0)
2314 {
2315 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
2316 if (IsDifferentSign(y0, ya))
2317 {
2318 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2319 coef,
2320 deriv,
2321 x0,
2322 xa,
2323 y0,
2324 ya,
2325 xError)))
2326 return true; // first interval
2327 }
2328 if (xb < x1)
2329 {
2330 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2331 if (IsDifferentSign(ya, yb))
2332 {
2333 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2334 coef,
2335 deriv,
2336 xa,
2337 xb,
2338 ya,
2339 yb,
2340 xError)))
2341 return true;
2342 }
2343 if (IsDifferentSign(yb, y1))
2344 {
2345 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2346 coef,
2347 deriv,
2348 xb,
2349 x1,
2350 yb,
2351 y1,
2352 xError)))
2353 return true; // last interval
2354 }
2355 }
2356 else
2357 {
2358 if (IsDifferentSign(ya, y1))
2359 {
2360 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2361 coef,
2362 deriv,
2363 xa,
2364 x1,
2365 ya,
2366 y1,
2367 xError)))
2368 return true;
2369 }
2370 }
2371 }
2372 else
2373 {
2374 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2375 if (IsDifferentSign(y0, yb))
2376 {
2377 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2378 coef,
2379 deriv,
2380 x0,
2381 xb,
2382 y0,
2383 yb,
2384 xError)))
2385 return true;
2386 }
2387 if (IsDifferentSign(yb, y1))
2388 {
2389 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2390 coef,
2391 deriv,
2392 xb,
2393 x1,
2394 yb,
2395 y1,
2396 xError)))
2397 return true; // last interval
2398 }
2399 }
2400 }
2401 else
2402 {
2403 if (IsDifferentSign(y0, y1))
2404 {
2405 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2406 coef,
2407 deriv,
2408 x0,
2409 x1,
2410 y0,
2411 y1,
2412 xError)))
2413 return true;
2414 }
2415 }
2416 return false;
2417}
2418
2419//-------------------------------------------------------------------------------
2420
2421template <typename ftype, bool boundError, typename RootFinder, typename RootCallback>
2422inline bool CubicForEachRoot(RootCallback callback, ftype const coef[4], ftype xError)
2423{
2424 if (coef[3] != 0)
2425 {
2426 const ftype a = coef[3] * 3;
2427 const ftype b_2 = coef[2];
2428 const ftype c = coef[1];
2429
2430 const ftype deriv[4] = {c, 2 * b_2, a, 0};
2431
2432 const ftype delta_4 = b_2 * b_2 - a * c;
2433
2434 if (delta_4 > 0)
2435 {
2436 const ftype d_2 = Sqrt(delta_4);
2437 const ftype q = -(b_2 + MultSign(d_2, b_2));
2438 const ftype rv0 = q / a;
2439 const ftype rv1 = c / q;
2440 const ftype xa = Min(rv0, rv1);
2441 const ftype xb = Max(rv0, rv1);
2442
2443 const ftype ya = PolynomialEval<3, ftype>(coef, xa);
2444 const ftype yb = PolynomialEval<3, ftype>(coef, xb);
2445
2446 if (!IsDifferentSign(coef[3], ya))
2447 {
2448 ftype root;
2449 root = RootFinder::template FindOpenMin<3, ftype, boundError>(
2450 coef,
2451 deriv,
2452 xa,
2453 ya,
2454 xError);
2455 if (callback(root))
2456 return true;
2457 if constexpr (!boundError)
2458 {
2459 if (IsDifferentSign(ya, yb))
2460 {
2461 ftype defPoly[4];
2462 PolynomialDeflate<3>(defPoly, coef, root);
2463 return QuadraticForEachRoot(callback, defPoly);
2464 }
2465 }
2466 else
2467 {
2468 if (IsDifferentSign(ya, yb))
2469 {
2470 if (callback(RootFinder::template FindClosed<3, ftype, boundError>(
2471 coef,
2472 deriv,
2473 xa,
2474 xb,
2475 ya,
2476 yb,
2477 xError)))
2478 return true;
2479 if (callback(RootFinder::template FindOpenMax<3, ftype, boundError>(
2480 coef,
2481 deriv,
2482 xb,
2483 yb,
2484 xError)))
2485 return true;
2486 return false;
2487 }
2488 }
2489 }
2490 else
2491 {
2492 if (callback(RootFinder::template FindOpenMax<3, ftype, boundError>(
2493 coef,
2494 deriv,
2495 xb,
2496 yb,
2497 xError)))
2498 return true;
2499 }
2500 }
2501 else
2502 {
2503 ftype x_inf = -b_2 / a;
2504 ftype y_inf = PolynomialEval<3, ftype>(coef, x_inf);
2505 if (IsDifferentSign(coef[3], y_inf))
2506 {
2507 if (callback(RootFinder::template FindOpenMax<3, ftype, boundError>(
2508 coef,
2509 deriv,
2510 x_inf,
2511 y_inf,
2512 xError)))
2513 return true;
2514 }
2515 else
2516 {
2517 if (callback(RootFinder::template FindOpenMin<3, ftype, boundError>(
2518 coef,
2519 deriv,
2520 x_inf,
2521 y_inf,
2522 xError)))
2523 return true;
2524 }
2525 }
2526 return false;
2527 }
2528 else
2529 return QuadraticForEachRoot<ftype>(callback, coef);
2530}
2531
2532//-------------------------------------------------------------------------------
2533// Higher Order Polynomials
2534//-------------------------------------------------------------------------------
2535
2536template <int N, typename ftype, bool boundError, typename RootFinder>
2537inline int
2538PolynomialRoots(ftype roots[N], ftype const coef[N + 1], ftype x0, ftype x1, ftype xError)
2539{
2540 if constexpr (N == 1)
2541 return LinearRoot<ftype>(*roots, coef, x0, x1);
2542 else if constexpr (N == 2)
2543 return QuadraticRoots<ftype>(roots, coef, x0, x1);
2544 else if constexpr (N == 3)
2545 return CubicRoots<ftype, boundError, RootFinder>(roots, coef, x0, x1, xError);
2546 else if (coef[N] == 0)
2547 return PolynomialRoots<N - 1, ftype, boundError, RootFinder>(roots, coef, x0, x1, xError);
2548 else
2549 {
2550 ftype y0 = PolynomialEval<N, ftype>(coef, x0);
2551 ftype deriv[N];
2552 PolynomialDerivative<N, ftype>(deriv, coef);
2553 ftype derivRoots[N - 1];
2554 int nd = PolynomialRoots<N - 1, ftype, boundError, RootFinder>(
2555 derivRoots,
2556 deriv,
2557 x0,
2558 x1,
2559 xError);
2560 ftype x[N + 1] = {x0};
2561 ftype y[N + 1] = {y0};
2562 for (int i = 0; i < nd; ++i)
2563 {
2564 x[i + 1] = derivRoots[i];
2565 y[i + 1] = PolynomialEval<N, ftype>(coef, derivRoots[i]);
2566 }
2567 x[nd + 1] = x1;
2568 y[nd + 1] = PolynomialEval<N, ftype>(coef, x1);
2569 int nr = 0;
2570 for (int i = 0; i <= nd; ++i)
2571 {
2572 if (IsDifferentSign(y[i], y[i + 1]))
2573 {
2574 roots[nr++] = RootFinder::template FindClosed<N, ftype, boundError>(
2575 coef,
2576 deriv,
2577 x[i],
2578 x[i + 1],
2579 y[i],
2580 y[i + 1],
2581 xError);
2582 }
2583 }
2584 return nr;
2585 }
2586}
2587
2588//-------------------------------------------------------------------------------
2589
2590template <int N, typename ftype, bool boundError, typename RootFinder>
2591inline int PolynomialRoots(ftype roots[N], ftype const coef[N + 1], ftype xError)
2592{
2593 if constexpr (N == 1)
2594 return LinearRoot<ftype>(*roots, coef);
2595 else if constexpr (N == 2)
2596 return QuadraticRoots<ftype>(roots, coef);
2597 else if constexpr (N == 3)
2598 return CubicRoots<ftype, boundError, RootFinder>(roots, coef, xError);
2599 else if (coef[N] == 0)
2600 return PolynomialRoots<N - 1, ftype, boundError, RootFinder>(roots, coef, xError);
2601 else
2602 {
2603 ftype deriv[N];
2604 PolynomialDerivative<N, ftype>(deriv, coef);
2605 ftype derivRoots[N - 1];
2606 int nd = PolynomialRoots<N - 1, ftype, boundError, RootFinder>(derivRoots, deriv, xError);
2607 if ((N & 1) || ((N & 1) == 0 && nd > 0))
2608 {
2609 int nr = 0;
2610 ftype xa = derivRoots[0];
2611 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2612 if (IsDifferentSign(coef[N], ya) != (N & 1))
2613 {
2614 roots[0] = RootFinder::template FindOpenMin<N, ftype, boundError>(
2615 coef,
2616 deriv,
2617 xa,
2618 ya,
2619 xError);
2620 nr = 1;
2621 }
2622 for (int i = 1; i < nd; ++i)
2623 {
2624 ftype xb = derivRoots[i];
2625 ftype yb = PolynomialEval<N, ftype>(coef, xb);
2626 if (IsDifferentSign(ya, yb))
2627 {
2628 roots[nr++] = RootFinder::template FindClosed<N, ftype, boundError>(
2629 coef,
2630 deriv,
2631 xa,
2632 xb,
2633 ya,
2634 yb,
2635 xError);
2636 }
2637 xa = xb;
2638 ya = yb;
2639 }
2640 if (IsDifferentSign(coef[N], ya))
2641 {
2642 roots[nr++] = RootFinder::template FindOpenMax<N, ftype, boundError>(
2643 coef,
2644 deriv,
2645 xa,
2646 ya,
2647 xError);
2648 }
2649 return nr;
2650 }
2651 else
2652 {
2653 if constexpr (N & 1)
2654 {
2655 roots[0] = RootFinder::template FindOpen<N, ftype, boundError>(coef, deriv, xError);
2656 return 1;
2657 }
2658 else
2659 return 0; // this should not happen
2660 }
2661 }
2662}
2663
2664//-------------------------------------------------------------------------------
2665
2666template <int N, typename ftype, bool boundError, typename RootFinder>
2667inline bool
2668PolynomialFirstRoot(ftype& root, ftype const coef[N + 1], ftype x0, ftype x1, ftype xError)
2669{
2670 if constexpr (N == 1)
2671 return LinearRoot<ftype>(root, coef, x0, x1);
2672 if constexpr (N == 2)
2673 return QuadraticFirstRoot<ftype>(root, coef, x0, x1);
2674 else if constexpr (N == 3)
2675 return CubicFirstRoot<ftype, boundError, RootFinder>(root, coef, x0, x1, xError);
2676 else if (coef[N] == 0)
2677 return PolynomialFirstRoot<N - 1, ftype, boundError, RootFinder>(
2678 root,
2679 coef,
2680 x0,
2681 x1,
2682 xError);
2683 else
2684 {
2685 ftype y0 = PolynomialEval<N, ftype>(coef, x0);
2686 ftype deriv[N];
2687 PolynomialDerivative<N, ftype>(deriv, coef);
2688 bool done = PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2689 [&](ftype xa) {
2690 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2691 if (IsDifferentSign(y0, ya))
2692 {
2693 root = RootFinder::template FindClosed<N, ftype, boundError>(
2694 coef,
2695 deriv,
2696 x0,
2697 xa,
2698 y0,
2699 ya,
2700 xError);
2701 return true;
2702 }
2703 x0 = xa;
2704 y0 = ya;
2705 return false;
2706 },
2707 deriv,
2708 x0,
2709 x1,
2710 xError);
2711 if (done)
2712 return true;
2713 else
2714 {
2715 ftype y1 = PolynomialEval<N, ftype>(coef, x1);
2716 if (IsDifferentSign(y0, y1))
2717 {
2718 root = RootFinder::template FindClosed<N, ftype, boundError>(
2719 coef,
2720 deriv,
2721 x0,
2722 x1,
2723 y0,
2724 y1,
2725 xError);
2726 return true;
2727 }
2728 else
2729 return false;
2730 }
2731 }
2732}
2733
2734//-------------------------------------------------------------------------------
2735
2736template <int N, typename ftype, bool boundError, typename RootFinder>
2737inline bool PolynomialFirstRoot(ftype& root, ftype const coef[N + 1], ftype xError)
2738{
2739 if constexpr (N == 1)
2740 return LinearRoot<ftype>(root, coef);
2741 if constexpr (N == 2)
2742 return QuadraticFirstRoot<ftype>(root, coef);
2743 else if constexpr (N == 3)
2744 return CubicFirstRoot<ftype, boundError, RootFinder>(root, coef, xError);
2745 else if (coef[N] == 0)
2746 return PolynomialFirstRoot<N - 1, ftype, boundError, RootFinder>(root, coef, xError);
2747 else
2748 {
2749 ftype x0 = -std::numeric_limits<ftype>::infinity();
2750 ftype y0 = (((N & 1) == 0) ^ (coef[N] < 0)) ? std::numeric_limits<ftype>::infinity() :
2751 -std::numeric_limits<ftype>::infinity();
2752 bool firstInterval = true;
2753 ftype deriv[N];
2754 PolynomialDerivative<N, ftype>(deriv, coef);
2755 bool done = PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2756 [&](ftype xa) {
2757 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2758 if (IsDifferentSign(y0, ya))
2759 {
2760 if (firstInterval)
2761 {
2762 root = RootFinder::template FindOpenMin<N, ftype, boundError>(
2763 coef,
2764 deriv,
2765 xa,
2766 ya,
2767 xError);
2768 }
2769 else
2770 {
2771 root = RootFinder::template FindClosed<N, ftype, boundError>(
2772 coef,
2773 deriv,
2774 x0,
2775 xa,
2776 y0,
2777 ya,
2778 xError);
2779 }
2780 return true;
2781 }
2782 firstInterval = false;
2783 x0 = xa;
2784 y0 = ya;
2785 return false;
2786 },
2787 deriv,
2788 xError);
2789 if (done)
2790 return true;
2791 else
2792 {
2793 if constexpr ((N & 1) == 1)
2794 {
2795 if (firstInterval)
2796 {
2797 root = RootFinder::template FindOpen<N, ftype, boundError>(coef, deriv, xError);
2798 }
2799 else
2800 {
2801 root = RootFinder::template FindOpenMax<N, ftype, boundError>(
2802 coef,
2803 deriv,
2804 x0,
2805 y0,
2806 xError);
2807 }
2808 return true;
2809 }
2810 else
2811 return false;
2812 }
2813 }
2814}
2815
2816//-------------------------------------------------------------------------------
2817
2818template <int N, typename ftype, bool boundError, typename RootFinder>
2819inline bool PolynomialHasRoot(ftype const coef[N + 1], ftype x0, ftype x1, ftype xError)
2820{
2821 if constexpr (N == 2)
2822 return QuadraticHasRoot<ftype>(coef, x0, x1);
2823 else if constexpr (N == 3)
2824 return CubicHasRoot<ftype>(coef, x0, x1);
2825 else if (coef[N] == 0)
2826 return PolynomialHasRoot<N - 1, ftype, boundError, RootFinder>(coef, x0, x1, xError);
2827 else
2828 {
2829 ftype y0 = PolynomialEval<N, ftype>(coef, x0);
2830 ftype y1 = PolynomialEval<N, ftype>(coef, x1);
2831 if (IsDifferentSign(y0, y1))
2832 return true;
2833 [[maybe_unused]] bool foundRoot = false;
2834 ftype deriv[N];
2835 PolynomialDerivative<N, ftype>(deriv, coef);
2836 return PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2837 [&](ftype xa) {
2838 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2839 return (IsDifferentSign(y0, ya));
2840 },
2841 deriv,
2842 x0,
2843 x1,
2844 xError);
2845 }
2846}
2847
2848//-------------------------------------------------------------------------------
2849
2850template <int N, typename ftype, bool boundError, typename RootFinder>
2851inline bool PolynomialHasRoot(ftype const coef[N + 1], ftype xError)
2852{
2853 if constexpr (N == 2)
2854 return QuadraticHasRoot<ftype>(coef);
2855 else if constexpr (N == 3)
2856 return CubicHasRoot<ftype>(coef);
2857 else if (coef[N] == 0)
2858 return PolynomialHasRoot<N - 1, ftype, boundError, RootFinder>(coef);
2859 else if constexpr ((N & 1) == 1)
2860 return true;
2861 else
2862 {
2863 ftype y0 = (coef[N] < 0) ? -std::numeric_limits<ftype>::infinity() :
2864 std::numeric_limits<ftype>::infinity();
2865 ftype deriv[N];
2866 PolynomialDerivative<N, ftype>(deriv, coef);
2867 return PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2868 [&](ftype xa) {
2869 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2870 if (IsDifferentSign(y0, ya))
2871 return true;
2872 return false;
2873 },
2874 deriv,
2875 xError);
2876 }
2877}
2878
2879//-------------------------------------------------------------------------------
2880
2881template <int N, typename ftype, bool boundError, typename RootFinder, typename RootCallback>
2882inline bool PolynomialForEachRoot(
2883 RootCallback callback,
2884 ftype const coef[N + 1],
2885 ftype x0,
2886 ftype x1,
2887 ftype xError)
2888{
2889 if constexpr (N == 2)
2890 return QuadraticForEachRoot<ftype, RootCallback>(callback, coef, x0, x1);
2891 else if constexpr (N == 3)
2892 return CubicForEachRoot<ftype, boundError, RootFinder, RootCallback>(
2893 callback,
2894 coef,
2895 x0,
2896 x1,
2897 xError);
2898 else if (coef[N] == 0)
2899 return PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder, RootCallback>(
2900 callback,
2901 coef,
2902 x0,
2903 x1,
2904 xError);
2905 else
2906 {
2907 ftype y0 = PolynomialEval<N, ftype>(coef, x0);
2908 ftype deriv[N];
2909 PolynomialDerivative<N, ftype>(deriv, coef);
2910 bool done = PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2911 [&](ftype xa) {
2912 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2913 if (IsDifferentSign(y0, ya))
2914 {
2915 ftype root;
2916 root = RootFinder::template FindClosed<N, ftype, boundError>(
2917 coef,
2918 deriv,
2919 x0,
2920 xa,
2921 y0,
2922 ya,
2923 xError);
2924 if (callback(root))
2925 return true;
2926 }
2927 x0 = xa;
2928 y0 = ya;
2929 return false;
2930 },
2931 deriv,
2932 x0,
2933 x1,
2934 xError);
2935 if (done)
2936 return true;
2937 else
2938 {
2939 ftype y1 = PolynomialEval<N, ftype>(coef, x1);
2940 if (IsDifferentSign(y0, y1))
2941 {
2942 return callback(RootFinder::template FindClosed<N, ftype, boundError>(
2943 coef,
2944 deriv,
2945 x0,
2946 x1,
2947 y0,
2948 y1,
2949 xError));
2950 }
2951 return false;
2952 }
2953 }
2954}
2955
2956//-------------------------------------------------------------------------------
2957
2958template <int N, typename ftype, bool boundError, typename RootFinder, typename RootCallback>
2959inline bool PolynomialForEachRoot(RootCallback callback, ftype const coef[N + 1], ftype xError)
2960{
2961 if constexpr (N == 2)
2962 return QuadraticForEachRoot<ftype, RootCallback>(callback, coef);
2963 else if constexpr (N == 3)
2964 return CubicForEachRoot<ftype, boundError, RootFinder, RootCallback>(
2965 callback,
2966 coef,
2967 xError);
2968 else if (coef[N] == 0)
2969 return PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder, RootCallback>(
2970 callback,
2971 coef,
2972 xError);
2973 else
2974 {
2975 ftype x0 = -std::numeric_limits<ftype>::infinity();
2976 ftype y0 = (((N & 1) == 0) ^ (coef[N] < 0)) ? std::numeric_limits<ftype>::infinity() :
2977 -std::numeric_limits<ftype>::infinity();
2978 bool firstInterval = true;
2979 ftype deriv[N];
2980 PolynomialDerivative<N, ftype>(deriv, coef);
2981 bool done = PolynomialForEachRoot<N - 1, ftype, boundError, RootFinder>(
2982 [&](ftype xa) {
2983 ftype ya = PolynomialEval<N, ftype>(coef, xa);
2984 if (IsDifferentSign(y0, ya))
2985 {
2986 ftype root;
2987 if (firstInterval)
2988 {
2989 root = RootFinder::template FindOpenMin<N, ftype, boundError>(
2990 coef,
2991 deriv,
2992 xa,
2993 ya,
2994 xError);
2995 }
2996 else
2997 {
2998 root = RootFinder::template FindClosed<N, ftype, boundError>(
2999 coef,
3000 deriv,
3001 x0,
3002 xa,
3003 y0,
3004 ya,
3005 xError);
3006 }
3007 if (callback(root))
3008 return true;
3009 }
3010 firstInterval = false;
3011 x0 = xa;
3012 y0 = ya;
3013 return false;
3014 },
3015 deriv,
3016 xError);
3017 if (done)
3018 return true;
3019 else
3020 {
3021 if (IsDifferentSign(y0, coef[N]))
3022 {
3023 ftype root;
3024 if constexpr ((N & 1) == 1)
3025 {
3026 if (firstInterval)
3027 {
3028 root = RootFinder::template FindOpen<N, ftype, boundError>(
3029 coef,
3030 deriv,
3031 xError);
3032 }
3033 else
3034 {
3035 root = RootFinder::template FindOpenMax<N, ftype, boundError>(
3036 coef,
3037 deriv,
3038 x0,
3039 y0,
3040 xError);
3041 }
3042 }
3043 else
3044 {
3045 root = RootFinder::template FindOpenMax<N, ftype, boundError>(
3046 coef,
3047 deriv,
3048 x0,
3049 y0,
3050 xError);
3051 }
3052 return callback(root);
3053 }
3054 return false;
3055 }
3056 }
3057}
3058
3059//-------------------------------------------------------------------------------
3060} // namespace cy
3061
3062#endif // _CY_POLYNOMIAL_H_INCLUDED_
3063
3064// clang-format on
3065
3066} // namespace pbat::math::polynomial::detail
3067
3068#include "pbat/warning/Pop.h"
3069
3070#include "pbat/Aliases.h"
3071
3072#include <array>
3073#include <utility>
3074
3075namespace pbat::math::polynomial {
3076
3077namespace detail {
3078template <class T, auto N>
3079using CArray = T[N];
3080} // namespace detail
3081
3093template <auto N, class TScalar = Scalar>
3094inline bool HasRoot(
3095 std::array<TScalar, N + 1> const& coeffs,
3096 TScalar min = std::numeric_limits<TScalar>::lowest(),
3097 TScalar max = std::numeric_limits<TScalar>::max())
3098{
3099 return detail::cy::PolynomialHasRoot<N, TScalar, true>(
3100 *reinterpret_cast<detail::CArray<TScalar, N + 1> const*>(coeffs.data()),
3101 min,
3102 max,
3103 TScalar(0));
3104}
3105
3117template <auto N, class TScalar = Scalar>
3118inline std::array<TScalar, N> Roots(
3119 std::array<TScalar, N + 1> const& coeffs,
3120 TScalar min = std::numeric_limits<TScalar>::lowest(),
3121 TScalar max = std::numeric_limits<TScalar>::max())
3122{
3123 std::array<TScalar, N> roots;
3124 roots.fill(std::numeric_limits<TScalar>::quiet_NaN());
3125 [[maybe_unused]] int const nRoots = detail::cy::PolynomialRoots<N, TScalar, true>(
3126 *reinterpret_cast<detail::CArray<TScalar, N>*>(roots.data()),
3127 *reinterpret_cast<detail::CArray<TScalar, N + 1> const*>(coeffs.data()),
3128 min,
3129 max,
3130 TScalar(0));
3131 return roots;
3132}
3133
3152template <auto N, class FOnRoot, class TScalar = Scalar>
3153inline bool ForEachRoot(
3154 FOnRoot&& fOnRoot,
3155 std::array<TScalar, N + 1> const& coeffs,
3156 TScalar min = std::numeric_limits<TScalar>::lowest(),
3157 TScalar max = std::numeric_limits<TScalar>::max())
3158{
3159 return detail::cy::PolynomialForEachRoot<N, TScalar, true>(
3160 std::forward<FOnRoot>(fOnRoot),
3161 *reinterpret_cast<detail::CArray<TScalar, N + 1> const*>(coeffs.data()),
3162 min,
3163 max,
3164 TScalar(0));
3165}
3166
3167} // namespace pbat::math::polynomial
3168
3169#endif // PBAT_MATH_POLYNOMIAL_ROOTS_H
pbat::math::polynomial::detail::CArray
T[N] CArray
C-style array alias.
Definition Roots.h:3079
pbat::geometry::sdf::sign
TScalar sign(TScalar x)
Sign function, i.e. .
Definition Primitive.h:32
pbat::math::polynomial::Roots
std::array< TScalar, N > Roots(std::array< TScalar, N+1 > const &coeffs, TScalar min=std::numeric_limits< TScalar >::lowest(), TScalar max=std::numeric_limits< TScalar >::max())
Computes all real roots of a degree N polynomial in the range [min,max].
Definition Roots.h:3118
pbat::math::polynomial::HasRoot
bool HasRoot(std::array< TScalar, N+1 > const &coeffs, TScalar min=std::numeric_limits< TScalar >::lowest(), TScalar max=std::numeric_limits< TScalar >::max())
Check if a polynomial has a root in the range [min,max].
Definition Roots.h:3094
pbat::math::polynomial::ForEachRoot
bool ForEachRoot(FOnRoot &&fOnRoot, std::array< TScalar, N+1 > const &coeffs, TScalar min=std::numeric_limits< TScalar >::lowest(), TScalar max=std::numeric_limits< TScalar >::max())
Computes the each real root of a degree N polynomial in the range [min,max] in increasing order.
Definition Roots.h:3153