From 03ad72afbb22a39055ab5b48d52608d09e2654e1 Mon Sep 17 00:00:00 2001 From: Tonis Date: Mon, 20 Oct 2025 12:04:28 +0300 Subject: [PATCH] Quaternion math improvements (#2524) * Add radians to deg function * Quaternion math fixes * Formatting, use splat/swizzling, divide into multiple tests. --------- Co-authored-by: tonis2 Co-authored-by: Christoffer Lerno --- lib/std/math/math.c3 | 5 ++ lib/std/math/quaternion.c3 | 47 ++++++++++--- lib/std/math/vector.c3 | 19 +----- test/unit/stdlib/math/quaternion.c3 | 101 ++++++++++++++++++++-------- 4 files changed, 116 insertions(+), 56 deletions(-) diff --git a/lib/std/math/math.c3 b/lib/std/math/math.c3 index 48c551803..8170affc2 100644 --- a/lib/std/math/math.c3 +++ b/lib/std/math/math.c3 @@ -73,6 +73,11 @@ faultdef OVERFLOW, MATRIX_INVERSE_DOESNT_EXIST; *> macro deg_to_rad(x) => x * PI / 180; +<* + @require types::is_numerical($typeof(x)) : `The input must be a numerical value or numerical vector` +*> +macro rad_to_deg(x) => x * 180 / PI; + <* @require types::is_numerical($typeof(x)) : `The input must be a numerical value or numerical vector` *> diff --git a/lib/std/math/quaternion.c3 b/lib/std/math/quaternion.c3 index eb9d844ad..2728d9ae1 100644 --- a/lib/std/math/quaternion.c3 +++ b/lib/std/math/quaternion.c3 @@ -32,12 +32,15 @@ macro QuaternionNumber QuaternionNumber.sub(self, QuaternionNumber b) @operator( macro QuaternionNumber QuaternionNumber.negate(self) @operator(-) => { .v = -self.v }; macro QuaternionNumber QuaternionNumber.sub_each(self, Real b) => { .v = self.v - b }; macro QuaternionNumber QuaternionNumber.scale(self, Real s) @operator_s(*) => { .v = self.v * s }; +macro QuaternionNumber.to_angle(self) => 2 * math::acos(self.v.w); macro QuaternionNumber QuaternionNumber.normalize(self) => { .v = self.v.normalize() }; macro Real QuaternionNumber.length(self) => self.v.length(); macro QuaternionNumber QuaternionNumber.lerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount) }; +fn QuaternionNumber QuaternionNumber.nlerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount).normalize() }; + macro Matrix4f QuaternionNumber.to_matrixf(&self) => into_matrix(self, Matrix4f); macro Matrix4 QuaternionNumber.to_matrix(&self) => into_matrix(self, Matrix4); -fn QuaternionNumber QuaternionNumber.nlerp(self, QuaternionNumber q2, Real amount) => { .v = self.v.lerp(q2.v, amount).normalize() }; + fn QuaternionNumber QuaternionNumber.invert(self) { @@ -47,6 +50,8 @@ fn QuaternionNumber QuaternionNumber.invert(self) return { self.v[0] * -inv_length, self.v[1] * -inv_length, self.v[2] * -inv_length, self.v[3] * inv_length }; } +fn QuaternionNumber QuaternionNumber.conjugate(&self) => { -self.v.x, -self.v.y, -self.v.z, self.v.w }; + fn QuaternionNumber QuaternionNumber.slerp(self, QuaternionNumber q2, Real amount) { QuaternionNumber result = {}; @@ -77,13 +82,35 @@ fn QuaternionNumber QuaternionNumber.slerp(self, QuaternionNumber q2, Real amoun } fn QuaternionNumber QuaternionNumber.mul(self, QuaternionNumber b) @operator(*) -{ - return { self.i * b.l + self.l * b.i + self.j * b.k - self.k * b.j, - self.j * b.l + self.l * b.j + self.k * b.i - self.i * b.k, - self.k * b.l + self.l * b.k + self.i * b.j - self.j * b.i, - self.l * b.l - self.i * b.i - self.j * self.j - self.k * self.k }; +{ + Real[<3>] q1_axis = { self.v.x, self.v.y, self.v.z }; + Real[<3>] q2_axis = { b.v.x, b.v.y, b.v.z }; + + Real scalar = (self.v.w * b.v.w - q1_axis.dot(q2_axis)); + Real[<3>] axis = self.v.w * q2_axis + b.v.w * q1_axis + q1_axis.cross(q2_axis); + + return { ...axis, scalar }; } + +fn QuaternionNumber from_axis_angle(Real[<3>] axis, Real angle) +{ + Real[<3>] normal_axis = axis.normalize(); + Real half_angle = angle * 0.5; + Real sin_half = math::sin(half_angle); + + return { ...(normal_axis * sin_half), math::cos(half_angle) }; +} + +fn Real[<3>] QuaternionNumber.rotate_vec3(self, Real[<3>] vector) @operator(*) +{ + QuaternionNumber p = { ...vector, 0 }; + QuaternionNumber result = self * p * self.conjugate(); + return result.v.xyz; +} + + + macro into_matrix(QuaternionNumber* q, $Type) @private { QuaternionNumber rotation = q.normalize(); @@ -93,9 +120,9 @@ macro into_matrix(QuaternionNumber* q, $Type) @private var w = rotation.l; return ($Type) { - 1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w, 0, - 2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w, 0, - 2*x*z - 2*y*w, 2*y*z + 2*x*w , 1 - 2*x*x - 2*y*y, 0, - 0.0, 0.0, 0.0, 1.0, + 1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w, 0, + 2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w, 0, + 2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y, 0, + 0.0, 0.0, 0.0, 1.0, }; } \ No newline at end of file diff --git a/lib/std/math/vector.c3 b/lib/std/math/vector.c3 index 81e162da2..ca6c25a4c 100644 --- a/lib/std/math/vector.c3 +++ b/lib/std/math/vector.c3 @@ -41,8 +41,8 @@ fn double double[<3>].angle(self, double[<3>] v2) => angle3(self, v2); fn float[<3>] float[<3>].refract(self, float[<3>] n, float r) => refract3(self, n, r); fn double[<3>] double[<3>].refract(self, double[<3>] n, double r) => refract3(self, n, r); -fn float[<3>] float[<3>].rotate_quat(self, Quaternionf q) => rotate_by_quat3(self, q); -fn double[<3>] double[<3>].rotate_quat(self, Quaternion q) => rotate_by_quat3(self, q); +fn float[<3>] float[<3>].rotate_quat(self, Quaternionf q) => q * self; +fn double[<3>] double[<3>].rotate_quat(self, Quaternion q) => q * self; fn float[<3>] float[<3>].rotate_axis(self, float[<3>] axis, float angle) => rotate_axis_angle(self, axis, angle); fn double[<3>] double[<3>].rotate_axis(self, double[<3>] axis, double angle) => rotate_axis_angle(self, axis, angle); @@ -144,21 +144,6 @@ macro void ortho_normalize3(v1, v2) @private *v2 = v1n.cross(vn1); } -macro rotate_by_quat3(v, q) @private -{ - return ($typeof(v)){ - v[0] * (q.i * q.i + q.l * q.l - q.j * q.j - q.k * q.k) - + v[1] * (2 * q.i * q.j - 2 * q.l * q.k) - + v[2] * (2 * q.i * q.k - 2 * q.l * q.j), - v[0] * (2 * q.l * q.k + 2 * q.i * q.j) - + v[1] * (q.l * q.l - q.i * q.i + q.j * q.j - q.k * q.k) - + v[2] * (-2 * q.l * q.i + 2 * q.j * q.k), - v[0] * (-2 * q.l * q.j + 2 * q.i * q.k) - + v[1] * (2 * q.l * q.i + 2 * q.j * q.k) - + v[2] * (q.l * q.l - q.i * q.i - q.j * q.j + q.k * q.k) - }; -} - macro rotate_axis_angle(v, axis, angle) @private { axis = axis.normalize(); diff --git a/test/unit/stdlib/math/quaternion.c3 b/test/unit/stdlib/math/quaternion.c3 index 2438b1de6..912ffccf9 100644 --- a/test/unit/stdlib/math/quaternion.c3 +++ b/test/unit/stdlib/math/quaternion.c3 @@ -1,38 +1,81 @@ -module math_quaternion @test; +module math_tests::quaternion @test; import std::math; -fn void test() +fn void test_rad_to_deg() { - { - Quaternion rotation = QUATERNION_IDENTITY; - Quaternionf rotation_f = QUATERNIONF_IDENTITY; - assert(rotation.v == {0,0,0,1}); - assert(rotation.v == {0,0,0,1}); + Quaternion value = { 0, 0.7071068, 0.7071068, 0 }; + double angle_radians = value.to_angle(); + assert(math::round_to_decimals(angle_radians, 2) == 3.14); + assert(math::rad_to_deg(angle_radians) == 180.0); +} + +fn void test_rotation() +{ + double[<3>] axis = { 3, 0, 3 }; + Quaternion rotation = { 0.5, 0, 0.5, 0 }; + double[<3>] result = { 1.500000, 0.000000, 1.500000 }; + + assert(rotation * axis == result); + assert(rotation.rotate_vec3(axis) == result); + assert(axis.rotate_quat(rotation) == result); +} +fn void test_conjugate() +{ + Quaternion value = { 0.5, 0.2, 1.0, 0.5 }; + Quaternion inverse = value.conjugate(); + assert(inverse.v == { -0.500000, -0.200000, -1.000000, 0.500000 }); +} + +fn void test_identity() +{ + Quaternion rotation = QUATERNION_IDENTITY; + Quaternionf rotation_f = QUATERNIONF_IDENTITY; + assert(rotation.v == { 0, 0, 0, 1 }); + assert(rotation.v == { 0, 0, 0, 1 }); +} + +fn void test_rotation_identity() +{ + Quaternion rotation = QUATERNION_IDENTITY; + Matrix4 identity_matrix = MATRIX4_IDENTITY; + + Quaternionf rotation_f = QUATERNIONF_IDENTITY; + Matrix4f identity_matrix_f = MATRIX4F_IDENTITY; + + assert((double[<16>])rotation.to_matrix().m == (double[<16>])identity_matrix.m); + assert((float[<16>])rotation_f.to_matrixf().m == (float[<16>])identity_matrix_f.m); +} + +fn void test_to_matrix() +{ + Matrix4 result = { + 0.428571, -0.285714, 0.857143, 0.000000, + 0.857143, 0.428571, -0.285714, 0.000000, + -0.285714, 0.857143, 0.428571, 0.000000, + 0.000000, 0.000000, 0.000000, 1.000000 }; - { - Quaternion rotation = QUATERNION_IDENTITY; - Matrix4 identity_matrix = MATRIX4_IDENTITY; + Matrix4 rotation = (Quaternion) { 0.5, 0.5, 0.5, 1 }.to_matrix(); + Matrix4f rotation_f = (Quaternionf) { 0.5, 0.5, 0.5, 1 }.to_matrixf(); - Quaternionf rotation_f = QUATERNIONF_IDENTITY; - Matrix4f identity_matrix_f = MATRIX4F_IDENTITY; + assert(math::round_to_decimals((double[<16>])result.m, 2) == math::round_to_decimals((double[<16>])rotation.m, 2)); + assert(math::round_to_decimals((float[<16>])result.m, 2) == math::round_to_decimals((float[<16>])rotation_f.m, 2)); +} +fn void test_normalize() +{ + Quaternionf value = quaternion::from_axis_angle({ 1, 0, 0 }, math::PI).normalize(); + assert(math::round_to_decimals(value.v, 2) == { 1, 0, 0, 0 }); +} - assert((double[<16>])rotation.to_matrix().m == (double[<16>])identity_matrix.m); - assert((float[<16>])rotation_f.to_matrixf().m == (float[<16>])identity_matrix_f.m); - }; +fn void test_normalize2() +{ + Quaternionf value = quaternion::from_axis_angle({ 0, 1, 0 }, math::PI / 2).normalize(); + assert(math::round_to_decimals(value.v, 4) == { 0, 0.7071, 0, 0.7071 }); +} - { - Matrix4 result = { - 0.428571, -0.285714, 0.857143, 0.000000, - 0.857143, 0.428571, -0.285714, 0.000000, - -0.285714, 0.857143, 0.428571, 0.000000, - 0.000000, 0.000000, 0.000000, 1.000000 - }; - - Matrix4 rotation = (Quaternion) {0.5, 0.5, 0.5, 1}.to_matrix(); - Matrix4f rotation_f = (Quaternionf) {0.5, 0.5, 0.5, 1}.to_matrixf(); - - assert(math::round_to_decimals((double[<16>])result.m, 2) == math::round_to_decimals((double[<16>])rotation.m, 2)); - assert(math::round_to_decimals((float[<16>])result.m, 2) == math::round_to_decimals((float[<16>])rotation_f.m, 2)); - }; +fn void test_mult() +{ + Quaternionf rotation = quaternion::from_axis_angle({ 0.0f, 1.0f, 0.0f }, (float)math::deg_to_rad(90.0f)); + float[<3>] rotate_point = rotation * (float[<3>]){ 1, 0, 0 }; + assert(math::round_to_decimals(rotate_point, 2) == (float[<3>]){ 0, 0, -1.0 }); } \ No newline at end of file