Force library

src/drone.zig

@@ -8,15 +8,16 @@ const Drone = struct {
     attitude: vec3.QuatF64,
 };
 
-const Propeller = struct {
+const Propellers = struct {
-    center_of_gravity_to_propeller_body: vec3.Vec3F64,
+    cg_to_prop: vec3.Vec3F64,
+    angular_velocity: f64,
     thrust_coefficient: f64,
     torque_drag_coefficient: f64,
 };
 
-const Simulation = struct {
+const QuadCopterSim = struct {
     drone: Drone,
-    propellers: std.ArrayList(Propeller),
+    propellers: [4]Propellers,
 };
 
 pub fn body_position_to_inertial(

src/forces.zig

@@ -6,6 +6,31 @@ 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 {
+    const body_thrust = vec3.Vec3F64.init(
+        0,
+        0,
+        thrust_coefficient * angular_velocity * angular_velocity,
+    );
+
+    return vec3.quatApply(attitude, body_thrust);
+}
+
+pub fn drag(
+    drag_coefficient: f64,
+    velocity: vec3.Vec3F64,
+) vec3.Vec3F64 {
+    return .init(
+        velocity.x() * drag_coefficient,
+        velocity.y() * drag_coefficient,
+        velocity.z() * drag_coefficient,
+    );
+}
+
 test "gravity is correct" {
     const mass = 10;
     const fg = gravity(mass);
@@ -13,3 +38,24 @@ test "gravity is correct" {
     try std.testing.expect(std.math.approxEqAbs(f64, fg.y(), 0, 1e-12));
     try std.testing.expect(std.math.approxEqAbs(f64, fg.z(), constants.g * mass, 1e-12));
 }
+
+test "thrust is correct" {
+    const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0);
+    const thrust_coefficient = 1;
+    const angular_velocity = 100;
+
+    const thrust = propThrust(attitude, thrust_coefficient, angular_velocity);
+
+    try std.testing.expectApproxEqAbs(0, thrust.x(), 1e-11);
+    try std.testing.expectApproxEqAbs(100 * 100, thrust.y(), 1e-11);
+    try std.testing.expectApproxEqAbs(0, thrust.z(), 1e-11);
+}
+
+test "drag is correct" {
+    const velocity = vec3.Vec3F64.init(0, 1, 1);
+    const f_drag = drag(0.5, velocity);
+
+    try std.testing.expectApproxEqAbs(0, f_drag.x(), 1e-11);
+    try std.testing.expectApproxEqAbs(0.5, f_drag.y(), 1e-11);
+    try std.testing.expectApproxEqAbs(0.5, f_drag.z(), 1e-11);
+}

src/vec3.zig

@@ -346,4 +346,11 @@ test "quat apply to vec " {
     try std.testing.expect(std.math.approxEqAbs(f64, rotated.x(), -2, 1e-12));
     try std.testing.expect(std.math.approxEqAbs(f64, rotated.y(), 1, 1e-12));
     try std.testing.expect(std.math.approxEqAbs(f64, rotated.z(), 1, 1e-12));
+
+    const forward_body = Vec3F64.init(1, 0, 0);
+    const forward_inertial = quatApply(quat, forward_body);
+
+    try std.testing.expectApproxEqAbs(@as(f64, 0), forward_inertial.x(), 1e-12);
+    try std.testing.expectApproxEqAbs(@as(f64, 1), forward_inertial.y(), 1e-12);
+    try std.testing.expectApproxEqAbs(@as(f64, 0), forward_inertial.z(), 1e-12);
 }