mQuat.h 5.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239
  1. //-----------------------------------------------------------------------------
  2. // Copyright (c) 2012 GarageGames, LLC
  3. //
  4. // Permission is hereby granted, free of charge, to any person obtaining a copy
  5. // of this software and associated documentation files (the "Software"), to
  6. // deal in the Software without restriction, including without limitation the
  7. // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  8. // sell copies of the Software, and to permit persons to whom the Software is
  9. // furnished to do so, subject to the following conditions:
  10. //
  11. // The above copyright notice and this permission notice shall be included in
  12. // all copies or substantial portions of the Software.
  13. //
  14. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  19. // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  20. // IN THE SOFTWARE.
  21. //-----------------------------------------------------------------------------
  22. #ifndef _MQUAT_H_
  23. #define _MQUAT_H_
  24. #ifndef _MPOINT3_H_
  25. #include "math/mPoint3.h"
  26. #endif
  27. #ifndef USE_TEMPLATE_MATRIX
  28. class MatrixF;
  29. #else
  30. template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
  31. typedef Matrix<F32, 4, 4> MatrixF;
  32. #endif
  33. class AngAxisF;
  34. //----------------------------------------------------------------------------
  35. // unit quaternion class:
  36. class QuatF
  37. {
  38. //-------------------------------------- Public static constants
  39. public:
  40. const static QuatF Identity;
  41. public:
  42. F32 x,y,z,w;
  43. QuatF() :x(0.0f), y(0.0f), z(0.0f), w(1.0f) {} //identity constructor
  44. QuatF( F32 _x, F32 _y, F32 _z, F32 w );
  45. QuatF( const Point3F &axis, F32 angle );
  46. QuatF( const MatrixF & m );
  47. QuatF( const AngAxisF & a );
  48. QuatF( const EulerF & e );
  49. QuatF& set( F32 _x, F32 _y, F32 _z, F32 _w );
  50. QuatF& set( const Point3F &axis, F32 angle );
  51. QuatF& set( const MatrixF & m );
  52. QuatF& set( const AngAxisF & a );
  53. QuatF& set( const EulerF & e );
  54. S32 operator ==( const QuatF & c ) const;
  55. S32 operator !=( const QuatF & c ) const;
  56. QuatF& operator *=( const QuatF & c );
  57. QuatF& operator /=( const QuatF & c );
  58. QuatF& operator +=( const QuatF & c );
  59. QuatF& operator -=( const QuatF & c );
  60. QuatF& operator *=( F32 a );
  61. QuatF& operator /=( F32 a );
  62. QuatF operator-( const QuatF &c ) const;
  63. QuatF operator*( F32 a ) const;
  64. QuatF& square();
  65. QuatF& neg();
  66. F32 dot( const QuatF &q ) const;
  67. MatrixF* setMatrix( MatrixF * mat ) const;
  68. QuatF& normalize();
  69. QuatF& inverse();
  70. QuatF& identity();
  71. S32 isIdentity() const;
  72. QuatF& slerp( const QuatF & q, F32 t );
  73. QuatF& extrapolate( const QuatF & q1, const QuatF & q2, F32 t );
  74. QuatF& interpolate( const QuatF & q1, const QuatF & q2, F32 t );
  75. F32 angleBetween( const QuatF & q );
  76. Point3F& mulP(const Point3F& a, Point3F* r) const; // r = p * this
  77. QuatF& mul(const QuatF& a, const QuatF& b); // This = a * b
  78. // Vectors passed in must be normalized
  79. QuatF& shortestArc( const VectorF &normalizedA, const VectorF &normalizedB );
  80. };
  81. // a couple simple utility methods
  82. inline F32 QuatIsEqual(F32 a,F32 b,F32 epsilon = 0.0001f) { return mFabs(a-b) < epsilon; }
  83. inline F32 QuatIsZero(F32 a,F32 epsilon = 0.0001f) { return mFabs(a) < epsilon; }
  84. //----------------------------------------------------------------------------
  85. // quaternion implementation:
  86. inline QuatF::QuatF( F32 _x, F32 _y, F32 _z, F32 _w )
  87. {
  88. set( _x, _y, _z, _w );
  89. }
  90. inline QuatF::QuatF( const Point3F &axis, F32 angle )
  91. {
  92. set( axis, angle );
  93. }
  94. inline QuatF::QuatF( const AngAxisF & a )
  95. {
  96. set( a );
  97. }
  98. inline QuatF::QuatF( const EulerF & e )
  99. {
  100. set( e );
  101. }
  102. inline QuatF::QuatF( const MatrixF & m )
  103. {
  104. set( m );
  105. }
  106. inline QuatF& QuatF::set( F32 _x, F32 _y, F32 _z, F32 _w )
  107. {
  108. x = _x;
  109. y = _y;
  110. z = _z;
  111. w = _w;
  112. return *this;
  113. }
  114. inline int QuatF::operator ==( const QuatF & c ) const
  115. {
  116. QuatF a = *this;
  117. QuatF b = c;
  118. a.normalize();
  119. b.normalize();
  120. b.inverse();
  121. a *= b;
  122. return a.isIdentity();
  123. }
  124. inline int QuatF::isIdentity() const
  125. {
  126. return QuatIsZero( x ) && QuatIsZero( y ) && QuatIsZero( z );
  127. }
  128. inline QuatF& QuatF::identity()
  129. {
  130. x = 0.0f;
  131. y = 0.0f;
  132. z = 0.0f;
  133. w = 1.0f;
  134. return *this;
  135. }
  136. inline int QuatF::operator !=( const QuatF & c ) const
  137. {
  138. return !operator==( c );
  139. }
  140. inline QuatF& QuatF::operator +=( const QuatF & c )
  141. {
  142. x += c.x;
  143. y += c.y;
  144. z += c.z;
  145. w += c.w;
  146. return *this;
  147. }
  148. inline QuatF& QuatF::operator -=( const QuatF & c )
  149. {
  150. x -= c.x;
  151. y -= c.y;
  152. z -= c.z;
  153. w -= c.w;
  154. return *this;
  155. }
  156. inline QuatF& QuatF::operator *=( F32 a )
  157. {
  158. x *= a;
  159. y *= a;
  160. z *= a;
  161. w *= a;
  162. return *this;
  163. }
  164. inline QuatF& QuatF::operator /=( F32 a )
  165. {
  166. x /= a;
  167. y /= a;
  168. z /= a;
  169. w /= a;
  170. return *this;
  171. }
  172. inline QuatF QuatF::operator -( const QuatF &c ) const
  173. {
  174. return QuatF( x - c.x,
  175. y - c.y,
  176. z - c.z,
  177. w - c.w );
  178. }
  179. inline QuatF QuatF::operator *( F32 a ) const
  180. {
  181. return QuatF( x * a,
  182. y * a,
  183. z * a,
  184. w * a );
  185. }
  186. inline QuatF& QuatF::neg()
  187. {
  188. x = -x;
  189. y = -y;
  190. z = -z;
  191. w = -w;
  192. return *this;
  193. }
  194. inline F32 QuatF::dot( const QuatF &q ) const
  195. {
  196. return mClampF(w*q.w + x*q.x + y*q.y + z*q.z, -1.0f, 1.0f);
  197. }
  198. inline F32 QuatF::angleBetween( const QuatF & q )
  199. {
  200. // angle between two normalized quaternions.
  201. return mAcos(q.dot(*this)) * 2.0f;
  202. }
  203. #endif // _MQUAT_H_