34#include <NDEVR/Angle.h>
35#include <NDEVR/Vector.h>
43 template<
class t_type_a,
class t_type_b>
44 constexpr static bool operator>(
const Angle<t_type_a>&
angle_a,
const Angle<t_type_b>&
angle_b)
52 template<
class t_type_a,
class t_type_b>
53 constexpr static bool operator<(
const Angle<t_type_a>&
angle_a,
const Angle<t_type_b>&
angle_b)
61 template<
class t_type_a,
class t_type_b>
62 constexpr static bool operator>=(
const Angle<t_type_a>&
angle_a,
const Angle<t_type_b>&
angle_b)
70 template<
class t_type_a,
class t_type_b>
71 constexpr static bool operator<=(
const Angle<t_type_a>&
angle_a,
const Angle<t_type_b>&
angle_b)
78 template<
class t_type>
103 template<
class t_type>
106 return ::sin(angle.template as<RADIANS>());
113 template<
class t_type>
114 t_type
sin(
const t_type& angle)
123 template<
class t_type>
127 return sin(internal_angle);
134 template<
class t_type>
137 return ::cos(angle.template as<RADIANS>());
144 template<
class t_type>
145 t_type
cos(
const t_type& angle)
155 template<
class t_type>
158 return sin(angle) /
cos(angle);
165 template<
class t_type>
168 return ::tan(angle.template as<RADIANS>());
176 template<
class t_type>
177 t_type
tan(
const t_type& angle)
188 template<
class t_type>
189 constexpr static Angle<t_type> operator-(
const Angle<t_type>&
angle_a,
const Angle<t_type>&
angle_b)
199 template<
class t_type>
200 constexpr static Angle<t_type> operator+(
const Angle<t_type>&
angle_a,
const Angle<t_type>&
angle_b)
211 template<
class t_type>
212 constexpr static Angle<t_type> difference(
const Angle<t_type>&
angle_a,
const Angle<t_type>&
angle_b)
215 if (angle <= Angle<t_type>(
DEGREES, 180.0f))
218 return Angle<t_type>(
DEGREES, -360.0f) + angle;
227 template<u
int01 t_dims,
class t_type>
228 constexpr static Vector<t_dims, Angle<t_type>> difference(
const Vector<t_dims, Angle<t_type>>&
angle_a,
const Vector<t_dims, Angle<t_type>>&
angle_b)
230 Vector<t_dims, Angle<t_type>> angle;
231 for (
uint01 dim = 0; dim < t_dims; ++dim)
245 template<
class t_type>
246 constexpr static Angle<t_type> Average(
const Angle<t_type>&
angle_a,
const Angle<t_type>&
angle_b)
260 template<u
int01 t_dims,
class t_type>
261 constexpr static Vector<t_dims, Angle<t_type>> Average(
const Vector<t_dims, Angle<t_type>>&
angle_a,
const Vector<t_dims, Angle<t_type>>&
angle_b)
263 Vector<t_dims, Angle<t_type>> angle;
264 for (
uint01 dim = 0; dim < t_dims; ++dim)
277 template<
class t_type>
278 constexpr static Angle<t_type> operator*(
const Angle<t_type>&
angle_a,
const Angle<t_type>&
angle_b)
289 template<
class t_type>
301 template<
class t_type,
class t_angle_type>
302 constexpr static Angle<t_angle_type> operator*(
const Angle<t_angle_type>& angle, t_type mult)
313 template<
class t_type,
class t_angle_type>
314 constexpr static Angle<t_angle_type> operator*(t_type mult,
const Angle<t_angle_type>& angle)
324 template<
class t_type,
class t_vector_type>
325 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
326 operator*(
const t_vector_type& angle,
const t_type& mult)
328 t_vector_type product;
329 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
330 product[dim] = angle[dim] * mult;
340 template<
class t_type,
class t_vector_type>
341 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
342 operator*(
const t_vector_type& angle,
const t_type& mult)
344 t_vector_type product;
345 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
346 product[dim] = angle[dim] * mult;
356 template<u
int01 t_dims,
class t_type,
class t_vector_type>
357 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<fltp08>>, t_vector_type>::value, t_vector_type>::type
360 t_vector_type product;
361 for (
uint01 dim = 0; dim < t_dims; ++dim)
362 product[dim] = angle[dim] * mult[dim];
372 template<u
int01 t_dims,
class t_type,
class t_vector_type>
373 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<sint04>>, t_vector_type>::value, t_vector_type>::type
376 t_vector_type product;
377 for (
uint01 dim = 0; dim < t_dims; ++dim)
378 product[dim] = angle[dim] * mult[dim];
382 template<
class t_type,
class t_vector_type>
383 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
384 operator*(
const t_type& mult,
const t_vector_type& angle)
386 t_vector_type product;
387 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
388 product[dim] = mult * angle[dim];
391 template<
class t_type,
class t_vector_type>
392 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
393 operator*(
const t_type& mult,
const t_vector_type& angle)
395 t_vector_type product;
396 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
397 product[dim] = mult * angle[dim];
400 template<u
int01 t_dims,
class t_type,
class t_vector_type>
401 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<fltp08>>, t_vector_type>::value, t_vector_type>::type
404 t_vector_type product;
405 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
406 product[dim] = mult[dim] * angle[dim];
409 template<u
int01 t_dims,
class t_type,
class t_vector_type>
410 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<sint04>>, t_vector_type>::value, t_vector_type>::type
413 t_vector_type product;
414 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
415 product[dim] = mult[dim] * angle[dim];
418 template<
class t_type,
class t_vector_type>
419 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
420 operator*(
const t_vector_type& mult,
const t_vector_type& angle)
422 t_vector_type product;
423 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
424 product[dim] = mult[dim] * angle[dim];
427 template<
class t_type,
class t_vector_type>
428 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
429 operator*(
const t_vector_type& mult,
const t_vector_type& angle)
431 t_vector_type product;
432 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
433 product[dim] = mult[dim] * angle[dim];
442 template<
class t_type,
class t_angle_type>
443 constexpr static Angle<t_angle_type> operator/(
const Angle<t_angle_type>& num, t_type den)
454 template<
class t_type,
class t_vector_type,
class t_angle_type>
455 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<t_angle_type>>::value, t_vector_type>::type
456 operator/(
const t_vector_type& angle,
const t_type& den)
459 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
460 div[dim] = angle[dim] / den;
470 template<
class t_type,
class t_vector_type>
471 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<fltp08>>::value, t_vector_type>::type
472 operator/(
const t_vector_type& angle,
const t_type& den)
475 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
476 div[dim] = angle[dim] / den;
486 template<
class t_type,
class t_vector_type>
487 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<sint04>>::value, t_vector_type>::type
488 operator/(
const t_vector_type& angle,
const t_type& den)
491 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
492 div[dim] = angle[dim] / den;
502 template<u
int01 t_dims,
class t_type,
class t_vector_type,
class t_angle_type>
503 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<t_angle_type>>, t_vector_type>::value, t_vector_type>::type
507 for (
uint01 dim = 0; dim < t_dims; ++dim)
508 div[dim] = angle[dim] / den[dim];
518 template<u
int01 t_dims,
class t_vector_type,
class t_angle_type>
519 constexpr typename std::enable_if<std::is_base_of<Vector<t_dims, Angle<t_angle_type>>, t_vector_type>::value, Vector<t_dims, fltp08>>::type
523 for (
uint01 dim = 0; dim < t_dims; ++dim)
524 div[dim] = angle[dim] / den[dim];
533 template<u
int01 t_dims,
class t_angle_type>
537 for (
uint01 dim = 0; dim < t_dims; ++dim)
538 div[dim] = angle[dim] / den[dim];
542 template<
class t_type,
class t_angle_type>
543 constexpr static Angle<t_angle_type> operator/(t_type num,
const Angle<t_angle_type>& den)
547 template<
class t_type,
class t_vector_type,
class t_angle_type>
548 constexpr typename std::enable_if<IsVecType<t_vector_type, Angle<t_angle_type>>::value, t_vector_type>::type
549 operator/(t_type num,
const t_vector_type& angle)
552 for (
uint01 dim = 0; dim < t_vector_type::NumberOfDimensions(); ++dim)
553 div[dim] = num / angle[dim];
556 template<
class t_angle_type>
557 constexpr static Angle<t_angle_type>& operator+=(Angle<t_angle_type>& angle,
const Angle<t_angle_type>& add)
562 template<
class t_angle_type>
563 constexpr static Angle<t_angle_type>& operator-=(Angle<t_angle_type>& angle,
const Angle<t_angle_type>& sub)
568 template<
class t_type,
class t_angle_type>
569 constexpr static Angle<t_angle_type>& operator*=(Angle<t_angle_type>& angle,
const t_type& mult)
571 angle = angle * mult;
574 template<u
int01 t_dims,
class t_type,
class t_angle_type>
575 constexpr static Vector<t_dims, Angle<t_angle_type>>& operator*=(Vector<t_dims, Angle<t_angle_type>>& angle,
const t_type& mult)
577 for (
uint01 dim = 0; dim < t_dims; ++dim)
578 angle[dim] = angle[dim] * mult;
581 template<u
int01 t_dims,
class t_type,
class t_angle_type>
582 constexpr static Vector<t_dims, Angle<t_angle_type>>& operator*=(Vector<t_dims, Angle<t_angle_type>>& angle,
const Vector<t_dims, t_type>& mult)
584 for (
uint01 dim = 0; dim < t_dims; ++dim)
585 angle[dim] = angle[dim] * mult[dim];
588 template<
class t_angle_type>
589 constexpr static Angle<t_angle_type>& operator*=(Angle<t_angle_type>& angle,
const Angle<t_angle_type>& mult)
591 angle = angle * mult;
595 template<
class t_type,
class t_angle_type>
596 constexpr static Angle<t_angle_type>& operator/=(Angle<t_angle_type>& angle,
const t_type& mult)
598 angle = angle / mult;
601 template<u
int01 t_dims,
class t_type,
class t_angle_type>
602 constexpr static Vector<t_dims, Angle<t_angle_type>>& operator/=(Vector<t_dims, Angle<t_angle_type>>& angle,
const t_type& den)
604 for (
uint01 dim = 0; dim < t_dims; ++dim)
605 angle[dim] = angle[dim] / den;
608 template<u
int01 t_dims,
class t_type,
class t_angle_type>
609 constexpr static Vector<t_dims, Angle<t_angle_type>>& operator/=(Vector<t_dims, Angle<t_angle_type>>& angle,
const Vector<t_dims, t_type>& den)
611 for (
uint01 dim = 0; dim < t_dims; ++dim)
612 angle[dim] = angle[dim] / den[dim];
616 template<u
int01 t_dims,
class t_angle_type>
620 for (
uint01 dim = 0; dim < t_dims; ++dim)
630 template<u
int01 t_dims,
class t_angle_type>
634 for (
uint01 dim = 0; dim < t_dims; ++dim)
644 template<
class t_angle_type>
656 template<
class t_new_type, u
int01 t_dims,
class t_angle_type>
660 for(
uint01 dim = 0; dim < t_dims; ++dim)
661 angle[dim] = old[dim].
template toTypeAngle<t_new_type>();
693 template<
class t_angle_type, u
int01 t_dims,
class t_type>
713 template<
class t_angle_type>
719 template<
class t_angle_type, u
int01 t_dims,
class t_type>
726 template<
class t_angle_type, u
int01 t_dims,
class t_type>
732 template<
class t_angle_type, u
int01 t_dims,
class t_type>
736 for (
uint01 i = 0; i < t_dims; i++)
738 return orient_vector;
740 template<
bool t_normalized, u
int01 t_dims,
class t_angle_type>
744 for (
uint01 i = 0; i < t_dims; i++)
745 orient_vector[i] = ray[i].as(angle_type);
746 return orient_vector;
762 template<
class t_angle_type,
class t_type>
767 if (::fabs(
cos(pitch)) < 0.001)
769 else if (::fabs(
cos(yaw)) < 0.01)
778 template<
class t_angle_type>
781 fltp08 suggested_pitch_value = angle[
PITCH].template as<DEGREES>();
782 fltp08 our_pitch_value = reference[
PITCH].template as<DEGREES>();
784 fltp08 suggested_yaw_value = angle[
YAW].template as<DEGREES>();
785 fltp08 our_yaw_value = reference[
YAW].template as<DEGREES>();
787 fltp08 suggested_roll_value = angle[
ROLL].template as<DEGREES>();
788 fltp08 our_roll_value = reference[
ROLL].template as<DEGREES>();
790 if (suggested_pitch_value - our_pitch_value > 180)
791 our_pitch_value += 360;
792 else if (our_pitch_value - suggested_pitch_value > 180)
793 suggested_pitch_value += 360;
795 if (suggested_yaw_value - our_yaw_value > 180)
796 our_yaw_value += 360;
797 else if (our_yaw_value - suggested_yaw_value > 180)
798 suggested_yaw_value += 360;
800 if (suggested_roll_value - our_roll_value > 180)
801 our_roll_value += 360;
802 else if (our_roll_value - suggested_roll_value > 180)
803 suggested_roll_value += 360;
805 bool flip = ::abs(suggested_roll_value - our_roll_value) >= 90.0f && ::abs(suggested_yaw_value - our_yaw_value) >= 90.0f;
827 template<u
int01 t_dims,
class t_angle_type>
833 template<u
int01 t_dims,
class t_angle_type>
Logic for converting between Euler angles and basic rotations or normals.
Definition AngleFunctions.h:669
static Vector< 3, Angle< t_angle_type > > NormalToOrientation(const Vector< 3, t_type > &normal, const Angle< t_angle_type > &yaw=Angle< t_angle_type >(0))
Definition AngleFunctions.h:763
static Vector< 3, Angle< t_angle_type > > NormalizeOrientation(Vector< 3, Angle< t_angle_type > > angle, const Vector< 3, Angle< t_angle_type > > &reference)
Definition AngleFunctions.h:779
static Vector< 3, Angle< fltp08 > > QuaternionToOrientation(const Vector< 4, fltp08 > &quaternion)
Converts a quaternion to Euler angles (Roll, Pitch and Yaw)
static constexpr Angle< t_angle_type > Heading(const Vector< t_dims, t_type > ray)
Definition AngleFunctions.h:727
static constexpr Vector< t_dims, fltp08 > Orientation(AngleType angle_type, const Vector< t_dims, Angle< t_angle_type > > &ray)
Definition AngleFunctions.h:741
static Angle< t_angle_type > Rotation(const Vector< t_dims, t_type > &left, const Vector< t_dims, t_type > &middle, const Vector< t_dims, t_type > &right)
Definition AngleFunctions.h:694
static Angle< t_angle_type > Inclination(const Angle< t_angle_type > &roll, const Angle< t_angle_type > &pitch)
Definition AngleFunctions.h:714
static const fltp08 * CalcIndexSin()
static constexpr Vector< t_dims, Angle< t_angle_type > > Orientation(AngleType angle_type, const Vector< t_dims, t_type > &ray)
Definition AngleFunctions.h:733
static constexpr Angle< t_angle_type > Inclination(const Vector< t_dims, t_type > ray)
Definition AngleFunctions.h:720
The primary angle storage class for this API. Stores an angle in an optimized format.
Definition StringStream.h:540
A fixed-size array with better performance compared to dynamic containers.
Definition Vector.hpp:60
constexpr t_type magnitudeSquared() const
Definition Vector.hpp:426
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type tan(const Angle< t_type > &angle)
Performs optimized tangent operation on the given angle using pre-computed lookup table for optimal s...
Definition AngleFunctions.h:156
t_type dot(const Vector< t_dims, t_type > &v1, const Vector< t_dims, t_type > &v2)
Definition VectorFunctions.hpp:1030
@ ROLL
Definition Angle.h:45
@ YAW
Definition Angle.h:47
@ PITCH
Definition Angle.h:46
constexpr Vector< t_dims, Angle< t_angle_type > > quantize(const Vector< t_dims, Angle< t_angle_type > > &value, Angle< t_angle_type > d=Angle< t_angle_type >(DEGREES, 1.0))
Definition AngleFunctions.h:828
@ angle_b
Definition Triangle.hpp:56
@ angle_a
Definition Triangle.hpp:55
uint8_t uint01
-Defines an alias representing a 1 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:80
AngleType
The possible units that can be used by the angle class. These are returned and used for real world an...
Definition Angle.h:56
@ RADIANS
Definition Angle.h:57
@ DEGREES
Definition Angle.h:58
@ INTERNAL_ANGLE
Definition Angle.h:59
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type cos(const Angle< t_type > &angle)
Performs optimized cosine operation on the given angle using pre-computed lookup table for optimal sp...
Definition AngleFunctions.h:124
constexpr Vector< t_dims, Angle< t_new_type > > ToTypeAngle(const Vector< t_dims, Angle< t_angle_type > > &old)
Converts a Vector of one angle to a different container type.
Definition AngleFunctions.h:657
t_type sqrt(const t_type &value)
Definition VectorFunctions.hpp:1225
constexpr t_to cast(const Angle< t_from > &value)
Definition Angle.h:375
uint16_t uint02
-Defines an alias representing a 2 byte, unsigned integer -Can represent exact integer values 0 throu...
Definition BaseValues.hpp:88
constexpr Angle< t_angle_type > abs(const Angle< t_angle_type > &value)
Changes an input with a negative sign, to a positive sign.
Definition AngleFunctions.h:645
std::enable_if<!ObjectInfo< t_type >::Float, fltp08 >::type sin(const Angle< t_type > &angle)
Performs optimized sine operation on the given angle using pre-computed lookup table for optimal spee...
Definition AngleFunctions.h:79
const fltp08 *const s_index_sin
constexpr Vector< t_dims, Angle< t_angle_type > > operator%(const Vector< t_dims, Angle< t_angle_type > > &vec_a, const Vector< t_dims, Angle< t_angle_type > > &vec_b)
Definition AngleFunctions.h:617
@ Y
Definition BaseValues.hpp:169
@ X
Definition BaseValues.hpp:167
@ Z
Definition BaseValues.hpp:171
double fltp08
Defines an alias representing an 8 byte floating-point number.
Definition BaseValues.hpp:149