From 923574ec48b472069f7a179642a828bdae1c4a95 Mon Sep 17 00:00:00 2001
From: Alex Selimov <alex@alexselimov.com>
Date: Sat, 8 Aug 2026 21:41:17 -0400
Subject: [PATCH] Add moment calculation for propeller

---
 src/drone.zig  | 10 +++++-----
 src/forces.zig | 39 ++++++++++++++++++++++++++++-----------
 2 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/src/drone.zig b/src/drone.zig
index 7786c16..eef40c0 100644
--- a/src/drone.zig
+++ b/src/drone.zig
@@ -1,23 +1,23 @@
 const vec3 = @import("./vec3.zig");
 const std = @import("std");
 
-const Drone = struct {
+pub const Drone = struct {
     mass_kg: f64,
     center_of_gravity_inertial: vec3.Vec3F64,
     moment_of_inertia: vec3.Mat3F64,
     attitude: vec3.QuatF64,
 };
 
-const Propellers = struct {
+pub const Propeller = struct {
     cg_to_prop: vec3.Vec3F64,
     angular_velocity: f64,
     thrust_coefficient: f64,
-    torque_drag_coefficient: f64,
+    moment_coefficient: f64,
 };
 
-const QuadCopterSim = struct {
+pub const QuadCopterSim = struct {
     drone: Drone,
-    propellers: [4]Propellers,
+    propellers: [4]Propeller,
 };
 
 pub fn body_position_to_inertial(
diff --git a/src/forces.zig b/src/forces.zig
index a1797f4..f9bf191 100644
--- a/src/forces.zig
+++ b/src/forces.zig
@@ -1,23 +1,32 @@
 const constants = @import("./constants.zig");
 const vec3 = @import("./vec3.zig");
+const drone = @import("./drone.zig");
 const std = @import("std");
 
 pub fn gravity(mass: f64) vec3.Vec3F64 {
     return .init(0, 0, mass * constants.g);
 }
 
-pub fn propThrust(
-    attitude: vec3.QuatF64,
-    thrust_coefficient: f64,
-    angular_velocity: f64,
-) vec3.Vec3F64 {
+pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3F64, vec3.Vec3F64 } {
     const body_thrust = vec3.Vec3F64.init(
         0,
         0,
-        thrust_coefficient * angular_velocity * angular_velocity,
+        p.thrust_coefficient * p.angular_velocity * p.angular_velocity,
     );
 
-    return vec3.quatApply(attitude, body_thrust);
+    const force = vec3.quatApply(attitude, body_thrust);
+
+    // pitching/rolling moment induced by thrust force
+    const force_moment = vec3.vec3Cross(p.cg_to_prop, body_thrust);
+
+    // Yaw moment induced by roation of propeller
+    const drag_moment = vec3.Vec3F64.init(
+        0,
+        0,
+        p.moment_coefficient * p.angular_velocity * p.angular_velocity,
+    );
+
+    return .{ force, vec3.vec3Add(force_moment, drag_moment) };
 }
 
 pub fn drag(
@@ -41,14 +50,22 @@ test "gravity is correct" {
 
 test "thrust is correct" {
     const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0);
-    const thrust_coefficient = 1;
-    const angular_velocity = 100;
+    const prop: drone.Propeller = .{
+        .cg_to_prop = vec3.Vec3F64.init(1, 1, 0),
+        .angular_velocity = 100,
+        .thrust_coefficient = 1,
+        .moment_coefficient = 2,
+    };
 
-    const thrust = propThrust(attitude, thrust_coefficient, angular_velocity);
+    const expected_thrust_mag = 100 * 100;
+    const thrust, const momentum = propThrust(attitude, prop);
 
     try std.testing.expectApproxEqAbs(0, thrust.x(), 1e-11);
-    try std.testing.expectApproxEqAbs(100 * 100, thrust.y(), 1e-11);
+    try std.testing.expectApproxEqAbs(expected_thrust_mag, thrust.y(), 1e-11);
     try std.testing.expectApproxEqAbs(0, thrust.z(), 1e-11);
+    try std.testing.expectApproxEqAbs(expected_thrust_mag, momentum.x(), 1e-11);
+    try std.testing.expectApproxEqAbs(-expected_thrust_mag, momentum.y(), 1e-11);
+    try std.testing.expectApproxEqAbs(100 * 100 * 2, momentum.z(), 1e-11);
 }
 
 test "drag is correct" {