turbomath.cpp
Go to the documentation of this file.
1 /*
2  *
3  * BSD 3-Clause License
4  *
5  * Copyright (c) 2017, James Jackson - BYU MAGICC Lab, Provo UT
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright notice, this
12  * list of conditions and the following disclaimer.
13  *
14  * * Redistributions in binary form must reproduce the above copyright notice,
15  * this list of conditions and the following disclaimer in the documentation
16  * and/or other materials provided with the distribution.
17  *
18  * * Neither the name of the copyright holder nor the names of its
19  * contributors may be used to endorse or promote products derived from
20  * this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
23  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <turbomath/turbomath.h>
35 #include <string.h>
36 
37 namespace turbomath
38 {
39 
41  x(*arr), y(*(arr+1)), z(*(arr+2))
42 {
43  arr[0] = v.arr[0];
44  arr[1] = v.arr[1];
45  arr[2] = v.arr[2];
46 }
47 
49  x(*arr), y(*(arr+1)), z(*(arr+2))
50 {}
51 
52 Vector::Vector(float x_, float y_, float z_) :
53  arr{x_, y_, z_},
54  x(*arr), y(*(arr+1)), z(*(arr+2))
55 {}
56 
57 
58 float Vector::norm() const
59 {
60  return 1.0f/inv_sqrt(x*x + y*y + z*z);
61 }
62 
63 
64 float Vector::sqrd_norm() const
65 {
66  return x*x + y*y + z*z;
67 }
68 
69 
71 {
72  float recip_norm = inv_sqrt(x*x + y*y + z*z);
73  x *= recip_norm;
74  y *= recip_norm;
75  z *= recip_norm;
76  return *this;
77 }
78 
79 
81 {
82  float recip_norm = inv_sqrt(x*x + y*y + z*z);
83  Vector out(x*recip_norm, y*recip_norm, z*recip_norm);
84  return out;
85 }
86 
88 {
89  arr[0] = v.arr[0];
90  arr[1] = v.arr[1];
91  arr[2] = v.arr[2];
92  return *this;
93 }
94 
95 
97 {
98  return Vector(x + v.x, y + v.y, z + v.z);
99 }
100 
101 
103 {
104  return Vector(x - v.x, y - v.y, z - v.z);
105 }
106 
107 
109 {
110  x += v.x;
111  y += v.y;
112  z += v.z;
113  return *this;
114 }
115 
116 
118 {
119  x -= v.x;
120  y -= v.y;
121  z -= v.z;
122  return *this;
123 }
124 
125 
127 {
128  return Vector(x*s, y*s, z*s);
129 }
130 
131 
133 {
134  return Vector(x/s, y/s, z/s);
135 }
136 
137 
139 {
140  x *= s;
141  y *= s;
142  z *= s;
143  return *this;
144 }
145 
146 
148 {
149  x /= s;
150  y /= s;
151  z /= s;
152  return *this;
153 }
154 
155 
156 float Vector::dot(const Vector &v) const
157 {
158  return x*v.x + y*v.y + z*v.z;
159 }
160 
161 
162 Vector Vector::cross(const Vector &v) const
163 {
164  return Vector(y * v.z - z * v.y,
165  z * v.x - x * v.z,
166  x * v.y - y * v.x);
167 }
168 
169 Quaternion::Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f)
170 {}
171 
172 Quaternion::Quaternion(float w_, float x_, float y_, float z_) : w(w_), x(x_), y(y_), z(z_)
173 {}
174 
176 {
177  from_two_unit_vectors(u, v);
178 }
179 
180 Quaternion::Quaternion(float roll, float pitch, float yaw)
181 {
182  from_RPY(roll, pitch, yaw);
183 }
184 
186 {
187  float recip_norm = inv_sqrt(w*w + x*x + y*y + z*z);
188  w *= recip_norm;
189  x *= recip_norm;
190  y *= recip_norm;
191  z *= recip_norm;
192 
193  // Make sure the quaternion is canonical (w is always positive)
194  if (w < 0.0f)
195  {
196  w *= -1.0f;
197  x *= -1.0f;
198  y *= -1.0f;
199  z *= -1.0f;
200  }
201 
202  return *this;
203 }
204 
206 {
207  return Quaternion(w*q.w - x*q.x - y*q.y - z*q.z,
208  w*q.x + x*q.w - y*q.z + z*q.y,
209  w*q.y + x*q.z + y*q.w - z*q.x,
210  w*q.z - x*q.y + y*q.x + z*q.w);
211 }
212 
214 {
215  w = w*q.w - x*q.x - y*q.y - z*q.z;
216  x = w*q.x + x*q.w - y*q.z + z*q.y;
217  y = w*q.y + x*q.z + y*q.w - z*q.x;
218  z = w*q.z - x*q.y + y*q.x + z*q.w;
219  return *this;
220 }
221 
223 {
224  return Vector((1.0f - 2.0f*y*y - 2.0f*z*z) * v.x + (2.0f*(x*y + w*z))*v.y + 2.0f*(x*z - w*y)*v.z,
225  (2.0f*(x*y - w*z)) * v.x + (1.0f - 2.0f*x*x - 2.0f*z*z) * v.y + 2.0f*(y*z + w*x)*v.z,
226  (2.0f*(x*z + w*y)) * v.x + 2.0f*(y*z - w*x)*v.y + (1.0f - 2.0f*x*x - 2.0f*y*y)*v.z);
227 }
228 
230 {
231  return rotate(v);
232 }
233 
235 {
236  return Quaternion(w, -x, -y, -z);
237 }
238 
240 {
241  x *= -1.0f;
242  y *= -1.0f;
243  z *= -1.0f;
244  return *this;
245 }
246 
248 {
249  // Adapted From the Ogre3d source code
250  // https://bitbucket.org/sinbad/ogre/src/9db75e3ba05c/OgreMain/include/OgreVector3.h?fileviewer=file-view-default#cl-651
251  float d = u.dot(v);
252  if (d >= 1.0f)
253  {
254  w = 1.0f;
255  x = 0.0f;
256  y = 0.0f;
257  z = 0.0f;
258  return *this;
259  }
260  else
261  {
262  float invs = inv_sqrt(2.0f*(1.0f+d));
263  Vector xyz = u.cross(v)*invs;
264  w = 0.5f/invs;
265  x = xyz.x;
266  y = xyz.y;
267  z = xyz.z;
268  }
269  normalize();
270  return *this;
271 }
272 
273 Quaternion &Quaternion::from_RPY(float roll, float pitch, float yaw)
274 {
275  // p 259 of "Small unmanned aircraft: Theory and Practice" by Randy Beard and Tim McLain
276  float cp = turbomath::cos(roll/2.0);
277  float sp = turbomath::sin(roll/2.0);
278  float ct = turbomath::cos(pitch/2.0);
279  float st = turbomath::sin(pitch/2.0);
280  float cs = turbomath::cos(yaw/2.0);
281  float ss = turbomath::sin(yaw/2.0);
282 
283  w = cs*ct*cp + ss*st*sp;
284  x = cs*ct*sp - ss*st*cp;
285  y = cs*st*cp + ss*ct*sp;
286  z = ss*ct*cp - cs*st*sp;
287 
288  normalize();
289  return *this;
290 }
291 
293 {
294  Quaternion dq = q.inverse() * (*this);
295  if (dq.w < 0.0)
296  {
297  dq.w *= -1.0;
298  dq.x *= -1.0;
299  dq.y *= -1.0;
300  dq.z *= -1.0;
301  }
302  return log(dq);
303 }
304 
305 void Quaternion::get_RPY(float *roll, float *pitch, float *yaw) const
306 {
307  *roll = turbomath::atan2(2.0f * (w*x + y*z), 1.0f - 2.0f * (x*x + y*y));
308  *pitch = turbomath::asin(2.0f*(w*y - z*x));
309  *yaw = turbomath::atan2(2.0f * (w*z + x*y), 1.0f - 2.0f * (y*y + z*z));
310 }
311 
312 
313 
314 #ifndef M_PI
315 #define M_PI 3.14159265359
316 #endif
317 
318 static const float atan_max_x = 1.000000;
319 static const float atan_min_x = 0.000000;
320 static const float atan_scale_factor = 41720.240162;
321 static const int16_t atan_num_entries = 125;
322 static const int16_t atan_lookup_table[125] =
323 {
324  0, 334, 667, 1001, 1335, 1668, 2001, 2334, 2666, 2999,
325  3331, 3662, 3993, 4323, 4653, 4983, 5311, 5639, 5967, 6293,
326  6619, 6944, 7268, 7592, 7914, 8235, 8556, 8875, 9194, 9511,
327  9827, 10142, 10456, 10768, 11080, 11390, 11699, 12006, 12313, 12617,
328  12921, 13223, 13524, 13823, 14120, 14417, 14711, 15005, 15296, 15586,
329  15875, 16162, 16447, 16731, 17013, 17293, 17572, 17849, 18125, 18399,
330  18671, 18941, 19210, 19477, 19742, 20006, 20268, 20528, 20786, 21043,
331  21298, 21551, 21802, 22052, 22300, 22546, 22791, 23034, 23275, 23514,
332  23752, 23988, 24222, 24454, 24685, 24914, 25142, 25367, 25591, 25814,
333  26034, 26253, 26471, 26686, 26900, 27113, 27324, 27533, 27740, 27946,
334  28150, 28353, 28554, 28754, 28952, 29148, 29343, 29537, 29728, 29919,
335  30108, 30295, 30481, 30665, 30848, 31030, 31210, 31388, 31566, 31741,
336  31916, 32089, 32260, 32431, 32599,
337 };
338 
339 static const float asin_max_x = 1.000000;
340 static const float asin_min_x = 0.000000;
341 static const float asin_scale_factor = 20860.120081;
342 static const int16_t asin_num_entries = 200;
343 static const int16_t asin_lookup_table[200] =
344 {
345  0, 104, 209, 313, 417, 522, 626, 730, 835, 939,
346  1043, 1148, 1252, 1357, 1461, 1566, 1671, 1775, 1880, 1985,
347  2090, 2194, 2299, 2404, 2509, 2614, 2720, 2825, 2930, 3035,
348  3141, 3246, 3352, 3458, 3564, 3669, 3775, 3881, 3988, 4094,
349  4200, 4307, 4413, 4520, 4627, 4734, 4841, 4948, 5056, 5163,
350  5271, 5379, 5487, 5595, 5703, 5811, 5920, 6029, 6138, 6247,
351  6356, 6465, 6575, 6685, 6795, 6905, 7015, 7126, 7237, 7348,
352  7459, 7570, 7682, 7794, 7906, 8019, 8131, 8244, 8357, 8471,
353  8584, 8698, 8812, 8927, 9042, 9157, 9272, 9388, 9504, 9620,
354  9737, 9854, 9971, 10089, 10207, 10325, 10444, 10563, 10682, 10802,
355  10922, 11043, 11164, 11285, 11407, 11530, 11652, 11776, 11899, 12024,
356  12148, 12273, 12399, 12525, 12652, 12779, 12907, 13035, 13164, 13293,
357  13424, 13554, 13686, 13817, 13950, 14083, 14217, 14352, 14487, 14623,
358  14760, 14898, 15036, 15176, 15316, 15457, 15598, 15741, 15885, 16029,
359  16175, 16321, 16469, 16618, 16767, 16918, 17070, 17224, 17378, 17534,
360  17691, 17849, 18009, 18170, 18333, 18497, 18663, 18830, 19000, 19171,
361  19343, 19518, 19695, 19874, 20055, 20239, 20424, 20613, 20803, 20997,
362  21194, 21393, 21596, 21802, 22012, 22225, 22443, 22664, 22891, 23122,
363  23359, 23601, 23849, 24104, 24366, 24637, 24916, 25204, 25504, 25816,
364  26143, 26485, 26847, 27232, 27644, 28093, 28588, 29149, 29814, 30680,
365 };
366 
367 
368 static const float max_pressure = 106598.405011;
369 static const float min_pressure = 69681.635473;
370 static const float pressure_scale_factor = 10.754785;
371 static const int16_t pressure_num_entries = 200;
372 static const int16_t pressure_lookup_table[200] =
373 {
374  32767, 32544, 32321, 32098, 31876, 31655, 31434, 31213, 30993, 30773,
375  30554, 30335, 30117, 29899, 29682, 29465, 29248, 29032, 28816, 28601,
376  28386, 28172, 27958, 27745, 27532, 27319, 27107, 26895, 26684, 26473,
377  26263, 26053, 25843, 25634, 25425, 25217, 25009, 24801, 24594, 24387,
378  24181, 23975, 23769, 23564, 23359, 23155, 22951, 22748, 22544, 22341,
379  22139, 21937, 21735, 21534, 21333, 21133, 20932, 20733, 20533, 20334,
380  20135, 19937, 19739, 19542, 19344, 19148, 18951, 18755, 18559, 18364,
381  18169, 17974, 17780, 17586, 17392, 17199, 17006, 16813, 16621, 16429,
382  16237, 16046, 15855, 15664, 15474, 15284, 15095, 14905, 14716, 14528,
383  14339, 14151, 13964, 13777, 13590, 13403, 13217, 13031, 12845, 12659,
384  12474, 12290, 12105, 11921, 11737, 11554, 11370, 11188, 11005, 10823,
385  10641, 10459, 10278, 10096, 9916, 9735, 9555, 9375, 9195, 9016,
386  8837, 8658, 8480, 8302, 8124, 7946, 7769, 7592, 7415, 7239,
387  7063, 6887, 6711, 6536, 6361, 6186, 6012, 5837, 5664, 5490,
388  5316, 5143, 4970, 4798, 4626, 4454, 4282, 4110, 3939, 3768,
389  3597, 3427, 3257, 3087, 2917, 2748, 2578, 2409, 2241, 2072,
390  1904, 1736, 1569, 1401, 1234, 1067, 901, 734, 568, 402,
391  237, 71, -94, -259, -424, -588, -752, -916, -1080, -1243,
392  -1407, -1570, -1732, -1895, -2057, -2219, -2381, -2543, -2704, -2865,
393  -3026, -3187, -3347, -3507, -3667, -3827, -3987, -4146, -4305, -4464,
394 };
395 
396 static const float sin_max_x = 3.141593;
397 static const float sin_min_x = 0.000000;
398 static const float sin_scale_factor = 32767.000000;
399 static const int16_t sin_num_entries = 125;
400 static const int16_t sin_lookup_table[125] =
401 {
402  0, 823, 1646, 2468, 3289, 4107, 4922, 5735, 6544, 7349,
403  8149, 8944, 9733, 10516, 11293, 12062, 12824, 13578, 14323, 15059,
404  15786, 16502, 17208, 17904, 18588, 19260, 19920, 20568, 21202, 21823,
405  22431, 23024, 23602, 24166, 24715, 25247, 25764, 26265, 26749, 27216,
406  27666, 28099, 28513, 28910, 29289, 29648, 29990, 30312, 30615, 30899,
407  31163, 31408, 31633, 31837, 32022, 32187, 32331, 32454, 32558, 32640,
408  32702, 32744, 32764, 32764, 32744, 32702, 32640, 32558, 32454, 32331,
409  32187, 32022, 31837, 31633, 31408, 31163, 30899, 30615, 30312, 29990,
410  29648, 29289, 28910, 28513, 28099, 27666, 27216, 26749, 26265, 25764,
411  25247, 24715, 24166, 23602, 23024, 22431, 21823, 21202, 20568, 19920,
412  19260, 18588, 17904, 17208, 16502, 15786, 15059, 14323, 13578, 12824,
413  12062, 11293, 10516, 9733, 8944, 8149, 7349, 6544, 5735, 4922,
414  4107, 3289, 2468, 1646, 823
415 };
416 
417 float fsign(float y)
418 {
419  return (0.0f < y) - (y < 0.0f);
420 }
421 
422 float cos(float x)
423 {
424  return sin(M_PI/2.0 - x);
425 }
426 
427 float sin(float x)
428 {
429  // wrap down to +/x PI
430  while (x > M_PI)
431  x -= 2.0*M_PI;
432 
433  while (x <= -M_PI)
434  x += 2.0*M_PI;
435 
436  // sin is symmetric
437  if (x < 0)
438  return -1.0*sin(-x);
439 
440  // wrap onto (0, PI)
441  if (x > M_PI)
442  return -1.0*sin(x - M_PI);
443 
444  // Now, all we have left is the range 0 to PI, use the lookup table
445  float t = (x - sin_min_x)/(sin_max_x - sin_min_x) * static_cast<float>(sin_num_entries);
446  int16_t index = static_cast<int16_t>(t);
447  float delta_x = t - index;
448 
449  if (index >= sin_num_entries)
450  return sin_lookup_table[sin_num_entries - 1]/sin_scale_factor;
451  else if (index < sin_num_entries - 1)
452  return sin_lookup_table[index]/sin_scale_factor + delta_x * (sin_lookup_table[index + 1] -
453  sin_lookup_table[index])/sin_scale_factor;
454  else
455  return sin_lookup_table[index]/sin_scale_factor + delta_x * (sin_lookup_table[index] - sin_lookup_table[index -
456  1])/sin_scale_factor;
457 }
458 
459 
460 float atan(float x)
461 {
462  // atan is symmetric
463  if (x < 0)
464  {
465  return -1.0*atan(-1.0*x);
466  }
467  // This uses a sweet identity to wrap the domain of atan onto (0,1)
468  if (x > 1.0)
469  {
470  return M_PI/2.0 - atan(1.0/x);
471  }
472 
473  float t = (x - atan_min_x)/(atan_max_x - atan_min_x) * static_cast<float>(atan_num_entries);
474  int16_t index = static_cast<int16_t>(t);
475  float delta_x = t - index;
476 
477  if (index >= atan_num_entries)
478  return atan_lookup_table[atan_num_entries-1]/atan_scale_factor;
479  else if (index < atan_num_entries - 1)
480  return atan_lookup_table[index]/atan_scale_factor + delta_x * (atan_lookup_table[index + 1] -
481  atan_lookup_table[index])/atan_scale_factor;
482  else
483  return atan_lookup_table[index]/atan_scale_factor + delta_x * (atan_lookup_table[index] - atan_lookup_table[index -
484  1])/atan_scale_factor;
485 }
486 
487 
488 float atan2(float y, float x)
489 {
490  // algorithm from wikipedia: https://en.wikipedia.org/wiki/Atan2
491  if (x == 0.0)
492  {
493  if (y < 0.0)
494  {
495  return - M_PI/2.0;
496  }
497  else if (y > 0.0)
498  {
499  return M_PI/2.0;
500  }
501  else
502  {
503  return 0.0;
504  }
505  }
506 
507  float arctan = atan(y/x);
508 
509  if (x < 0.0)
510  {
511  if (y < 0)
512  {
513  return arctan - M_PI;
514  }
515  else
516  {
517  return arctan + M_PI;
518  }
519  }
520 
521  else
522  {
523  return arctan;
524  }
525 }
526 
527 
528 float asin(float x)
529 {
530  if (x < 0.0)
531  {
532  return -1.0*asin(-1.0*x);
533  }
534 
535  float t = (x - asin_min_x)/(asin_max_x - asin_min_x) * static_cast<float>(asin_num_entries);
536  int16_t index = static_cast<int16_t>(t);
537  float delta_x = t - index;
538 
539  if (index >= asin_num_entries)
540  return asin_lookup_table[asin_num_entries - 1]/asin_scale_factor;
541  else if (index < asin_num_entries - 1)
542  return asin_lookup_table[index]/asin_scale_factor + delta_x * (asin_lookup_table[index + 1] -
543  asin_lookup_table[index])/asin_scale_factor;
544  else
545  return asin_lookup_table[index]/asin_scale_factor + delta_x * (asin_lookup_table[index] - asin_lookup_table[index -
546  1])/asin_scale_factor;
547 }
548 
549 float alt(float press)
550 {
551 
552  if (press < max_pressure && press > min_pressure)
553  {
554  float t = (press - min_pressure)/(max_pressure - min_pressure) * static_cast<float>(pressure_num_entries);
555  int16_t index = static_cast<int16_t>(t);
556  float delta_x = t - index;
557 
558  if (index >= pressure_num_entries)
559  return asin_lookup_table[pressure_num_entries - 1]/pressure_scale_factor;
560  else if (index < pressure_num_entries - 1)
561  return pressure_lookup_table[index]/pressure_scale_factor + delta_x * (pressure_lookup_table[index + 1] -
562  pressure_lookup_table[index])/pressure_scale_factor;
563  else
564  return pressure_lookup_table[index]/pressure_scale_factor + delta_x * (pressure_lookup_table[index] -
565  pressure_lookup_table[index - 1])/pressure_scale_factor;
566  }
567  else
568  return 0.0;
569 }
570 
571 float fabs(float x)
572 {
573  if (x < 0)
574  return -x;
575  else
576  return x;
577 
578 }
579 
580 float inv_sqrt(float x)
581 {
582  volatile float x2;
583  volatile float_converter_t y, i;
584  const float threehalfs = 1.5F;
585 
586  x2 = x * 0.5F;
587  y.fvalue = x;
588  i.ivalue = y.ivalue; // evil floating point bit level hacking
589  i.ivalue = 0x5f3759df - (i.ivalue >> 1);
590  y.fvalue = i.fvalue;
591  y.fvalue = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 1st iteration
592  y.fvalue = y.fvalue * (threehalfs - (x2 * y.fvalue * y.fvalue)); // 2nd iteration, this can be removed
593 
594  return fabs(y.fvalue);
595 }
596 
597 } // namespace turbomath
float dot(const Vector &v) const
Definition: turbomath.cpp:156
d
static const float atan_max_x
Definition: turbomath.cpp:318
Vector cross(const Vector &v) const
Definition: turbomath.cpp:162
float atan(float x)
Definition: turbomath.cpp:460
Vector operator*(const Vector &v) const
Definition: turbomath.cpp:229
float cos(float x)
Definition: turbomath.cpp:422
float sqrd_norm() const
Definition: turbomath.cpp:64
float sin(float x)
Definition: turbomath.cpp:427
Vector & operator=(const Vector &v)
Definition: turbomath.cpp:87
float atan2(float y, float x)
Definition: turbomath.cpp:488
float fsign(float y)
Definition: turbomath.cpp:417
#define M_PI
Definition: turbomath.cpp:315
Vector & normalize()
Definition: turbomath.cpp:70
static const int16_t sin_lookup_table[125]
Definition: turbomath.cpp:400
static const int16_t asin_lookup_table[200]
Definition: turbomath.cpp:343
float alt(float press)
Definition: turbomath.cpp:549
float fabs(float x)
Definition: turbomath.cpp:571
Quaternion & from_RPY(float roll, float pitch, float yaw)
Definition: turbomath.cpp:273
static const float sin_max_x
Definition: turbomath.cpp:396
void get_RPY(float *roll, float *pitch, float *yaw) const
Definition: turbomath.cpp:305
static const float atan_scale_factor
Definition: turbomath.cpp:320
Vector normalized() const
Definition: turbomath.cpp:80
Quaternion & from_two_unit_vectors(const Vector &u, const Vector &v)
Definition: turbomath.cpp:247
float asin(float x)
Definition: turbomath.cpp:528
static const float max_pressure
Definition: turbomath.cpp:368
static const float atan_min_x
Definition: turbomath.cpp:319
static const int16_t sin_num_entries
Definition: turbomath.cpp:399
Vector operator+(const Vector &v) const
Definition: turbomath.cpp:96
Vector boxminus(const Quaternion &q) const
Definition: turbomath.cpp:292
Quaternion & normalize()
Definition: turbomath.cpp:185
static const float asin_max_x
Definition: turbomath.cpp:339
Quaternion & invert()
Definition: turbomath.cpp:239
static const int16_t pressure_lookup_table[200]
Definition: turbomath.cpp:372
Quaternion inverse() const
Definition: turbomath.cpp:234
float inv_sqrt(float x)
Definition: turbomath.cpp:580
Vector operator*(float s) const
Definition: turbomath.cpp:126
float norm() const
Definition: turbomath.cpp:58
Vector operator/(float s) const
Definition: turbomath.cpp:132
static const float asin_scale_factor
Definition: turbomath.cpp:341
static const float sin_min_x
Definition: turbomath.cpp:397
static const int16_t atan_lookup_table[125]
Definition: turbomath.cpp:322
static const int16_t pressure_num_entries
Definition: turbomath.cpp:371
Vector & operator-=(const Vector &v)
Definition: turbomath.cpp:117
static const int16_t asin_num_entries
Definition: turbomath.cpp:342
float arr[3]
Definition: turbomath.h:66
Vector & operator/=(float s)
Definition: turbomath.cpp:147
static const float pressure_scale_factor
Definition: turbomath.cpp:370
static const float sin_scale_factor
Definition: turbomath.cpp:398
Vector & operator+=(const Vector &v)
Definition: turbomath.cpp:108
Vector & operator*=(float s)
Definition: turbomath.cpp:138
Vector operator-(const Vector &v) const
Definition: turbomath.cpp:102
static const float asin_min_x
Definition: turbomath.cpp:340
static const float min_pressure
Definition: turbomath.cpp:369
static Vector log(const Quaternion &q)
Definition: turbomath.h:129
Vector rotate(const Vector &v) const
Definition: turbomath.cpp:222
Quaternion & operator*=(const Quaternion &q)
Definition: turbomath.cpp:213
static const int16_t atan_num_entries
Definition: turbomath.cpp:321


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:26