From f5fefc7b86e14d4c26e8393c3d80e0af6fd9dcb6 Mon Sep 17 00:00:00 2001 From: SirLynix Date: Thu, 7 Dec 2023 16:50:22 +0100 Subject: [PATCH] Math: Add AngleBetween and RotateTowards for Vector3 and Quaternion --- include/Nazara/Math/Quaternion.hpp | 8 +++++--- include/Nazara/Math/Quaternion.inl | 25 ++++++++++++++++++++++-- include/Nazara/Math/Vector3.hpp | 1 + include/Nazara/Math/Vector3.inl | 31 +++++++++++++++--------------- 4 files changed, 45 insertions(+), 20 deletions(-) diff --git a/include/Nazara/Math/Quaternion.hpp b/include/Nazara/Math/Quaternion.hpp index da7fbb093..f5286c53a 100644 --- a/include/Nazara/Math/Quaternion.hpp +++ b/include/Nazara/Math/Quaternion.hpp @@ -8,15 +8,14 @@ #define NAZARA_MATH_QUATERNION_HPP #include +#include +#include #include namespace Nz { struct SerializationContext; - template class EulerAngles; - template class Vector3; - template class Quaternion { public: @@ -31,6 +30,7 @@ namespace Nz constexpr Quaternion(Quaternion&&) = default; ~Quaternion() = default; + RadianAngle AngleBetween(const Quaternion& vec) const; constexpr bool ApproxEqual(const Quaternion& quat, T maxDifference = std::numeric_limits::epsilon()) const; Quaternion& ComputeW(); @@ -75,12 +75,14 @@ namespace Nz constexpr bool operator>(const Quaternion& quat) const; constexpr bool operator>=(const Quaternion& quat) const; + static RadianAngle AngleBetween(const Quaternion& lhs, const Quaternion& rhs); static constexpr bool ApproxEqual(const Quaternion& lhs, const Quaternion& rhs, T maxDifference = std::numeric_limits::epsilon()); static constexpr Quaternion Identity(); static constexpr Quaternion Lerp(const Quaternion& from, const Quaternion& to, T interpolation); static Quaternion LookAt(const Vector3& forward, const Vector3& up); static Quaternion Normalize(const Quaternion& quat, T* length = nullptr); static Quaternion RotationBetween(const Vector3& from, const Vector3& to); + static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, RadianAngle maxRotation); static Quaternion Mirror(Quaternion quat, const Vector3& axis); static Quaternion Slerp(const Quaternion& from, const Quaternion& to, T interpolation); static constexpr Quaternion Zero(); diff --git a/include/Nazara/Math/Quaternion.inl b/include/Nazara/Math/Quaternion.inl index 8e3b35ea5..e730f89e3 100644 --- a/include/Nazara/Math/Quaternion.inl +++ b/include/Nazara/Math/Quaternion.inl @@ -4,8 +4,6 @@ #include #include -#include -#include #include #include #include @@ -116,6 +114,13 @@ namespace Nz { } + template + RadianAngle Quaternion::AngleBetween(const Quaternion& quat) const + { + T alpha = Vector3::DotProduct(Vector3(x, y, z), Vector3(quat.x, quat.y, quat.z)); + return std::acos(Nz::Clamp(alpha, T(-1.0), T(1.0))); + } + template constexpr bool Quaternion::ApproxEqual(const Quaternion& quat, T maxDifference) const { @@ -571,6 +576,12 @@ namespace Nz return z >= quat.z; } + template + RadianAngle Quaternion::AngleBetween(const Quaternion& lhs, const Quaternion& rhs) + { + return lhs.AngleBetween(rhs); + } + template constexpr bool Quaternion::ApproxEqual(const Quaternion& lhs, const Quaternion& rhs, T maxDifference) { @@ -687,6 +698,16 @@ namespace Nz } } + template + Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, RadianAngle maxRotation) + { + RadianAngle rotationBetween = AngleBetween(from, to); + if (rotationBetween < maxRotation) + return to; + + return Slerp(from, to, std::min(maxRotation.value / rotationBetween.value), 1.f); + } + template Quaternion Quaternion::Mirror(Quaternion quat, const Vector3& axis) { diff --git a/include/Nazara/Math/Vector3.hpp b/include/Nazara/Math/Vector3.hpp index fb222396b..2e0da1b40 100644 --- a/include/Nazara/Math/Vector3.hpp +++ b/include/Nazara/Math/Vector3.hpp @@ -105,6 +105,7 @@ namespace Nz static constexpr Vector3 Lerp(const Vector3& from, const Vector3& to, T interpolation); static constexpr Vector3 Max(const Vector3& lhs, const Vector3& rhs); static constexpr Vector3 Min(const Vector3& lhs, const Vector3& rhs); + static Vector3 RotateTowards(const Vector3& from, const Vector3& to, RadianAngle maxAngle); static Vector3 Normalize(const Vector3& vec); static constexpr Vector3 Right(); static constexpr T SquaredDistance(const Vector3& vec1, const Vector3& vec2); diff --git a/include/Nazara/Math/Vector3.inl b/include/Nazara/Math/Vector3.inl index c0e34d232..219ef77df 100644 --- a/include/Nazara/Math/Vector3.inl +++ b/include/Nazara/Math/Vector3.inl @@ -123,7 +123,7 @@ namespace Nz * * \param vec The other vector to measure the angle with * - * \remark The vectors do not need to be normalised + * \remark The vectors need to be normalised * \remark Produce a NazaraError if one of the vec components is null with NAZARA_MATH_SAFE defined * \throw std::domain_error if NAZARA_MATH_SAFE is defined and one of the vec components is null * @@ -132,20 +132,7 @@ namespace Nz template RadianAngle Vector3::AngleBetween(const Vector3& vec) const { - // sqrt(a) * sqrt(b) = sqrt(a*b) - T divisor = std::sqrt(GetSquaredLength() * vec.GetSquaredLength()); - - #if NAZARA_MATH_SAFE - if (NumberEquals(divisor, T(0.0))) - { - std::string error("Division by zero"); - - NazaraError(error); - throw std::domain_error(std::move(error)); - } - #endif - - T alpha = DotProduct(vec) / divisor; + T alpha = DotProduct(vec); return std::acos(Nz::Clamp(alpha, T(-1.0), T(1.0))); } @@ -835,6 +822,20 @@ namespace Nz return min; } + template + Vector3 Vector3::RotateTowards(const Vector3& from, const Vector3& to, RadianAngle maxAngle) + { + // https://gamedev.stackexchange.com/a/203036 + RadianAngle angleBetween = from.AngleBetween(to); + if (angleBetween < maxAngle) + return to; + + Vector3 axis = CrossProduct(from, to); + + Quaternion rotationIncrement = Quaternion(maxAngle, axis); + return rotationIncrement * from; + } + /*! * \brief Gives the normalized vector * \return A normalized vector from the vec