API tools faq
paste
Advertisement
Swiftkill

quat.h

Swiftkill
Sep 8th, 2023
801
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 16.91 KB | None | 0 0
raw download clone embed print report
  1. /**************************************************************************//**
  2. *   @file  quat.h
  3. *   @brief Описание класса кватерниона.
  4. *
  5. *   @date
  6. *   @todo  комментарии
  7. ******************************************************************************/
  8.  
  9. // Что такое кватернион?
  10. // ---------------------
  11. // кватернион - это точка в  4-мерном комплексном пространстве.
  12. // Q = { Qx, Qy, Qz, Qw }
  13. //
  14. // Зачем нужны кватернионы?
  15. // ------------------------
  16. // Множество всех кватернионов, принадлежащих сфере с радиусом 1
  17. // соответствует множеству всех возможных преобразований вращения в
  18. // трехмерном эвклидовом пространстве. Вращение проще описать
  19. // в пространстве кватернионов, чем в пространстве матриц вращения.
  20. // * Кватернион идентичен 4хмерному вектору, проще хранить в массивах,
  21. // в виде float или _m128
  22. // * Нет проблемы выбора способа хранения
  23. // * Будучи маленьким
  24. // * Умножение вектора на кватернион содержит меньше умножений
  25. //
  26. // Как использовать кватернионы?
  27. // -----------------------------
  28. // Следует знать как получить кватернион из матрицы и  наооборот
  29. // Поворот вектора v может быть записан следующим образом:
  30. //
  31. //  v' = v * R
  32. //
  33. // 1) вектор v' описывает ту же точку в другой системе координат
  34. // 2) вектор v' описывает точку, повернутую в соответствии с
  35. // параметрами преобразования
  36. //
  37. //
  38. // Кватернионы и вращение вокруг оси
  39. // ---------------------------------
  40. // Предположим  мы хотим записать поворот на угол (theta)
  41. // вокруг оси , описанной единичным вектором ({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. // Если вектор А единичен, то метрика кватерниона тоже.
  48. //
  49. // Матрица 3x3 , соответствующая кватерниону
  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. // Потядок вращений для углов Эйлера:
  72. //
  73. // вращает ССК вокруг изначальной x на 'roll' (крен)
  74. // вращает ССК вокруг изначальной y на 'pitch' (тангаж)
  75. // вращает ССК вокруг изначальной z на '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. // Углы задаются в радианах
  88.  
  89. //******************************************************************************
  90. //  NOTA BENE: Кватернион д.б. единичен!!!! И все вектора и матрицы
  91. //             должны быть нормализованы. Иначе будет плохо!!!
  92. //******************************************************************************
  93. static const U32 LENGTHOFQUAT = 4;
  94.  
  95. class CQuaternion
  96. {
  97.     F32 mQ[LENGTHOFQUAT];
  98. public:
  99.     /// Создает единичный кватернион (0,0,0,1)
  100.     CQuaternion();                     
  101.     /// Создает кватернион поворота из Matrix4
  102.     explicit CQuaternion(const CMatrix4 &mat); 
  103.     /// Создает кватернион поворота из Matrix3
  104.     explicit CQuaternion(const CMatrix3 &mat);     
  105.     /// Создает нормализованный кватернион на основе вектора (x, y, z, w)
  106.     CQuaternion(F32 x, F32 y, F32 z, F32 w);       
  107.     /// Создает кватернион  поворота вокруг оси vec на угол angle (рад)
  108.     CQuaternion(F32 angle, const CVector4F &vec);
  109.     /// Создает кватернион  поворота вокруг оси vec на угол angle (рад)
  110.     CQuaternion(F32 angle, const CVector3F &vec);  
  111.     /// Создает нормализованный кватернион из массива
  112.     CQuaternion(const F32 *q); 
  113.     /// Создает кватернион из базиса ССК [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.     // Единичный?
  122.     bool isIdentity() const;
  123.     // Не единичный?
  124.     bool isNotIdentity() const;
  125.     /// Проверяет конечность значений
  126.     bool isFinite() const;     
  127.     /// округляет значения для имитации целочисленного представления
  128.     void quantize16(F32 lower, F32 upper);                 
  129.     /// округляет значения для имитации целочисленного представления
  130.     void quantize8(F32 lower, F32 upper);                  
  131.     /// единичный кватернион
  132.     void loadIdentity();                                   
  133.  
  134.     const CQuaternion&  set(F32 x, F32 y, F32 z, F32 w);        /// нормализованный кватернион на основе вектора(x, y, z, w)
  135.     const CQuaternion&  set(const CQuaternion &quat);           /// копирование
  136.     const CQuaternion&  set(const F32 *q);                      /// нормализованный кватернион (quat[VX], quat[VY], quat[VZ], quat[VW])
  137.     const CQuaternion&  set(const CMatrix3 &mat);               /// Создает кватернион поворота из матрицы
  138.     const CQuaternion&  set(const CMatrix4 &mat);               /// Создает кватернион поворота из матрицы
  139.  
  140.     /// кватернион, соответствующий вращению вокруг оси (x,y,z) на угол angle (рад)
  141.     const CQuaternion&  setAngleAxis(F32 angle, F32 x, F32 y, F32 z);  
  142.     /// кватернион, соответствующий вращению вокруг оси vec на угол angle (рад)
  143.     const CQuaternion&  setAngleAxis(F32 angle, const CVector3F &vec); 
  144.     /// кватернион, соответствующий вращению вокруг оси vec на угол angle (рад)
  145.     const CQuaternion&  setAngleAxis(F32 angle, const CVector4F &vec); 
  146.     /// кватернион, соответсвующий  углам Эйлера
  147.     const CQuaternion&  setEulerAngles(F32 roll, F32 pitch, F32 yaw);  
  148.  
  149.     CMatrix4    getMatrix4(void) const;                        
  150.     CMatrix3    getMatrix3(void) const;            
  151.    
  152.     /// Возвращает угол и ось вращения   
  153.     void        getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const;
  154.     /// Возвращает угол и ось вращения
  155.     void        getAngleAxis(F32* angle, CVector3F &vec) const;
  156.      /// Возвращает углы эйлера
  157.     void        getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
  158.  
  159.     /// Нормализует и возвращает прежнюю магнитуду
  160.     F32 normalize();   
  161.  
  162.     /// Сопряженный кватернион
  163.     const CQuaternion&  conjugate(void);
  164.  
  165.     /// Зеркальное вращение
  166.     const CQuaternion&  transpose();
  167.  
  168.     /// кратчайшее вращение от направления а к b
  169.     void  shortestArc(const CVector3F &a, const CVector3F &b); 
  170.     /// урезает поворот до пределов конуса с заданным углом
  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);     // сопряжение
  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.     // вращение вектора а
  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.     // скалярное произведение
  193.     friend F32 dot(const CQuaternion &a, const CQuaternion &b);
  194.     // Интерполяция (t = 0 ... 1) от p к q
  195.     friend CQuaternion lerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  196.     // Интерполяция (t = 0 ... 1) от единичного кв. к q     
  197.     friend CQuaternion lerp(F32 t, const CQuaternion &q);                  
  198.     // Сферическая интерполяция от p к q     
  199.     friend CQuaternion slerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  200.     // Сферическая интерполяция от единичного кв. к q    
  201.     friend CQuaternion slerp(F32 t, const CQuaternion &q); 
  202.     // Нормализованная интерполяция от p к q                     
  203.     friend CQuaternion nlerp(F32 t, const CQuaternion &p, const CQuaternion &q);
  204.     // Нормализованная интерполяция от единичного кв. к 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:Не следует вставлять сюда нормализацию. Слишком накладно для временных значний, да и округления накапливаются
  247.     // для нормализации следует использовать set()
  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 Можно ли избавиться от 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.         // Углы никогда не должны быть  за пределами [PI, -PI]
  319.         // и нам нужен кратчайший поворот.
  320.         // Т.к. acos определен на отрезке [0, PI], а мы умножаем угол на два,
  321.         // то мы можем выйти за пределы отрезка. Если заменить угл на соответствующий в
  322.         // другой полуплоскости и инвертировать ось, это будет соответствовать
  323.         // вращению в другом направлении (правило правой руки)
  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. /// Отражение (зеркальный поворот)
  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!