From d9e4e06c788c1903689f850c2a76bc338e949924 Mon Sep 17 00:00:00 2001 From: Alex Selimov Date: Thu, 6 Aug 2026 09:37:03 -0400 Subject: [PATCH] Refactor base float type to f64 for accuracy and add gravity --- src/constants.zig | 1 + src/drone.zig | 57 +++++----- src/forces.zig | 15 +++ src/main.zig | 1 + src/vec3.zig | 267 +++++++++++++++++++++++----------------------- 5 files changed, 183 insertions(+), 158 deletions(-) create mode 100644 src/constants.zig create mode 100644 src/forces.zig diff --git a/src/constants.zig b/src/constants.zig new file mode 100644 index 0000000..e574c41 --- /dev/null +++ b/src/constants.zig @@ -0,0 +1 @@ +pub const g: f64 = 9.80665; diff --git a/src/drone.zig b/src/drone.zig index 806b9ee..93aa413 100644 --- a/src/drone.zig +++ b/src/drone.zig @@ -2,16 +2,16 @@ const vec3 = @import("./vec3.zig"); const std = @import("std"); const Drone = struct { - mass: f32, - cg_inertial: vec3.Vec3F32, - moment_of_inertia: vec3.Mat3F32, - attitude: vec3.QuatF32, + mass_kg: f64, + center_of_gravity_inertial: vec3.Vec3F64, + moment_of_inertia: vec3.Mat3F64, + attitude: vec3.QuatF64, }; const Propeller = struct { - cg_to_prop_body: vec3.Vec3F32, - thrust_coefficient: f32, - torque_drag_coefficient: f32, + center_of_gravity_to_propeller_body: vec3.Vec3F64, + thrust_coefficient: f64, + torque_drag_coefficient: f64, }; const Simulation = struct { @@ -20,28 +20,35 @@ const Simulation = struct { }; pub fn body_position_to_inertial( - cg: vec3.Vec3F32, - attitude: vec3.QuatF32, - body_vec: vec3.Vec3F32, -) vec3.Vec3F32 { - const intertial_vec = vec3.quatApply(attitude, body_vec); - return vec3.vec3Add(cg, intertial_vec); + center_of_gravity: vec3.Vec3F64, + attitude: vec3.QuatF64, + body_vector: vec3.Vec3F64, +) vec3.Vec3F64 { + const inertial_offset = vec3.quatApply(attitude.conjugate(), body_vector); + return vec3.vec3Add(center_of_gravity, inertial_offset); } test "Body position to inertial" { - const cg: vec3.Vec3F32 = .init(1, 1, 1); - const body_vec: vec3.Vec3F32 = .init(1, 0, 0); - const attitude = vec3.yawPitchRollToQuat(std.math.pi / 2.0, 0, 0); + const center_of_gravity: vec3.Vec3F64 = .init(1, 1, 1); + const body_vector: vec3.Vec3F64 = .init(1, 0, 0); - const inertial_vec = body_position_to_inertial(cg, attitude, body_vec); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.x(), 1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.y(), 2, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.z(), 1, 1e-7)); + var attitude = vec3.yawPitchRollToQuat(0, 0, 0); + var inertial_position = body_position_to_inertial(center_of_gravity, attitude, body_vector); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.x(), 2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.y(), 1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.z(), 1, 1e-12)); - const attitude_2 = vec3.yawPitchRollToQuat(std.math.pi, 0, 0); - const inertial_vec_2 = body_position_to_inertial(cg, attitude_2, body_vec); + attitude = vec3.yawPitchRollToQuat(std.math.pi / 2.0, 0, 0); + inertial_position = body_position_to_inertial(center_of_gravity, attitude, body_vector); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.x(), 0, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.y(), 1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.z(), 1, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.x(), 1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.y(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.z(), 1, 1e-12)); + + attitude = vec3.yawPitchRollToQuat(std.math.pi, 0, 0); + inertial_position = body_position_to_inertial(center_of_gravity, attitude, body_vector); + + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.x(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.y(), 1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, inertial_position.z(), 1, 1e-12)); } diff --git a/src/forces.zig b/src/forces.zig new file mode 100644 index 0000000..4c00794 --- /dev/null +++ b/src/forces.zig @@ -0,0 +1,15 @@ +const constants = @import("./constants.zig"); +const vec3 = @import("./vec3.zig"); +const std = @import("std"); + +pub fn gravity(mass: f64) vec3.Vec3F64 { + return .init(0, 0, mass * constants.g); +} + +test "gravity is correct" { + const mass = 10; + const fg = gravity(mass); + try std.testing.expect(std.math.approxEqAbs(f64, fg.x(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, fg.y(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, fg.z(), constants.g * mass, 1e-12)); +} diff --git a/src/main.zig b/src/main.zig index c8cc000..e6e2016 100644 --- a/src/main.zig +++ b/src/main.zig @@ -3,4 +3,5 @@ const std = @import("std"); test "main" { _ = @import("./drone.zig"); _ = @import("./vec3.zig"); + _ = @import("./forces.zig"); } diff --git a/src/vec3.zig b/src/vec3.zig index 2742bfb..6703b24 100644 --- a/src/vec3.zig +++ b/src/vec3.zig @@ -1,51 +1,51 @@ const std = @import("std"); -pub const Vec3F32 = struct { - data: @Vector(4, f32), +pub const Vec3F64 = struct { + data: @Vector(4, f64), - pub fn init(_x: f32, _y: f32, _z: f32) Vec3F32 { - return .{ .data = .{ _x, _y, _z, 0 } }; + pub fn init(x_value: f64, y_value: f64, z_value: f64) Vec3F64 { + return .{ .data = .{ x_value, y_value, z_value, 0 } }; } - pub fn x(self: Vec3F32) f32 { + pub fn x(self: Vec3F64) f64 { return self.data[0]; } - pub fn y(self: Vec3F32) f32 { + pub fn y(self: Vec3F64) f64 { return self.data[1]; } - pub fn z(self: Vec3F32) f32 { + pub fn z(self: Vec3F64) f64 { return self.data[2]; } }; -pub fn vec3Add(a: Vec3F32, b: Vec3F32) Vec3F32 { +pub fn vec3Add(a: Vec3F64, b: Vec3F64) Vec3F64 { return .{ .data = a.data + b.data }; } -pub fn vec3Sub(a: Vec3F32, b: Vec3F32) Vec3F32 { +pub fn vec3Sub(a: Vec3F64, b: Vec3F64) Vec3F64 { return .{ .data = a.data - b.data }; } -pub fn vec3Dot(a: Vec3F32, b: Vec3F32) f32 { +pub fn vec3Dot(a: Vec3F64, b: Vec3F64) f64 { return @reduce(.Add, a.data * b.data); } -pub fn vec3Cross(a: Vec3F32, b: Vec3F32) Vec3F32 { - const x = a.y() * b.z() - a.z() * b.y(); - const y = a.z() * b.x() - a.x() * b.z(); - const z = a.x() * b.y() - a.y() * b.x(); - return .init(x, y, z); +pub fn vec3Cross(a: Vec3F64, b: Vec3F64) Vec3F64 { + const x_component = a.y() * b.z() - a.z() * b.y(); + const y_component = a.z() * b.x() - a.x() * b.z(); + const z_component = a.x() * b.y() - a.y() * b.x(); + return .init(x_component, y_component, z_component); } -pub const Mat3F32 = struct { - row1: Vec3F32, - row2: Vec3F32, - row3: Vec3F32, +pub const Mat3F64 = struct { + row1: Vec3F64, + row2: Vec3F64, + row3: Vec3F64, }; -pub fn mat3Add(a: Mat3F32, b: Mat3F32) Mat3F32 { +pub fn mat3Add(a: Mat3F64, b: Mat3F64) Mat3F64 { return .{ .row1 = vec3Add(a.row1, b.row1), .row2 = vec3Add(a.row2, b.row2), @@ -53,7 +53,7 @@ pub fn mat3Add(a: Mat3F32, b: Mat3F32) Mat3F32 { }; } -pub fn mat3Sub(a: Mat3F32, b: Mat3F32) Mat3F32 { +pub fn mat3Sub(a: Mat3F64, b: Mat3F64) Mat3F64 { return .{ .row1 = vec3Sub(a.row1, b.row1), .row2 = vec3Sub(a.row2, b.row2), @@ -61,20 +61,20 @@ pub fn mat3Sub(a: Mat3F32, b: Mat3F32) Mat3F32 { }; } -pub fn mat3Mul(a: Mat3F32, b: Mat3F32) Mat3F32 { - const c_row1: Vec3F32 = .init( +pub fn mat3Mul(a: Mat3F64, b: Mat3F64) Mat3F64 { + const c_row1: Vec3F64 = .init( a.row1.x() * b.row1.x() + a.row1.y() * b.row2.x() + a.row1.z() * b.row3.x(), a.row1.x() * b.row1.y() + a.row1.y() * b.row2.y() + a.row1.z() * b.row3.y(), a.row1.x() * b.row1.z() + a.row1.y() * b.row2.z() + a.row1.z() * b.row3.z(), ); - const c_row2: Vec3F32 = .init( + const c_row2: Vec3F64 = .init( a.row2.x() * b.row1.x() + a.row2.y() * b.row2.x() + a.row2.z() * b.row3.x(), a.row2.x() * b.row1.y() + a.row2.y() * b.row2.y() + a.row2.z() * b.row3.y(), a.row2.x() * b.row1.z() + a.row2.y() * b.row2.z() + a.row2.z() * b.row3.z(), ); - const c_row3: Vec3F32 = .init( + const c_row3: Vec3F64 = .init( a.row3.x() * b.row1.x() + a.row3.y() * b.row2.x() + a.row3.z() * b.row3.x(), a.row3.x() * b.row1.y() + a.row3.y() * b.row2.y() + a.row3.z() * b.row3.y(), a.row3.x() * b.row1.z() + a.row3.y() * b.row2.z() + a.row3.z() * b.row3.z(), @@ -83,7 +83,7 @@ pub fn mat3Mul(a: Mat3F32, b: Mat3F32) Mat3F32 { return .{ .row1 = c_row1, .row2 = c_row2, .row3 = c_row3 }; } -pub fn vec3MulMat3(a: Mat3F32, b: Vec3F32) Vec3F32 { +pub fn vec3MulMat3(a: Mat3F64, b: Vec3F64) Vec3F64 { return .init( vec3Dot(a.row1, b), vec3Dot(a.row2, b), @@ -94,56 +94,57 @@ pub fn vec3MulMat3(a: Mat3F32, b: Vec3F32) Vec3F32 { // Uses the Quaternion definition of q = w + xi + yj + zk // and with i^2 = j^2 = k^2 = ijk = -1 // I grabbed a lot of this math from https://imadrahmoune.com/rotations-with-quaternions/ -pub const QuatF32 = struct { - data: @Vector(4, f32), - pub fn init(_x: f32, _y: f32, _z: f32, _w: f32) QuatF32 { - return .{ .data = .{ _x, _y, _z, _w } }; +pub const QuatF64 = struct { + data: @Vector(4, f64), + pub fn init(x_value: f64, y_value: f64, z_value: f64, w_value: f64) QuatF64 { + return .{ .data = .{ x_value, y_value, z_value, w_value } }; } - pub fn x(self: QuatF32) f32 { + pub fn x(self: QuatF64) f64 { return self.data[0]; } - pub fn y(self: QuatF32) f32 { + pub fn y(self: QuatF64) f64 { return self.data[1]; } - pub fn z(self: QuatF32) f32 { + pub fn z(self: QuatF64) f64 { return self.data[2]; } - pub fn w(self: QuatF32) f32 { + pub fn w(self: QuatF64) f64 { return self.data[3]; } - pub fn mag(q: QuatF32) f32 { + pub fn mag(q: QuatF64) f64 { return std.math.sqrt(@reduce(.Add, q.data * q.data)); } - pub fn normalized(q: QuatF32) QuatF32 { + pub fn normalized(q: QuatF64) QuatF64 { const magnitude = q.mag(); - return .{ .data = q.data / @as(@Vector(4, f32), @splat(magnitude)) }; + return .{ .data = q.data / @as(@Vector(4, f64), @splat(magnitude)) }; } - pub fn conjugate(q: QuatF32) QuatF32 { - return .{ .data = q.data * @as(@Vector(4, f32), .{ -1, -1, -1, 1 }) }; + pub fn conjugate(q: QuatF64) QuatF64 { + return .{ .data = q.data * @as(@Vector(4, f64), .{ -1, -1, -1, 1 }) }; } }; -pub fn yawPitchRollToQuat(yaw: f32, pitch: f32, roll: f32) QuatF32 { - const cx = @cos(roll / 2); - const sx = @sin(roll / 2); - const cy = @cos(pitch / 2); - const sy = @sin(pitch / 2); - const cz = @cos(yaw / 2); - const sz = @sin(yaw / 2); +pub fn yawPitchRollToQuat(yaw_radians: f64, pitch_radians: f64, roll_radians: f64) QuatF64 { + const cos_roll = @cos(roll_radians / 2); + const sin_roll = @sin(roll_radians / 2); + const cos_pitch = @cos(pitch_radians / 2); + const sin_pitch = @sin(pitch_radians / 2); + const cos_yaw = @cos(yaw_radians / 2); + const sin_yaw = @sin(yaw_radians / 2); - return .init( - sx * cy * cz - cx * sy * sz, - cx * sy * cz + sx * cy * sz, - cx * cy * sz - sx * sy * cz, - cx * cy * cz + sx * sy * sz, + const quat: QuatF64 = .init( + sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw, + cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw, + cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw, + cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw, ); + return quat.normalized(); } -pub fn quatMul(a: QuatF32, b: QuatF32) QuatF32 { +pub fn quatMul(a: QuatF64, b: QuatF64) QuatF64 { return .init( a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), a.w() * b.y() - a.x() * b.z() + a.y() * b.w() + a.z() * b.x(), @@ -152,37 +153,37 @@ pub fn quatMul(a: QuatF32, b: QuatF32) QuatF32 { ); } -pub fn quatApply(a: QuatF32, b: Vec3F32) Vec3F32 { - const quat_v = quatMul(quatMul(a, .init(b.x(), b.y(), b.z(), 0)), a.conjugate()); - return .init(quat_v.data[0], quat_v.data[1], quat_v.data[2]); +pub fn quatApply(a: QuatF64, b: Vec3F64) Vec3F64 { + const rotated_quat = quatMul(quatMul(a, .init(b.x(), b.y(), b.z(), 0)), a.conjugate()); + return .init(rotated_quat.data[0], rotated_quat.data[1], rotated_quat.data[2]); } test "vec3Add adds properly" { - const a = Vec3F32.init(1, 2, 3); - const b = Vec3F32.init(3, 1, 0); + const a = Vec3F64.init(1, 2, 3); + const b = Vec3F64.init(3, 1, 0); const c = vec3Add(a, b); - try std.testing.expect(std.math.approxEqAbs(f32, c.x(), 4, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 3, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, c.x(), 4, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.y(), 3, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.z(), 3, 1e-12)); } test "vec3Sub subs properly" { - const a = Vec3F32.init(1, 2, 3); - const b = Vec3F32.init(3, 1, 0); + const a = Vec3F64.init(1, 2, 3); + const b = Vec3F64.init(3, 1, 0); const c = vec3Sub(a, b); - try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, c.x(), -2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.y(), 1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.z(), 3, 1e-12)); } test "vec3Dot dots properly" { - const a = Vec3F32.init(1, 2, 3); - const b = Vec3F32.init(3, 1, 0); - try std.testing.expect(std.math.approxEqAbs(f32, vec3Dot(a, b), 5, 1e-7)); + const a = Vec3F64.init(1, 2, 3); + const b = Vec3F64.init(3, 1, 0); + try std.testing.expect(std.math.approxEqAbs(f64, vec3Dot(a, b), 5, 1e-12)); } test "vec3Cross crosses properly" { - const a = Vec3F32.init(1, 2, 3); - const b = Vec3F32.init(3, 1, 0); + const a = Vec3F64.init(1, 2, 3); + const b = Vec3F64.init(3, 1, 0); const c = vec3Cross(a, b); @@ -192,15 +193,15 @@ test "vec3Cross crosses properly" { } test "Mat3 add works" { - const a: Mat3F32 = .{ - .row1 = Vec3F32.init(1, 2, 3), - .row2 = Vec3F32.init(0, 1, 5), - .row3 = Vec3F32.init(0, 0, 9), + const a: Mat3F64 = .{ + .row1 = Vec3F64.init(1, 2, 3), + .row2 = Vec3F64.init(0, 1, 5), + .row3 = Vec3F64.init(0, 0, 9), }; - const b: Mat3F32 = .{ - .row1 = Vec3F32.init(0, 1, 0), - .row2 = Vec3F32.init(0, 1, 1), - .row3 = Vec3F32.init(0, 0, 1), + const b: Mat3F64 = .{ + .row1 = Vec3F64.init(0, 1, 0), + .row2 = Vec3F64.init(0, 1, 1), + .row3 = Vec3F64.init(0, 0, 1), }; const c = mat3Add(a, b); @@ -217,15 +218,15 @@ test "Mat3 add works" { } test "Mat3 sub works" { - const a: Mat3F32 = .{ - .row1 = Vec3F32.init(1, 2, 3), - .row2 = Vec3F32.init(0, 1, 5), - .row3 = Vec3F32.init(0, 0, 9), + const a: Mat3F64 = .{ + .row1 = Vec3F64.init(1, 2, 3), + .row2 = Vec3F64.init(0, 1, 5), + .row3 = Vec3F64.init(0, 0, 9), }; - const b: Mat3F32 = .{ - .row1 = Vec3F32.init(0, 1, 0), - .row2 = Vec3F32.init(0, 1, 1), - .row3 = Vec3F32.init(0, 0, 1), + const b: Mat3F64 = .{ + .row1 = Vec3F64.init(0, 1, 0), + .row2 = Vec3F64.init(0, 1, 1), + .row3 = Vec3F64.init(0, 0, 1), }; const c = mat3Sub(a, b); @@ -242,15 +243,15 @@ test "Mat3 sub works" { } test "Mat3 mul works" { - const a: Mat3F32 = .{ - .row1 = Vec3F32.init(1, 2, 3), - .row2 = Vec3F32.init(0, 1, 5), - .row3 = Vec3F32.init(0, 0, 9), + const a: Mat3F64 = .{ + .row1 = Vec3F64.init(1, 2, 3), + .row2 = Vec3F64.init(0, 1, 5), + .row3 = Vec3F64.init(0, 0, 9), }; - const b: Mat3F32 = .{ - .row1 = Vec3F32.init(0, 1, 0), - .row2 = Vec3F32.init(0, 1, 1), - .row3 = Vec3F32.init(0, 0, 1), + const b: Mat3F64 = .{ + .row1 = Vec3F64.init(0, 1, 0), + .row2 = Vec3F64.init(0, 1, 1), + .row3 = Vec3F64.init(0, 0, 1), }; const c = mat3Mul(a, b); @@ -267,28 +268,28 @@ test "Mat3 mul works" { } test "vec3 mul mat3 works" { - const b: Mat3F32 = .{ - .row1 = Vec3F32.init(0, 1, 0), - .row2 = Vec3F32.init(0, 1, 1), - .row3 = Vec3F32.init(0, 0, 1), + const b: Mat3F64 = .{ + .row1 = Vec3F64.init(0, 1, 0), + .row2 = Vec3F64.init(0, 1, 1), + .row3 = Vec3F64.init(0, 0, 1), }; - const a = Vec3F32.init(1, 2, 3); + const a = Vec3F64.init(1, 2, 3); const c = vec3MulMat3(b, a); - try std.testing.expect(std.math.approxEqAbs(f32, c.x(), 2, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 5, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, c.x(), 2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.y(), 5, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.z(), 3, 1e-12)); } test "quat magnitude" { - const q: QuatF32 = .init(1.0, 2.0, 3.0, 4.0); - try std.testing.expect(std.math.approxEqAbs(f32, q.mag(), std.math.sqrt(30.0), 1e-7)); + const q: QuatF64 = .init(1.0, 2.0, 3.0, 4.0); + try std.testing.expect(std.math.approxEqAbs(f64, q.mag(), std.math.sqrt(30.0), 1e-12)); } test "quat normalized" { - const q: QuatF32 = .init(1.0, 2.0, 3.0, 4.0); - const expected: QuatF32 = .init( + const q: QuatF64 = .init(1.0, 2.0, 3.0, 4.0); + const expected: QuatF64 = .init( 1.0 / std.math.sqrt(30.0), 2.0 / std.math.sqrt(30.0), 3.0 / std.math.sqrt(30.0), @@ -297,52 +298,52 @@ test "quat normalized" { const normalized = q.normalized(); - try std.testing.expect(std.math.approxEqAbs(f32, normalized.x(), expected.x(), 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, normalized.y(), expected.y(), 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, normalized.z(), expected.z(), 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, normalized.w(), expected.w(), 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, normalized.x(), expected.x(), 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, normalized.y(), expected.y(), 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, normalized.z(), expected.z(), 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, normalized.w(), expected.w(), 1e-12)); } test "quat mul" { - const a: QuatF32 = .init(2, 3, 4, 1); - const b: QuatF32 = .init(0, 1, 0, 1); + const a: QuatF64 = .init(2, 3, 4, 1); + const b: QuatF64 = .init(0, 1, 0, 1); const c = quatMul(a, b); - try std.testing.expect(std.math.approxEqAbs(f32, c.w(), -2, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 4, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 6, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, c.w(), -2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.x(), -2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.y(), 4, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, c.z(), 6, 1e-12)); } test "quat euler" { const quat = yawPitchRollToQuat(0.5, 0.5, 1); - try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0.3963648, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0.3252922, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), 0.0954433, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), 0.8532119, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.x(), 0.3963648, 1e-6)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.y(), 0.3252922, 1e-6)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.z(), 0.0954433, 1e-6)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.w(), 0.8532119, 1e-6)); } test "quat conjugate" { - const quat: QuatF32 = .init(1, 1, 1, 1); + const quat: QuatF64 = .init(1, 1, 1, 1); const conj = quat.conjugate(); - try std.testing.expect(std.math.approxEqAbs(f32, conj.x(), -1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, conj.y(), -1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, conj.z(), -1, 1e-7)); - try std.testing.expect(std.math.approxEqAbs(f32, conj.w(), 1, 1e-7)); + try std.testing.expect(std.math.approxEqAbs(f64, conj.x(), -1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, conj.y(), -1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, conj.z(), -1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, conj.w(), 1, 1e-12)); } test "quat apply to vec " { - const quat: QuatF32 = yawPitchRollToQuat(std.math.pi / 2.0, 0, 0); + const quat: QuatF64 = yawPitchRollToQuat(std.math.pi / 2.0, 0, 0); - try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0, 1e-6)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0, 1e-6)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), @sin(std.math.pi / 4.0), 1e-6)); - try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), @cos(std.math.pi / 4.0), 1e-6)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.x(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.y(), 0, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.z(), @sin(std.math.pi / 4.0), 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, quat.w(), @cos(std.math.pi / 4.0), 1e-12)); const rotated = quatApply(quat, .init(1, 2, 1)); - try std.testing.expect(std.math.approxEqAbs(f32, rotated.x(), -2, 1e-6)); - try std.testing.expect(std.math.approxEqAbs(f32, rotated.y(), 1, 1e-6)); - try std.testing.expect(std.math.approxEqAbs(f32, rotated.z(), 1, 1e-6)); + try std.testing.expect(std.math.approxEqAbs(f64, rotated.x(), -2, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, rotated.y(), 1, 1e-12)); + try std.testing.expect(std.math.approxEqAbs(f64, rotated.z(), 1, 1e-12)); }