Add basic drone data structures
This commit is contained in:
parent
0fc5ccd020
commit
e27ba9a558
3 changed files with 52 additions and 269 deletions
47
src/drone.zig
Normal file
47
src/drone.zig
Normal file
|
|
@ -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));
|
||||
}
|
||||
268
src/main.zig
268
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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 } };
|
||||
Loading…
Add table
Add a link
Reference in a new issue