1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#pragma once
4
5#include "Prerequisites/BsPrerequisitesUtil.h"
6#include "Math/BsDegree.h"
7#include "Math/BsRadian.h"
8#include "Math/BsVector3.h"
9
10namespace bs
11{
12 /** @addtogroup Implementation
13 * @{
14 */
15
16 namespace impl
17 {
18 /** Helper method for implementing variable-parameter Math::min. */
19 template<typename T>
20 const T& min(const T& in)
21 {
22 return in;
23 }
24
25 /** Helper method for implementing variable-parameter Math::min. */
26 template<typename A, typename B>
27 std::common_type_t<A, B> min(const A& a, const B& b)
28 {
29 return a < b ? a : b;
30 }
31
32 /** Helper method for implementing variable-parameter Math::min. */
33 template<typename A, typename B, typename ...Args>
34 std::common_type_t<A, B, Args...> min(const A& a, const B& b, const Args& ...args)
35 {
36 return min(min(a, b), min(args...));
37 }
38
39 /** Helper method for implementing variable-parameter Math::max. */
40 template<typename T>
41 const T& max(const T& in)
42 {
43 return in;
44 }
45
46 /** Helper method for implementing variable-parameter Math::max. */
47 template<typename A, typename B>
48 std::common_type_t<A, B> max(const A& a, const B& b)
49 {
50 return a > b ? a : b;
51 }
52
53 /** Helper method for implementing variable-parameter Math::max. */
54 template<typename A, typename B, typename ...Args>
55 std::common_type_t<A, B, Args...> max(const A& a, const B& b, const Args& ...args)
56 {
57 return max(max(a, b), max(args...));
58 }
59
60 /** Helper method for implementing Math::gcd. */
61 template <typename A, typename B>
62 std::common_type_t<A, B> gcd(const A& a, const B& b)
63 {
64 return (b == 0) ? a : gcd(b, a % b);
65 }
66
67 /** Helper method for implementing Math::lcm. */
68 template <typename A, typename B>
69 std::common_type_t<A, B> lcm(const A& a, const B& b)
70 {
71 return (a * b) / gcd(a, b);
72 }
73 }
74
75 /** @} */
76
77 /** @addtogroup Math
78 * @{
79 */
80
81 /** Utility class providing common scalar math operations. */
82 class BS_UTILITY_EXPORT Math
83 {
84 public:
85 static constexpr float BIGGEST_FLOAT_SMALLER_THAN_ONE = 0.99999994f;
86
87 /** Inverse cosine. */
88 static Radian acos(float val);
89
90 /** Inverse sine. */
91 static Radian asin(float val);
92
93 /** Inverse tangent. */
94 static Radian atan(float val) { return Radian(std::atan(val)); }
95
96 /** Inverse tangent with two arguments, returns angle between the X axis and the point. */
97 static Radian atan2(float y, float x) { return Radian(std::atan2(y, x)); }
98
99 /** Cosine. */
100 static float cos(const Radian& val) { return (float)std::cos(val.valueRadians()); }
101
102 /** Cosine. */
103 static float cos(float val) { return (float)std::cos(val); }
104
105 /** Sine. */
106 static float sin(const Radian& val) { return (float)std::sin(val.valueRadians()); }
107
108 /** Sine. */
109 static float sin(float val) { return (float)std::sin(val); }
110
111 /** Tangent. */
112 static float tan(const Radian& val) { return (float)std::tan(val.valueRadians()); }
113
114 /** Tangent. */
115 static float tan(float val) { return (float)std::tan(val); }
116
117 /** Square root. */
118 static float sqrt(float val) { return (float)std::sqrt(val); }
119
120 /** Square root. */
121 static Radian sqrt(const Radian& val) { return Radian(std::sqrt(val.valueRadians())); }
122
123 /** Square root. */
124 static Degree sqrt(const Degree& val) { return Degree(std::sqrt(val.valueDegrees())); }
125
126 /** Square root followed by an inverse. */
127 static float invSqrt(float val);
128
129 /** Returns square of the provided value. */
130 static float sqr(float val) { return val * val; }
131
132 /** Returns base raised to the provided power. */
133 static float pow(float base, float exponent) { return (float)std::pow(base, exponent); }
134
135 /** Returns euler number (e) raised to the provided power. */
136 static float exp(float val) { return (float)std::exp(val); }
137
138 /** Returns natural (base e) logarithm of the provided value. */
139 static float log(float val) { return (float)std::log(val); }
140
141 /** Returns base 2 logarithm of the provided value. */
142 static float log2(float val) { return (float)(std::log(val) / LOG2); }
143
144 /** Returns base N logarithm of the provided value. */
145 static float logN(float base, float val) { return (float)(std::log(val) / std::log(base)); }
146
147 /** Returns the sign of the provided value as 1 or -1. */
148 static float sign(float val);
149
150 /** Returns the sign of the provided value as 1 or -1. */
151 static Radian sign(const Radian& val) { return Radian(sign(val.valueRadians())); }
152
153 /** Returns the sign of the provided value as 1 or -1. */
154 static Degree sign(const Degree& val) { return Degree(sign(val.valueDegrees())); }
155
156 /** Returns the absolute value. */
157 static float abs(float val) { return float(std::fabs(val)); }
158
159 /** Returns the absolute value. */
160 static Degree abs(const Degree& val) { return Degree(std::fabs(val.valueDegrees())); }
161
162 /** Returns the absolute value. */
163 static Radian abs(const Radian& val) { return Radian(std::fabs(val.valueRadians())); }
164
165 /** Returns the nearest integer equal or higher to the provided value. */
166 static float ceil(float val) { return (float)std::ceil(val); }
167
168 /**
169 * Returns the nearest integer equal or higher to the provided value. If you are sure the input is positive use
170 * ceilToPosInt() for a slightly faster operation.
171 */
172 static int32_t ceilToInt(float val)
173 {
174 assert(val >= std::numeric_limits<int32_t>::min() && val <= std::numeric_limits<int32_t>::max());
175
176 // Positive values need offset in order to truncate towards positive infinity (cast truncates towards zero)
177 return val >= 0.0f ? (int32_t)(val + BIGGEST_FLOAT_SMALLER_THAN_ONE) : (int32_t)val;
178 }
179
180 /**
181 * Returns the nearest integer equal or higher to the provided value. Value must be non-negative. Slightly faster
182 * than ceilToInt().
183 */
184 static uint32_t ceilToPosInt(float val)
185 {
186 assert(val >= 0 && val <= std::numeric_limits<uint32_t>::max());
187
188 return (uint32_t)(val + BIGGEST_FLOAT_SMALLER_THAN_ONE);
189 }
190
191 /** Returns the integer nearest to the provided value. */
192 static float round(float val) { return (float)std::floor(val + 0.5f); }
193
194 /** Returns the integer nearest to the provided value. */
195 static float fastRound(float val) { return (val >= 0) ? (float)(val + 0.5f) : (float)(val - 0.5f); }
196
197 /**
198 * Returns the integer nearest to the provided value. If you are sure the input is positive use roundToPosInt()
199 * for a slightly faster operation.
200 */
201 static int32_t roundToInt(float val) { return floorToInt(val + 0.5f); }
202
203 /**
204 * Returns the integer nearest to the provided value. Value must be non-negative. Slightly faster than roundToInt().
205 */
206 static uint32_t roundToPosInt(float val) { return floorToPosInt(val + 0.5f); }
207
208 /**
209 * Divides an integer by another integer and returns the result, rounded up. Only works if both integers are
210 * positive.
211 */
212 template<class T>
213 static constexpr T divideAndRoundUp(T n, T d) { return (n + d - 1) / d; }
214
215 /** Returns the nearest integer equal or lower of the provided value. */
216 static float floor(float val) { return (float)std::floor(val); }
217
218 /** Returns the nearest integer equal or lower of the provided value. */
219 static float fastFloor(float val) { return (val >= 0) ? (float)val : (float)val - 1.0f; }
220
221 /**
222 * Returns the nearest integer equal or lower of the provided value. If you are sure the input is positive
223 * use floorToPosInt() for a slightly faster operation.
224 */
225 static int floorToInt(float val)
226 {
227 assert(val >= std::numeric_limits<int32_t>::min() && val <= std::numeric_limits<int32_t>::max());
228
229 // Negative values need offset in order to truncate towards negative infinity (cast truncates towards zero)
230 return val >= 0.0f ? (int32_t)val : (int32_t)(val - BIGGEST_FLOAT_SMALLER_THAN_ONE);
231 }
232
233 /**
234 * Returns the nearest integer equal or lower of the provided value. Value must be non-negative. Slightly faster
235 * than floorToInt().
236 */
237 static uint32_t floorToPosInt(float val)
238 {
239 assert(val >= 0 && val <= std::numeric_limits<uint32_t>::max());
240
241 return (uint32_t)val;
242 }
243
244 /** Rounds @p x to the nearest multiple of @p multiple. */
245 static float roundToMultiple(float x, float multiple)
246 {
247 return floor((x + multiple * 0.5f) / multiple) * multiple;
248 }
249
250 /** Clamp a value within an inclusive range. */
251 template <typename T>
252 static T clamp(T val, T minval, T maxval)
253 {
254 assert (minval <= maxval && "Invalid clamp range");
255 return std::max(std::min(val, maxval), minval);
256 }
257
258 /** Clamp a value within an inclusive range [0..1]. */
259 template <typename T>
260 static T clamp01(T val)
261 {
262 return std::max(std::min(val, (T)1), (T)0);
263 }
264
265 /** Returns the fractional part of a floating point number. */
266 static float frac(float val)
267 {
268 return val - (float)(int32_t)val;
269 }
270
271 /** Returns a floating point remainder for (@p val / @p length). */
272 static float repeat(float val, float length)
273 {
274 return val - floor(val / length) * length;
275 }
276
277 /**
278 * Wraps the value in range [0, length) and reverses the direction every @p length increment. This results in
279 * @p val incrementing until @p length, then decrementing back to 0, and so on.
280 */
281 static float pingPong(float val, float length)
282 {
283 val = repeat(val, length * 2.0f);
284 return length - fabs(val - length);
285 }
286
287 /** Checks if the value is a valid number. */
288 static bool isNaN(float f)
289 {
290 return f != f;
291 }
292
293 /** Check if the value is a prime number. */
294 static bool isPrime(int n)
295 {
296 if (n < 2)
297 return false;
298
299 if (n % 2 == 0)
300 return n == 2;
301
302 if (n % 3 == 0)
303 return n == 3;
304
305 int d = 5;
306 while (d * d <= n)
307 {
308 if (n % d == 0)
309 return false;
310
311 d += 2;
312
313 if (n % d == 0)
314 return false;
315 d += 4;
316 }
317
318 return true;
319 }
320
321 /** Performs smooth Hermite interpolation between values. */
322 static float smoothStep(float val1, float val2, float t)
323 {
324 t = clamp((t - val1) / (val2 - val1), 0.0f, 1.0f);
325 return t * t * (3.0f - 2.0f * t);
326 }
327
328 /**
329 * Performs quintic interpolation where @p val is the value to map onto a quintic S-curve. @p val should be in
330 * [0, 1] range.
331 */
332 static float quintic(float val)
333 {
334 return val * val * val * (val * (val * 6.0f - 15.0f) + 10.0f);
335 }
336
337 /**
338 * Performs cubic interpolation between two values bound between two other values where @p f is the alpha value.
339 * It should range from 0.0f to 1.0f. If it is 0.0f the method returns @p val2. If it is 1.0f it returns @p val3.
340 */
341 static float cubic(float val1, float val2, float val3, float val4, float f)
342 {
343 float t = (val4 - val3) - (val1 - val2);
344 return f * f * f * t + f * f * ((val1 - val2) - t) + f * (val3 - val1) + val2;
345 }
346
347 /** Compare two floats, using tolerance for inaccuracies. */
348 static bool approxEquals(float a, float b,
349 float tolerance = std::numeric_limits<float>::epsilon())
350 {
351 return fabs(b - a) <= tolerance;
352 }
353
354 /** Compare two doubles, using tolerance for inaccuracies. */
355 static bool approxEquals(double a, double b,
356 double tolerance = std::numeric_limits<double>::epsilon())
357 {
358 return fabs(b - a) <= tolerance;
359 }
360
361 /** Compare two 2D vectors, using tolerance for inaccuracies. */
362 static bool approxEquals(const Vector2& a, const Vector2& b,
363 float tolerance = std::numeric_limits<float>::epsilon());
364
365 /** Compare two 3D vectors, using tolerance for inaccuracies. */
366 static bool approxEquals(const Vector3& a, const Vector3& b,
367 float tolerance = std::numeric_limits<float>::epsilon());
368
369 /** Compare two 4D vectors, using tolerance for inaccuracies. */
370 static bool approxEquals(const Vector4& a, const Vector4& b,
371 float tolerance = std::numeric_limits<float>::epsilon());
372
373 /** Compare two quaternions, using tolerance for inaccuracies. */
374 static bool approxEquals(const Quaternion& a, const Quaternion& b,
375 float tolerance = std::numeric_limits<float>::epsilon());
376
377 /** Calculates the tangent space vector for a given set of positions / texture coords. */
378 static Vector3 calculateTriTangent(const Vector3& position1, const Vector3& position2,
379 const Vector3& position3, float u1, float v1, float u2, float v2, float u3, float v3);
380
381 /************************************************************************/
382 /* TRIG APPROXIMATIONS */
383 /************************************************************************/
384
385 /**
386 * Sine function approximation.
387 *
388 * @param[in] val Angle in range [0, pi/2].
389 *
390 * @note Evaluates trigonometric functions using polynomial approximations.
391 */
392 static float fastSin0(const Radian& val) { return (float)fastASin0(val.valueRadians()); }
393
394 /**
395 * Sine function approximation.
396 *
397 * @param[in] val Angle in range [0, pi/2].
398 *
399 * @note Evaluates trigonometric functions using polynomial approximations.
400 */
401 static float fastSin0(float val);
402
403 /**
404 * Sine function approximation.
405 *
406 * @param[in] val Angle in range [0, pi/2].
407 *
408 * @note
409 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastSin0.
410 */
411 static float fastSin1(const Radian& val) { return (float)fastASin1(val.valueRadians()); }
412
413 /**
414 * Sine function approximation.
415 *
416 * @param[in] val Angle in range [0, pi/2].
417 *
418 * @note
419 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastSin0.
420 */
421 static float fastSin1(float val);
422
423 /**
424 * Cosine function approximation.
425 *
426 * @param[in] val Angle in range [0, pi/2].
427 *
428 * @note Evaluates trigonometric functions using polynomial approximations.
429 */
430 static float fastCos0(const Radian& val) { return (float)fastACos0(val.valueRadians()); }
431
432 /**
433 * Cosine function approximation.
434 *
435 * @param[in] val Angle in range [0, pi/2].
436 *
437 * @note Evaluates trigonometric functions using polynomial approximations.
438 */
439 static float fastCos0(float val);
440
441 /**
442 * Cosine function approximation.
443 *
444 * @param[in] val Angle in range [0, pi/2].
445 *
446 * @note
447 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastCos0.
448 */
449 static float fastCos1(const Radian& val) { return (float)fastACos1(val.valueRadians()); }
450
451 /**
452 * Cosine function approximation.
453 *
454 * @param[in] val Angle in range [0, pi/2].
455 *
456 * @note
457 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastCos0.
458 */
459 static float fastCos1(float val);
460
461 /**
462 * Tangent function approximation.
463 *
464 * @param[in] val Angle in range [0, pi/4].
465 *
466 * @note Evaluates trigonometric functions using polynomial approximations.
467 */
468 static float fastTan0(const Radian& val) { return (float)fastATan0(val.valueRadians()); }
469
470 /**
471 * Tangent function approximation.
472 *
473 * @param[in] val Angle in range [0, pi/4].
474 *
475 * @note Evaluates trigonometric functions using polynomial approximations.
476 */
477 static float fastTan0(float val);
478
479 /**
480 * Tangent function approximation.
481 *
482 * @param[in] val Angle in range [0, pi/4].
483 *
484 * @note
485 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastTan0.
486 */
487 static float fastTan1(const Radian& val) { return (float)fastATan1(val.valueRadians()); }
488
489 /**
490 * Tangent function approximation.
491 *
492 * @param[in] val Angle in range [0, pi/4].
493 *
494 * @note
495 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastTan0.
496 */
497 static float fastTan1(float val);
498
499 /**
500 * Inverse sine function approximation.
501 *
502 * @param[in] val Angle in range [0, 1].
503 *
504 * @note Evaluates trigonometric functions using polynomial approximations.
505 */
506 static float fastASin0(const Radian& val) { return (float)fastASin0(val.valueRadians()); }
507
508 /**
509 * Inverse sine function approximation.
510 *
511 * @param[in] val Angle in range [0, 1].
512 *
513 * @note Evaluates trigonometric functions using polynomial approximations.
514 */
515 static float fastASin0(float val);
516
517 /**
518 * Inverse sine function approximation.
519 *
520 * @param[in] val Angle in range [0, 1].
521 *
522 * @note
523 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastASin0.
524 */
525 static float fastASin1(const Radian& val) { return (float)fastASin1(val.valueRadians()); }
526
527 /**
528 * Inverse sine function approximation.
529 *
530 * @param[in] val Angle in range [0, 1].
531 *
532 * @note
533 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastASin0.
534 */
535 static float fastASin1(float val);
536
537 /**
538 * Inverse cosine function approximation.
539 *
540 * @param[in] val Angle in range [0, 1].
541 *
542 * @note Evaluates trigonometric functions using polynomial approximations.
543 */
544 static float fastACos0(const Radian& val) { return (float)fastACos0(val.valueRadians()); }
545
546 /**
547 * Inverse cosine function approximation.
548 *
549 * @param[in] val Angle in range [0, 1].
550 *
551 * @note Evaluates trigonometric functions using polynomial approximations.
552 */
553 static float fastACos0(float val);
554
555 /**
556 * Inverse cosine function approximation.
557 *
558 * @param[in] val Angle in range [0, 1].
559 *
560 * @note
561 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastACos0.
562 */
563 static float fastACos1(const Radian& val) { return (float)fastACos1(val.valueRadians()); }
564
565 /**
566 * Inverse cosine function approximation.
567 *
568 * @param[in] val Angle in range [0, 1].
569 *
570 * @note
571 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastACos0.
572 */
573 static float fastACos1(float val);
574
575 /**
576 * Inverse tangent function approximation.
577 *
578 * @param[in] val Angle in range [-1, 1].
579 *
580 * @note Evaluates trigonometric functions using polynomial approximations.
581 */
582 static float fastATan0(const Radian& val) { return (float)fastATan0(val.valueRadians()); }
583
584 /**
585 * Inverse tangent function approximation.
586 *
587 * @param[in] val Angle in range [-1, 1].
588 *
589 * @note Evaluates trigonometric functions using polynomial approximations.
590 */
591 static float fastATan0(float val);
592
593 /**
594 * Inverse tangent function approximation.
595 *
596 * @param[in] val Angle in range [-1, 1].
597 *
598 * @note
599 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastATan0.
600 */
601 static float fastATan1(const Radian& val) { return (float)fastATan1(val.valueRadians()); }
602
603 /**
604 * Inverse tangent function approximation.
605 *
606 * @param[in] val Angle in range [-1, 1].
607 *
608 * @note
609 * Evaluates trigonometric functions using polynomial approximations. Slightly better (and slower) than fastATan0.
610 */
611 static float fastATan1(float val);
612
613 /**
614 * Linearly interpolates between the two values using @p t. t should be in [0, 1] range, where t = 0 corresponds
615 * to @p min value, while t = 1 corresponds to @p max value.
616 */
617 template <typename T>
618 static T lerp(float t, T min, T max)
619 {
620 return (1.0f - t) * min + t * max;
621 }
622 /**
623 * Determines the position of a value between two other values. Returns 0 if @p value is less or equal than
624 * @p min, 1 if @p value is equal or greater than @p max, and value in range (0, 1) otherwise.
625 */
626 template <typename T>
627 static float invLerp(T val, T min, T max)
628 {
629 return clamp01((val - min) / std::max(max - min, 0.0001F));
630 }
631
632 /** Returns the minimum value of the two provided. */
633 template <typename A, typename B>
634 static std::common_type_t<A, B> min(const A& a, const B& b)
635 {
636 return impl::min(a, b);
637 }
638
639 /** Returns the minimum value of all the values provided. */
640 template <typename A, typename B, typename... Args>
641 static std::common_type_t<A, B, Args...> min(const A& a, const B& b, const Args&... args)
642 {
643 return impl::min(a, b, args...);
644 }
645
646 /** Returns the maximum value of the two provided. */
647 template <typename A, typename B>
648 static std::common_type_t<A, B> max(const A& a, const B& b)
649 {
650 return impl::max(a, b);
651 }
652
653 /** Returns the maximum value of all the values provided. */
654 template <typename A, typename B, typename... Args>
655 static std::common_type_t<A, B, Args...> max(const A& a, const B& b, const Args&... args)
656 {
657 return impl::max(a, b, args...);
658 }
659
660 /** Return the greater common divisor between two values. */
661 template <typename A, typename B>
662 static std::common_type_t<A, B> gcd(const A& a, const B& b)
663 {
664 return impl::gcd(a, b);
665 }
666
667 /** Return the least common multiple between two values. */
668 template <typename A, typename B>
669 static std::common_type_t<A, B> lcm(const A& a, const B& b)
670 {
671 return impl::lcm(a, b);
672 }
673
674 /**
675 * Solves the linear equation with the parameters A, B. Returns number of roots found and the roots themselves will
676 * be output in the @p roots array.
677 *
678 * @param[in] A First variable.
679 * @param[in] B Second variable.
680 * @param[out] roots Must be at least size of 1.
681 *
682 * @note Only returns real roots.
683 */
684 template <typename T>
685 static UINT32 solveLinear(T A, T B, T* roots)
686 {
687 if (!approxEquals(A, (T)0))
688 {
689 roots[0] = -B / A;
690 return 1;
691 }
692
693 roots[0] = 0.0f;
694 return 1;
695 }
696
697 /**
698 * Solves the quadratic equation with the parameters A, B, C. Returns number of roots found and the roots themselves
699 * will be output in the @p roots array.
700 *
701 * @param[in] A First variable.
702 * @param[in] B Second variable.
703 * @param[in] C Third variable.
704 * @param[out] roots Must be at least size of 2.
705 *
706 * @note Only returns real roots.
707 */
708 template <typename T>
709 static UINT32 solveQuadratic(T A, T B, T C, T* roots)
710 {
711 if (!approxEquals(A, (T)0))
712 {
713 T p = B / (2 * A);
714 T q = C / A;
715 T D = p * p - q;
716
717 if (!approxEquals(D, (T)0))
718 {
719 if (D < (T)0)
720 return 0;
721
722 T sqrtD = sqrt(D);
723 roots[0] = sqrtD - p;
724 roots[1] = -sqrtD - p;
725
726 return 2;
727 }
728 else
729 {
730 roots[0] = -p;
731 roots[1] = -p;
732
733 return 1;
734 }
735 }
736 else
737 {
738 return solveLinear(B, C, roots);
739 }
740 }
741
742 /**
743 * Solves the cubic equation with the parameters A, B, C, D. Returns number of roots found and the roots themselves
744 * will be output in the @p roots array.
745 *
746 * @param[in] A First variable.
747 * @param[in] B Second variable.
748 * @param[in] C Third variable.
749 * @param[in] D Fourth variable.
750 * @param[out] roots Must be at least size of 3.
751 *
752 * @note Only returns real roots.
753 */
754 template <typename T>
755 static UINT32 solveCubic(T A, T B, T C, T D, T* roots)
756 {
757 static const T THIRD = (1 / (T)3);
758
759 T invA = 1 / A;
760 A = B * invA;
761 B = C * invA;
762 C = D * invA;
763
764 T sqA = A * A;
765 T p = THIRD * (-THIRD * sqA + B);
766 T q = ((T)0.5) * ((2 / (T)27) * A * sqA - THIRD * A * B + C);
767
768 T cbp = p * p * p;
769 D = q * q + cbp;
770
771 UINT32 numRoots = 0;
772 if (!approxEquals(D, (T)0))
773 {
774 if (D < 0.0)
775 {
776 T phi = THIRD * ::acos(-q / sqrt(-cbp));
777 T t = 2 * sqrt(-p);
778
779 roots[0] = t * cos(phi);
780 roots[1] = -t * cos(phi + PI * THIRD);
781 roots[2] = -t * cos(phi - PI * THIRD);
782
783 numRoots = 3;
784 }
785 else
786 {
787 T sqrtD = sqrt(D);
788 T u = cbrt(sqrtD + fabs(q));
789
790 if (q > (T)0)
791 roots[0] = -u + p / u;
792 else
793 roots[0] = u - p / u;
794
795 numRoots = 1;
796 }
797 }
798 else
799 {
800 if (!approxEquals(q, (T)0))
801 {
802 T u = cbrt(-q);
803 roots[0] = 2 * u;
804 roots[1] = -u;
805
806 numRoots = 2;
807 }
808 else
809 {
810 roots[0] = 0.0f;
811 numRoots = 1;
812 }
813 }
814
815 T sub = THIRD * A;
816 for (UINT32 i = 0; i < numRoots; i++)
817 roots[i] -= sub;
818
819 return numRoots;
820 }
821
822 /**
823 * Solves the quartic equation with the parameters A, B, C, D, E. Returns number of roots found and the roots
824 * themselves will be output in the @p roots array.
825 *
826 * @param[in] A First variable.
827 * @param[in] B Second variable.
828 * @param[in] C Third variable.
829 * @param[in] D Fourth variable.
830 * @param[in] E Fifth variable.
831 * @param[out] roots Must be at least size of 4.
832 *
833 * @note Only returns real roots.
834 */
835 template <typename T>
836 static UINT32 solveQuartic(T A, T B, T C, T D, T E, T* roots)
837 {
838 T invA = 1 / A;
839 A = B * invA;
840 B = C * invA;
841 C = D * invA;
842 D = E * invA;
843
844 T sqA = A*A;
845 T p = -(3 / (T)8) * sqA + B;
846 T q = (1 / (T)8) * sqA * A - (T)0.5 * A * B + C;
847 T r = -(3 / (T)256) * sqA * sqA + (1 / (T)16) * sqA * B - (1 / (T)4) * A * C + D;
848
849 UINT32 numRoots = 0;
850 if (!approxEquals(r, (T)0))
851 {
852 T cubicA = 1;
853 T cubicB = -(T)0.5 * p ;
854 T cubicC = -r;
855 T cubicD = (T)0.5 * r * p - (1 / (T)8) * q * q;
856
857 solveCubic(cubicA, cubicB, cubicC, cubicD, roots);
858 T z = roots[0];
859
860 T u = z * z - r;
861 T v = 2 * z - p;
862
863 if (approxEquals(u, T(0)))
864 u = 0;
865 else if (u > 0)
866 u = sqrt(u);
867 else
868 return 0;
869
870 if (approxEquals(v, T(0)))
871 v = 0;
872 else if (v > 0)
873 v = sqrt(v);
874 else
875 return 0;
876
877 T quadraticA = 1;
878 T quadraticB = q < 0 ? -v : v;
879 T quadraticC = z - u;
880
881 numRoots = solveQuadratic(quadraticA, quadraticB, quadraticC, roots);
882
883 quadraticA = 1;
884 quadraticB = q < 0 ? v : -v;
885 quadraticC = z + u;
886
887 numRoots += solveQuadratic(quadraticA, quadraticB, quadraticC, roots + numRoots);
888 }
889 else
890 {
891 numRoots = solveCubic(q, p, (T)0, (T)1, roots);
892 roots[numRoots++] = 0;
893 }
894
895 T sub = (1/(T)4) * A;
896 for (UINT32 i = 0; i < numRoots; i++)
897 roots[i] -= sub;
898
899 return numRoots;
900 }
901
902 /**
903 * Evaluates a cubic Hermite curve at a specific point.
904 *
905 * @param[in] t Parameter that at which to evaluate the curve, in range [0, 1].
906 * @param[in] pointA Starting point (at t=0).
907 * @param[in] pointB Ending point (at t=1).
908 * @param[in] tangentA Starting tangent (at t=0).
909 * @param[in] tangentB Ending tangent (at t = 1).
910 * @return Evaluated value at @p t.
911 */
912 template<class T>
913 static T cubicHermite(float t, const T& pointA, const T& pointB, const T& tangentA, const T& tangentB)
914 {
915 float t2 = t * t;
916 float t3 = t2 * t;
917
918 float a = 2 * t3 - 3 * t2 + 1;
919 float b = t3 - 2 * t2 + t;
920 float c = -2 * t3 + 3 * t2;
921 float d = t3 - t2;
922
923 return a * pointA + b * tangentA + c * pointB + d * tangentB;
924 }
925
926 /**
927 * Evaluates the first derivative of a cubic Hermite curve at a specific point.
928 *
929 * @param[in] t Parameter that at which to evaluate the curve, in range [0, 1].
930 * @param[in] pointA Starting point (at t=0).
931 * @param[in] pointB Ending point (at t=1).
932 * @param[in] tangentA Starting tangent (at t=0).
933 * @param[in] tangentB Ending tangent (at t = 1).
934 * @return Evaluated value at @p t.
935 */
936 template<class T>
937 static T cubicHermiteD1(float t, const T& pointA, const T& pointB, const T& tangentA, const T& tangentB)
938 {
939 float t2 = t * t;
940
941 float a = 6 * t2 - 6 * t;
942 float b = 3 * t2 - 4 * t + 1;
943 float c = -6 * t2 + 6 * t;
944 float d = 3 * t2 - 2 * t;
945
946 return a * pointA + b * tangentA + c * pointB + d * tangentB;
947 }
948
949 /**
950 * Calculates coefficients needed for evaluating a cubic curve in Hermite form. Assumes @p t has been normalized is
951 * in range [0, 1]. Tangents must be scaled by the length of the curve (length is the maximum value of @p t before
952 * it was normalized).
953 *
954 * @param[in] pointA Starting point (at t=0).
955 * @param[in] pointB Ending point (at t=1).
956 * @param[in] tangentA Starting tangent (at t=0).
957 * @param[in] tangentB Ending tangent (at t = 1).
958 * @param[out] coefficients Four coefficients for the cubic curve, in order [t^3, t^2, t, 1].
959 */
960 template<class T>
961 static void cubicHermiteCoefficients(const T& pointA, const T& pointB, const T& tangentA, const T& tangentB,
962 T (&coefficients)[4])
963 {
964 T diff = pointA - pointB;
965
966 coefficients[0] = 2 * diff + tangentA + tangentB;
967 coefficients[1] = -3 * diff - 2 * tangentA - tangentB;
968 coefficients[2] = tangentA;
969 coefficients[3] = pointA;
970 }
971
972 /**
973 * Calculates coefficients needed for evaluating a cubic curve in Hermite form. Assumes @p t is in range
974 * [0, @p length]. Tangents must not be scaled by @p length.
975 *
976 * @param[in] pointA Starting point (at t=0).
977 * @param[in] pointB Ending point (at t=length).
978 * @param[in] tangentA Starting tangent (at t=0).
979 * @param[in] tangentB Ending tangent (at t=length).
980 * @param[in] length Maximum value the curve will be evaluated at.
981 * @param[out] coefficients Four coefficients for the cubic curve, in order [t^3, t^2, t, 1].
982 */
983 template<class T>
984 static void cubicHermiteCoefficients(const T& pointA, const T& pointB, const T& tangentA, const T& tangentB,
985 float length, T (&coefficients)[4])
986 {
987 float length2 = length * length;
988 float invLength2 = 1.0f / length2;
989 float invLength3 = 1.0f / (length2 * length);
990
991 T scaledTangentA = tangentA * length;
992 T scaledTangentB = tangentB * length;
993
994 T diff = pointA - pointB;
995
996 coefficients[0] = (2 * diff + scaledTangentA + scaledTangentB) * invLength3;
997 coefficients[1] = (-3 * diff - 2 * scaledTangentA - scaledTangentB) * invLength2;
998 coefficients[2] = tangentA;
999 coefficients[3] = pointA;
1000 }
1001
1002 /**
1003 * Calculates the Romberg Integration.
1004 *
1005 * @param[in] a Lower bound.
1006 * @param[in] b Upper bound.
1007 * @param[in] order Order of the function.
1008 * @param[in] integrand Function to integrate.
1009 * @return Integrated function.
1010 */
1011 template <typename T>
1012 static T rombergIntegration(T a, T b, int order, const std::function<T(T)> integrand)
1013 {
1014 T h[order + 1];
1015 T r[order + 1][order + 1];
1016
1017 for (int i = 1; i < order + 1; ++i)
1018 h[i] = (b - a) / Math::pow(2, i - 1);
1019
1020 r[1][1] = h[1] / 2 * (integrand(a) + integrand(b));
1021
1022 for (int i = 2; i < order + 1; ++i)
1023 {
1024 T coeff = 0;
1025 for (int k = 1; k <= Math::pow(2, i - 2); ++k)
1026 coeff += integrand(a + (2 * k - 1) * h[i]);
1027
1028 r[i][1] = 0.5 * (r[i - 1][1] + h[i - 1] * coeff);
1029 }
1030
1031 for (int i = 2; i < order + 1; ++i)
1032 {
1033 for (int j = 2; j <= i; ++j)
1034 r[i][j] = r[i][j - 1] + (r[i][j - 1] - r[i - 1][j - 1]) / (Math::pow(4, j - 1) - 1);
1035 }
1036
1037 return r[order][order];
1038 }
1039
1040 /**
1041 * Calculates the Gaussian Quadrature.
1042 *
1043 * @param[in] a Lower bound.
1044 * @param[in] b Upper bound.
1045 * @param[in] roots Roots of the function.
1046 * @param[in] coefficients Coefficients of the function.
1047 * @param[in] integrand Function to integrate.
1048 * @return Gaussian Quadrature integration.
1049 */
1050 template <typename T>
1051 static T gaussianQuadrature(T a, T b, T* roots, T* coefficients, const std::function <T(T)>& integrand)
1052 {
1053 const T half = (T)0.5;
1054 const T radius = half * (b - a);
1055 const T center = half * (b + a);
1056 T res = (T)0;
1057
1058 for (UINT32 i = 0; i < sizeof(roots) / sizeof(*roots); ++i)
1059 res += coefficients[i] * integrand(radius * roots[i] + center);
1060
1061 res *= radius;
1062
1063 return res;
1064 }
1065
1066 static constexpr float POS_INFINITY = std::numeric_limits<float>::infinity();
1067 static constexpr float NEG_INFINITY = -std::numeric_limits<float>::infinity();
1068 static constexpr float PI = 3.14159265358979323846f;
1069 static constexpr float TWO_PI = (float)(2.0f * PI);
1070 static constexpr float HALF_PI = (float)(0.5f * PI);
1071 static constexpr float QUARTER_PI = (float)(0.25f * PI);
1072 static constexpr float INV_PI = (float)(1 / PI);
1073 static constexpr float INV_HALF_PI = (float)(INV_PI / 2);
1074 static constexpr float INV_TWO_PI = (float)(2.0f * INV_PI);
1075 static constexpr float DEG2RAD = PI / 180.0f;
1076 static constexpr float RAD2DEG = 180.0f / PI;
1077 static constexpr float SQRT2 = 1.4142135623730951f;
1078 static constexpr float INV_SQRT2 = (float)(1.0f / SQRT2);
1079 static const float LOG2;
1080 };
1081
1082 /** @} */
1083}
1084