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


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:20