diff --git a/src/drone.zig b/src/drone.zig
new file mode 100644
index 0000000..806b9ee
--- /dev/null
+++ b/src/drone.zig
@@ -0,0 +1,47 @@
+const vec3 = @import("./vec3.zig");
+const std = @import("std");
+
+const Drone = struct {
+    mass: f32,
+    cg_inertial: vec3.Vec3F32,
+    moment_of_inertia: vec3.Mat3F32,
+    attitude: vec3.QuatF32,
+};
+
+const Propeller = struct {
+    cg_to_prop_body: vec3.Vec3F32,
+    thrust_coefficient: f32,
+    torque_drag_coefficient: f32,
+};
+
+const Simulation = struct {
+    drone: Drone,
+    propellers: std.ArrayList(Propeller),
+};
+
+pub fn body_position_to_inertial(
+    cg: vec3.Vec3F32,
+    attitude: vec3.QuatF32,
+    body_vec: vec3.Vec3F32,
+) vec3.Vec3F32 {
+    const intertial_vec = vec3.quatApply(attitude, body_vec);
+    return vec3.vec3Add(cg, intertial_vec);
+}
+
+test "Body position to inertial" {
+    const cg: vec3.Vec3F32 = .init(1, 1, 1);
+    const body_vec: vec3.Vec3F32 = .init(1, 0, 0);
+    const attitude = vec3.yawPitchRollToQuat(std.math.pi / 2.0, 0, 0);
+
+    const inertial_vec = body_position_to_inertial(cg, attitude, body_vec);
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.x(), 1, 1e-7));
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.y(), 2, 1e-7));
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec.z(), 1, 1e-7));
+
+    const attitude_2 = vec3.yawPitchRollToQuat(std.math.pi, 0, 0);
+    const inertial_vec_2 = body_position_to_inertial(cg, attitude_2, body_vec);
+
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.x(), 0, 1e-7));
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.y(), 1, 1e-7));
+    try std.testing.expect(std.math.approxEqAbs(f32, inertial_vec_2.z(), 1, 1e-7));
+}
diff --git a/src/main.zig b/src/main.zig
index 5db6ce0..c8cc000 100644
--- a/src/main.zig
+++ b/src/main.zig
@@ -1,270 +1,6 @@
 const std = @import("std");
 
