Updated Quaternion and added Test

GlitchyEngine.Test/src/Math/QuaternionTest.bf

@@ -0,0 +1,199 @@
+using System;
+using GlitchyEngine.Math;
+
+namespace GlitchyEngine.Test.Math
+{
+	/**
+	 * @Note Testing 180° Angles is quite stupid as 180° = -180° and thus the result is quite dependant of floating point precision...
+	 */
+	static class QuaternionTest
+	{
+		static bool QuatEquals(Quaternion q1, Quaternion q2, float delta = 0.0001f)
+		{
+			return Math.Abs(q1.X - q2.X) < delta && Math.Abs(q1.Y - q2.Y) < delta &&
+				Math.Abs(q1.Z - q2.Z) < delta && Math.Abs(q1.W - q2.W) < delta;
+		}
+
+		static bool Vector3Equals(Vector3 v1, Vector3 v2, float delta = 0.0001f)
+		{
+			return Math.Abs(v1.X - v2.X) < delta && Math.Abs(v1.Y - v2.Y) < delta &&
+				Math.Abs(v1.Z - v2.Z) < delta;
+		}
+
+		static bool FloatEquals(float l, float r, float delta = 0.0001f)
+		{
+			return Math.Abs(l - r) < delta;
+		}
+
+		[Test]
+		public static void TestAxisAngleToQuaternion()
+		{
+			// angle in degrees
+			void Test(Vector3 axis, float angle, Quaternion expectedQuat)
+			{
+				float angleRad = MathHelper.ToRadians(angle);
+				Quaternion result = .FromAxisAngle(axis, angleRad);
+
+				Quaternion resultNormalized = .Normalize(result);
+				Quaternion expectedNormalized = .Normalize(expectedQuat);
+
+				Test.Assert(QuatEquals(resultNormalized, expectedNormalized));
+			}
+
+			// Reference for conversions: https://www.andre-gaschler.com/rotationconverter/
+			
+			Test(.(0, 0, 0), 55, Quaternion.Identity);
+
+			// x: 90°
+			Test(.(1, 0, 0), 90, Quaternion(1, 0, 0, 1));
+			// y: 90°
+			Test(.(0, 1, 0), 90, Quaternion(0, 1, 0, 1));
+			// z: 90°
+			Test(.(0, 0, 1), 90, Quaternion(0, 0, 1, 1));
+
+			Test(.(1, 1, 0), 90, Quaternion(0.5f, 0.5f, 0, 0.707107f));
+
+			Test(.(1, 1, 1), 90, Quaternion(0.4082483f, 0.4082483f, 0.4082483f, 0.7071068f));
+
+			Test(.(0.3f, 0.5f, 0.4f), -25, Quaternion(-0.0918276f, -0.1530459f, -0.1224367f, 0.976296f));
+
+			Test(.(0.3f, -0.5f, 0.4f), -25, Quaternion(-0.0918276f, 0.1530459f, -0.1224367f, 0.976296f));
+		}
+		
+		[Test]
+		public static void TestQuaternionToAxisAngle()
+		{
+			// angle in degrees
+			void Test(Quaternion quat, Vector3 expectedAxis, float expectedAngle)
+			{
+				Quaternion quatNrm = .Normalize(quat);
+				(Vector3 resultAxis, float resultAngle) = quatNrm.ToAxisAngle();
+
+				resultAxis.Normalize();
+				resultAngle = MathHelper.ToDegrees(resultAngle);
+
+				Vector3 nrmExpectedAxis = .Normalize(expectedAxis);
+
+				Test.Assert(Vector3Equals(resultAxis, nrmExpectedAxis) && FloatEquals(expectedAngle, resultAngle));
+			}
+
+			// Reference for conversions: https://www.andre-gaschler.com/rotationconverter/
+			
+			Test(Quaternion.Identity, .(0, 0, 0), 0);
+
+			// x: 90°
+			Test(Quaternion(1, 0, 0, 1), .(1, 0, 0), 90);
+			// y: 90°
+			Test(Quaternion(0, 1, 0, 1), .(0, 1, 0), 90);
+			// z: 90°
+			Test(Quaternion(0, 0, 1, 1), .(0, 0, 1), 90);
+
+			Test(Quaternion(0.5f, 0.5f, 0, 0.707107f), .(1, 1, 0), 90);
+
+			Test(Quaternion(0.4082483f, 0.4082483f, 0.4082483f, 0.7071068f), .(1, 1, 1), 90);
+
+			Test(Quaternion(-0.0918276f, -0.1530459f, -0.1224367f, 0.976296f), .(-0.4242643f, -0.7071067f, -0.5656853f), 24.9999988f);
+			Test(Quaternion(-0.0918276f, 0.1530459f, -0.1224367f, 0.976296f), .(-0.4242643f, 0.7071067f, -0.5656853f), 24.9999988f);
+		}
+
+		[Test]
+		public static void TestEulerToQuaternion()
+		{
+			void Test(Vector3 inEuler, Quaternion expectedQuat)
+			{
+				Quaternion result = .FromEulerAngles(MathHelper.ToRadians(inEuler.Y), MathHelper.ToRadians(inEuler.X), MathHelper.ToRadians(inEuler.Z));
+
+				Test.Assert(QuatEquals(result, expectedQuat));
+			}
+
+			Test(Vector3.Zero, Quaternion.Identity);
+
+			// X: 90°
+			Test(.(90, 0, 0), Quaternion(1, 0, 0, 1)..Normalize());
+			
+			// Y: 90°
+			Test(.(0, 90, 0), Quaternion(0, 1, 0, 1)..Normalize());
+
+			// Z: 90°
+			Test(.(0, 0, 90), Quaternion(0, 0, 1, 1)..Normalize());
+			
+			// X: 90° Y: 90°
+			Test(.(90, 90, 0), Quaternion(1, 1, -1, 1)..Normalize());
+
+			// 90° 45° 30°
+			Test(.(90, 45, 30), Quaternion(0.7010574f, 0.4304593f, -0.092296f, 0.5609855f)..Normalize());
+
+			// 105° 90° 0°
+			Test(.(105, 90, 0), Quaternion(0.560986f, 0.430459f, -0.560986f, 0.430459f)..Normalize());
+			
+			// 180° 90° -75°
+			Test(.(180, 90, -75), Quaternion(0.5609855f, -0.4304593f, -0.5609855f, 0.4304593f)..Normalize());
+		}
+		
+		[Test]
+		public static void TestQuaternionToEuler()
+		{
+			void Test(Quaternion inQuat, Vector3 expectedEuler)
+			{
+				Quaternion nrmInQuat = .Normalize(inQuat);
+
+				Vector3 result = Quaternion.ToEulerAngles(nrmInQuat);
+				Vector3 resultDeg = MathHelper.ToDegrees(result);
+
+				Test.Assert(Vector3Equals(resultDeg, expectedEuler));
+			}
+
+			Test(Quaternion.Identity, Vector3.Zero);
+
+			// X: 90°
+			Test(Quaternion(1, 0, 0, 1), .(90, 0, 0));
+			
+			// Y: 90°
+			Test(Quaternion(0, 1, 0, 1), .(0, 90, 0));
+
+			// Z: 90°
+			Test(Quaternion(0, 0, 1, 1), .(0, 0, 90));
+			
+			// X: 90° Y: 90°
+			Test(Quaternion(1, 1, -1, 1), .(90, 90, 0));
+
+			// 90° 45° 30°
+			Test(Quaternion(0.7010574f, 0.4304593f, -0.092296f, 0.5609855f), .(90, 45, 30));
+
+			// 105° 90° 0°
+			Test(Quaternion(0.560986f, 0.430459f, -0.560986f, 0.430459f), .(105, 90, 0));
+			
+			// 180° 90° -75°
+			Test(Quaternion(0.5609855f, -0.4304593f, -0.5609855f, 0.4304593f), .(180, 90, -75));
+		}
+		
+		[Test]
+		public static void TestEulerToQautToEuler()
+		{
+			void Test(float yaw, float pitch, float roll)
+			{
+				Quaternion quat = .FromEulerAngles(MathHelper.ToRadians(yaw), MathHelper.ToRadians(pitch), MathHelper.ToRadians(roll));
+
+				Vector3 result = Quaternion.ToEulerAngles(quat);
+
+				result = .(MathHelper.ToDegrees(result.X), MathHelper.ToDegrees(result.Y), MathHelper.ToDegrees(result.Z));
+
+				Test.Assert(Vector3Equals(.(pitch, yaw, roll), result));
+			}
+
+			Test(0, 90, 0);
+			Test(90, 0, 0);
+			Test(0, 0, 90);
+
+			Test(90, 90, 0);
+			
+			Test(90, 45, 0);
+
+			Test(90, 45, 30);
+
+			Test(105, 90, 0);
+
+			Test(179, 90, -75);
+		}
+	}
+}

