Aether3D Game Engine
Quaternion.hpp
1 #ifndef QUATERNION_H
2 #define QUATERNION_H
3 
4 #include <cmath>
5 #include "Vec3.hpp"
6 #include "Matrix.hpp"
7 
8 namespace ae3d
9 {
11  struct Quaternion
12  {
14  Quaternion() : x( 0 ), y( 0 ), z( 0 ), w( 1 ) {}
15 
20  Quaternion( const Vec3& vec, float aW ) : x( vec.x ), y( vec.y ), z( vec.z ), w( aW ) {}
21 
23  Quaternion( const Quaternion& v ) = default;
24 
26  Quaternion( Quaternion&& v ) noexcept = default;
27 
29  Quaternion& operator=( Quaternion&& ) noexcept = default;
30 
32  Quaternion& operator=( const Quaternion& ) = default;
33 
40  Vec3 operator*( const Vec3& vec ) const
41  {
42  // FIXME: vec was originally normalized here but I removed normalization because it caused
43  // too fast camera movement and errorneous movement at slow speeds. [2014-12-08]
44 
45  Quaternion vecQuat, resQuat;
46  vecQuat.x = vec.x;
47  vecQuat.y = vec.y;
48  vecQuat.z = vec.z;
49  vecQuat.w = 0.0f;
50 
51  resQuat = vecQuat * Conjugate();
52  resQuat = *this * resQuat;
53 
54  return Vec3( resQuat.x, resQuat.y, resQuat.z );
55  }
56 
62  Quaternion operator*( const Quaternion& aQ ) const
63  {
64  return Quaternion( Vec3( w * aQ.x + x * aQ.w + y * aQ.z - z * aQ.y,
65  w * aQ.y + y * aQ.w + z * aQ.x - x * aQ.z,
66  w * aQ.z + z * aQ.w + x * aQ.y - y * aQ.x ),
67  w * aQ.w - x * aQ.x - y * aQ.y - z * aQ.z );
68  }
69 
75  bool operator==( const Quaternion& q ) const
76  {
77  const float acceptableDelta = 0.00001f;
78 
79  return std::fabs(x - q.x) < acceptableDelta &&
80  std::fabs(y - q.y) < acceptableDelta &&
81  std::fabs(z - q.z) < acceptableDelta &&
82  std::fabs(w - q.w) < acceptableDelta;
83  }
84 
90  bool operator!=( const Quaternion& q ) const
91  {
92  return !(*this == q);
93  }
94 
102  {
103  return Quaternion( Vec3( -x, -y, -z ), w );
104  }
105 
112  float FindTwist( const Vec3& axis ) const
113  {
114  // Get the plane the axis is a normal of.
115  Vec3 orthonormal1, orthonormal2;
116  FindOrthonormals( axis, orthonormal1, orthonormal2 );
117 
118  Vec3 transformed = *this * orthonormal1;// = Vec3.Transform(orthonormal1, q);
119 
120  //project transformed vector onto plane
121  Vec3 flattened = transformed - axis * Vec3::Dot( transformed, axis );
122  flattened = flattened.Normalized();
123 
124  // get angle between original vector and projected transform to get angle around normal
125  return std::acos( Vec3::Dot( orthonormal1, flattened ) );
126  }
127 
132  void GetAxisAngle( Vec3& outAxis, float& outAngleRad ) const
133  {
134  const float scale = std::sqrt( x * x + y * y + z * z );
135 
136  if (std::fabs( scale ) < 0.00001f)
137  {
138  outAxis = Vec3( 0, 0, 0 );
139  }
140  else
141  {
142  outAxis.x = x / scale;
143  outAxis.y = y / scale;
144  outAxis.z = z / scale;
145  }
146 
147  outAngleRad = std::acos( w ) * 2;
148  }
149 
152  static Quaternion FromEuler( const Vec3& euler )
153  {
154  const Vec3 xAxis( 1, 0, 0 );
155  const Vec3 yAxis( 0, 1, 0 );
156  const Vec3 zAxis( 0, 0, 1 );
157 
158  Quaternion qx, qy, qz, qt;
159  qx.FromAxisAngle( xAxis, euler.x );
160  qy.FromAxisAngle( yAxis, euler.y );
161  qz.FromAxisAngle( zAxis, euler.z );
162  qt = qx * qy;
163  return qt * qz;
164  }
165 
170  void FromAxisAngle( const Vec3& axis, float angleDeg )
171  {
172  float angleRad = angleDeg * (3.1418693659f / 180.0f);
173 
174  angleRad *= 0.5f;
175 
176  const float sinAngle = std::sin( angleRad );
177 
178  x = axis.x * sinAngle;
179  y = axis.y * sinAngle;
180  z = axis.z * sinAngle;
181  w = std::cos( angleRad );
182  }
183 
188  static Quaternion CreateFromAxisAngle( const Vec3& axis, float angleDeg )
189  {
190  Quaternion q;
191  q.FromAxisAngle( axis, angleDeg );
192  return q;
193  }
194 
196  void FromMatrix( const Matrix44& mat )
197  {
198  const float trace = 1.0f + mat.m[0] + mat.m[5] + mat.m[10];
199 
200  if (trace > 0.0f)
201  {
202  const float S = sqrtf( trace ) * 2.0f;
203  x = ( mat.m[9] - mat.m[6] ) / S;
204  y = ( mat.m[2] - mat.m[8] ) / S;
205  z = ( mat.m[4] - mat.m[1] ) / S;
206  w = 0.25f * S;
207  }
208  else if (mat.m[0] > mat.m[5] && mat.m[0] > mat.m[10])
209  {
210  const float S = std::sqrt( 1.0f + mat.m[0] - mat.m[5] - mat.m[10] ) * 2.0f;
211  x = 0.25f * S;
212  y = (mat.m[4] + mat.m[1] ) / S;
213  z = (mat.m[2] + mat.m[8] ) / S;
214  w = (mat.m[9] - mat.m[6] ) / S;
215  }
216  else if (mat.m[5] > mat.m[10])
217  {
218  const float S = std::sqrt( 1.0f + mat.m[5] - mat.m[0] - mat.m[10] ) * 2.0f;
219  x = (mat.m[4] + mat.m[1] ) / S;
220  y = 0.25f * S;
221  z = (mat.m[9] + mat.m[6] ) / S;
222  w = (mat.m[2] - mat.m[8] ) / S;
223  }
224  else
225  {
226  const float S = std::sqrt( 1.0f + mat.m[10] - mat.m[0] - mat.m[5] ) * 2.0f;
227  x = (mat.m[2] + mat.m[8] ) / S;
228  y = (mat.m[9] + mat.m[6] ) / S;
229  z = 0.25f * S;
230  w = (mat.m[4] - mat.m[1] ) / S;
231  }
232  }
233 
239  void GetMatrix( Matrix44& outMatrix ) const
240  {
241  const float x2 = x * x;
242  const float y2 = y * y;
243  const float z2 = z * z;
244  const float xy = x * y;
245  const float xz = x * z;
246  const float yz = y * z;
247  const float wx = w * x;
248  const float wy = w * y;
249  const float wz = w * z;
250 
251  outMatrix.m[ 0] = 1 - 2 * (y2 + z2);
252  outMatrix.m[ 1] = 2 * (xy - wz);
253  outMatrix.m[ 2] = 2 * (xz + wy);
254  outMatrix.m[ 3] = 0;
255  outMatrix.m[ 4] = 2 * (xy + wz);
256  outMatrix.m[ 5] = 1 - 2 * (x2 + z2);
257  outMatrix.m[ 6] = 2 * (yz - wx);
258  outMatrix.m[ 7] = 0;
259  outMatrix.m[ 8] = 2 * (xz - wy);
260  outMatrix.m[ 9] = 2 * (yz + wx);
261  outMatrix.m[10] = 1 - 2 * (x2 + y2);
262  outMatrix.m[11] = 0;
263  outMatrix.m[12] = 0;
264  outMatrix.m[13] = 0;
265  outMatrix.m[14] = 0;
266  outMatrix.m[15] = 1;
267  }
268 
273  Vec3 GetEuler() const
274  {
275  Vec3 out;
276  out.z = std::atan2( 2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z );
277  out.y = std::asin( 2 * x * y + 2 * z * w );
278  out.x = std::atan2( 2 * x * w - 2 * y * z, 1 - 2 * x * x - 2 * z * z );
279  return out / (3.14159265358979f / 180.0f);
280  }
281 
283  void Normalize()
284  {
285  const float mag2 = w * w + x * x + y * y + z * z;
286  const float acceptableDelta = 0.00001f;
287 
288  if (std::fabs( mag2 ) > acceptableDelta && std::fabs( mag2 - 1.0f ) > acceptableDelta)
289  {
290  const float oneOverMag = 1.0f / std::sqrt( mag2 );
291 
292  x *= oneOverMag;
293  y *= oneOverMag;
294  z *= oneOverMag;
295  w *= oneOverMag;
296  }
297  }
298 
300  float x;
302  float y;
304  float z;
306  float w;
307 
308  private:
309  void FindOrthonormals( const Vec3& normal, Vec3& orthonormal1, Vec3& orthonormal2 ) const
310  {
311  Matrix44 orthoX( 90, 0, 0 );
312  Matrix44 orthoY( 0, 90, 0 );
313 
314  Vec3 ww;
315  Matrix44::TransformDirection( normal, orthoX, &ww );
316  const float dot = Vec3::Dot( normal, ww );
317 
318  if (std::fabs( dot ) > 0.6f)
319  {
320  Matrix44::TransformDirection( normal, orthoY, &ww );
321  }
322 
323  ww = ww.Normalized();
324 
325  orthonormal1 = Vec3::Cross( normal, ww );
326  orthonormal1 = orthonormal1.Normalized();
327  orthonormal2 = Vec3::Cross( normal, orthonormal1 );
328  orthonormal2 = orthonormal2.Normalized();
329  }
330  };
331 }
332 #endif
Quaternion()
Constructor.
Definition: Quaternion.hpp:14
void FromMatrix(const Matrix44 &mat)
Definition: Quaternion.hpp:196
float x
X coordinate.
Definition: Vec3.hpp:15
Definition: AudioClip.hpp:4
static Quaternion FromEuler(const Vec3 &euler)
Definition: Quaternion.hpp:152
Quaternion operator*(const Quaternion &aQ) const
Applies rotation aQ to this quaternion and returns the result.
Definition: Quaternion.hpp:62
Vec3 GetEuler() const
Definition: Quaternion.hpp:273
float z
Definition: Quaternion.hpp:304
3-component vector.
Definition: Vec3.hpp:12
void GetMatrix(Matrix44 &outMatrix) const
Definition: Quaternion.hpp:239
float y
Definition: Quaternion.hpp:302
bool operator==(const Quaternion &q) const
Definition: Quaternion.hpp:75
static Quaternion CreateFromAxisAngle(const Vec3 &axis, float angleDeg)
Definition: Quaternion.hpp:188
float w
Definition: Quaternion.hpp:306
Quaternion Conjugate() const
Definition: Quaternion.hpp:101
float x
Definition: Quaternion.hpp:300
float z
Z coordinate.
Definition: Vec3.hpp:19
static Vec3 Cross(const Vec3 &v1, const Vec3 &v2)
Cross product.
Definition: Vec3.hpp:28
void Normalize()
Definition: Quaternion.hpp:283
float FindTwist(const Vec3 &axis) const
Finds twist angle around axis.
Definition: Quaternion.hpp:112
static float Dot(const Vec3 &v1, const Vec3 &v2)
Definition: Vec3.hpp:100
void FromAxisAngle(const Vec3 &axis, float angleDeg)
Definition: Quaternion.hpp:170
void GetAxisAngle(Vec3 &outAxis, float &outAngleRad) const
Definition: Quaternion.hpp:132
Vec3 Normalized() const
Definition: Vec3.hpp:311
float y
Y coordinate.
Definition: Vec3.hpp:17
Quaternion(const Vec3 &vec, float aW)
Definition: Quaternion.hpp:20
bool operator!=(const Quaternion &q) const
Definition: Quaternion.hpp:90
Stores an orientation.
Definition: Quaternion.hpp:11