zigsdof/src/forces.zig

const constants = @import("./constants.zig");
const vec3 = @import("./vec3.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 {
    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);
    try std.testing.expect(std.math.approxEqAbs(f64, fg.x(), 0, 1e-12));
    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);
}