Leonetienne/Eule
Homemade math library, mainly targetted towards computer graphics
Public Member Functions | Friends | List of all members
Eule::Quaternion Class Reference

3D rotation representation More...

#include <Quaternion.h>

Public Member Functions

 Quaternion ()
 
 Quaternion (const Vector4d values)
 Constructs by these raw values. More...
 
 Quaternion (const Quaternion &q)
 Copies this existing Quaternion. More...
 
 Quaternion (const Vector3d eulerAngles)
 Creates an quaternion from euler angles. More...
 
 ~Quaternion ()
 
Quaternion operator= (const Quaternion &q)
 Copies. More...
 
Quaternion operator* (const Quaternion &q) const
 Multiplies (applies) More...
 
Quaternion operator/ (Quaternion &q) const
 Divides (applies) More...
 
Quaternion & operator*= (const Quaternion &q)
 Also multiplies. More...
 
Quaternion & operator/= (const Quaternion &q)
 Also divides. More...
 
Vector3d operator* (const Vector3d &p) const
 Will transform a 3d point around its origin. More...
 
bool operator== (const Quaternion &q) const
 
bool operator!= (const Quaternion &q) const
 
Quaternion Inverse () const
 
Quaternion Conjugate () const
 
Quaternion UnitQuaternion () const
 
Vector3d RotateVector (const Vector3d &vec) const
 Will rotate a vector by this quaternion. More...
 
Vector3d ToEulerAngles () const
 Will return euler angles representing this Quaternion's rotation. More...
 
Matrix4x4 ToRotationMatrix () const
 Will return a rotation matrix representing this Quaternions rotation. More...
 
Vector4d GetRawValues () const
 Will return the raw four-dimensional values. More...
 
Quaternion AngleBetween (const Quaternion &other) const
 Will return the value between two Quaternion's as another Quaternion. More...
 
void SetRawValues (const Vector4d values)
 Will set the raw four-dimensional values. More...
 
Quaternion Lerp (const Quaternion &other, double t) const
 Will return the lerp result between two quaternions. More...
 

Friends

std::ostream & operator<< (std::ostream &os, const Quaternion &q)
 
std::wostream & operator<< (std::wostream &os, const Quaternion &q)
 

Detailed Description

3D rotation representation

Definition at line 10 of file Quaternion.h.

Constructor & Destructor Documentation

◆ Quaternion() [1/4]

Quaternion::Quaternion ( )

Definition at line 11 of file Quaternion.cpp.

12 {
13  v = Vector4d(0, 0, 0, 1);
14  return;
15 }

◆ Quaternion() [2/4]

Quaternion::Quaternion ( const Vector4d  values)
explicit

Constructs by these raw values.

Definition at line 17 of file Quaternion.cpp.

18 {
19  v = values;
20  return;
21 }

◆ Quaternion() [3/4]

Quaternion::Quaternion ( const Quaternion &  q)

Copies this existing Quaternion.

Definition at line 23 of file Quaternion.cpp.

24 {
25  v = q.v;
26  return;
27 }

◆ Quaternion() [4/4]

Quaternion::Quaternion ( const Vector3d  eulerAngles)

Creates an quaternion from euler angles.

Definition at line 29 of file Quaternion.cpp.

30 {
31  Vector3d eulerRad = eulerAngles * Deg2Rad;
32 
33  #ifndef _EULE_NO_INTRINSICS_
34 
35  // Calculate sine and cos values
36  __m256d __vec = _mm256_set_pd(0, eulerRad.z, eulerRad.y, eulerRad.x);
37  __vec = _mm256_mul_pd(__vec, _mm256_set1_pd(0.5));
38  __m256d __cos;
39  __m256d __sin = _mm256_sincos_pd(&__cos, __vec);
40 
41  // Create multiplication vectors
42  double sin[4];
43  double cos[4];
44 
45  _mm256_storeu_pd(sin, __sin);
46  _mm256_storeu_pd(cos, __cos);
47 
48  __m256d __a = _mm256_set_pd(cos[0], cos[0], sin[0], cos[0]);
49  __m256d __b = _mm256_set_pd(cos[1], sin[1], cos[1], cos[1]);
50  __m256d __c = _mm256_set_pd(sin[2], cos[2], cos[2], cos[2]);
51 
52  __m256d __d = _mm256_set_pd(sin[0], sin[0], cos[0], sin[0]);
53  __m256d __e = _mm256_set_pd(sin[1], cos[1], sin[1], sin[1]);
54  __m256d __f = _mm256_set_pd(cos[2], sin[2], sin[2], sin[2]);
55 
56  // Multiply them
57  __m256d __abc;
58  __abc = _mm256_mul_pd(__a, __b);
59  __abc = _mm256_mul_pd(__abc, __c);
60 
61  __m256d __def;
62  __def = _mm256_mul_pd(__d, __e);
63  __def = _mm256_mul_pd(__def, __f);
64 
65  // Extract results
66  double abc[4];
67  double def[4];
68 
69  _mm256_storeu_pd(abc, __abc);
70  _mm256_storeu_pd(def, __def);
71 
72  // Sum them up
73  v.w = abc[0] + def[0];
74  v.x = abc[1] - def[1];
75  v.y = abc[2] + def[2];
76  v.z = abc[3] - def[3];
77 
78  #else
79 
80  const double cy = cos(eulerRad.z * 0.5);
81  const double sy = sin(eulerRad.z * 0.5);
82  const double cp = cos(eulerRad.y * 0.5);
83  const double sp = sin(eulerRad.y * 0.5);
84  const double cr = cos(eulerRad.x * 0.5);
85  const double sr = sin(eulerRad.x * 0.5);
86 
87  v.w = cr * cp * cy + sr * sp * sy;
88  v.x = sr * cp * cy - cr * sp * sy;
89  v.y = cr * sp * cy + sr * cp * sy;
90  v.z = cr * cp * sy - sr * sp * cy;
91 
92  #endif
93 
94  return;
95 }

