Untested timestep function

src/drone.zig

@@ -3,9 +3,15 @@ const std = @import("std");
 
 pub const Drone = struct {
     mass_kg: f64,
-    center_of_gravity_inertial: vec3.Vec3F64,
     moment_of_inertia: vec3.Mat3F64,
-    attitude: vec3.QuatF64,
+    drag_coeff: f64,
+};
+
+pub const State = struct {
+    x_cg: vec3.Vec3F64,
+    v: vec3.Vec3F64,
+    q: vec3.QuatF64,
+    omega: vec3.Vec3F64,
 };
 
 pub const Propeller = struct {

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;
+}

src/vec3.zig

@@ -32,7 +32,7 @@ pub fn vec3Dot(a: Vec3F64, b: Vec3F64) f64 {
     return @reduce(.Add, a.data * b.data);
 }
 
-pub fn vec3MulScalar(a: Vec3F64, b: f64) Vec3F64 {
+pub fn vec3Scale(a: Vec3F64, b: f64) Vec3F64 {
     return .init(a.x() * b, a.y() * b, a.z() * b);
 }
 
@@ -95,7 +95,7 @@ pub fn mat3Det(a: Mat3F64) f64 {
 
 pub fn mat3Inv(a: Mat3F64) Mat3F64 {
     const inv_det = 1.0 / mat3Det(a);
-    const row1 = vec3MulScalar(
+    const row1 = vec3Scale(
         .init(
             a.row2.y() * a.row3.z() - a.row2.z() * a.row3.y(),
             a.row1.z() * a.row3.y() - a.row1.y() * a.row3.z(),
@@ -103,7 +103,7 @@ pub fn mat3Inv(a: Mat3F64) Mat3F64 {
         ),
         inv_det,
     );
-    const row2 = vec3MulScalar(
+    const row2 = vec3Scale(
         .init(
             a.row2.z() * a.row3.x() + a.row2.x() * a.row3.z(),
             a.row1.x() * a.row3.z() - a.row1.z() * a.row3.x(),
@@ -111,7 +111,7 @@ pub fn mat3Inv(a: Mat3F64) Mat3F64 {
         ),
         inv_det,
     );
-    const row3 = vec3MulScalar(
+    const row3 = vec3Scale(
         .init(
             a.row2.x() * a.row3.y() + a.row2.y() * a.row3.x(),
             a.row1.y() * a.row3.x() - a.row1.x() * a.row3.y(),
@@ -201,6 +201,14 @@ pub fn quatApply(a: QuatF64, b: Vec3F64) Vec3F64 {
     return .init(rotated_quat.data[0], rotated_quat.data[1], rotated_quat.data[2]);
 }
 
+pub fn quatScale(a: QuatF64, b: f64) QuatF64 {
+    return .init(a.x() * b, a.y() * b, a.z() * b, a.w() * b);
+}
+
+pub fn quatAdd(a: QuatF64, b: QuatF64) QuatF64 {
+    return .init(a.x() + b.x(), a.y() + b.y(), a.z() + b.z(), a.w() + b.w());
+}
+
 test "vec3Add adds properly" {
     const a = Vec3F64.init(1, 2, 3);
     const b = Vec3F64.init(3, 1, 0);
@@ -327,8 +335,6 @@ test "mat inverse works" {
     };
 
     const ainv = mat3Inv(a);
-    std.debug.print("{}", .{ainv});
-    std.debug.print("{}", .{ainv.row2.z()});
 
     try std.testing.expectApproxEqAbs(1, ainv.row1.x(), 1e-12);
     try std.testing.expectApproxEqAbs(-2, ainv.row1.y(), 1e-12);
@@ -378,15 +384,25 @@ test "quat normalized" {
 }
 
 test "quat mul" {
-    const a: QuatF64 = .init(2, 3, 4, 1);
-    const b: QuatF64 = .init(0, 1, 0, 1);
+    var a: QuatF64 = .init(2, 3, 4, 1);
+    var b: QuatF64 = .init(0, 1, 0, 1);
 
-    const c = quatMul(a, b);
+    var c = quatMul(a, b);
 
     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));
+
+    a = .init(0.27059805, -0.65328148, 0.27059805, 0.65328148);
+    b = .init(0.70710678, 0, 0, 0.70710678);
+    c = quatMul(a, b);
+
+    const expected: QuatF64 = .init(0.65328148, -0.27059805, 0.65328148, 0.27059805);
+    try std.testing.expectApproxEqAbs(expected.x(), c.x(), 1e-7);
+    try std.testing.expectApproxEqAbs(expected.y(), c.y(), 1e-7);
+    try std.testing.expectApproxEqAbs(expected.z(), c.z(), 1e-7);
+    try std.testing.expectApproxEqAbs(expected.w(), c.w(), 1e-7);
 }
 
 test "quat euler" {