Aether3D Game Engine
Quaternion.hpp
1 #ifndef QUATERNION_H
2 #define QUATERNION_H
3 
4 #ifndef INCLUDE_MATH_H
5 # include <cmath>
6 # define INCLUDE_MATH_H
7 #endif
8 
9 #ifndef VEC3_H
10 # include "Vec3.hpp"
11 #endif
12 #ifndef MATRIX_H
13 # include "Matrix.hpp"
14 #endif
15 
18 {
19  public:
21  Quaternion() : x( 0 ), y( 0 ), z( 0 ), w( 1 ) {}
22 
27  Quaternion( const Vec3& vec, float aW ) : x( vec.x ), y( vec.y ), z( vec.z ), w( aW ) {}
28 
35  Vec3 operator*( const Vec3& vec ) const
36  {
37  // FIXME: vec was originally normalized here but I removed normalization because it caused
38  // too fast camera movement and errorneous movement at slow speeds. [2014-12-08]
39 
40  Quaternion vecQuat, resQuat;
41  vecQuat.x = vec.x;
42  vecQuat.y = vec.y;
43  vecQuat.z = vec.z;
44  vecQuat.w = 0.0f;
45 
46  resQuat = vecQuat * Conjugate();
47  resQuat = *this * resQuat;
48 
49  return Vec3( resQuat.x, resQuat.y, resQuat.z );
50  }
51 
57  Quaternion operator*( const Quaternion& aQ ) const
58  {
59  return Quaternion( Vec3( w * aQ.x + x * aQ.w + y * aQ.z - z * aQ.y,
60  w * aQ.y + y * aQ.w + z * aQ.x - x * aQ.z,
61  w * aQ.z + z * aQ.w + x * aQ.y - y * aQ.x ),
62  w * aQ.w - x * aQ.x - y * aQ.y - z * aQ.z );
63  }
64 
70  bool operator==( const Quaternion& q ) const
71  {
72  return x == q.x && y == q.y && z == q.z && w == q.w;
73  }
74 
80  bool operator!=( const Quaternion& q ) const
81  {
82  return !(*this == q);
83  }
84 
92  {
93  return Quaternion( Vec3( -x, -y, -z ), w );
94  }
95 
102  float FindTwist( const Vec3& axis ) const
103  {
104  // Get the plane the axis is a normal of.
105  Vec3 orthonormal1, orthonormal2;
106  FindOrthonormals( axis, orthonormal1, orthonormal2 );
107 
108  Vec3 transformed = *this * orthonormal1;// = Vec3.Transform(orthonormal1, q);
109 
110  //project transformed vector onto plane
111  Vec3 flattened = transformed - axis * Vec3::Dot( transformed, axis );
112  flattened = flattened.Normalized();
113 
114  // get angle between original vector and projected transform to get angle around normal
115  return std::acos( Vec3::Dot( orthonormal1, flattened ) );
116  }
117 
122  void GetAxisAngle( Vec3& outAxis, float& outAngleRad ) const
123  {
124  const float scale = std::sqrt( x * x + y * y + z * z );
125 
126  if (std::fabs( scale ) < 0.00001f)
127  {
128  outAxis = Vec3( 0, 0, 0 );
129  }
130  else
131  {
132  outAxis.x = x / scale;
133  outAxis.y = y / scale;
134  outAxis.z = z / scale;
135  }
136 
137  outAngleRad = std::acos( w ) * 2;
138  }
139 
144  void FromEuler( const Vec3& euler )
145  {
146  const Vec3 xAxis( 1, 0, 0 );
147  const Vec3 yAxis( 0, 1, 0 );
148  const Vec3 zAxis( 0, 0, 1 );
149 
150  Quaternion qx, qy, qz, qt;
151  qx.FromAxisAngle( xAxis, euler.x );
152  qy.FromAxisAngle( yAxis, euler.y );
153  qz.FromAxisAngle( zAxis, euler.z );
154  qt = qx * qy;
155  *this = qt * qz;
156  }
157 
162  void FromAxisAngle( const Vec3& axis, float angleDeg )
163  {
164  float angleRad = angleDeg * (3.1418693659f / 180.0f);
165 
166  angleRad *= 0.5f;
167 
168  const float sinAngle = std::sin( angleRad );
169 
170  x = axis.x * sinAngle;
171  y = axis.y * sinAngle;
172  z = axis.z * sinAngle;
173  w = std::cos( angleRad );
174  }
175 
180  static Quaternion CreateFromAxisAngle( const Vec3& axis, float angleDeg )
181  {
182  Quaternion q;
183  q.FromAxisAngle( axis, angleDeg );
184  return q;
185  }
186 
188  void FromMatrix( const Matrix44& mat )
189  {
190  const float trace = 1.0f + mat.m[0] + mat.m[5] + mat.m[10];
191 
192  if (trace > 0.0f)
193  {
194  const float S = sqrtf( trace ) * 2.0f;
195  x = ( mat.m[9] - mat.m[6] ) / S;
196  y = ( mat.m[2] - mat.m[8] ) / S;
197  z = ( mat.m[4] - mat.m[1] ) / S;
198  w = 0.25f * S;
199  }
200  else if (mat.m[0] > mat.m[5] && mat.m[0] > mat.m[10])
201  {
202  const float S = std::sqrt( 1.0f + mat.m[0] - mat.m[5] - mat.m[10] ) * 2.0f;
203  x = 0.25f * S;
204  y = (mat.m[4] + mat.m[1] ) / S;
205  z = (mat.m[2] + mat.m[8] ) / S;
206  w = (mat.m[9] - mat.m[6] ) / S;
207  }
208  else if (mat.m[5] > mat.m[10])
209  {
210  const float S = std::sqrt( 1.0f + mat.m[5] - mat.m[0] - mat.m[10] ) * 2.0f;
211  x = (mat.m[4] + mat.m[1] ) / S;
212  y = 0.25f * S;
213  z = (mat.m[9] + mat.m[6] ) / S;
214  w = (mat.m[2] - mat.m[8] ) / S;
215  }
216  else
217  {
218  const float S = std::sqrt( 1.0f + mat.m[10] - mat.m[0] - mat.m[5] ) * 2.0f;
219  x = (mat.m[2] + mat.m[8] ) / S;
220  y = (mat.m[9] + mat.m[6] ) / S;
221  z = 0.25f * S;
222  w = (mat.m[4] - mat.m[1] ) / S;
223  }
224  }
225 
231  void GetMatrix( Matrix44& outMatrix ) const
232  {
233  const float x2 = x * x;
234  const float y2 = y * y;
235  const float z2 = z * z;
236  const float xy = x * y;
237  const float xz = x * z;
238  const float yz = y * z;
239  const float wx = w * x;
240  const float wy = w * y;
241  const float wz = w * z;
242 
243  outMatrix.m[ 0] = 1 - 2 * (y2 + z2);
244  outMatrix.m[ 1] = 2 * (xy - wz);
245  outMatrix.m[ 2] = 2 * (xz + wy);
246  outMatrix.m[ 3] = 0;
247  outMatrix.m[ 4] = 2 * (xy + wz);
248  outMatrix.m[ 5] = 1 - 2 * (x2 + z2);
249  outMatrix.m[ 6] = 2 * (yz - wx);
250  outMatrix.m[ 7] = 0;
251  outMatrix.m[ 8] = 2 * (xz - wy);
252  outMatrix.m[ 9] = 2 * (yz + wx);
253  outMatrix.m[10] = 1 - 2 * (x2 + y2);
254  outMatrix.m[11] = 0;
255  outMatrix.m[12] = 0;
256  outMatrix.m[13] = 0;
257  outMatrix.m[14] = 0;
258  outMatrix.m[15] = 1;
259  }
260 
265  Vec3 GetEuler() const
266  {
267  Vec3 out;
268  out.z = std::atan2( 2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z );
269  out.y = std::asin( 2 * x * y + 2 * z * w );
270  out.x = std::atan2( 2 * x * w - 2 * y * z, 1 - 2 * x * x - 2 * z * z );
271  return out / (3.14159265358979f / 180.0f);
272  }
273 
275  void Normalize()
276  {
277  const float mag2 = w * w + x * x + y * y + z * z;
278  const float acceptableDelta = 0.00001f;
279 
280  if (std::fabs( mag2 ) > acceptableDelta && std::fabs( mag2 - 1.0f ) > acceptableDelta)
281  {
282  const float oneOverMag = 1.0f / std::sqrt( mag2 );
283 
284  x *= oneOverMag;
285  y *= oneOverMag;
286  z *= oneOverMag;
287  w *= oneOverMag;
288  }
289  }
290 
291  float x, y, z, w;
292 
293 private:
294 
296  void FindOrthonormals( const Vec3& normal, Vec3& orthonormal1, Vec3& orthonormal2 ) const
297  {
298  Matrix44 orthoX( 90, 0, 0 );
299  Matrix44 orthoY( 0, 90, 0 );
300 
301  Vec3 ww;
302  Matrix44::TransformDirection( normal, orthoX, &ww );
303  const float dot = Vec3::Dot( normal, ww );
304 
305  if (std::fabs( dot ) > 0.6f)
306  {
307  Matrix44::TransformDirection( normal, orthoY, &ww );
308  }
309 
310  ww = ww.Normalized();
311 
312  orthonormal1 = Vec3::Cross( normal, ww );
313  orthonormal1 = orthonormal1.Normalized();
314  orthonormal2 = Vec3::Cross( normal, orthonormal1 );
315  orthonormal2 = orthonormal2.Normalized();
316  }
317 };
318 #endif
void GetAxisAngle(Vec3 &outAxis, float &outAngleRad) const
Definition: Quaternion.hpp:122
static Vec3 Cross(const Vec3 &v1, const Vec3 &v2)
Cross product.
Definition: Vec3.hpp:33
float m[16]
Member data, row-major.
Definition: Matrix.hpp:176
bool operator!=(const Quaternion &q) const
Definition: Quaternion.hpp:80
Stores an orientation.
Definition: Quaternion.hpp:17
Quaternion(const Vec3 &vec, float aW)
Definition: Quaternion.hpp:27
float FindTwist(const Vec3 &axis) const
Finds twist angle around axis.
Definition: Quaternion.hpp:102
void Normalize()
Normalizes the quaternion if it&#39;s not near unit-length already.
Definition: Quaternion.hpp:275
void FromAxisAngle(const Vec3 &axis, float angleDeg)
Definition: Quaternion.hpp:162
static float Dot(const Vec3 &v1, const Vec3 &v2)
Definition: Vec3.hpp:74
void GetMatrix(Matrix44 &outMatrix) const
Definition: Quaternion.hpp:231
Quaternion operator*(const Quaternion &aQ) const
Applies rotation aQ to this quaternion and returns the result.
Definition: Quaternion.hpp:57
static void TransformDirection(const Vec3 &dir, const Matrix44 &mat, Vec3 *out)
Definition: Matrix.cpp:190
void FromEuler(const Vec3 &euler)
Definition: Quaternion.hpp:144
Vec3 GetEuler() const
Definition: Quaternion.hpp:265
Vec3 Normalized() const
Definition: Vec3.hpp:343
3-component vector.
Definition: Vec3.hpp:22
bool operator==(const Quaternion &q) const
Definition: Quaternion.hpp:70
void FromMatrix(const Matrix44 &mat)
Definition: Quaternion.hpp:188
Quaternion Conjugate() const
Definition: Quaternion.hpp:91
Row-major 4x4 Matrix.
Definition: Matrix.hpp:20
static Quaternion CreateFromAxisAngle(const Vec3 &axis, float angleDeg)
Definition: Quaternion.hpp:180
Vec3 operator*(const Vec3 &vec) const
Multiplying a quaternion q with a vector v applies the q-rotation to vec.
Definition: Quaternion.hpp:35
Quaternion()
Constructor.
Definition: Quaternion.hpp:21