diff --git a/GlitchyEngine.Test/src/Math/QuaternionTest.bf b/GlitchyEngine.Test/src/Math/QuaternionTest.bf new file mode 100644 index 0000000..7da9870 --- /dev/null +++ b/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); + } + } +} diff --git a/GlitchyEngine/src/Math/Quaternion.bf b/GlitchyEngine/src/Math/Quaternion.bf index 52fde9a..d1c011e 100644 --- a/GlitchyEngine/src/Math/Quaternion.bf +++ b/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 float Scalar => W; + public Vector3 Vector + { + 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] - public static implicit operator Vector4(in Self value) => *(Vector4*)&value; + Vector3 v = l.Vector * r.Vector + (l.W * r.Vector) + (r.W * l.Vector); + 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] - public static implicit operator Quaternion(in Vector4 value) => *(Quaternion*)&value; - - //public static Quaternion operator +(Self left, Self right) => return .(); - - /* + return result; + } + 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), - left.Scalar * right.Scalar - Vector3.Dot(left.Axis, right.Axis)); + return l.X == r.X && l.Y == r.Y && l.Z == r.Z && l.W == r.W; } - */ - //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; + } } }