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


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:16