1 // Copyright 2014 Emilie Gillet.
2 //
3 // Author: Emilie Gillet (emilie.o.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 arc-tangent routines.
28
29 #ifndef STMLIB_DSP_ATAN_H_
30 #define STMLIB_DSP_ATAN_H_
31
32 #include "stmlib/stmlib.h"
33
34 #include "stmlib/dsp/rsqrt.h"
35
36 #include <cmath>
37
38 namespace stmlib {
39
fast_atan2(float y,float x)40 static inline uint16_t fast_atan2(float y, float x) {
41 static const uint32_t sign_mask = 0x80000000;
42 static const float b = 0.596227f;
43 uint32_t ux_s = sign_mask & unsafe_bit_cast<uint32_t, float>(x);
44 uint32_t uy_s = sign_mask & unsafe_bit_cast<uint32_t, float>(y);
45 uint32_t offset = ((~ux_s & uy_s) >> 29 | ux_s >> 30) << 14;
46 float bxy_a = fabs(b * x * y);
47 float num = bxy_a + y * y;
48 float atan_1q = num / (x * x + bxy_a + num);
49 uint32_t uatan_2q = (ux_s ^ uy_s) | unsafe_bit_cast<uint32_t, float>(atan_1q);
50 return unsafe_bit_cast<float, uint32_t>(uatan_2q) * 16384 + offset;
51 }
52
53 extern const uint16_t atan_lut[513];
54
fast_atan2r(float y,float x,float * r)55 static inline uint16_t fast_atan2r(float y, float x, float* r) {
56 float squared_magnitude = x * x + y * y;
57 if (squared_magnitude == 0.0f) {
58 *r = 0.0f;
59 return 0.0f;
60 }
61 float rinv = fast_rsqrt_carmack(squared_magnitude);
62 *r = rinv * squared_magnitude;
63
64 static const uint32_t sign_mask = 0x80000000;
65 uint32_t ux_s = sign_mask & unsafe_bit_cast<uint32_t, float>(x);
66 uint32_t uy_s = sign_mask & unsafe_bit_cast<uint32_t, float>(y);
67 uint32_t quadrant = ((~ux_s & uy_s) >> 29 | ux_s >> 30);
68 uint16_t angle = 0;
69 x = fabs(x);
70 y = fabs(y);
71 if (y > x) {
72 angle = 16384 - atan_lut[static_cast<uint32_t>(x * rinv * 512.0f + 0.5f)];
73 } else {
74 angle = atan_lut[static_cast<uint32_t>(y * rinv * 512.0f + 0.5f)];
75 }
76 if (ux_s ^ uy_s) {
77 angle = -angle;
78 }
79 return angle + (quadrant << 14);
80 }
81
82 } // namespace stmlib
83
84 #endif // STMLIB_DSP_ATAN_H_
85