Untested timestep function

src/integration.zig

@@ -0,0 +1,39 @@
+const drone = @import("./drone.zig");
+const vec3 = @import("./vec3.zig");
+const forces = @import("./forces.zig");
+
+pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *drone.State) void {
+    var force = vec3.Vec3F64.init(0, 0, 0);
+    var moment = vec3.Vec3F64.init(0, 0, 0);
+
+    for (props) |prop| {
+        const prop_force, const prop_moment = forces.propThrust(s.attitude, prop);
+        force = vec3.vec3Add(force, prop_force);
+        moment = vec3.vec3Add(moment, prop_moment);
+    }
+
+    force = vec3.vec3Add(force, forces.drag(drone.drag_coeff, s.v));
+    force = vec3.vec3Add(force, forces.gravity(d.mass_kg));
+
+    const accel = vec3.vec3Scale(force, 1.0 / d.mass_kg);
+    s.v = vec3.vec3Add(s.v, vec3.vec3Scale(accel, dt));
+    s.x_cg = vec3.vec3Add(s.x_cg, vec3.vec3Scale(accel, dt));
+
+    const I_omega = vec3.vec3MulMat3(d.moment_of_inertia, s.omega);
+    const omega_dot = vec3.vec3MulMat3(
+        vec3.mat3Inv(d.moment_of_inertia),
+        vec3.vec3Sub(moment, vec3.vec3Cross(s.omega, I_omega)),
+    );
+
+    s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt));
+    const half_omege_quat: vec3.QuatF64 = .init(
+        s.omega.x() * 0.5,
+        s.omega.y() * 0.5,
+        s.omega.z() * 0.5,
+        0,
+    );
+
+    const qdot = vec3.quatMul(half_omege_quat, s.q);
+    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
+    return;
+}