aselimov/zigsdof
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add single timestep test to verify math

Browse source
This commit is contained in:
Alex Selimov 2026-08-12 20:34:58 -04:00
parent 78f5db1a9e
commit 8ab8af992a
1 changed files with 10 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/integration.zig
View file

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