Math: Add AngleBetween and RotateTowards for Vector3 and Quaternion

include/Nazara/Math/Quaternion.hpp

@@ -8,15 +8,14 @@
 #define NAZARA_MATH_QUATERNION_HPP
 
 #include <Nazara/Math/Angle.hpp>
+#include <Nazara/Math/EulerAngles.hpp>
+#include <Nazara/Math/Vector3.hpp>
 #include <string>
 
 namespace Nz
 {
 	struct SerializationContext;
 
-	template<typename T> class EulerAngles;
-	template<typename T> class Vector3;
-
 	template<typename T> class Quaternion
 	{
 		public:
@@ -31,6 +30,7 @@ namespace Nz
 			constexpr Quaternion(Quaternion&&) = default;
 			~Quaternion() = default;
 
+			RadianAngle<T> AngleBetween(const Quaternion& vec) const;
 			constexpr bool ApproxEqual(const Quaternion& quat, T maxDifference = std::numeric_limits<T>::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<T> AngleBetween(const Quaternion& lhs, const Quaternion& rhs);
 			static constexpr bool ApproxEqual(const Quaternion& lhs, const Quaternion& rhs, T maxDifference = std::numeric_limits<T>::epsilon());
 			static constexpr Quaternion Identity();
 			static constexpr Quaternion Lerp(const Quaternion& from, const Quaternion& to, T interpolation);
 			static Quaternion LookAt(const Vector3<T>& forward, const Vector3<T>& up);
 			static Quaternion Normalize(const Quaternion& quat, T* length = nullptr);
 			static Quaternion RotationBetween(const Vector3<T>& from, const Vector3<T>& to);
+			static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, RadianAngle<T> maxRotation);
 			static Quaternion Mirror(Quaternion quat, const Vector3<T>& axis);
 			static Quaternion Slerp(const Quaternion& from, const Quaternion& to, T interpolation);
 			static constexpr Quaternion Zero();

include/Nazara/Math/Quaternion.inl

@@ -4,8 +4,6 @@
 
 #include <Nazara/Core/Algorithm.hpp>
 #include <Nazara/Math/Config.hpp>
-#include <Nazara/Math/EulerAngles.hpp>
-#include <Nazara/Math/Vector3.hpp>
 #include <cstring>
 #include <limits>
 #include <sstream>
@@ -116,6 +114,13 @@ namespace Nz
 	{
 	}
 
+	template<typename T>
+	RadianAngle<T> Quaternion<T>::AngleBetween(const Quaternion& quat) const
+	{
+		T alpha = Vector3<T>::DotProduct(Vector3<T>(x, y, z), Vector3<T>(quat.x, quat.y, quat.z));
+		return std::acos(Nz::Clamp(alpha, T(-1.0), T(1.0)));
+	}
+
 	template<typename T>
 	constexpr bool Quaternion<T>::ApproxEqual(const Quaternion& quat, T maxDifference) const
 	{
@@ -571,6 +576,12 @@ namespace Nz
 			return z >= quat.z;
 	}
 
+	template<typename T>
+	RadianAngle<T> Quaternion<T>::AngleBetween(const Quaternion& lhs, const Quaternion& rhs)
+	{
+		return lhs.AngleBetween(rhs);
+	}
+
 	template<typename T>
 	constexpr bool Quaternion<T>::ApproxEqual(const Quaternion& lhs, const Quaternion& rhs, T maxDifference)
 	{
@@ -687,6 +698,16 @@ namespace Nz
 		}
 	}
 
+	template<typename T>
+	Quaternion<T> Quaternion<T>::RotateTowards(const Quaternion& from, const Quaternion& to, RadianAngle<T> maxRotation)
+	{
+		RadianAngle<T> rotationBetween = AngleBetween(from, to);
+		if (rotationBetween < maxRotation)
+			return to;
+
+		return Slerp(from, to, std::min(maxRotation.value / rotationBetween.value), 1.f);
+	}
+
 	template<typename T>
 	Quaternion<T> Quaternion<T>::Mirror(Quaternion quat, const Vector3<T>& axis)
 	{

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<T> maxAngle);
 			static Vector3 Normalize(const Vector3& vec);
 			static constexpr Vector3 Right();
 			static constexpr T SquaredDistance(const Vector3& vec1, const Vector3& vec2);

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<typename T>
 	RadianAngle<T> Vector3<T>::AngleBetween(const Vector3& vec) const
 	{
-		// sqrt(a) * sqrt(b) = sqrt(a*b)
+		T alpha = DotProduct(vec);
-		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;
 		return std::acos(Nz::Clamp(alpha, T(-1.0), T(1.0)));
 	}
 
@@ -835,6 +822,20 @@ namespace Nz
 		return min;
 	}
 
+	template<typename T>
+	Vector3<T> Vector3<T>::RotateTowards(const Vector3& from, const Vector3& to, RadianAngle<T> maxAngle)
+	{
+		// https://gamedev.stackexchange.com/a/203036
+		RadianAngle<T> angleBetween = from.AngleBetween(to);
+		if (angleBetween < maxAngle)
+			return to;
+
+		Vector3 axis = CrossProduct(from, to);
+
+		Quaternion<T> rotationIncrement = Quaternion(maxAngle, axis);
+		return rotationIncrement * from;
+	}
+
 	/*!
 	* \brief Gives the normalized vector
 	* \return A normalized vector from the vec