GlitchyEngine/src/Math/Quaternion.bf

@@ -8,7 +8,6 @@ namespace GlitchyEngine.Math
 		public const Quaternion One = .(1.0f, 1.0f, 1.0f, 1.0f);
 		public const Quaternion Identity = .(0.0f, 0.0f, 0.0f, 1.0f);
 
-		// qv: (X, Y, Z), sv: W
 		public float X, Y, Z, W;
 
 		public this() => this = default;
@@ -45,8 +44,22 @@ namespace GlitchyEngine.Math
 			W = vector.W;
 		}
 
-		public Vector3 Axis => .(X, Y, Z);
+		public Vector3 Vector
-		public float Scalar => W;
+		{
+			get => .(X, Y, Z);
+			set mut
+			{
+				X = value.X;
+				Y = value.Y;
+				Z = value.Z;
+			}
+		}
+		
+		public float Scalar
+		{
+			get => W;
+			set mut => W = value;
+		}
 
 		public void Normalize() mut
 		{
@@ -173,20 +186,25 @@ namespace GlitchyEngine.Math
 		public static Self operator -(Self l, Self r) => Self(l.X - r.X, l.Y - r.Y, l.Z - r.Z, l.W - r.W);
 
 		public static Self operator *(float l, Self r) => Self(l * r.X, l * r.Y, l * r.Z, l * r.W);
+		
+		public static Self operator /(Self l, float r) => Self(l.X * r, l.Y * r, l.Z * r, l.W * r);
 
+		public static Self operator *(Self l, Self r)
+		{
+			Quaternion result;
 
-		[Inline]
+			Vector3 v = l.Vector * r.Vector + (l.W * r.Vector) + (r.W * l.Vector);
-		public static implicit operator Vector4(in Self value) => *(Vector4*)&value;
+			result.X = v.X;
+			result.Y = v.Y;
+			result.Z = v.Z;
+			result.W = (l.W * r.W) - Vector3.Dot(l.Vector, r.Vector);
 
-		[Inline]
+			return result;
-		public static implicit operator Quaternion(in Vector4 value) => *(Quaternion*)&value;
+		}
-
+		
-		//public static Quaternion operator +(Self left, Self right) => return .();
-
-		/*
 		public static Quaternion Conjugate(Quaternion q)
 		{
-			return Quaternion(-q.Axis, q.Scalar);
+			return Quaternion(-q.Vector, q.Scalar);
 		}
 
 		public static Quaternion Inverse(Quaternion q)
@@ -195,16 +213,134 @@ namespace GlitchyEngine.Math
 
 			float magSquared = LengthSquared(q);
 
-			return Quaternion(conjugate / magSquared);
+			return conjugate / magSquared;
 		}
 
-		public static Quaternion operator *(Quaternion left, Quaternion right)
+		[Inline]
+		public static implicit operator Vector4(in Self value) => *(Vector4*)&value;
+
+		[Inline]
+		public static implicit operator Quaternion(in Vector4 value) => *(Quaternion*)&value;
+
+		public static bool operator ==(Quaternion l, Quaternion r)
 		{
-			return Quaternion(left.Scalar * right.Axis + right.Scalar * left.Axis + Vector3.Cross(left.Axis, right.Axis),
+			return l.X == r.X && l.Y == r.Y && l.Z == r.Z && l.W == r.W;
-				left.Scalar * right.Scalar - Vector3.Dot(left.Axis, right.Axis));
 		}
-		*/
 
-		//public static Quaternion operator /(Quaternion left, float right) => Quaternion(left.X / right, left.Y / right, left.Z / right, left.W / right);
+		public static bool operator !=(Quaternion l, Quaternion r)
+		{
+			return l.X != r.X && l.Y != r.Y && l.Z != r.Z && l.W != r.W;
+		}
+
+		public (Vector3 Axis, float Angle) ToAxisAngle()
+		{
+			// scalar part = cos(θ/2)
+			// So, we can extract the angle directly.
+			float angle = 2.0f * Math.Acos(W);
+
+			// vector part = axis * sin(θ/2)
+			// In other words, the vector part is the axis, but with length of sin(θ/2).
+			// We assume quaternion is unit length, so subtracting w^2 gives us length of just vector part (aka sin(θ/2)).
+			float length = Math.Sqrt(1.0f - (W * W));
+
+			Vector3 axis;
+
+			// Normalize vector part to get the axis!
+			if(length == 0)
+			{
+			    axis = Vector3.Zero;
+			}
+			else
+			{
+			    length = 1.0f / length;
+			    axis.X = X * length;
+			    axis.Y = Y * length;
+			    axis.Z = Z * length;
+			}
+
+			return (axis, angle);
+		}
+
+		public static Quaternion FromAxisAngle(Vector3 axis, float angle)
+		{
+			float lengthSq = axis.MagnitudeSquared();
+
+			if(lengthSq == 0)
+			{
+				return .Identity;
+			}
+
+			float halfAngle = angle * 0.5f;
+
+			float sin = Math.Sin(halfAngle) / Math.Sqrt(lengthSq);
+
+			Quaternion result;
+			result.X = axis.X * sin;
+			result.Y = axis.Y * sin;
+			result.Z = axis.Z * sin;
+			result.W = Math.Cos(halfAngle);
+
+			return result;
+		}
+
+		// Assumes YZX-Order meaning Y applied first, Z second and x last
+		public static Quaternion FromEulerAngles(float yaw, float pitch, float roll)
+		{
+			float halfYaw = yaw / 2.0f;
+			float halfPitch = pitch / 2.0f;
+			float halfRoll = roll / 2.0f;
+
+			float cosYaw = Math.Cos(halfYaw);//heading
+			float sinYaw = Math.Sin(halfYaw);
+			float cosRoll = Math.Cos(halfRoll);//attitude
+			float sinRoll = Math.Sin(halfRoll);
+			float cosPitch = Math.Cos(halfPitch);//bank
+			float sinPitch = Math.Sin(halfPitch);
+
+			float cosYawCosRoll = cosYaw * cosRoll;
+			float sinYawSinRoll = sinYaw * sinRoll;
+			float cosYawSinRoll = cosYaw * sinRoll;
+			float sinYawCosRoll = sinYaw * cosRoll;
+
+			Quaternion result;
+
+			result.W = cosYawCosRoll * cosPitch - sinYawSinRoll * sinPitch;
+			result.X = cosYawCosRoll * sinPitch + sinYawSinRoll * cosPitch;
+			result.Y = sinYawCosRoll * cosPitch + cosYawSinRoll * sinPitch;
+			result.Z = cosYawSinRoll * cosPitch - sinYawCosRoll * sinPitch;
+			
+			return result;
+		}
+
+		public static Vector3 ToEulerAngles(Quaternion q1)
+		{
+			// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
+
+			Vector3 result;
+
+		    float sqw = q1.W*q1.W;
+		    float sqx = q1.X*q1.X;
+		    float sqy = q1.Y*q1.Y;
+		    float sqz = q1.Z*q1.Z;
+			float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
+			float test = q1.X*q1.Y + q1.Z*q1.W;
+			if (test > 0.499f*unit) { // singularity at north pole
+				result.Y = 2.0f * Math.Atan2(q1.X,q1.W);
+				result.Z = Math.PI_f / 2.0f;
+				result.X = 0.0f;
+				return result;
+			}
+			if (test < -0.499f*unit) { // singularity at south pole
+				result.Y = -2.0f * Math.Atan2(q1.X,q1.W);
+				result.Z = -Math.PI_f / 2.0f;
+				result.X = 0.0f;
+				return result;
+			}
+		    result.Y = Math.Atan2(2*q1.Y*q1.W-2*q1.X*q1.Z , sqx - sqy - sqz + sqw);
+			result.Z = Math.Asin(2*test/unit);
+			result.X = Math.Atan2(2*q1.X*q1.W-2*q1.Y*q1.Z , -sqx + sqy - sqz + sqw);
+
+			return result;
+		}
 	}
 }