-const Vec3F32Simd = @Vector(3, f32);
-const Vec4F32Simd = @Vector(4, f32);
-
-const Vec3F32Struct = struct {
-    x: f32,
-    y: f32,
-    z: f32,
-};
-
-pub fn vec3_struct_add(a: Vec3F32Struct, b: Vec3F32Struct) Vec3F32Struct {
-    return .{
-        .x = a.x + b.x,
-        .y = a.y + b.y,
-        .z = a.z + b.z,
-    };
-}
-
-pub fn vec3_struct_mul(a: Vec3F32Struct, b: Vec3F32Struct) Vec3F32Struct {
-    return .{
-        .x = a.x * b.x,
-        .y = a.y * b.y,
-        .z = a.z * b.z,
-    };
-}
-
-pub fn vec3_struct_sub(a: Vec3F32Struct, b: Vec3F32Struct) Vec3F32Struct {
-    return .{
-        .x = a.x - b.x,
-        .y = a.y - b.y,
-        .z = a.z - b.z,
-    };
-}
-
-pub fn vec3_struct_kernel(a: Vec3F32Struct, x: Vec3F32Struct, c: Vec3F32Struct) Vec3F32Struct {
-    const mixed = vec3_struct_add(vec3_struct_sub(vec3_struct_add(a, x), c), vec3_struct_mul(vec3_struct_sub(a, c), c));
-    return .{
-        .x = @sqrt(@abs(mixed.x) + 1.0),
-        .y = @sqrt(@abs(mixed.y) + 1.0),
-        .z = @sqrt(@abs(mixed.z) + 1.0),
-    };
-}
-
-pub fn vec3_simd_add(a: Vec3F32Simd, b: Vec3F32Simd) Vec3F32Simd {
-    return a + b;
-}
-
-pub fn vec3_simd_mul(a: Vec3F32Simd, b: Vec3F32Simd) Vec3F32Simd {
-    return a * b;
-}
-
-pub fn vec3_simd_sub(a: Vec3F32Simd, b: Vec3F32Simd) Vec3F32Simd {
-    return a - b;
-}
-
-pub fn vec3_simd_kernel(a: Vec3F32Simd, x: Vec3F32Simd, c: Vec3F32Simd) Vec3F32Simd {
-    const mixed = vec3_simd_add(vec3_simd_sub(vec3_simd_add(a, x), c), vec3_simd_mul(vec3_simd_sub(a, c), c));
-    return @sqrt(@abs(mixed) + @as(Vec3F32Simd, @splat(1.0)));
-}
-
-pub fn vec4_simd_add(a: Vec4F32Simd, b: Vec4F32Simd) Vec4F32Simd {
-    return a + b;
-}
-pub fn vec4_simd_mul(a: Vec4F32Simd, b: Vec4F32Simd) Vec4F32Simd {
-    return a * b;
-}
-
-pub fn vec4_simd_sub(a: Vec4F32Simd, b: Vec4F32Simd) Vec4F32Simd {
-    return a - b;
-}
-
-pub fn vec4_simd_kernel(a: Vec4F32Simd, x: Vec4F32Simd, c: Vec4F32Simd) Vec4F32Simd {
-    const mixed = vec4_simd_add(vec4_simd_sub(vec4_simd_add(a, x), c), vec4_simd_mul(vec4_simd_sub(a, c), c));
-    return @sqrt(@abs(mixed) + @as(Vec4F32Simd, @splat(1.0)));
-}
-
-pub fn bench_vec3F32Simd(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-
-    var a: Vec3F32Simd = .{ 0, 0, 0 };
-    const x: Vec3F32Simd = .{ prng.float(f32), prng.float(f32), prng.float(f32) };
-    const c: Vec3F32Simd = .{ prng.float(f32), prng.float(f32), prng.float(f32) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = vec3_simd_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec3F32Simd took {} with final answer {}\n", .{ end - start, a });
-}
-
-pub fn bench_vec4F32Simd(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-    var a: Vec4F32Simd = .{ 0, 0, 0, 0 };
-    const x: Vec4F32Simd = .{ prng.float(f32), prng.float(f32), prng.float(f32), prng.float(f32) };
-    const c: Vec4F32Simd = .{ prng.float(f32), prng.float(f32), prng.float(f32), prng.float(f32) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = vec4_simd_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec4F32Simd took {} with final answer {}\n", .{ end - start, a });
-}
-
-pub fn bench_vec3F32Struct(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-    var a: Vec3F32Struct = .{ .x = 0, .y = 0, .z = 0 };
-    const x: Vec3F32Struct = .{ .x = prng.float(f32), .y = prng.float(f32), .z = prng.float(f32) };
-    const c: Vec3F32Struct = .{ .x = prng.float(f32), .y = prng.float(f32), .z = prng.float(f32) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = vec3_struct_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec3F32Struct took {} with final answer {}\n", .{ end - start, a });
-}
-
-const Vec3F64Struct = struct {
-    x: f64,
-    y: f64,
-    z: f64,
-};
-
-const Vec3F64Simd = @Vector(3, f64);
-const Vec4F64Simd = @Vector(4, f64);
-
-pub fn f64vec3_struct_add(a: Vec3F64Struct, b: Vec3F64Struct) Vec3F64Struct {
-    return .{
-        .x = a.x + b.x,
-        .y = a.y + b.y,
-        .z = a.z + b.z,
-    };
-}
-
-pub fn f64vec3_struct_mul(a: Vec3F64Struct, b: Vec3F64Struct) Vec3F64Struct {
-    return .{
-        .x = a.x * b.x,
-        .y = a.y * b.y,
-        .z = a.z * b.z,
-    };
-}
-
-pub fn f64vec3_struct_sub(a: Vec3F64Struct, b: Vec3F64Struct) Vec3F64Struct {
-    return .{
-        .x = a.x - b.x,
-        .y = a.y - b.y,
-        .z = a.z - b.z,
-    };
-}
-
-pub fn f64vec3_struct_kernel(a: Vec3F64Struct, x: Vec3F64Struct, c: Vec3F64Struct) Vec3F64Struct {
-    const mixed = f64vec3_struct_add(f64vec3_struct_sub(f64vec3_struct_add(a, x), c), f64vec3_struct_mul(f64vec3_struct_sub(a, c), c));
-    return .{
-        .x = @sqrt(@abs(mixed.x) + 1.0),
-        .y = @sqrt(@abs(mixed.y) + 1.0),
-        .z = @sqrt(@abs(mixed.z) + 1.0),
-    };
-}
-
-pub fn f64vec3_simd_add(a: Vec3F64Simd, b: Vec3F64Simd) Vec3F64Simd {
-    return a + b;
-}
-
-pub fn f64vec3_simd_mul(a: Vec3F64Simd, b: Vec3F64Simd) Vec3F64Simd {
-    return a * b;
-}
-
-pub fn f64vec3_simd_sub(a: Vec3F64Simd, b: Vec3F64Simd) Vec3F64Simd {
-    return a - b;
-}
-
-pub fn f64vec3_simd_kernel(a: Vec3F64Simd, x: Vec3F64Simd, c: Vec3F64Simd) Vec3F64Simd {
-    const mixed = f64vec3_simd_add(f64vec3_simd_sub(f64vec3_simd_add(a, x), c), f64vec3_simd_mul(f64vec3_simd_sub(a, c), c));
-    return @sqrt(@abs(mixed) + @as(Vec3F64Simd, @splat(1.0)));
-}
-
-pub fn f64vec4_simd_add(a: Vec4F64Simd, b: Vec4F64Simd) Vec4F64Simd {
-    return a + b;
-}
-pub fn f64vec4_simd_mul(a: Vec4F64Simd, b: Vec4F64Simd) Vec4F64Simd {
-    return a * b;
-}
-
-pub fn f64vec4_simd_sub(a: Vec4F64Simd, b: Vec4F64Simd) Vec4F64Simd {
-    return a - b;
-}
-
-pub fn f64vec4_simd_kernel(a: Vec4F64Simd, x: Vec4F64Simd, c: Vec4F64Simd) Vec4F64Simd {
-    const mixed = f64vec4_simd_add(f64vec4_simd_sub(f64vec4_simd_add(a, x), c), f64vec4_simd_mul(f64vec4_simd_sub(a, c), c));
-    return @sqrt(@abs(mixed) + @as(Vec4F64Simd, @splat(1.0)));
-}
-
-pub fn bench_f64vec3F64Simd(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-
-    var a: Vec3F64Simd = .{ 0, 0, 0 };
-    const x: Vec3F64Simd = .{ prng.float(f64), prng.float(f64), prng.float(f64) };
-    const c: Vec3F64Simd = .{ prng.float(f64), prng.float(f64), prng.float(f64) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = f64vec3_simd_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec3F64Simd took {} with final answer {}\n", .{ end - start, a });
-}
-
-pub fn bench_f64vec4F64Simd(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-    var a: Vec4F64Simd = .{ 0, 0, 0, 0 };
-    const x: Vec4F64Simd = .{ prng.float(f64), prng.float(f64), prng.float(f64), prng.float(f64) };
-    const c: Vec4F64Simd = .{ prng.float(f64), prng.float(f64), prng.float(f64), prng.float(f64) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = f64vec4_simd_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec4F64Simd took {} with final answer {}\n", .{ end - start, a });
-}
-
-pub fn bench_f64vec3F64Struct(io: std.Io) void {
-    var rng: std.Random.DefaultPrng = .init(42);
-    var prng = rng.random();
-    var a: Vec3F64Struct = .{ .x = 0, .y = 0, .z = 0 };
-    const x: Vec3F64Struct = .{ .x = prng.float(f64), .y = prng.float(f64), .z = prng.float(f64) };
-    const c: Vec3F64Struct = .{ .x = prng.float(f64), .y = prng.float(f64), .z = prng.float(f64) };
-
-    var i: usize = 0;
-    const start = std.Io.Clock.real.now(io).toMicroseconds();
-    while (i < 100_000_000) : (i += 1) {
-        a = f64vec3_struct_kernel(a, x, c);
-    }
-    const end = std.Io.Clock.real.now(io).toMicroseconds();
-
-    std.debug.print("Vec3F64Struct took {} with final answer {}\n", .{ end - start, a });
-}
-pub fn main(init: std.process.Init) !void {
-    std.debug.print("Suggested lanes for f32: {}\n", .{std.simd.suggestVectorLength(f32).?});
-    std.debug.print("Suggested lanes for f64: {}\n\n", .{std.simd.suggestVectorLength(f64).?});
-
-    bench_vec3F32Simd(init.io);
-    bench_vec4F32Simd(init.io);
-    bench_vec3F32Struct(init.io);
-    bench_f64vec3F64Simd(init.io);
-    bench_f64vec4F64Simd(init.io);
-    bench_f64vec3F64Struct(init.io);
-}
-
 test "main" {
-    //_ = @import("./f64vec3/src/vec3.zig");
+    _ = @import("./drone.zig");
+    _ = @import("./vec3.zig");
 }
diff --git a/src/types/vec3.zig b/src/vec3.zig
similarity index 99%
rename from src/types/vec3.zig
rename to src/vec3.zig
index 8465161..2742bfb 100644
--- a/src/types/vec3.zig
+++ b/src/vec3.zig
@@ -1,6 +1,6 @@
 const std = @import("std");
 
-const Vec3F32 = struct {
+pub const Vec3F32 = struct {
     data: @Vector(4, f32),
 
     pub fn init(_x: f32, _y: f32, _z: f32) Vec3F32 {
@@ -39,7 +39,7 @@ pub fn vec3Cross(a: Vec3F32, b: Vec3F32) Vec3F32 {
     return .init(x, y, z);
 }
 
-const Mat3F32 = struct {
+pub const Mat3F32 = struct {
     row1: Vec3F32,
     row2: Vec3F32,
     row3: Vec3F32,
@@ -94,7 +94,7 @@ pub fn vec3MulMat3(a: Mat3F32, b: Vec3F32) Vec3F32 {
 // Uses the Quaternion definition of q = w + xi + yj + zk
 // and with i^2 = j^2 = k^2 = ijk = -1
 // I grabbed a lot of this math from https://imadrahmoune.com/rotations-with-quaternions/
-const QuatF32 = struct {
+pub const QuatF32 = struct {
     data: @Vector(4, f32),
     pub fn init(_x: f32, _y: f32, _z: f32, _w: f32) QuatF32 {
         return .{ .data = .{ _x, _y, _z, _w } };