From f2519fd085a7af27d77a38ef3b43e26d9a7172b2 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Fri, 11 Feb 2022 14:48:13 +0100 Subject: [PATCH] Fixed bug in Quaternion that didn't allow for operator/ with a const-qualified operand b --- Eule/Quaternion.cpp | 640 ++++++++++++++++++++++---------------------- Eule/Quaternion.h | 2 +- 2 files changed, 320 insertions(+), 322 deletions(-) diff --git a/Eule/Quaternion.cpp b/Eule/Quaternion.cpp index 50f4989..79fd915 100644 --- a/Eule/Quaternion.cpp +++ b/Eule/Quaternion.cpp @@ -10,328 +10,326 @@ #include "gcccompat.h" #endif -using namespace Eule; +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/ (const 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 + { + const std::lock_guard lock(lock_inverseCache); + + 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 + { + const std::lock_guard lock(lock_eulerCache); + + 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 + { + const std::lock_guard lock(lock_matrixCache); + + 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; + } -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 -{ - const std::lock_guard lock(lock_inverseCache); - - 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 -{ - const std::lock_guard lock(lock_eulerCache); - - 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 -{ - const std::lock_guard lock(lock_matrixCache); - - 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 << "]"; diff --git a/Eule/Quaternion.h b/Eule/Quaternion.h index e36fe57..5a13de6 100644 --- a/Eule/Quaternion.h +++ b/Eule/Quaternion.h @@ -31,7 +31,7 @@ namespace Eule Quaternion operator* (const Quaternion& q) const; //! Divides (applies) - Quaternion operator/ (Quaternion& q) const; + Quaternion operator/ (const Quaternion& q) const; //! Also multiplies Quaternion& operator*= (const Quaternion& q);