From 8ab8af992a92b0ef8f931a2e802bc55c36d91a5a Mon Sep 17 00:00:00 2001 From: Alex Selimov Date: Wed, 12 Aug 2026 20:34:58 -0400 Subject: [PATCH] Add single timestep test to verify math --- src/integration.zig | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/integration.zig b/src/integration.zig index 7e5cf65..1e7578c 100644 --- a/src/integration.zig +++ b/src/integration.zig @@ -36,7 +36,7 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro ); const qdot = vec3.quatMul(half_omege_quat, s.q); - s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)); + s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized(); return; } @@ -76,4 +76,13 @@ test "Validate timesteps" { try std.testing.expectApproxEqAbs(0, state.x_cg.x(), 1e-12); try std.testing.expectApproxEqAbs(0, state.x_cg.y(), 1e-12); try std.testing.expectApproxEqAbs(-0.24975483375, state.x_cg.z(), 1e-11); + + try std.testing.expectApproxEqAbs(0, state.omega.x(), 1e-12); + try std.testing.expectApproxEqAbs(50, state.omega.y(), 1e-12); + try std.testing.expectApproxEqAbs(2.5, state.omega.z(), 1e-11); + + try std.testing.expectApproxEqAbs(0, state.q.x(), 1e-12); + try std.testing.expectApproxEqAbs(0.12403234937465502, state.q.y(), 1e-12); + try std.testing.expectApproxEqAbs(0.006201617468732751, state.q.z(), 1e-11); + try std.testing.expectApproxEqAbs(0.9922587949972401, state.q.w(), 1e-11); }