shishantbiswas/c3c
1
1
Fork 0
mirror of https://github.com/c3lang/c3c.git synced 2026-02-27 12:01:16 +00:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion math improvements (#2524)

Browse Source 
* Add radians to deg function

* Quaternion math fixes

* Formatting, use splat/swizzling, divide into multiple tests.

---------

Co-authored-by: tonis2 <tanton@paysure.solutions>
Co-authored-by: Christoffer Lerno <christoffer@aegik.com>
This commit is contained in:
Tonis
2025-10-20 12:04:28 +03:00
committed by GitHub
parent b924ede71a
commit 03ad72afbb
4 changed files with 116 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
lib/std/math/math.c3
View File

@@ -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`
*>

37
lib/std/math/quaternion.c3
View File

@@ -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 = {};
@@ -78,12 +83,34 @@ 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();

19
lib/std/math/vector.c3
View File

@@ -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();

53
test/unit/stdlib/math/quaternion.c3
View File

@@ -1,15 +1,40 @@
module math_quaternion @test;
module math_tests::quaternion @test;
import std::math;
fn void test()
fn void test_rad_to_deg()
{
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;
@@ -19,8 +44,9 @@ fn void test()
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,
@@ -34,5 +60,22 @@ fn void test()
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 });
}
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 });
}
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 });
}