1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file MathFunc.cpp
16         @author Jukka Jyl�nki
17         @brief Common mathematical functions. */
18 #include "MathFunc.h"
19 #include "SSEMath.h"
20 #ifdef MATH_ENABLE_STL_SUPPORT
21 #include <utility>
22 #include <algorithm>
23 #endif
24
25 #include "myassert.h"
26 #include "float2.h"
27 #include "grisu3.h"
28
29 #ifdef WIN32
30 #include "../Math/InclWindows.h"
31 #endif
32
33 #ifdef MATH_SSE2
34 #include "sse_mathfun.h"
35 #endif
36
37 MATH_BEGIN_NAMESPACE
38
39 bool mathBreakOnAssume = false;
40
41 void SetMathBreakOnAssume(bool isEnabled)
42 {
43         mathBreakOnAssume = isEnabled;
44 }
45
46 /// Returns the current state of the math break-on-assume flag.
47 bool MathBreakOnAssume()
48 {
49         return mathBreakOnAssume;
50 }
51
52 bool AssumeFailed()
53 {
54         if (mathBreakOnAssume)
55         {
56 #if defined(WIN32) && !defined(WIN8RT) // Win8 metro apps don't have DebugBreak.
57                 DebugBreak();
58 #endif
59         }
60         return mathBreakOnAssume;
61 }
62
63 #if defined(__EMSCRIPTEN__) && !defined(MATH_USE_SINCOS_LOOKUPTABLE)
64 // On Emscripten using lookup tables has been profiled to be significantly faster.
65 #define MATH_USE_SINCOS_LOOKUPTABLE
66 #endif
67
68 #ifdef MATH_USE_SINCOS_LOOKUPTABLE
69
70 // A lookup table implementation adapted from http://www.flipcode.com/archives/Fast_Trigonometry_Functions_Using_Lookup_Tables.shtml
71 #define MAX_CIRCLE_ANGLE           65536
72 #define HALF_MAX_CIRCLE_ANGLE     (MAX_CIRCLE_ANGLE/2)
73 #define QUARTER_MAX_CIRCLE_ANGLE  (MAX_CIRCLE_ANGLE/4)
74 #define MASK_MAX_CIRCLE_ANGLE     (MAX_CIRCLE_ANGLE - 1)
75 #define PI                        3.14159265358979323846f
76 static float fast_cossin_table[MAX_CIRCLE_ANGLE];           // Declare table of fast cosinus and sinus
77
78 class Init_fast_cossin_table
79 {
80 public:
81         Init_fast_cossin_table()
82         {
83                 // Build cossin table
84                 for(int i = 0; i < MAX_CIRCLE_ANGLE; i++)
85                         fast_cossin_table[i] = (float)sin((double)i * PI / HALF_MAX_CIRCLE_ANGLE);
86         }
87 };
88 Init_fast_cossin_table static_initializer;
89
90 static inline float sin_lookuptable(float n)
91 {
92         int i = (int)(n * (HALF_MAX_CIRCLE_ANGLE / PI));
93         if (i < 0) return fast_cossin_table[MAX_CIRCLE_ANGLE - ((-i) & MASK_MAX_CIRCLE_ANGLE)];
94         else return fast_cossin_table[i & MASK_MAX_CIRCLE_ANGLE];
95 }
96
97 static inline float cos_lookuptable(float n)
98 {
99         int i = (int)(n * (HALF_MAX_CIRCLE_ANGLE / PI));
100         if (i < 0) return fast_cossin_table[(QUARTER_MAX_CIRCLE_ANGLE - i) & MASK_MAX_CIRCLE_ANGLE];
101         else return fast_cossin_table[(QUARTER_MAX_CIRCLE_ANGLE + i) & MASK_MAX_CIRCLE_ANGLE];
102 }
103
104 static inline void sincos_lookuptable(float n, float &sinOut, float &cosOut)
105 {
106         int i = (int)(n * (HALF_MAX_CIRCLE_ANGLE / PI));
107         i = (i >= 0) ? (i & MASK_MAX_CIRCLE_ANGLE) : (MAX_CIRCLE_ANGLE - ((-i) & MASK_MAX_CIRCLE_ANGLE));
108         sinOut = fast_cossin_table[i];
109         cosOut = fast_cossin_table[(MAX_CIRCLE_ANGLE + QUARTER_MAX_CIRCLE_ANGLE - i) & MASK_MAX_CIRCLE_ANGLE];
110 }
111
112 #endif
113
114 #ifdef MATH_SSE2
115 static const __m128 pi2 = _mm_set1_ps(2.f*pi);
116 #endif
117
118 float Sin(float angleRadians)
119 {
120 #ifdef MATH_USE_SINCOS_LOOKUPTABLE
121         return sin_lookuptable(angleRadians);
122 #elif defined(MATH_SSE2)
123         // Do range reduction by 2pi before calling sin - this enchances precision of sin_ps a lot
124         return s4f_x(sin_ps(modf_ps(setx_ps(angleRadians), pi2)));
125 #else
126         return sinf(angleRadians);
127 #endif
128 }
129
130 float Cos(float angleRadians)
131 {
132 #ifdef MATH_USE_SINCOS_LOOKUPTABLE
133         return cos_lookuptable(angleRadians);
134 #elif defined(MATH_SSE2)
135         // Do range reduction by 2pi before calling cos - this enchances precision of cos_ps a lot
136         return s4f_x(cos_ps(modf_ps(setx_ps(angleRadians), pi2)));
137 #else
138         return cosf(angleRadians);
139 #endif
140 }
141
142 float Tan(float angleRadians)
143 {
144         return tanf(angleRadians);
145 }
146
147 void SinCos(float angleRadians, float &outSin, float &outCos)
148 {
149 #ifdef MATH_USE_SINCOS_LOOKUPTABLE
150         return sincos_lookuptable(angleRadians, outSin, outCos);
151 #elif defined(MATH_SSE2)
152         __m128 angle = modf_ps(setx_ps(angleRadians), pi2);
153         __m128 sin, cos;
154         sincos_ps(angle, &sin, &cos);
155         outSin = s4f_x(sin);
156         outCos = s4f_x(cos);
157 #else
158         outSin = Sin(angleRadians);
159         outCos = Cos(angleRadians);
160 #endif
161 }
162
163 void SinCos2(const float4 &angleRadians, float4 &outSin, float4 &outCos)
164 {
165 #ifdef MATH_SSE2
166         __m128 angle = modf_ps(angleRadians.v, pi2);
167         sincos_ps(angle, &outSin.v, &outCos.v);
168 #else
169         SinCos(angleRadians.x, outSin.x, outCos.x);
170         SinCos(angleRadians.y, outSin.y, outCos.y);
171 #endif
172 }
173
174 void SinCos3(const float4 &angleRadians, float4 &outSin, float4 &outCos)
175 {
176 #ifdef MATH_SSE2
177         __m128 angle = modf_ps(angleRadians.v, pi2);
178         sincos_ps(angle, &outSin.v, &outCos.v);
179 #else
180         SinCos(angleRadians.x, outSin.x, outCos.x);
181         SinCos(angleRadians.y, outSin.y, outCos.y);
182         SinCos(angleRadians.z, outSin.z, outCos.z);
183 #endif
184 }
185
186 void SinCos4(const float4 &angleRadians, float4 &outSin, float4 &outCos)
187 {
188 #ifdef MATH_SSE2
189         __m128 angle = modf_ps(angleRadians.v, pi2);
190         sincos_ps(angle, &outSin.v, &outCos.v);
191 #else
192         SinCos(angleRadians.x, outSin.x, outCos.x);
193         SinCos(angleRadians.y, outSin.y, outCos.y);
194         SinCos(angleRadians.z, outSin.z, outCos.z);
195         SinCos(angleRadians.w, outSin.w, outCos.w);
196 #endif
197 }
198
199 float Asin(float x)
200 {
201         return asinf(x);
202 }
203
204 float Acos(float x)
205 {
206         return acosf(x);
207 }
208
209 float Atan(float x)
210 {
211         return atanf(x);
212 }
213
214 float Atan2(float y, float x)
215 {
216         return atan2f(y, x);
217 }
218
219 float Sinh(float x)
220 {
221         return sinhf(x);
222 }
223
224 float Cosh(float x)
225 {
226         return coshf(x);
227 }
228
229 float Tanh(float x)
230 {
231         return tanhf(x);
232 }
233
234 bool IsPow2(u32 number)
235 {
236         return (number & (number-1)) == 0;
237 }
238
239 bool IsPow2(u64 number)
240 {
241         return (number & (number-1)) == 0;
242 }
243
244 u32 RoundUpPow2(u32 x)
245 {
246         assert(sizeof(u32) == 4);
247         --x;
248         x |= x >> 1;
249         x |= x >> 2;
250         x |= x >> 4;
251         x |= x >> 8;
252         x |= x >> 16;
253         ++x;
254
255         return x;
256 }
257
258 u64 RoundUpPow2(u64 x)
259 {
260         assert(sizeof(u64) == 8);
261         --x;
262         x |= x >> 1;
263         x |= x >> 2;
264         x |= x >> 4;
265         x |= x >> 8;
266         x |= x >> 16;
267         x |= x >> 32;
268         ++x;
269
270         return x;
271 }
272
273 u32 RoundDownPow2(u32 x)
274 {
275         assert(sizeof(u32) == 4);
276         x |= x >> 1;
277         x |= x >> 2;
278         x |= x >> 4;
279         x |= x >> 8;
280         x |= x >> 16;
281         return x - (x >> 1);
282 }
283
284 u64 RoundDownPow2(u64 x)
285 {
286         assert(sizeof(u64) == 8);
287         x |= x >> 1;
288         x |= x >> 2;
289         x |= x >> 4;
290         x |= x >> 8;
291         x |= x >> 16;
292         x |= x >> 32;
293         return x - (x >> 1);
294 }
295
296 int RoundIntUpToMultipleOfPow2(int x, int n)
297 {
298         assert(IsPow2(n));
299         return (x + n-1) & ~(n-1);
300 }
301
302 s64 RoundIntUpToMultipleOfPow2(s64 x, s64 n)
303 {
304         assert(IsPow2(n));
305         return (x + n-1) & ~(n-1);
306 }
307
308 float Pow(float base, float exponent)
309 {
310         return pow(base, exponent);
311 }
312
313 float Exp(float exponent)
314 {
315         return exp(exponent);
316 }
317
318 float Log(float base, float value)
319 {
320         return log(value) / log(base);
321 }
322
323 float Log2(float value)
324 {
325         return Log(2.f, value);
326 }
327
328 float Ln(float value)
329 {
330         return log(value);
331 }
332
333 float Log10(float value)
334 {
335         return Log(10.f, value);
336 }
337
338 float Ceil(float x)
339 {
340         return ceilf(x);
341 }
342
343 int CeilInt(float x)
344 {
345         return (int)ceilf(x);
346 }
347
348 float Floor(float x)
349 {
350         return floorf(x);
351 }
352
353 int FloorInt(float x)
354 {
355         return (int)floorf(x);
356 }
357
358 float Round(float x)
359 {
360         return Floor(x+0.5f);
361 }
362
363 int RoundInt(float x)
364 {
365         return (int)Round(x);
366 }
367
368 float Sign(float x)
369 {
370         return x >= 0.f ? 1.f : -1.f;
371 }
372
373 float SignOrZero(float x, float epsilon)
374 {
375         return Abs(x) <= epsilon ? 0.f : Sign(x);
376 }
377
378 float Lerp(float a, float b, float t)
379 {
380         return a + t * (b-a);
381 }
382
383 float LerpMod(float a, float b, float mod, float t)
384 {
385         a = ModPos(a, mod);
386         b = ModPos(b, mod);
387         if (Abs(b-a) * 2.f <= mod)
388                 return Lerp(a, b, t);
389         else
390         {
391                 if (a < b)
392                         return ModPos(Lerp(a + mod, b, t), mod);
393                 else
394                         return ModPos(Lerp(a, b + mod, t), mod);
395         }
396 }
397
398 float InvLerp(float a, float b, float x)
399 {
400         assume(Abs(b-a) > eps);
401         return (x - a) / (b - a);
402 }
403
404 float Step(float y, float x)
405 {
406         return (x >= y) ? 1.f : 0.f;
407 }
408
409 float SmoothStep(float min, float max, float x)
410 {
411         return x <= min ? 0.f : (x >= max ? 1.f : (x - min) / (max - min));
412 }
413
414 float PingPongMod(float x, float mod)
415 {
416         x = Mod(x, mod * 2.f);
417         return x >= mod ? (2.f * mod - x) : x;
418 }
419
420 float Mod(float x, float mod)
421 {
422         return fmod(x, mod);
423 }
424
425 float Mod(float x, int mod)
426 {
427         ///@todo Optimize.
428         return fmod(x, (float)mod);
429 }
430
431 float ModPos(float x, float mod)
432 {
433         float m = fmod(x, mod);
434         return m >= 0.f ? m : (m + mod);
435 }
436
437 float ModPos(float x, int mod)
438 {
439         ///@todo Optimize.
440         return ModPos(x, (float)mod);
441 }
442
443 float Frac(float x)
444 {
445         return x - Floor(x);
446 }
447
448 /** Uses a recursive approach, not the fastest/brightest method.
449         Note that 13! = 6227020800 overflows already.
450         @return n! = n * (n-1) * (n-2) * ... * 1. */
451 int Factorial(int n)
452 {
453         int result = 1;
454         for(int i = 2; i <= n; i++)
455                 result *= i;
456         return result;
457 }
458
459 /** @return Binomial coefficients with recursion, i.e. n choose k, C(n,k) or nCk. */
460 int CombinatorialRec(int n, int k)
461 {
462         /* We could do:
463                         return factorial(n)/(factorial(n-k)*factorial(k));
464                 But prefer the recursive approach instead, because it's not so prone
465                 to numerical overflow. This approach uses the idea of the Pascal triangle. */
466
467         if (k <= 0 || k >= n)
468                 return 1;
469         else
470                 return CombinatorialRec(n-1,k-1) + CombinatorialRec(n-1,k);
471 }
472
473 /** @return Binomial coefficients by tabulation, i.e. n choose k, C(n,k) or nCk. */
474 int CombinatorialTab(int n, int k)
475 {
476         if (k == 0 || k == n)
477                 return 1;
478         if (k < 0 || k > n)
479                 return 0;
480         // We use two auxiliary tables, one of size n-2 and one of size n-3.
481         int *table = new int[2*(k+1)]; ///@todo We can lower this size.
482         for(int i = 0; i < 2*(k+1); ++i)
483                 table[i] = 1;
484         int *t1 = &table[0];
485         int *t2 = &table[k+1];
486         // Iteratively fill the tables.
487         for(int i = 2; i <= n; ++i)
488         {
489                 for(int j = Max(1, i-n+k); j <= Min(k,i-1); ++j)
490                         t1[j] = t2[j] + t2[j-1];
491                 Swap(t1, t2);
492         }
493         int c = t2[k];
494         delete[] table;
495         return c;
496 }
497
498 float PowUInt(float base, u32 exponent)
499 {
500         // 'Fast Exponentiation': We interpret exponent in base two and calculate the power by
501         // squaring and multiplying by base.
502
503         // Find the highest bit that is set.
504         u32 e = 0x80000000;
505         while((exponent & e) == 0 && e > 0)
506                 e >>= 1;
507
508         float val = 1.f;
509         do
510         {
511                 val *= val; // Shifts the exponent one place left
512                 val *= (exponent & e) != 0 ? base : 1.f; // Adds a 1 as the LSB of the exponent
513                 e >>= 1;
514         } while(e > 0);
515
516         return val;
517 }
518
519 /** @param base Exponent base value.
520         @param exponent Integer exponent to raise base to.
521         @return pow(base,exponent) but optimized because we only use integer exponent. */
522 float PowInt(float base, int exponent)
523 {
524         if (exponent == 0)
525                 return 1.f;
526         else if (exponent < 0)
527                 return 1.f / PowUInt(base, (u32)-exponent);
528         else
529                 return PowUInt(base, (u32)exponent);
530 }
531
532 char *SerializeFloat(float f, char *dstStr)
533 {
534         if (!IsNan(f))
535         {
536                 int numChars = dtoa_grisu3((double)f, dstStr);
537                 return dstStr + numChars;
538         }
539         else
540         {
541                 u32 u = ReinterpretAsU32(f);
542                 int numChars = sprintf(dstStr, "NaN(%8X)", (unsigned int)u);
543                 return dstStr + numChars;
544         }
545 }
546
547 float DeserializeFloat(const char *str, const char **outEndStr)
548 {
549         if (!str)
550                 return FLOAT_NAN;
551         while(*str > 0 && *str <= ' ')
552                 ++str;
553         if (*str == 0)
554                 return FLOAT_NAN;
555         if (MATH_NEXT_WORD_IS(str, "NaN("))
556         {
557                 str += strlen("NaN("); //MATH_SKIP_WORD(str, "NaN(");
558                 u32 x;
559                 int n = sscanf(str, "%X", (unsigned int *)&x);
560                 if (n != 1)
561                         return FLOAT_NAN;
562                 while(*str != 0)
563                 {
564                         ++str;
565                         if (*str == ')')
566                         {
567                                 ++str;
568                                 break;
569                         }
570                 }
571                 if (outEndStr)
572                         *outEndStr = str;
573                 return ReinterpretAsFloat(x);
574         }
575         float f;
576
577         if (!strncmp(str, "-inf", 4)) { f = -FLOAT_INF; str += 4; }
578         else if (!strncmp(str, "inf", 3)) { f = FLOAT_INF; str += 3; }
579         else f = (float)strtod(str, const_cast<char**>(&str));
580
581         while(*str > 0 && *str <= ' ')
582                 ++str;
583         if (*str == ',' || *str == ';')
584                 ++str;
585         while(*str > 0 && *str <= ' ')
586                 ++str;
587         if (outEndStr)
588                 *outEndStr = str;
589         return f;
590 }
591
592 int hexstr_to_u64(const char *str, uint64_t *u)
593 {
594         assert(u);
595         *u = 0;
596         const char *s = str;
597         for(int i = 0; i <= 16; ++i)
598         {
599                 char ch = *s;
600                 if (ch >= '0' && ch <= '9')
601                         *u = 16 * *u + ch - '0';
602                 else if (ch >= 'a' && ch <= 'f')
603                         *u = 16 * *u + 10 + ch - 'a';
604                 else if (ch >= 'A' && ch <= 'F')
605                         *u = 16 * *u + 10 + ch - 'A';
606                 else
607                         break;
608                 ++s;
609         }
610         return (int)(s - str);
611 }
612
613 double DeserializeDouble(const char *str, const char **outEndStr)
614 {
615         if (!str)
616                 return (double)FLOAT_NAN;
617         while(*str > 0 && *str <= ' ')
618                 ++str;
619         if (*str == 0)
620                 return (double)FLOAT_NAN;
621         if (MATH_NEXT_WORD_IS(str, "NaN("))
622         {
623                 str += strlen("NaN("); //MATH_SKIP_WORD(str, "NaN(");
624
625                 // Read 64-bit unsigned hex representation of the NaN. TODO: Make this more efficient without using sscanf.
626                 uint64_t u;
627                 int nChars = hexstr_to_u64(str, &u);
628                 str += nChars;
629                 while(*str != 0)
630                 {
631                         ++str;
632                         if (*str == ')')
633                         {
634                                 ++str;
635                                 break;
636                         }
637                 }
638                 if (outEndStr)
639                         *outEndStr = str;
640                 return ReinterpretAsDouble(u);
641         }
642         double f;
643         
644         if (!strncmp(str, "-inf", 4)) { f = (double)-FLOAT_INF; str += 4; }
645         else if (!strncmp(str, "inf", 3)) { f = (double)FLOAT_INF; str += 3; }
646         else f = strtod(str, const_cast<char**>(&str));
647
648         while(*str > 0 && *str <= ' ')
649                 ++str;
650         if (*str == ',' || *str == ';')
651                 ++str;
652         while(*str > 0 && *str <= ' ')
653                 ++str;
654         if (outEndStr)
655                 *outEndStr = str;
656         return f;
657 }
658
659 MATH_END_NAMESPACE