41 x(*arr), y(*(arr+1)), z(*(arr+2))
83 Vector out(
x*recip_norm,
y*recip_norm, z*recip_norm);
158 return x*v.
x +
y*v.
y +
z*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);
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;
315 #define M_PI 3.14159265359 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,
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,
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,
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
419 return (0.0
f < y) - (y < 0.0f);
446 int16_t index =
static_cast<int16_t
>(t);
447 float delta_x = t - index;
449 if (index >= sin_num_entries)
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;
455 return sin_lookup_table[index]/sin_scale_factor + delta_x * (sin_lookup_table[index] - sin_lookup_table[index -
456 1])/sin_scale_factor;
465 return -1.0*
atan(-1.0*x);
474 int16_t index =
static_cast<int16_t
>(t);
475 float delta_x = t - index;
477 if (index >= atan_num_entries)
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;
483 return atan_lookup_table[index]/atan_scale_factor + delta_x * (atan_lookup_table[index] - atan_lookup_table[index -
484 1])/atan_scale_factor;
507 float arctan =
atan(y/x);
513 return arctan -
M_PI;
517 return arctan +
M_PI;
532 return -1.0*
asin(-1.0*x);
536 int16_t index =
static_cast<int16_t
>(t);
537 float delta_x = t - index;
539 if (index >= asin_num_entries)
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;
545 return asin_lookup_table[index]/asin_scale_factor + delta_x * (asin_lookup_table[index] - asin_lookup_table[index -
546 1])/asin_scale_factor;
552 if (press < max_pressure && press > min_pressure)
555 int16_t index =
static_cast<int16_t
>(t);
556 float delta_x = t - index;
558 if (index >= pressure_num_entries)
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;
564 return pressure_lookup_table[index]/pressure_scale_factor + delta_x * (pressure_lookup_table[index] -
565 pressure_lookup_table[index - 1])/pressure_scale_factor;
584 const float threehalfs = 1.5F;
float dot(const Vector &v) const
static const float atan_max_x
Vector cross(const Vector &v) const
Vector operator*(const Vector &v) const
Vector & operator=(const Vector &v)
float atan2(float y, float x)
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
void get_RPY(float *roll, float *pitch, float *yaw) const
static const float atan_scale_factor
Vector normalized() const
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+(const Vector &v) const
Vector boxminus(const Quaternion &q) const
static const float asin_max_x
static const int16_t pressure_lookup_table[200]
Quaternion inverse() const
Vector operator*(float s) const
Vector operator/(float s) const
static const float asin_scale_factor
static const float sin_min_x
static const int16_t atan_lookup_table[125]
static const int16_t pressure_num_entries
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)
Vector operator-(const Vector &v) const
static const float asin_min_x
static const float min_pressure
static Vector log(const Quaternion &q)
Vector rotate(const Vector &v) const
Quaternion & operator*=(const Quaternion &q)
static const int16_t atan_num_entries