1 // Copyright 2014 Olivier Gillet.
2 //
3 // Author: Olivier Gillet (ol.gillet@gmail.com)
4 //
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11 //
12 // The above copyright notice and this permission notice shall be included in
13 // all copies or substantial portions of the Software.
14 //
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21 // THE SOFTWARE.
22 //
23 // See http://creativecommons.org/licenses/MIT/ for more information.
24 //
25 // -----------------------------------------------------------------------------
26 //
27 // Fast reciprocal of square-root routines.
28
29 #ifndef STMLIB_DSP_RSQRT_H_
30 #define STMLIB_DSP_RSQRT_H_
31
32 #include "stmlib/stmlib.h"
33
34 namespace stmlib {
35
36 template<typename To, typename From>
37 struct unsafe_bit_cast_t {
38 union {
39 From from;
40 To to;
41 };
42 };
43
44 template<typename To, typename From>
unsafe_bit_cast(From from)45 To unsafe_bit_cast(From from) {
46 unsafe_bit_cast_t<To, From> u;
47 u.from = from;
48 return u.to;
49 }
50
51
fast_rsqrt_carmack(float x)52 static inline float fast_rsqrt_carmack(float x) {
53 uint32_t i;
54 float x2, y;
55 const float threehalfs = 1.5f;
56 y = x;
57 i = unsafe_bit_cast<uint32_t, float>(y);
58 i = 0x5f3759df - (i >> 1);
59 y = unsafe_bit_cast<float, uint32_t>(i);
60 x2 = x * 0.5f;
61 y = y * (threehalfs - (x2 * y * y));
62 return y;
63 }
64
fast_rsqrt_accurate(float fp0)65 static inline float fast_rsqrt_accurate(float fp0) {
66 float _min = 1.0e-38;
67 float _1p5 = 1.5;
68 float fp1, fp2, fp3;
69
70 uint32_t q = unsafe_bit_cast<uint32_t, float>(fp0);
71 fp2 = unsafe_bit_cast<float, uint32_t>(0x5F3997BB - ((q >> 1) & 0x3FFFFFFF));
72 fp1 = _1p5 * fp0 - fp0;
73 fp3 = fp2 * fp2;
74 if (fp0 < _min) {
75 return fp0 > 0 ? fp2 : 1000.0;
76 }
77 fp3 = _1p5 - fp1 * fp3;
78 fp2 = fp2 * fp3;
79 fp3 = fp2 * fp2;
80 fp3 = _1p5 - fp1 * fp3;
81 fp2 = fp2 * fp3;
82 fp3 = fp2 * fp2;
83 fp3 = _1p5 - fp1 * fp3;
84 return fp2 * fp3;
85 }
86
87 } // namespace stmlib
88
89 #endif // STMLIB_DSP_RSQRT_H_
90