Fix matrix test and add parsing from json

src/drone.zig

@@ -16,11 +16,21 @@ pub const State = struct {
 };
 
 pub const Propeller = struct {
-    cg_to_prop: vec3.Vec3F64,
+    x_cg: vec3.Vec3F64,
     angular_velocity: f64,
     thrust_coefficient: f64,
     moment_coefficient: f64,
     direction: constants.PropSpinDirection,
+
+    pub fn empty() Propeller {
+        return .{
+            .x_cg = .init(0, 0, 0),
+            .angular_velocity = 0,
+            .thrust_coefficient = 0,
+            .moment_coefficient = 0,
+            .direction = .cw,
+        };
+    }
 };
 
 pub const QuadCopterSim = struct {

src/forces.zig

@@ -17,7 +17,7 @@ pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3
     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);
+    const force_moment = vec3.vec3Cross(p.x_cg, body_thrust);
 
     // Yaw moment induced by roation of propeller
     const drag_moment = vec3.Vec3F64.init(
@@ -51,7 +51,7 @@ test "gravity is correct" {
 test "thrust is correct" {
     const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0);
     const prop: drone.Propeller = .{
-        .cg_to_prop = vec3.Vec3F64.init(1, 1, 0),
+        .x_cg = vec3.Vec3F64.init(1, 1, 0),
         .angular_velocity = 100,
         .thrust_coefficient = 1,
         .moment_coefficient = 2,

src/integration.zig

@@ -60,7 +60,7 @@ test "Validate timesteps" {
 
     const prop: drone.Propeller = .{
         .angular_velocity = 100,
-        .cg_to_prop = .init(1, 0, 0),
+        .x_cg = .init(1, 0, 0),
         .moment_coefficient = 0.1,
         .thrust_coefficient = 0.5,
         .direction = constants.PropSpinDirection.cw,

src/io.zig

@@ -0,0 +1,149 @@
+const std = @import("std");
+
+const constants = @import("./constants.zig");
+const drone = @import("./drone.zig");
+const vec3 = @import("./vec3.zig");
+
+const IoError = error{InputParsingError};
+
+const JsonProp = struct {
+    x_cg: [3]f64,
+    thrust_coefficient: f64,
+    moment_coefficient: f64,
+    direction: constants.PropSpinDirection,
+};
+const JsonSim = struct {
+    mass_kg: f64,
+    moment_of_inertia: [9]f64,
+    drag_coeff: f64,
+    propellers: [4]JsonProp,
+};
+
+pub fn jsonFileToSimConfig(
+    file_path: []u8,
+    allocator: std.mem.Allocator,
+    io: std.Io,
+) !drone.QuadCopterSim {
+    const cwd = std.Io.Dir.cwd();
+    const file = try cwd.openFile(io, file_path, .{ .mode = .read });
+    defer file.close(io);
+
+    var file_reader = file.reader(io, &.{});
+    const json = try file_reader.interface.allocRemaining(allocator, .unlimited);
+
+    return jsonStringToSimConfig(json, allocator, io);
+}
+
+pub fn jsonStringToSimConfig(
+    json_string: []const u8,
+    allocator: std.mem.Allocator,
+) !drone.QuadCopterSim {
+    const parsed = try std.json.parseFromSlice(JsonSim, allocator, json_string, .{});
+    defer parsed.deinit();
+
+    const d: drone.Drone = .{
+        .drag_coeff = parsed.value.drag_coeff,
+        .moment_of_inertia = .{
+            .row1 = .init(
+                parsed.value.moment_of_inertia[0],
+                parsed.value.moment_of_inertia[1],
+                parsed.value.moment_of_inertia[2],
+            ),
+            .row2 = .init(
+                parsed.value.moment_of_inertia[3],
+                parsed.value.moment_of_inertia[4],
+                parsed.value.moment_of_inertia[5],
+            ),
+            .row3 = .init(
+                parsed.value.moment_of_inertia[6],
+                parsed.value.moment_of_inertia[7],
+                parsed.value.moment_of_inertia[8],
+            ),
+        },
+        .mass_kg = parsed.value.mass_kg,
+    };
+    var propellers: [4]drone.Propeller = .{ .empty(), .empty(), .empty(), .empty() };
+    for (parsed.value.propellers, 0..) |parsed_prop, i| {
+        propellers[i].x_cg = .init(
+            parsed_prop.x_cg[0],
+            parsed_prop.x_cg[1],
+            parsed_prop.x_cg[2],
+        );
+        propellers[i].thrust_coefficient = parsed_prop.thrust_coefficient;
+        propellers[i].moment_coefficient = parsed_prop.moment_coefficient;
+        propellers[i].direction = parsed_prop.direction;
+    }
+    return .{
+        .propellers = propellers,
+        .drone = d,
+    };
+}
+
+test "Verify parse from Json String works" {
+    const TEST_JSON_STRING =
+        \\ { 
+        \\  "mass_kg": 0.5,
+        \\  "moment_of_inertia": [1,0,0,0,1,0,0,0,1],
+        \\  "drag_coeff": 0.25,
+        \\  "propellers": [
+        \\      {   "x_cg": [1,0,0], 
+        \\          "thrust_coefficient": 1.0,
+        \\          "moment_coefficient": 2.0,
+        \\          "direction": "cw"
+        \\      },
+        \\      {   "x_cg": [-1,0,0], 
+        \\          "thrust_coefficient": 2.0,
+        \\          "moment_coefficient": 1.0,
+        \\          "direction": "ccw"
+        \\      },
+        \\      {   "x_cg": [0,1,0], 
+        \\          "thrust_coefficient": 1.0,
+        \\          "moment_coefficient": 2.0,
+        \\          "direction": "cw"
+        \\      },
+        \\      {   "x_cg": [0,-1,0], 
+        \\          "thrust_coefficient": 2.0,
+        \\          "moment_coefficient": 1.0,
+        \\          "direction": "ccw"
+        \\      }
+        \\]
+        \\}
+    ;
+    const gpa = std.testing.allocator;
+    const sim_config = try jsonStringToSimConfig(TEST_JSON_STRING, gpa);
+
+    try std.testing.expectApproxEqAbs(0.5, sim_config.drone.mass_kg, 1e-12);
+    try std.testing.expectApproxEqAbs(0.25, sim_config.drone.drag_coeff, 1e-12);
+    try std.testing.expect(vec3.mat3ApproxEq(sim_config.drone.moment_of_inertia, vec3.Mat3F64.identity(), 1e-12));
+
+    const expected_x: [4]vec3.Vec3F64 = .{
+        .init(1, 0, 0),
+        .init(-1, 0, 0),
+        .init(0, 1, 0),
+        .init(0, -1, 0),
+    };
+
+    const expected_thrust: [4]f64 = .{ 1, 2, 1, 2 };
+    const expected_moment: [4]f64 = .{ 2, 1, 2, 1 };
+    const expected_direction: [4]constants.PropSpinDirection = .{ .cw, .ccw, .cw, .ccw };
+
+    var i: u8 = 0;
+    while (i < 4) : (i += 1) {
+        try std.testing.expectApproxEqAbs(
+            expected_thrust[i],
+            sim_config.propellers[i].thrust_coefficient,
+            1e-12,
+        );
+        try std.testing.expectApproxEqAbs(
+            expected_moment[i],
+            sim_config.propellers[i].moment_coefficient,
+            1e-12,
+        );
+
+        try std.testing.expect(
+            vec3.vec3ApproxEq(sim_config.propellers[i].x_cg, expected_x[i], 1e-12),
+        );
+
+        try std.testing.expect(sim_config.propellers[i].direction == expected_direction[i]);
+    }
+}

src/main.zig

@@ -5,4 +5,5 @@ test "main" {
     _ = @import("./vec3.zig");
     _ = @import("./forces.zig");
     _ = @import("./integration.zig");
+    _ = @import("./io.zig");
 }

src/vec3.zig

@@ -392,7 +392,7 @@ test "mat3approxEq works" {
     try std.testing.expect(mat3ApproxEq(a, b, 1e-12));
     b.row2.data[1] += 1e-11;
     try std.testing.expect(!mat3ApproxEq(a, b, 1e-12));
-    try std.testing.expect(mat3ApproxEq(a, b, 1e-12));
+    try std.testing.expect(mat3ApproxEq(a, b, 1e-10));
 }
 
 test "vec3 mul mat3 works" {