turbomath.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 James Jackson, BYU MAGICC Lab.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include <rosflight_utils/turbomath.h>
33 
34 #include <cstdint>
35 
36 static const int16_t atan_lookup_table[250] = {
37  0, 40, 80, 120, 160, 200, 240, 280, 320, 360, 400, 440, 480, 520, 559, 599, 639, 679,
38  719, 759, 798, 838, 878, 917, 957, 997, 1036, 1076, 1115, 1155, 1194, 1234, 1273, 1312, 1352, 1391,
39  1430, 1469, 1508, 1548, 1587, 1626, 1664, 1703, 1742, 1781, 1820, 1858, 1897, 1935, 1974, 2012, 2051, 2089,
40  2127, 2166, 2204, 2242, 2280, 2318, 2355, 2393, 2431, 2469, 2506, 2544, 2581, 2618, 2656, 2693, 2730, 2767,
41  2804, 2841, 2878, 2915, 2951, 2988, 3024, 3061, 3097, 3133, 3169, 3206, 3241, 3277, 3313, 3349, 3385, 3420,
42  3456, 3491, 3526, 3561, 3596, 3631, 3666, 3701, 3736, 3771, 3805, 3839, 3874, 3908, 3942, 3976, 4010, 4044,
43  4078, 4112, 4145, 4179, 4212, 4245, 4278, 4311, 4344, 4377, 4410, 4443, 4475, 4508, 4540, 4572, 4604, 4636,
44  4668, 4700, 4732, 4764, 4795, 4827, 4858, 4889, 4920, 4951, 4982, 5013, 5044, 5074, 5105, 5135, 5166, 5196,
45  5226, 5256, 5286, 5315, 5345, 5375, 5404, 5434, 5463, 5492, 5521, 5550, 5579, 5608, 5636, 5665, 5693, 5721,
46  5750, 5778, 5806, 5834, 5862, 5889, 5917, 5944, 5972, 5999, 6026, 6053, 6080, 6107, 6134, 6161, 6187, 6214,
47  6240, 6267, 6293, 6319, 6345, 6371, 6397, 6422, 6448, 6473, 6499, 6524, 6549, 6574, 6599, 6624, 6649, 6674,
48  6698, 6723, 6747, 6772, 6796, 6820, 6844, 6868, 6892, 6916, 6940, 6963, 6987, 7010, 7033, 7057, 7080, 7103,
49  7126, 7149, 7171, 7194, 7217, 7239, 7261, 7284, 7306, 7328, 7350, 7372, 7394, 7416, 7438, 7459, 7481, 7502,
50  7524, 7545, 7566, 7587, 7608, 7629, 7650, 7671, 7691, 7712, 7733, 7753, 7773, 7794, 7814, 7834};
51 
52 static const int16_t asin_lookup_table[250] = {
53  0, 40, 80, 120, 160, 200, 240, 280, 320, 360, 400, 440, 480, 520, 560, 600,
54  640, 681, 721, 761, 801, 841, 881, 921, 961, 1002, 1042, 1082, 1122, 1163, 1203, 1243,
55  1284, 1324, 1364, 1405, 1445, 1485, 1526, 1566, 1607, 1647, 1688, 1729, 1769, 1810, 1851, 1891,
56  1932, 1973, 2014, 2054, 2095, 2136, 2177, 2218, 2259, 2300, 2341, 2382, 2424, 2465, 2506, 2547,
57  2589, 2630, 2672, 2713, 2755, 2796, 2838, 2880, 2921, 2963, 3005, 3047, 3089, 3131, 3173, 3215,
58  3257, 3300, 3342, 3384, 3427, 3469, 3512, 3554, 3597, 3640, 3683, 3726, 3769, 3812, 3855, 3898,
59  3941, 3985, 4028, 4072, 4115, 4159, 4203, 4246, 4290, 4334, 4379, 4423, 4467, 4511, 4556, 4601,
60  4645, 4690, 4735, 4780, 4825, 4870, 4916, 4961, 5007, 5052, 5098, 5144, 5190, 5236, 5282, 5329,
61  5375, 5422, 5469, 5515, 5562, 5610, 5657, 5704, 5752, 5800, 5848, 5896, 5944, 5992, 6041, 6089,
62  6138, 6187, 6236, 6286, 6335, 6385, 6435, 6485, 6535, 6586, 6637, 6687, 6739, 6790, 6841, 6893,
63  6945, 6997, 7050, 7102, 7155, 7208, 7262, 7315, 7369, 7423, 7478, 7532, 7587, 7643, 7698, 7754,
64  7810, 7867, 7923, 7981, 8038, 8096, 8154, 8213, 8271, 8331, 8390, 8450, 8511, 8572, 8633, 8695,
65  8757, 8820, 8883, 8947, 9011, 9076, 9141, 9207, 9273, 9340, 9407, 9476, 9545, 9614, 9684, 9755,
66  9827, 9900, 9973, 10047, 10122, 10198, 10275, 10353, 10432, 10512, 10593, 10675, 10759, 10844, 10930, 11018,
67  11107, 11198, 11290, 11385, 11481, 11580, 11681, 11784, 11890, 11999, 12111, 12226, 12346, 12469, 12597, 12730,
68  12870, 13017, 13171, 13336, 13513, 13705, 13917, 14157, 14442, 14813};
69 
70 static const int16_t sin_lookup_table[250] = {
71  0, 63, 126, 188, 251, 314, 377, 440, 502, 565, 628, 691, 753, 816, 879, 941, 1004, 1066,
72  1129, 1191, 1253, 1316, 1378, 1440, 1502, 1564, 1626, 1688, 1750, 1812, 1874, 1935, 1997, 2059, 2120, 2181,
73  2243, 2304, 2365, 2426, 2487, 2548, 2608, 2669, 2730, 2790, 2850, 2910, 2970, 3030, 3090, 3150, 3209, 3269,
74  3328, 3387, 3446, 3505, 3564, 3623, 3681, 3740, 3798, 3856, 3914, 3971, 4029, 4086, 4144, 4201, 4258, 4315,
75  4371, 4428, 4484, 4540, 4596, 4652, 4707, 4762, 4818, 4873, 4927, 4982, 5036, 5090, 5144, 5198, 5252, 5305,
76  5358, 5411, 5464, 5516, 5569, 5621, 5673, 5724, 5776, 5827, 5878, 5929, 5979, 6029, 6079, 6129, 6179, 6228,
77  6277, 6326, 6374, 6423, 6471, 6518, 6566, 6613, 6660, 6707, 6753, 6800, 6845, 6891, 6937, 6982, 7026, 7071,
78  7115, 7159, 7203, 7247, 7290, 7333, 7375, 7417, 7459, 7501, 7543, 7584, 7624, 7665, 7705, 7745, 7785, 7824,
79  7863, 7902, 7940, 7978, 8016, 8053, 8090, 8127, 8163, 8200, 8235, 8271, 8306, 8341, 8375, 8409, 8443, 8477,
80  8510, 8543, 8575, 8607, 8639, 8671, 8702, 8733, 8763, 8793, 8823, 8852, 8881, 8910, 8938, 8966, 8994, 9021,
81  9048, 9075, 9101, 9127, 9152, 9178, 9202, 9227, 9251, 9274, 9298, 9321, 9343, 9365, 9387, 9409, 9430, 9451,
82  9471, 9491, 9511, 9530, 9549, 9567, 9585, 9603, 9620, 9637, 9654, 9670, 9686, 9701, 9716, 9731, 9745, 9759,
83  9773, 9786, 9799, 9811, 9823, 9834, 9846, 9856, 9867, 9877, 9887, 9896, 9905, 9913, 9921, 9929, 9936, 9943,
84  9950, 9956, 9961, 9967, 9972, 9976, 9980, 9984, 9987, 9990, 9993, 9995, 9997, 9998, 9999, 10000};
85 
86 float sign(float y)
87 {
88  return (0 < y) - (y < 0);
89 }
90 
91 float asin_lookup(float x)
92 {
93  static const float max = 1.0;
94  static const float min = 0.0;
95  static const int16_t num_entries = 250;
96  static float dx = 0.0;
97  static const float scale = 10000.0;
98 
99  float t = (x - min) / (max - min) * num_entries;
100  uint8_t index = (uint8_t)t;
101  dx = t - (float)index;
102 
103  if (index >= num_entries)
104  {
105  return max;
106  }
107  else if (index < num_entries - 1)
108  {
109  return (float)asin_lookup_table[index] / scale
110  + dx * (float)(asin_lookup_table[index + 1] - asin_lookup_table[index]) / scale;
111  }
112  else
113  {
114  return (float)asin_lookup_table[index] / scale
115  + dx * (float)(asin_lookup_table[index] - asin_lookup_table[index - 1]) / scale;
116  }
117 }
118 
119 float turboacos(float x)
120 {
121  if (x < 0)
122  return M_PI + asin_lookup(-x);
123  else
124  return M_PI - asin_lookup(x);
125 }
126 
127 float turboasin(float x)
128 {
129  if (x < 0)
130  return -asin_lookup(-x);
131  else
132  return asin_lookup(x);
133 }
134 
135 float sin_lookup(float x)
136 {
137  static const float max = 1.0;
138  static const float min = 0.0;
139  static const int16_t num_entries = 250;
140  static float dx = 0.0;
141  static const float scale = 10000.0;
142 
143  float t = (x - min) / (max - min) * num_entries;
144  uint8_t index = (uint8_t)t;
145  dx = t - (float)index;
146 
147  if (index >= num_entries)
148  return max;
149  else if (index < num_entries - 1)
150  return (float)sin_lookup_table[index] / scale
151  + dx * (float)(sin_lookup_table[index + 1] - sin_lookup_table[index]) / scale;
152  else
153  return (float)sin_lookup_table[index] / scale
154  + dx * (float)(sin_lookup_table[index] - sin_lookup_table[index - 1]) / scale;
155 }
156 
157 float turbosin(float x)
158 {
159  while (x > M_PI) x -= 2.0 * M_PI;
160  while (x <= -M_PI) x += 2.0 * M_PI;
161 
162  if (0 <= x && x <= M_PI / 2.0)
163  return sin_lookup(x);
164  else if (-M_PI / 2.0 <= x && x < 0)
165  return -sin_lookup(-x);
166  else if (M_PI / 2.0 < x)
167  return sin_lookup(M_PI - x);
168  else
169  return -sin_lookup(x + M_PI);
170 }
171 
172 float turbocos(float x)
173 {
174  return turbosin(x + M_PI);
175 }
176 
177 float atan_lookup(float x)
178 {
179  if (x < 0)
180  return -1 * atan_lookup(-1 * x);
181  if (x > 1.0)
182  return M_PI - atan_lookup(1.0 / x);
183 
184  static const float max = 1.0;
185  static const float min = 0.0;
186  static const int16_t num_entries = 250;
187  static float dx = 0.0;
188  static const float scale = 10000.0;
189 
190  float t = (x - min) / (max - min) * num_entries;
191  uint8_t index = (uint8_t)t;
192  dx = t - (float)index;
193 
194  if (index >= num_entries)
195  return max;
196  else if (index < num_entries - 1)
197  return (float)atan_lookup_table[index] / scale
198  + dx * (float)(atan_lookup_table[index + 1] - atan_lookup_table[index]) / scale;
199  else
200  return (float)atan_lookup_table[index] / scale
201  + dx * (float)(atan_lookup_table[index] - atan_lookup_table[index - 1]) / scale;
202 }
203 
204 float turboatan2(float y, float x)
205 {
206  if (y == 0)
207  {
208  if (x < 0)
209  return M_PI;
210  else
211  return 0;
212  }
213 
214  else if (x == 0)
215  {
216  return M_PI / 2.0 * sign(y);
217  }
218 
219  else
220  {
221  float arctan = atan_lookup(x / y);
222 
223  if (y > 0)
224  return M_PI / 2.0 - arctan;
225  else if (y < 0)
226  return -M_PI / 2.0 - arctan;
227  else if (x < 0)
228  return arctan + M_PI;
229  else
230  return arctan;
231  }
232 }
233 
234 double turbopow(double a, double b)
235 {
236  union
237  {
238  double d;
239  int x[2];
240  } u = {a};
241  u.x[1] = (int)(b * (u.x[1] - 1072632447) + 1072632447);
242  u.x[0] = 0;
243  return u.d;
244 }
245 
246 float turboInvSqrt(float x)
247 {
248  long i;
249  float x2, y;
250  const float threehalfs = 1.5F;
251 
252  x2 = x * 0.5F;
253  y = x;
254  i = *(long*)&y; // evil floating point bit level hacking
255  i = 0x5f3759df - (i >> 1);
256  y = *(float*)&i;
257  y = y * (threehalfs - (x2 * y * y)); // 1st iteration
258  // y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
259 
260  return y;
261 }
d
d
turboatan2
float turboatan2(float y, float x)
Definition: turbomath.cpp:204
sin_lookup_table
static const int16_t sin_lookup_table[250]
Definition: turbomath.cpp:70
sign
float sign(float y)
Definition: turbomath.cpp:86
turboasin
float turboasin(float x)
Definition: turbomath.cpp:127
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
asin_lookup
float asin_lookup(float x)
Definition: turbomath.cpp:91
atan_lookup_table
static const int16_t atan_lookup_table[250]
Definition: turbomath.cpp:36
b
struct _USB_OTG_GOTGCTL_TypeDef::@58 b
turboInvSqrt
float turboInvSqrt(float x)
Definition: turbomath.cpp:246
turbosin
float turbosin(float x)
Definition: turbomath.cpp:157
asin_lookup_table
static const int16_t asin_lookup_table[250]
Definition: turbomath.cpp:52
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
i
int i
sin_lookup
float sin_lookup(float x)
Definition: turbomath.cpp:135
min
int min(int a, int b)
turbopow
double turbopow(double a, double b)
Definition: turbomath.cpp:234
turbocos
float turbocos(float x)
Definition: turbomath.cpp:172
M_PI
#define M_PI
Definition: turbomath.h:36
turboacos
float turboacos(float x)
Definition: turbomath.cpp:119
atan_lookup
float atan_lookup(float x)
Definition: turbomath.cpp:177

rosflight_utils
Author(s):
autogenerated on Thu Apr 15 2021 05:10:06