◆ ~Quaternion()

Quaternion::~Quaternion ( )

Definition at line 97 of file Quaternion.cpp.

98 {
99  return;
100 }

Member Function Documentation

◆ AngleBetween()

Quaternion Quaternion::AngleBetween ( const Quaternion &  other) const

Will return the value between two Quaternion's as another Quaternion.

Definition at line 295 of file Quaternion.cpp.

296 {
297  return other * Conjugate();
298 }

◆ Conjugate()

Quaternion Quaternion::Conjugate ( ) const

Definition at line 187 of file Quaternion.cpp.

188 {
189  return Quaternion(Vector4d(-v.x, -v.y, -v.z, v.w));
190 }

◆ GetRawValues()

Vector4d Quaternion::GetRawValues ( ) const

Will return the raw four-dimensional values.

Definition at line 290 of file Quaternion.cpp.

291 {
292  return v;
293 }

◆ Inverse()

Quaternion Quaternion::Inverse ( ) const

Definition at line 175 of file Quaternion.cpp.

176 {
177  if (!isCacheUpToDate_inverse)
178  {
179  cache_inverse = (Conjugate() * (1.0 / v.SqrMagnitude())).v;
180 
181  isCacheUpToDate_inverse = true;
182  }
183 
184  return Quaternion(cache_inverse);
185 }

◆ Lerp()

Quaternion Quaternion::Lerp ( const Quaternion &  other,
double  t 
) const

Will return the lerp result between two quaternions.

Definition at line 309 of file Quaternion.cpp.

310 {
311  return Quaternion(v.Lerp(other.v, t)).UnitQuaternion();
312 }

◆ operator!=()

bool Quaternion::operator!= ( const Quaternion &  q) const

Definition at line 170 of file Quaternion.cpp.

171 {
172  return (!v.Similar(q.v)) && (!v.Similar(q.v * -1));
173 }

◆ operator*() [1/2]

Quaternion Quaternion::operator* ( const Quaternion &  q) const

Multiplies (applies)

Definition at line 111 of file Quaternion.cpp.

112 {
113  return Quaternion(Vector4d(
114  v.w * q.v.x + v.x * q.v.w + v.y * q.v.z - v.z * q.v.y,
115  v.w * q.v.y + v.y * q.v.w + v.z * q.v.x - v.x * q.v.z,
116  v.w * q.v.z + v.z * q.v.w + v.x * q.v.y - v.y * q.v.x,
117  v.w * q.v.w - v.x * q.v.x - v.y * q.v.y - v.z * q.v.z
118  ));
119 }

◆ operator*() [2/2]

Vector3d Quaternion::operator* ( const Vector3d &  p) const

Will transform a 3d point around its origin.

Definition at line 160 of file Quaternion.cpp.

161 {
162  return RotateVector(p);
163 }

◆ operator*=()

Quaternion & Quaternion::operator*= ( const Quaternion &  q)

Also multiplies.

Definition at line 131 of file Quaternion.cpp.

132 {
133  InvalidateCache();
134 
135  Vector4d bufr = v;
136  v.x = bufr.w * q.v.x + bufr.x * q.v.w + bufr.y * q.v.z - bufr.z * q.v.y; // x
137  v.y = bufr.w * q.v.y + bufr.y * q.v.w + bufr.z * q.v.x - bufr.x * q.v.z; // y
138  v.z = bufr.w * q.v.z + bufr.z * q.v.w + bufr.x * q.v.y - bufr.y * q.v.x; // z
139  v.w = bufr.w * q.v.w - bufr.x * q.v.x - bufr.y * q.v.y - bufr.z * q.v.z; // w
140 
141  return (*this);
142 }

