From 57f9898e07667e00fede342e79fc4bee42551693 Mon Sep 17 00:00:00 2001 From: Diren D Bharwani Date: Sun, 23 Oct 2022 20:03:18 +0800 Subject: [PATCH] Finished quaternion implementation --- SHADE_Engine/src/Math/SHQuaternion.cpp | 125 +++++++++++++++++++++---- SHADE_Engine/src/Math/SHQuaternion.h | 40 ++++---- SHADE_Managed/Quaternion.hxx | 17 ++++ SHADE_Managed/src/Math/Vector3.hxx | 1 + 4 files changed, 148 insertions(+), 35 deletions(-) create mode 100644 SHADE_Managed/Quaternion.hxx diff --git a/SHADE_Engine/src/Math/SHQuaternion.cpp b/SHADE_Engine/src/Math/SHQuaternion.cpp index 3878cea1..3564916a 100644 --- a/SHADE_Engine/src/Math/SHQuaternion.cpp +++ b/SHADE_Engine/src/Math/SHQuaternion.cpp @@ -211,15 +211,6 @@ namespace SHADE return XMVectorGetX(XMQuaternionDot(*this, rhs)); } - SHQuaternion SHQuaternion::RotateTowards(const SHQuaternion&, float) const noexcept - { - SHQuaternion result; - - // TODO (Diren) - - return result; - } - SHVec3 SHQuaternion::ToEuler() const noexcept { const float XX = x * x; @@ -317,19 +308,32 @@ namespace SHADE } - float SHQuaternion::Angle(const SHQuaternion&, const SHQuaternion&) noexcept + float SHQuaternion::Angle(const SHQuaternion& q1, const SHQuaternion& q2) noexcept { - // TODO (Diren) + XMVECTOR R = XMQuaternionMultiply(XMQuaternionConjugate(q1), q2); - return 0.0f; + const float RS = XMVectorGetW(R); + R = XMVector3Length(R); + return 2.0f * atan2f(XMVectorGetX(R), RS); } - SHQuaternion SHQuaternion::Lerp(const SHQuaternion&, const SHQuaternion&, float) noexcept + SHQuaternion SHQuaternion::Lerp(const SHQuaternion& q1, const SHQuaternion& q2, float t) noexcept { SHQuaternion result; - // TODO (Diren) + XMVECTOR R = XMVectorZero(); + if (XMVector4GreaterOrEqual(XMVector4Dot(q1, q2), R)) + { + R = XMVectorLerp(q1, q2, t); + } + else + { + const XMVECTOR X0 = XMVectorMultiply(q1, XMVectorReplicate(1.f - t)); + const XMVECTOR X1 = XMVectorMultiply(q2, XMVectorReplicate(t)); + R = XMVectorSubtract(X0, X1); + } + XMStoreFloat4(&result, XMQuaternionNormalize(R)); return result; } @@ -341,13 +345,102 @@ namespace SHADE return result; } - SHQuaternion SHQuaternion::Rotate(const SHVec3& , const SHVec3&) noexcept + SHQuaternion SHQuaternion::ClampedLerp(const SHQuaternion& q1, const SHQuaternion& q2, float t, float tMin, float tMax) noexcept { + return Lerp(q1, q2, std::clamp(t, tMin, tMax)); + } + + + SHQuaternion SHQuaternion::ClampedSlerp(const SHQuaternion& q1, const SHQuaternion& q2, float t, float tMin, float tMax) noexcept + { + return Slerp(q1, q2, std::clamp(t, tMin, tMax)); + } + + SHQuaternion SHQuaternion::FromToRotation(const SHVec3& from, const SHVec3& to) noexcept + { + // Melax, "The Shortest Arc Quaternion", Game Programming Gems + SHQuaternion result; - // TODO (Diren) + const XMVECTOR F = XMVector3Normalize(from); + const XMVECTOR T = XMVector3Normalize(to); + + const float dot = XMVectorGetX(XMVector3Dot(F, T)); + if (dot >= 1.f) + { + result = Identity; + } + else if (dot <= -1.f) + { + XMVECTOR axis = XMVector3Cross(F, SHVec3::Right); + if (XMVector3NearEqual(XMVector3LengthSq(axis), g_XMZero, g_XMEpsilon)) + { + axis = XMVector3Cross(F, SHVec3::Up); + } + + const XMVECTOR Q = XMQuaternionRotationAxis(axis, XM_PI); + XMStoreFloat4(&result, Q); + } + else + { + const XMVECTOR C = XMVector3Cross(F, T); + XMStoreFloat4(&result, C); + + const float s = sqrtf((1.f + dot) * 2.f); + result.x /= s; + result.y /= s; + result.z /= s; + result.w = s * 0.5f; + } return result; } + SHQuaternion SHQuaternion::LookRotation(const SHVec3& forward, const SHVec3& up) noexcept + { + SHQuaternion result; + + const SHQuaternion Q1 = FromToRotation(SHVec3::Forward, forward); + + const XMVECTOR C = XMVector3Cross(forward, up); + if (XMVector3NearEqual(XMVector3LengthSq(C), g_XMZero, g_XMEpsilon)) + { + // forward and up are co-linear + return Q1; + } + + SHVec3 qU; + XMStoreFloat3(&qU, XMQuaternionMultiply(Q1, SHVec3::Up)); + + const SHQuaternion Q2 = FromToRotation(qU, up); + + XMStoreFloat4(&result, XMQuaternionMultiply(Q2, Q1)); + + return result; + } + + SHQuaternion SHQuaternion::RotateTowards(const SHQuaternion& from, const SHQuaternion& to, float maxAngleInRad) noexcept + { + SHQuaternion result; + + // We can use the conjugate here instead of inverse assuming q1 & q2 are normalized. + const XMVECTOR R = XMQuaternionMultiply(XMQuaternionConjugate(from), to); + + const float RS = XMVectorGetW(R); + const XMVECTOR L = XMVector3Length(R); + const float angle = 2.f * atan2f(XMVectorGetX(L), RS); + if (angle > maxAngleInRad) + { + const XMVECTOR delta = XMQuaternionRotationAxis(R, maxAngleInRad); + const XMVECTOR Q = XMQuaternionMultiply(delta, from); + XMStoreFloat4(&result, Q); + } + else + { + // Don't overshoot. + result = to; + } + return result; + } + } // namespace SHADE \ No newline at end of file diff --git a/SHADE_Engine/src/Math/SHQuaternion.h b/SHADE_Engine/src/Math/SHQuaternion.h index cc1b5ff4..fa5b5d36 100644 --- a/SHADE_Engine/src/Math/SHQuaternion.h +++ b/SHADE_Engine/src/Math/SHQuaternion.h @@ -51,7 +51,6 @@ namespace SHADE SHQuaternion () noexcept; SHQuaternion (const SHVec4& vec4) noexcept; SHQuaternion (float x, float y, float z, float w) noexcept; - SHQuaternion (float yaw, float pitch, float roll) noexcept; // Conversion from other math types @@ -98,34 +97,37 @@ namespace SHADE /* Function Members */ /*---------------------------------------------------------------------------------*/ - void Invert () noexcept; + void Invert () noexcept; - [[nodiscard]] float Length () const noexcept; - [[nodiscard]] float LengthSquared () const noexcept; - [[nodiscard]] float Dot (const SHQuaternion& rhs) const noexcept; - [[nodiscard]] SHQuaternion RotateTowards (const SHQuaternion& target, float maxAngleInRad) const noexcept; + [[nodiscard]] float Length () const noexcept; + [[nodiscard]] float LengthSquared () const noexcept; + [[nodiscard]] float Dot (const SHQuaternion& rhs) const noexcept; - [[nodiscard]] SHVec3 ToEuler () const noexcept; - [[nodiscard]] std::string ToString () const noexcept; + [[nodiscard]] SHVec3 ToEuler () const noexcept; + [[nodiscard]] std::string ToString () const noexcept; /*---------------------------------------------------------------------------------*/ /* Static Function Members */ /*---------------------------------------------------------------------------------*/ - [[nodiscard]] static SHQuaternion FromEuler (const SHVec3& eulerAngles) noexcept; - [[nodiscard]] static SHQuaternion FromPitchYawRoll (float pitch, float yaw, float roll) noexcept; - [[nodiscard]] static SHQuaternion FromAxisAngle (const SHVec3& axis, float angle) noexcept; - [[nodiscard]] static SHQuaternion FromRotationMatrix(const SHMatrix& rotationMatrix) noexcept; + [[nodiscard]] static SHQuaternion FromEuler (const SHVec3& eulerAngles) noexcept; + [[nodiscard]] static SHQuaternion FromPitchYawRoll (float pitch, float yaw, float roll) noexcept; + [[nodiscard]] static SHQuaternion FromAxisAngle (const SHVec3& axis, float angle) noexcept; + [[nodiscard]] static SHQuaternion FromRotationMatrix(const SHMatrix& rotationMatrix) noexcept; - [[nodiscard]] static SHQuaternion Normalise (const SHQuaternion& q) noexcept; - [[nodiscard]] static SHQuaternion Conjugate (const SHQuaternion& q) noexcept; - [[nodiscard]] static SHQuaternion Inverse (const SHQuaternion& q) noexcept; - [[nodiscard]] static float Angle (const SHQuaternion& q1, const SHQuaternion& q2) noexcept; + [[nodiscard]] static SHQuaternion Normalise (const SHQuaternion& q) noexcept; + [[nodiscard]] static SHQuaternion Conjugate (const SHQuaternion& q) noexcept; + [[nodiscard]] static SHQuaternion Inverse (const SHQuaternion& q) noexcept; + [[nodiscard]] static float Angle (const SHQuaternion& q1, const SHQuaternion& q2) noexcept; - [[nodiscard]] static SHQuaternion Lerp (const SHQuaternion& q1, const SHQuaternion& q2, float t) noexcept; - [[nodiscard]] static SHQuaternion Slerp (const SHQuaternion& q1, const SHQuaternion& q2, float t) noexcept; + [[nodiscard]] static SHQuaternion Lerp (const SHQuaternion& q1, const SHQuaternion& q2, float t) noexcept; + [[nodiscard]] static SHQuaternion Slerp (const SHQuaternion& q1, const SHQuaternion& q2, float t) noexcept; + [[nodiscard]] static SHQuaternion ClampedLerp (const SHQuaternion& q1, const SHQuaternion& q2, float t, float tMin = 0.0f, float tMax = 1.0f) noexcept; + [[nodiscard]] static SHQuaternion ClampedSlerp (const SHQuaternion& q1, const SHQuaternion& q2, float t, float tMin = 0.0f, float tMax = 1.0f) noexcept; - [[nodiscard]] static SHQuaternion Rotate (const SHVec3& from, const SHVec3& to) noexcept; + [[nodiscard]] static SHQuaternion FromToRotation (const SHVec3& from, const SHVec3& to) noexcept; + [[nodiscard]] static SHQuaternion LookRotation (const SHVec3& forward, const SHVec3& up) noexcept; + [[nodiscard]] static SHQuaternion RotateTowards (const SHQuaternion& from, const SHQuaternion& to, float maxAngleInRad) noexcept; }; SHQuaternion operator*(float lhs, const SHQuaternion& rhs) noexcept; diff --git a/SHADE_Managed/Quaternion.hxx b/SHADE_Managed/Quaternion.hxx new file mode 100644 index 00000000..0b07a34e --- /dev/null +++ b/SHADE_Managed/Quaternion.hxx @@ -0,0 +1,17 @@ +/************************************************************************************//*! +\file Quaternion.hxx +\author Diren D Bharwani, diren.dbharwani, 390002520 +\par email: diren.dbharwani\@digipen.edu +\date Oct 23, 2022 +\brief Contains the definitions of Quaternion struct. + + Note: This file is written in C++17/CLI. + +Copyright (C) 2021 DigiPen Institute of Technology. +Reproduction or disclosure of this file or its contents without the prior written consent +of DigiPen Institute of Technology is prohibited. +*//*************************************************************************************/ + +#pragma once + +// TODO(Diren) diff --git a/SHADE_Managed/src/Math/Vector3.hxx b/SHADE_Managed/src/Math/Vector3.hxx index e6cdc7d4..8b66439c 100644 --- a/SHADE_Managed/src/Math/Vector3.hxx +++ b/SHADE_Managed/src/Math/Vector3.hxx @@ -11,6 +11,7 @@ Copyright (C) 2021 DigiPen Institute of Technology. Reproduction or disclosure of this file or its contents without the prior written consent of DigiPen Institute of Technology is prohibited. *//*************************************************************************************/ + #pragma once // Standard Libraries