49 return x *
x +
y *
y +
z *
z;
64 Vector out(
x * recip_norm,
y * recip_norm, z * recip_norm);
122 return x * v.
x +
y * v.
y +
z * v.
z;
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);
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;
182 (1.0
f - 2.0
f *
y *
y - 2.0
f *
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);
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;
271 #define M_PI 3.14159265359 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,
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,
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,
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};
345 return (0.0
f < y) - (y < 0.0f);
362 return -1.0 *
sin(-x);
370 int16_t index =
static_cast<int16_t
>(t);
371 float delta_x = t - index;
373 if (index >= sin_num_entries)
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;
379 return sin_lookup_table[index] / sin_scale_factor
380 + delta_x * (sin_lookup_table[index] - sin_lookup_table[index - 1]) / sin_scale_factor;
388 return -1.0 *
atan(-1.0 * x);
397 int16_t index =
static_cast<int16_t
>(t);
398 float delta_x = t - index;
400 if (index >= atan_num_entries)
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;
406 return atan_lookup_table[index] / atan_scale_factor
407 + delta_x * (atan_lookup_table[index] - atan_lookup_table[index - 1]) / atan_scale_factor;
429 float arctan =
atan(y / x);
435 return arctan -
M_PI;
439 return arctan +
M_PI;
453 return -1.0 *
asin(-1.0 * x);
457 int16_t index =
static_cast<int16_t
>(t);
458 float delta_x = t - index;
460 if (index >= asin_num_entries)
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;
466 return asin_lookup_table[index] / asin_scale_factor
467 + delta_x * (asin_lookup_table[index] - asin_lookup_table[index - 1]) / asin_scale_factor;
472 if (press < max_pressure && press > min_pressure)
475 int16_t index =
static_cast<int16_t
>(t);
476 float delta_x = t - index;
478 if (index >= pressure_num_entries)
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;
484 return pressure_lookup_table[index] / pressure_scale_factor
485 + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) / pressure_scale_factor;
503 const float threehalfs = 1.5F;
Vector operator-(const Vector &v) const
Vector boxminus(const Quaternion &q) const
Vector rotate(const Vector &v) const
static const float atan_max_x
Vector operator+(const Vector &v) const
Vector normalized() const
float atan2(float y, float x)
float dot(const Vector &v) const
Quaternion inverse() const
static const int16_t sin_lookup_table[125]
static const int16_t asin_lookup_table[200]
Quaternion & from_RPY(float roll, float pitch, float yaw)
static const float sin_max_x
static const float atan_scale_factor
Quaternion & from_two_unit_vectors(const Vector &u, const Vector &v)
static const float max_pressure
static const float atan_min_x
static const int16_t sin_num_entries
Vector operator/(float s) const
void get_RPY(float *roll, float *pitch, float *yaw) const
static const float asin_max_x
static const int16_t pressure_lookup_table[200]
Vector operator*(float s) const
static const float asin_scale_factor
Vector operator*(const Vector &v) const
static const float sin_min_x
static const int16_t atan_lookup_table[125]
static const int16_t pressure_num_entries
Vector cross(const Vector &v) const
Vector & operator-=(const Vector &v)
static const int16_t asin_num_entries
Vector & operator/=(float s)
static const float pressure_scale_factor
static const float sin_scale_factor
Vector & operator+=(const Vector &v)
Vector & operator*=(float s)
static const float asin_min_x
static const float min_pressure
static Vector log(const Quaternion &q)
Quaternion & operator*=(const Quaternion &q)
static const int16_t atan_num_entries