◆ operator/()

Quaternion Quaternion::operator/ ( Quaternion &  q) const

Divides (applies)

Definition at line 126 of file Quaternion.cpp.

127 {
128  return ((*this) * (q.Inverse()));
129 }

◆ operator/=()

Quaternion & Quaternion::operator/= ( const Quaternion &  q)

Also divides.

Definition at line 152 of file Quaternion.cpp.

153 {
154  InvalidateCache();
155 
156  (*this) = (*this) * q.Inverse();
157  return (*this);
158 }

◆ operator=()

Quaternion Quaternion::operator= ( const Quaternion &  q)

Copies.

Definition at line 102 of file Quaternion.cpp.

103 {
104  InvalidateCache();
105 
106  v = q.v;
107 
108  return (*this);
109 }

◆ operator==()

bool Quaternion::operator== ( const Quaternion &  q) const

Definition at line 165 of file Quaternion.cpp.

166 {
167  return (v.Similar(q.v)) || (v.Similar(q.v * -1));
168 }

◆ RotateVector()

Vector3d Quaternion::RotateVector ( const Vector3d &  vec) const

Will rotate a vector by this quaternion.

Definition at line 197 of file Quaternion.cpp.

198 {
199  Quaternion pure(Vector4d(vec.x, vec.y, vec.z, 0));
200 
201  //Quaternion f = Conjugate() * pure * (*this);
202  //Quaternion f = Inverse().Conjugate() * pure * (this->Inverse());
203 
204 
205  Quaternion f = Inverse() * pure * (*this);
206 
207  Vector3d toRet;
208  toRet.x = f.v.x;
209  toRet.y = f.v.y;
210  toRet.z = f.v.z;
211 
212  return toRet;
213 }

◆ SetRawValues()

void Quaternion::SetRawValues ( const Vector4d  values)

Will set the raw four-dimensional values.

Definition at line 300 of file Quaternion.cpp.

301 {
302  InvalidateCache();
303 
304  v = values;
305 
306  return;
307 }

◆ ToEulerAngles()

Vector3d Quaternion::ToEulerAngles ( ) const

Will return euler angles representing this Quaternion's rotation.

Definition at line 215 of file Quaternion.cpp.

216 {
217  if (!isCacheUpToDate_euler)
218  {
219  Vector3d euler;
220  // roll (x-axis rotation)
221  double sinr_cosp = 2.0 * (v.w * v.x + v.y * v.z);
222  double cosr_cosp = 1.0 - 2.0 * (v.x * v.x + v.y * v.y);
223  euler.x = std::atan2(sinr_cosp, cosr_cosp);
224 
225  // pitch (y-axis rotation)
226  double sinp = 2.0 * (v.w * v.y - v.z * v.x);
227  if (std::abs(sinp) >= 1)
228  euler.y = std::copysign(PI / 2, sinp); // use 90 degrees if out of range
229  else
230  euler.y = std::asin(sinp);
231 
232  // yaw (z-axis rotation)
233  double siny_cosp = 2.0 * (v.w * v.z + v.x * v.y);
234  double cosy_cosp = 1.0 - 2.0 * (v.y * v.y + v.z * v.z);
235  euler.z = std::atan2(siny_cosp, cosy_cosp);
236 
237  euler *= Rad2Deg;
238 
239  cache_euler = euler;
240  isCacheUpToDate_matrix = true;
241  }
242 
243  return cache_euler;
244 }

◆ ToRotationMatrix()

Matrix4x4 Quaternion::ToRotationMatrix ( ) const

Will return a rotation matrix representing this Quaternions rotation.

Definition at line 246 of file Quaternion.cpp.

247 {
248  if (!isCacheUpToDate_matrix)
249  {
250  Matrix4x4 m;
251 
252  const double sqx = v.x * v.x;
253  const double sqy = v.y * v.y;
254  const double sqz = v.z * v.z;
255  const double sqw = v.w * v.w;
256  const double x = v.x;
257  const double y = v.y;
258  const double z = v.z;
259  const double w = v.w;
260 
261  // invs (inverse square length) is only required if quaternion is not already normalised
262  double invs = 1.0 / (sqx + sqy + sqz + sqw);
263 
264  // since sqw + sqx + sqy + sqz =1/invs*invs
265 
266  // yaw (y)
267  m.c = ((2 * x * z) - (2 * w * y)) * invs;
268  m.f = (1 - (2 * sqx) - (2 * sqz)) * invs;
269  m.i = ((2 * x * z) + (2 * w * y)) * invs;
270 
271  // pitch (x)
272  m.a = (1 - (2 * sqy) - (2 * sqz)) * invs;
273  m.g = ((2 * y * z) + (2 * w * x)) * invs;
274  m.j = ((2 * y * z) - (2 * w * x)) * invs;
275 
276  // roll (z)
277  m.b = ((2 * x * v.y) + (2 * w * z)) * invs;
278  m.e = ((2 * x * v.y) - (2 * w * z)) * invs;
279  m.k = (1 - (2 * sqx) - (2 * sqy)) * invs;
280 
281  m.p = 1;
282 
283  cache_matrix = m;
284  isCacheUpToDate_matrix = true;
285  }
286 
287  return cache_matrix;
288 }

