API tools faq
paste
Advertisement
Swiftkill

quat.h translated

Swiftkill
Sep 8th, 2023 (edited)
958
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 14.08 KB | None | 0 0
raw download clone embed print report
  1. /************************************************ ************************//**
  2. * @filequat.h
  3. * @brief Description of the quaternion class.
  4. *
  5. * @date
  6. * @todo comments
  7. **************************************** ****************************/
  8.  
  9. // What is a quaternion?
  10. // ---------------------
  11. // a quaternion is a point in 4-dimensional complex space.
  12. // Q = { Qx, Qy, Qz, Qw }
  13. //
  14. // Why are quaternions needed?
  15. //-----------------------
  16. // Set of all quaternions belonging to a sphere with radius 1
  17. // corresponds to the set of all possible rotation transformations in
  18. // three-dimensional Euclidean space. Rotation is easier to describe
  19. // in quaternion space than in rotation matrix space.
  20. // * Quaternion is identical to a 4-dimensional vector, easier to store in arrays,
  21. // as float or _m128
  22. // * There is no problem of choosing a storage method
  23. // * Being small
  24. // * Multiplying a vector by a quaternion contains fewer multiplications
  25. //
  26. // How to use quaternions?
  27. // -----------------------------
  28. // You should know how to get a quaternion from a matrix and vice versa
  29. // The rotation of vector v can be written as follows:
  30. //
  31. // v' = v * R
  32. //
  33. // 1) vector v' describes the same point in a different coordinate system
  34. // 2) vector v' describes a point rotated in accordance with
  35. // conversion parameters
  36. //
  37. //
  38. // Quaternions and rotation around an axis
  39. // ---------------------------------
  40. // Suppose we want to record a rotation by an angle (theta)
  41. // around the axis described by the unit vector ({Ax, Ay, Az})...
  42. //
  43. // s = sin(0.5 * theta)
  44. // c = cos(0.5 * theta)
  45. // Q = { s * Ax, s * Ay, s * Az, c }
  46. //
  47. // If vector A is unit, then the quaternion metric is too.
  48. //
  49. // 3x3 matrix corresponding to the quaternion
  50. // -----------------------------------------
  51. // Q = { x, y, z ,w }
  52. //
  53. //     |                                                                    |
  54. //     | 1 - 2 * (y^2 + z^2)   2 * (x * y + z * w)     2 * (y * w - x * z)  |
  55. //     |                                                                    |
  56. // M = | 2 * (x * y - z * w)   1 - 2 * (x^2 + z^2)     2 * (y * z + x * w)  |
  57. //     |                                                                    |
  58. //     | 2 * (x * z + y * w)   2 * (y * z - x * w)     1 - 2 * (x^2 + y^2)  |
  59. //     |  
  60.  
  61. #ifndef QUAT_H
  62. #define QUAT_H
  63. #include "point2d.h"
  64.  
  65. class CVector4F;
  66. class CVector3F;
  67. class CVector3D;
  68. class CMatrix4;
  69. class CMatrix3;
  70.  
  71. // Order of rotations for Euler angles:
  72. //
  73. // rotates the SCS around the original x by 'roll'
  74. // rotates the SSC around the original y to 'pitch'
  75. // rotates the SSC around the original z to 'yaw'.
  76. //               .
  77. //              /|\ yaw axis
  78. //               |     __.
  79. //  ._        ___|      /| pitch axis
  80. // _||\       \\ |-.   /
  81. // \|| \_______\_|__\_/_______
  82. //  | _ _   o o o_o_o_o o   /_\__  ________\ roll axis
  83. //  //  /_______/    /__________/          /  
  84. // /_,-'       //   /
  85. //            /__,-'
  86. //
  87. // Angles are specified in radians
  88.  
  89. //************************************************ *******************************
  90. // NOTA BENE: Quaternion must be unary!!!! And all the vectors and matrices
  91. // must be normalized. Otherwise it will be bad!!!
  92. //************************************************ *******************************
  93. static const U32 LENGTHOFQUAT = 4;
  94.  
  95. class CQuaternion
  96. {
  97.     F32 mQ[LENGTHOFQUAT];
  98. public:
  99.     /// Creates a unit quaternion (0,0,0,1)
  100.     CQuaternion();                     
  101.     /// Creates a rotation quaternion from Matrix4
  102.     explicit CQuaternion(const CMatrix4 &mat); 
  103.     /// Creates a rotation quaternion from Matrix3
  104.     explicit CQuaternion(const CMatrix3 &mat);     
  105.     /// Creates a normalized quaternion based on the vector (x, y, z, w)
  106.     CQuaternion(F32 x, F32 y, F32 z, F32 w);       
  107.     /// Creates a quaternion of rotation around the vec axis by angle angle (rad)
  108.     CQuaternion(F32 angle, const CVector4F &vec);
  109.     /// Creates a quaternion of rotation around the vec axis by angle angle (rad)
  110.     CQuaternion(F32 angle, const CVector3F &vec);  
  111.     /// Creates a normalized quaternion from an array
  112.     CQuaternion(const F32 *q); 
  113.     /// Creates a quaternion from the SCS basis [x_axis ; y_axis ; z_axis]                 
  114.     CQuaternion(const CVector3F &x_axis,
  115.                  const CVector3F &y_axis,
  116.                  const CVector3F &z_axis);         
  117.  
  118.     F32 operator[](int idx) const { return mQ[idx]; }
  119.     F32 &operator[](int idx) { return mQ[idx]; }
  120.  
  121.     // Single?
  122.     bool isIdentity() const;
  123.     // Not single?
  124.     bool isNotIdentity() const;
  125.     /// Checks the finiteness of values
  126.     bool isFinite() const;     
  127.     /// rounds values ​​to simulate integer representation
  128.     void quantize16(F32 lower, F32 upper);                 
  129.     /// rounds values ​​to simulate integer representation
  130.     void quantize8(F32 lower, F32 upper);                  
  131.     /// unit quaternion
  132.     void loadIdentity();                                   
  133.  
  134.     const CQuaternion& set(F32 x, F32 y, F32 z, F32 w); /// normalized quaternion based on vector(x, y, z, w)
  135.     const CQuaternion& set(const CQuaternion &quat); /// copy
  136.     const CQuaternion& set(const F32 *q); /// normalized quaternion (quat[VX], quat[VY], quat[VZ], quat[VW])
  137.     const CQuaternion& set(const CMatrix3 &mat); /// Creates a rotation quaternion from a matrix
  138.     const CQuaternion& set(const CMatrix4 &mat); /// Creates a rotation quaternion from a matrix
  139.  
  140.     /// quaternion corresponding to rotation around the (x,y,z) axis by angle angle (rad)
  141.     const CQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z);   
  142.     /// quaternion corresponding to rotation around the vec axis by angle angle (rad)
  143.     const CQuaternion& setAngleAxis(F32 angle, const CVector3F &vec);  
  144.     /// quaternion corresponding to rotation around the vec axis by angle angle (rad)
  145.     const CQuaternion& setAngleAxis(F32 angle, const CVector4F &vec);  
  146.     /// quaternion corresponding to Euler angles
  147.     const CQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw);   
  148.  
  149.     CMatrix4 getMatrix4(void) const;                           
  150.     CMatrix3 getMatrix3(void) const;               
  151.    
  152.     /// Returns the angle and axis of rotation 
  153.     void getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const;   
  154.     /// Returns the angle and axis of rotation
  155.     void getAngleAxis(F32* angle, CVector3F &vec) const;
  156.      /// Returns the Euler angles
  157.     void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
  158.  
  159.     /// Normalizes and returns the previous magnitude
  160.     F32 normalize();   
  161.  
  162.     /// Conjugate quaternion
  163.     const CQuaternion& conjugate(void);
  164.  
  165.     /// Mirror rotation
  166.     const CQuaternion& transpose();
  167.  
  168.     /// shortest rotation from direction a to b
  169.     void shortestArc(const CVector3F &a, const CVector3F &b);  
  170.     /// trims the rotation to the limits of a cone with a given angle
  171.     const CQuaternion& constrain(F32 radians);                 
  172.  
  173.  
  174.     friend std::ostream& operator<<(std::ostream &s, const CQuaternion &a);    
  175.     friend CQuaternion operator+(const CQuaternion &a, const CQuaternion &b);  
  176.     friend CQuaternion operator-(const CQuaternion &a, const CQuaternion &b);  
  177.     friend CQuaternion operator-(const CQuaternion &a);
  178.     friend CQuaternion operator*(F32 a, const CQuaternion &q); 
  179.     friend CQuaternion operator*(const CQuaternion &q, F32 b); 
  180.     friend CQuaternion operator*(const CQuaternion &a, const CQuaternion &b);
  181.     friend CQuaternion operator~(const CQuaternion &a); // pairing
  182.     bool operator==(const CQuaternion &b) const;           
  183.     bool operator!=(const CQuaternion &b) const;       
  184.  
  185.     friend const CQuaternion& operator*=(CQuaternion &a, const CQuaternion &b);
  186.  
  187.     // rotation of vector a
  188.     friend CVector4F operator*(const CVector4F &a, const CQuaternion &rot);
  189.     friend CVector3F operator*(const CVector3F &a, const CQuaternion &rot);    
  190.     friend CVector3D operator*(const CVector3D &a, const CQuaternion &rot);
  191.  
  192.     // scalar product
  193.     friend F32 dot(const CQuaternion &a, const CQuaternion &b);
  194.     // Interpolation (t = 0 ... 1) from p to q
  195.     friend CQuaternion lerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  196.     // Interpolation (t = 0 ... 1) from unit square. to q      
  197.     friend CQuaternion lerp(F32 t, const CQuaternion &q);                  
  198.     // Spherical interpolation from p to q     
  199.     friend CQuaternion slerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  200.     // Spherical interpolation from unit square. to q  
  201.     friend CQuaternion slerp(F32 t, const CQuaternion &q); 
  202.     // Normalized interpolation from p to q                    
  203.     friend CQuaternion nlerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  204.     // Normalized interpolation from unit square. to q 
  205.     friend CQuaternion nlerp(F32 t, const CQuaternion &q);                         
  206.  
  207. };
  208.  
  209. inline bool CQuaternion::isFinite() const
  210. {
  211.     return (isfinite(mQ[VX]) && isfinite(mQ[VY]) && isfinite(mQ[VZ]) && isfinite(mQ[VS]));
  212. }
  213.  
  214. inline bool CQuaternion::isIdentity() const
  215. {
  216.     return
  217.         ( mQ[VX] == 0.f ) &&
  218.         ( mQ[VY] == 0.f ) &&
  219.         ( mQ[VZ] == 0.f ) &&
  220.         ( mQ[VS] == 1.f );
  221. }
  222.  
  223. inline bool CQuaternion::isNotIdentity() const
  224. {
  225.     return
  226.         ( mQ[VX] != 0.f ) ||
  227.         ( mQ[VY] != 0.f ) ||
  228.         ( mQ[VZ] != 0.f ) ||
  229.         ( mQ[VS] != 1.f );
  230. }
  231.  
  232. inline CQuaternion::CQuaternion(void)
  233. {
  234.     mQ[VX] = 0.f;
  235.     mQ[VY] = 0.f;
  236.     mQ[VZ] = 0.f;
  237.     mQ[VS] = 1.f;
  238. }
  239.  
  240. inline CQuaternion::CQuaternion(F32 x, F32 y, F32 z, F32 w)
  241. {
  242.     mQ[VX] = x;
  243.     mQ[VY] = y;
  244.     mQ[VZ] = z;
  245.     mQ[VS] = w;
  246.     //RN:No normalization should be inserted here. Too expensive for temporary values, and rounding adds up
  247.     // set() should be used for normalization
  248. }
  249.  
  250. inline CQuaternion::CQuaternion(const F32 *q)
  251. {
  252.     mQ[VX] = q[VX];
  253.     mQ[VY] = q[VY];
  254.     mQ[VZ] = q[VZ];
  255.     mQ[VS] = q[VW];
  256.  
  257.     normalize();
  258. }
  259.  
  260.  
  261. inline void CQuaternion::loadIdentity()
  262. {
  263.     mQ[VX] = 0.0f;
  264.     mQ[VY] = 0.0f;
  265.     mQ[VZ] = 0.0f;
  266.     mQ[VW] = 1.0f;
  267. }
  268.  
  269. inline const CQuaternion& CQuaternion::set(F32 x, F32 y, F32 z, F32 w)
  270. {
  271.     mQ[VX] = x;
  272.     mQ[VY] = y;
  273.     mQ[VZ] = z;
  274.     mQ[VS] = w;
  275.     normalize();
  276.     return (*this);
  277. }
  278.  
  279. inline const CQuaternion& CQuaternion::set(const CQuaternion &quat)
  280. {
  281.     mQ[VX] = quat.mQ[VX];
  282.     mQ[VY] = quat.mQ[VY];
  283.     mQ[VZ] = quat.mQ[VZ];
  284.     mQ[VW] = quat.mQ[VW];
  285.     normalize();
  286.     return (*this);
  287. }
  288.  
  289. inline const CQuaternion& CQuaternion::set(const F32 *q)
  290. {
  291.     mQ[VX] = q[VX];
  292.     mQ[VY] = q[VY];
  293.     mQ[VZ] = q[VZ];
  294.     mQ[VS] = q[VW];
  295.     normalize();
  296.     return (*this);
  297. }
  298.  
  299.  
  300. /// @todo Is it possible to get rid of sqrt... sin_a = VX*VX + VY*VY + VZ*VZ?
  301. // Copied from Matrix and Quaternion FAQ 1.12
  302. inline void CQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
  303. {
  304.     F32 cos_a = mQ[VW];
  305.     if (cos_a > 1.0f) cos_a = 1.0f;
  306.     if (cos_a < -1.0f) cos_a = -1.0f;
  307.  
  308.     F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
  309.  
  310.     if ( fabs( sin_a ) < 0.0005f )
  311.         sin_a = 1.0f;
  312.     else
  313.         sin_a = 1.f/sin_a;
  314.  
  315.     F32 temp_angle = 2.0f * (F32) acos( cos_a );
  316.     if (temp_angle > F_PI)
  317.     {
  318.         // Angles should never be outside [PI, -PI]
  319.         // and we need the shortest turn.
  320.         // Because acos is defined on the segment [0, PI], and we multiply the angle by two,
  321.         // then we can go beyond the segment. If you replace the angle with the corresponding one in
  322.         // another half-plane and invert the axis, this will correspond
  323.         // rotate in the other direction (right hand rule)
  324.         *angle = 2.f * F_PI - temp_angle;
  325.         *x = - mQ[VX] * sin_a;
  326.         *y = - mQ[VY] * sin_a;
  327.         *z = - mQ[VZ] * sin_a;
  328.     }
  329.     else
  330.     {
  331.         *angle = temp_angle;
  332.         *x = mQ[VX] * sin_a;
  333.         *y = mQ[VY] * sin_a;
  334.         *z = mQ[VZ] * sin_a;
  335.     }
  336. }
  337.  
  338. inline const CQuaternion& CQuaternion::conjugate()
  339. {
  340.     mQ[VX] *= -1.f;
  341.     mQ[VY] *= -1.f;
  342.     mQ[VZ] *= -1.f;
  343.     return (*this);
  344. }
  345.  
  346. /// Reflection (mirror rotation)
  347. inline const CQuaternion& CQuaternion::transpose()
  348. {
  349.     mQ[VX] *= -1.f;
  350.     mQ[VY] *= -1.f;
  351.     mQ[VZ] *= -1.f;
  352.     return (*this);
  353. }
  354.  
  355. inline CQuaternion operator+(const CQuaternion &a, const CQuaternion &b)
  356. {
  357.     return CQuaternion(
  358.         a.mQ[VX] + b.mQ[VX],
  359.         a.mQ[VY] + b.mQ[VY],
  360.         a.mQ[VZ] + b.mQ[VZ],
  361.         a.mQ[VW] + b.mQ[VW] );
  362. }
  363.  
  364.  
  365. inline CQuaternion operator-(const CQuaternion &a, const CQuaternion &b)
  366. {
  367.     return CQuaternion(
  368.         a.mQ[VX] - b.mQ[VX],
  369.         a.mQ[VY] - b.mQ[VY],
  370.         a.mQ[VZ] - b.mQ[VZ],
  371.         a.mQ[VW] - b.mQ[VW] );
  372. }
  373.  
  374.  
  375. inline CQuaternion operator-(const CQuaternion &a)
  376. {
  377.     return CQuaternion(
  378.         -a.mQ[VX],
  379.         -a.mQ[VY],
  380.         -a.mQ[VZ],
  381.         -a.mQ[VW] );
  382. }
  383.  
  384.  
  385. inline CQuaternion operator*(F32 a, const CQuaternion &q)
  386. {
  387.     return CQuaternion(
  388.         a * q.mQ[VX],
  389.         a * q.mQ[VY],
  390.         a * q.mQ[VZ],
  391.         a * q.mQ[VW] );
  392. }
  393.  
  394.  
  395. inline CQuaternion operator*(const CQuaternion &q, F32 a)
  396. {
  397.     return CQuaternion(
  398.         a * q.mQ[VX],
  399.         a * q.mQ[VY],
  400.         a * q.mQ[VZ],
  401.         a * q.mQ[VW] );
  402. }
  403.  
  404. inline CQuaternion operator~(const CQuaternion &a)
  405. {
  406.     CQuaternion q(a);
  407.     q.conjugate();
  408.     return q;
  409. }
  410.  
  411. inline bool CQuaternion::operator==(const CQuaternion &b) const
  412. {
  413.     return ( (mQ[VX] == b.mQ[VX])
  414.             &&(mQ[VY] == b.mQ[VY])
  415.             &&(mQ[VZ] == b.mQ[VZ])
  416.             &&(mQ[VS] == b.mQ[VS]));
  417. }
  418.  
  419. inline bool CQuaternion::operator!=(const CQuaternion &b) const
  420. {
  421.     return ( (mQ[VX] != b.mQ[VX])
  422.             ||(mQ[VY] != b.mQ[VY])
  423.             ||(mQ[VZ] != b.mQ[VZ])
  424.             ||(mQ[VS] != b.mQ[VS]));
  425. }
  426.  
  427. inline const CQuaternion& operator*=(CQuaternion &a, const CQuaternion &b)
  428. {
  429.     CQuaternion q(
  430.         b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a .mQ[1],
  431.         b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a .mQ[2],
  432.         b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a .mQ[0],
  433.         b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a .mQ[2]
  434.     );
  435.     a = q;
  436.     return a;
  437. }
  438.  
  439. const F32 ONE_PART_IN_A_MILLION = 0.000001f;
  440.  
  441. inline F32 CQuaternion::normalize()
  442. {
  443.     F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
  444.  
  445.     if (mag > FP_MAG_THRESHOLD)
  446.     {
  447.         // Floating point error can prevent some quaternions from achieving
  448.         // exact unity length. When trying to renormalize such quaternions we
  449.         // can oscillate between multiple quantized states. To prevent such
  450.         // drifts we only renomalize if the length is far enough from unity.
  451.         if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
  452.         {
  453.             F32 oomag = 1.f/mag;
  454.             mQ[VX] *= oomag;
  455.             mQ[VY] *= oomag;
  456.             mQ[VZ] *= oomag;
  457.             mQ[VS] *= oomag;
  458.         }
  459.     }
  460.     else
  461.     {
  462.         // we were given a very bad quaternion so we set it to identity
  463.         mQ[VX] = 0.f;
  464.         mQ[VY] = 0.f;
  465.         mQ[VZ] = 0.f;
  466.         mQ[VS] = 1.f;
  467.     }
  468.  
  469.     return mag;
  470. }
  471.  
  472.  
  473. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!