Quaternion.cpp
Go to the documentation of this file.
#include "Quaternion.h"
#include "Constants.h"
//#define _EULE_NO_INTRINSICS_
#ifndef _EULE_NO_INTRINSICS_
#include <immintrin.h>
#endif
using namespace Eule;
Quaternion::Quaternion()
{
v = Vector4d(0, 0, 0, 1);
return;
}
Quaternion::Quaternion(const Vector4d values)
{
v = values;
return;
}
Quaternion::Quaternion(const Quaternion& q)
{
v = q.v;
return;
}
Quaternion::Quaternion(const Vector3d eulerAngles)
{
Vector3d eulerRad = eulerAngles * Deg2Rad;
#ifndef _EULE_NO_INTRINSICS_
// Calculate sine and cos values
__m256d __vec = _mm256_set_pd(0, eulerRad.z, eulerRad.y, eulerRad.x);
__vec = _mm256_mul_pd(__vec, _mm256_set1_pd(0.5));
__m256d __cos;
__m256d __sin = _mm256_sincos_pd(&__cos, __vec);
// Create multiplication vectors
double sin[4];
double cos[4];
_mm256_storeu_pd(sin, __sin);
_mm256_storeu_pd(cos, __cos);
__m256d __a = _mm256_set_pd(cos[0], cos[0], sin[0], cos[0]);
__m256d __b = _mm256_set_pd(cos[1], sin[1], cos[1], cos[1]);
__m256d __c = _mm256_set_pd(sin[2], cos[2], cos[2], cos[2]);
__m256d __d = _mm256_set_pd(sin[0], sin[0], cos[0], sin[0]);
__m256d __e = _mm256_set_pd(sin[1], cos[1], sin[1], sin[1]);
__m256d __f = _mm256_set_pd(cos[2], sin[2], sin[2], sin[2]);
// Multiply them
__m256d __abc;
__abc = _mm256_mul_pd(__a, __b);
__abc = _mm256_mul_pd(__abc, __c);
__m256d __def;
__def = _mm256_mul_pd(__d, __e);
__def = _mm256_mul_pd(__def, __f);
// Extract results
double abc[4];
double def[4];
_mm256_storeu_pd(abc, __abc);
_mm256_storeu_pd(def, __def);
// Sum them up
v.w = abc[0] + def[0];
v.x = abc[1] - def[1];
v.y = abc[2] + def[2];
v.z = abc[3] - def[3];
#else
const double cy = cos(eulerRad.z * 0.5);
const double sy = sin(eulerRad.z * 0.5);
const double cp = cos(eulerRad.y * 0.5);
const double sp = sin(eulerRad.y * 0.5);
const double cr = cos(eulerRad.x * 0.5);
const double sr = sin(eulerRad.x * 0.5);
v.w = cr * cp * cy + sr * sp * sy;
v.x = sr * cp * cy - cr * sp * sy;
v.y = cr * sp * cy + sr * cp * sy;
v.z = cr * cp * sy - sr * sp * cy;
#endif
return;
}
Quaternion::~Quaternion()
{
return;
}
Quaternion Quaternion::operator= (const Quaternion& q)
{
InvalidateCache();
v = q.v;
return (*this);
}
Quaternion Quaternion::operator* (const Quaternion& q) const
{
return Quaternion(Vector4d(
v.w * q.v.x + v.x * q.v.w + v.y * q.v.z - v.z * q.v.y,
v.w * q.v.y + v.y * q.v.w + v.z * q.v.x - v.x * q.v.z,
v.w * q.v.z + v.z * q.v.w + v.x * q.v.y - v.y * q.v.x,
v.w * q.v.w - v.x * q.v.x - v.y * q.v.y - v.z * q.v.z
));
}
Quaternion Quaternion::operator*(const double scale) const
{
return Quaternion(v * scale);
}
Quaternion Quaternion::operator/ (Quaternion& q) const
{
return ((*this) * (q.Inverse()));
}
Quaternion& Quaternion::operator*= (const Quaternion& q)
{
InvalidateCache();
Vector4d bufr = v;
v.x = bufr.w * q.v.x + bufr.x * q.v.w + bufr.y * q.v.z - bufr.z * q.v.y; // x
v.y = bufr.w * q.v.y + bufr.y * q.v.w + bufr.z * q.v.x - bufr.x * q.v.z; // y
v.z = bufr.w * q.v.z + bufr.z * q.v.w + bufr.x * q.v.y - bufr.y * q.v.x; // z
v.w = bufr.w * q.v.w - bufr.x * q.v.x - bufr.y * q.v.y - bufr.z * q.v.z; // w
return (*this);
}
Quaternion& Quaternion::operator*=(const double scale)
{
InvalidateCache();
v *= scale;
return (*this);
}
Quaternion& Quaternion::operator/= (const Quaternion& q)
{
InvalidateCache();
(*this) = (*this) * q.Inverse();
return (*this);
}
Vector3d Quaternion::operator*(const Vector3d& p) const
{
return RotateVector(p);
}
bool Quaternion::operator== (const Quaternion& q) const
{
return (v.Similar(q.v)) || (v.Similar(q.v * -1));
}
bool Quaternion::operator!= (const Quaternion& q) const
{
return (!v.Similar(q.v)) && (!v.Similar(q.v * -1));
}
Quaternion Quaternion::Inverse() const
{
if (!isCacheUpToDate_inverse)
{
cache_inverse = (Conjugate() * (1.0 / v.SqrMagnitude())).v;
isCacheUpToDate_inverse = true;
}
return Quaternion(cache_inverse);
}
Quaternion Quaternion::Conjugate() const
{
return Quaternion(Vector4d(-v.x, -v.y, -v.z, v.w));
}
Quaternion Quaternion::UnitQuaternion() const
{
return (*this) * (1.0 / v.Magnitude());
}
Vector3d Quaternion::RotateVector(const Vector3d& vec) const
{
Quaternion pure(Vector4d(vec.x, vec.y, vec.z, 0));
//Quaternion f = Conjugate() * pure * (*this);
//Quaternion f = Inverse().Conjugate() * pure * (this->Inverse());
Quaternion f = Inverse() * pure * (*this);
Vector3d toRet;
toRet.x = f.v.x;
toRet.y = f.v.y;
toRet.z = f.v.z;
return toRet;
}
Vector3d Quaternion::ToEulerAngles() const
{
if (!isCacheUpToDate_euler)
{
Vector3d euler;
// roll (x-axis rotation)
double sinr_cosp = 2.0 * (v.w * v.x + v.y * v.z);
double cosr_cosp = 1.0 - 2.0 * (v.x * v.x + v.y * v.y);
euler.x = std::atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2.0 * (v.w * v.y - v.z * v.x);
if (std::abs(sinp) >= 1)
euler.y = std::copysign(PI / 2, sinp); // use 90 degrees if out of range
else
euler.y = std::asin(sinp);
// yaw (z-axis rotation)
double siny_cosp = 2.0 * (v.w * v.z + v.x * v.y);
double cosy_cosp = 1.0 - 2.0 * (v.y * v.y + v.z * v.z);
euler.z = std::atan2(siny_cosp, cosy_cosp);
euler *= Rad2Deg;
cache_euler = euler;
isCacheUpToDate_matrix = true;
}
return cache_euler;
}
Matrix4x4 Quaternion::ToRotationMatrix() const
{
if (!isCacheUpToDate_matrix)
{
Matrix4x4 m;
const double sqx = v.x * v.x;
const double sqy = v.y * v.y;
const double sqz = v.z * v.z;
const double sqw = v.w * v.w;
const double x = v.x;
const double y = v.y;
const double z = v.z;
const double w = v.w;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1.0 / (sqx + sqy + sqz + sqw);
// since sqw + sqx + sqy + sqz =1/invs*invs
// yaw (y)
m.c = ((2 * x * z) - (2 * w * y)) * invs;
m.f = (1 - (2 * sqx) - (2 * sqz)) * invs;
m.i = ((2 * x * z) + (2 * w * y)) * invs;
// pitch (x)
m.a = (1 - (2 * sqy) - (2 * sqz)) * invs;
m.g = ((2 * y * z) + (2 * w * x)) * invs;
m.j = ((2 * y * z) - (2 * w * x)) * invs;
// roll (z)
m.b = ((2 * x * v.y) + (2 * w * z)) * invs;
m.e = ((2 * x * v.y) - (2 * w * z)) * invs;
m.k = (1 - (2 * sqx) - (2 * sqy)) * invs;
m.p = 1;
cache_matrix = m;
isCacheUpToDate_matrix = true;
}
return cache_matrix;
}
Vector4d Quaternion::GetRawValues() const
{
return v;
}
Quaternion Quaternion::AngleBetween(const Quaternion& other) const
{
return other * Conjugate();
}
void Quaternion::SetRawValues(const Vector4d values)
{
InvalidateCache();
v = values;
return;
}
Quaternion Quaternion::Lerp(const Quaternion& other, double t) const
{
return Quaternion(v.Lerp(other.v, t)).UnitQuaternion();
}
void Quaternion::InvalidateCache()
{
isCacheUpToDate_euler = false;
isCacheUpToDate_matrix = false;
isCacheUpToDate_inverse = false;
return;
}
namespace Eule
{
std::ostream& operator<< (std::ostream& os, const Quaternion& q)
{
os << "[" << q.v << "]";
return os;
}
std::wostream& operator<<(std::wostream& os, const Quaternion& q)
{
os << L"[" << q.v << L"]";
return os;
}
}
Eule::Quaternion::operator/
Quaternion operator/(Quaternion &q) const
Divides (applies)
Definition: Quaternion.cpp:126
Eule::Matrix4x4::j
double & j
Definition: Matrix4x4.h:137
Eule::Quaternion::ToRotationMatrix
Matrix4x4 ToRotationMatrix() const
Will return a rotation matrix representing this Quaternions rotation.
Definition: Quaternion.cpp:246
Eule::Quaternion::GetRawValues
Vector4d GetRawValues() const
Will return the raw four-dimensional values.
Definition: Quaternion.cpp:290
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::Quaternion::AngleBetween
Quaternion AngleBetween(const Quaternion &other) const
Will return the value between two Quaternion's as another Quaternion.
Definition: Quaternion.cpp:295
Eule::Vector3< double >
Eule::Quaternion::Lerp
Quaternion Lerp(const Quaternion &other, double t) const
Will return the lerp result between two quaternions.
Definition: Quaternion.cpp:309
Eule::Vector4::x
T x
Definition: Vector4.h:88
Eule::Quaternion::UnitQuaternion
Quaternion UnitQuaternion() const
Definition: Quaternion.cpp:192
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
Constants.h
Eule::Matrix4x4::f
double & f
Definition: Matrix4x4.h:133
Eule::Quaternion::ToEulerAngles
Vector3d ToEulerAngles() const
Will return euler angles representing this Quaternion's rotation.
Definition: Quaternion.cpp:215
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::Quaternion::operator=
Quaternion operator=(const Quaternion &q)
Copies.
Definition: Quaternion.cpp:102
Eule::Vector4::w
T w
Definition: Vector4.h:91
Eule::Vector3::z
T z
Definition: Vector3.h:96
Eule::Quaternion::~Quaternion
~Quaternion()
Definition: Quaternion.cpp:97
Eule::Quaternion::operator*
Quaternion operator*(const Quaternion &q) const
Multiplies (applies)
Definition: Quaternion.cpp:111
Eule::Matrix4x4
A matrix 4x4 class representing a 3d transformation.
Definition: Matrix4x4.h:36
Quaternion.h
Eule::Quaternion::operator*=
Quaternion & operator*=(const Quaternion &q)
Also multiplies.
Definition: Quaternion.cpp:131
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::Quaternion::SetRawValues
void SetRawValues(const Vector4d values)
Will set the raw four-dimensional values.
Definition: Quaternion.cpp:300
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
Eule::operator<<
std::ostream & operator<<(std::ostream &os, const Matrix4x4 &m)
Definition: Matrix4x4.cpp:620
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::Quaternion::operator==
bool operator==(const Quaternion &q) const
Definition: Quaternion.cpp:165
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::Quaternion::operator!=
bool operator!=(const Quaternion &q) const
Definition: Quaternion.cpp:170
Eule::Quaternion::operator/=
Quaternion & operator/=(const Quaternion &q)
Also divides.
Definition: Quaternion.cpp:152
Eule
Definition: Collider.h:4
Eule::Vector3::y
T y
Definition: Vector3.h:95
Eule::Vector4< double >