◆ UnitQuaternion()

Quaternion Quaternion::UnitQuaternion ( ) const

Definition at line 192 of file Quaternion.cpp.

193 {
194  return (*this) * (1.0 / v.Magnitude());
195 }

Friends And Related Function Documentation

◆ operator<< [1/2]

std::ostream& operator<< ( std::ostream &  os,
const Quaternion &  q 
)
friend

Definition at line 325 of file Quaternion.cpp.

326  {
327  os << "[" << q.v << "]";
328  return os;
329  }

◆ operator<< [2/2]

std::wostream& operator<< ( std::wostream &  os,
const Quaternion &  q 
)
friend

Definition at line 331 of file Quaternion.cpp.

332  {
333  os << L"[" << q.v << L"]";
334  return os;
335  }
The documentation for this class was generated from the following files:
Eule::Matrix4x4::j
double & j
Definition: Matrix4x4.h:137
PI
static constexpr double PI
Pi up to 50 decimal places.
Definition: Constants.h:6
Eule::Matrix4x4::i
double & i
Definition: Matrix4x4.h:136
Eule::Vector3< double >
Eule::Vector4::x
T x
Definition: Vector4.h:88
Eule::Matrix4x4::e
double & e
Definition: Matrix4x4.h:132
Eule::Vector4::z
T z
Definition: Vector4.h:90
Eule::Vector4d
Vector4< double > Vector4d
Definition: Vector4.h:107
Eule::Matrix4x4::f
double & f
Definition: Matrix4x4.h:133
Deg2Rad
static constexpr double Deg2Rad
Factor to convert degrees to radians.
Definition: Constants.h:12
Eule::Matrix4x4::k
double & k
Definition: Matrix4x4.h:138
Eule::Vector4::Lerp
Vector4< double > Lerp(const Vector4< T > &other, double t) const
Will return a lerp result between this and another vector.
Definition: Vector4.cpp:287
Eule::Vector4::w
T w
Definition: Vector4.h:91
Eule::Vector3::z
T z
Definition: Vector3.h:96
Eule::Matrix4x4
A matrix 4x4 class representing a 3d transformation.
Definition: Matrix4x4.h:36
Eule::Vector4::Similar
bool Similar(const Vector4< T > &other, double epsilon=0.00001) const
Will compare if two vectors are similar to a certain epsilon value.
Definition: Vector4.cpp:162
Eule::Matrix4x4::b
double & b
Definition: Matrix4x4.h:129
Eule::Vector4::Magnitude
double Magnitude() const
Will compute the magnitude.
Definition: Vector4.cpp:38
Eule::Matrix4x4::g
double & g
Definition: Matrix4x4.h:134
Eule::Vector3::x
T x
Definition: Vector3.h:94
Eule::Matrix4x4::p
double & p
Definition: Matrix4x4.h:143
Eule::Quaternion::RotateVector
Vector3d RotateVector(const Vector3d &vec) const
Will rotate a vector by this quaternion.
Definition: Quaternion.cpp:197
Eule::Vector4::SqrMagnitude
double SqrMagnitude() const
Will compute the square magnitude.
Definition: Vector4.cpp:31
Eule::Quaternion::Quaternion
Quaternion()
Definition: Quaternion.cpp:11
Rad2Deg
static constexpr double Rad2Deg
Factor to convert radians to degrees.
Definition: Constants.h:15
Eule::Quaternion::Conjugate
Quaternion Conjugate() const
Definition: Quaternion.cpp:187
Eule::Quaternion::Inverse
Quaternion Inverse() const
Definition: Quaternion.cpp:175
Eule::Matrix4x4::c
double & c
Definition: Matrix4x4.h:130
Eule::Quaternion
3D rotation representation
Definition: Quaternion.h:10
Eule::Matrix4x4::a
double & a
Definition: Matrix4x4.h:128
Eule::Vector4::y
T y
Definition: Vector4.h:89
Eule::Vector3::y
T y
Definition: Vector3.h:95
Eule::Vector4
Representation of a 4d vector.
Definition: Vector2.h:8
Generated on Mon Nov 15 2021 16:10:18 for Leonetienne/Eule by   doxygen 1.8.17