Force library

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