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

rosflight_utils
Author(s):
autogenerated on Wed Jul 3 2019 20:00:31