Add basic drone data structures
This commit is contained in:
parent
0fc5ccd020
commit
e27ba9a558
3 changed files with 52 additions and 269 deletions
348
src/vec3.zig
Normal file
348
src/vec3.zig
Normal file
|
|
@ -0,0 +1,348 @@
|
|||
const std = @import("std");
|
||||
|
||||
pub const Vec3F32 = struct {
|
||||
data: @Vector(4, f32),
|
||||
|
||||
pub fn init(_x: f32, _y: f32, _z: f32) Vec3F32 {
|
||||
return .{ .data = .{ _x, _y, _z, 0 } };
|
||||
}
|
||||
|
||||
pub fn x(self: Vec3F32) f32 {
|
||||
return self.data[0];
|
||||
}
|
||||
|
||||
pub fn y(self: Vec3F32) f32 {
|
||||
return self.data[1];
|
||||
}
|
||||
|
||||
pub fn z(self: Vec3F32) f32 {
|
||||
return self.data[2];
|
||||
}
|
||||
};
|
||||
|
||||
pub fn vec3Add(a: Vec3F32, b: Vec3F32) Vec3F32 {
|
||||
return .{ .data = a.data + b.data };
|
||||
}
|
||||
|
||||
pub fn vec3Sub(a: Vec3F32, b: Vec3F32) Vec3F32 {
|
||||
return .{ .data = a.data - b.data };
|
||||
}
|
||||
|
||||
pub fn vec3Dot(a: Vec3F32, b: Vec3F32) f32 {
|
||||
return @reduce(.Add, a.data * b.data);
|
||||
}
|
||||
|
||||
pub fn vec3Cross(a: Vec3F32, b: Vec3F32) Vec3F32 {
|
||||
const x = a.y() * b.z() - a.z() * b.y();
|
||||
const y = a.z() * b.x() - a.x() * b.z();
|
||||
const z = a.x() * b.y() - a.y() * b.x();
|
||||
return .init(x, y, z);
|
||||
}
|
||||
|
||||
pub const Mat3F32 = struct {
|
||||
row1: Vec3F32,
|
||||
row2: Vec3F32,
|
||||
row3: Vec3F32,
|
||||
};
|
||||
|
||||
pub fn mat3Add(a: Mat3F32, b: Mat3F32) Mat3F32 {
|
||||
return .{
|
||||
.row1 = vec3Add(a.row1, b.row1),
|
||||
.row2 = vec3Add(a.row2, b.row2),
|
||||
.row3 = vec3Add(a.row3, b.row3),
|
||||
};
|
||||
}
|
||||
|
||||
pub fn mat3Sub(a: Mat3F32, b: Mat3F32) Mat3F32 {
|
||||
return .{
|
||||
.row1 = vec3Sub(a.row1, b.row1),
|
||||
.row2 = vec3Sub(a.row2, b.row2),
|
||||
.row3 = vec3Sub(a.row3, b.row3),
|
||||
};
|
||||
}
|
||||
|
||||
pub fn mat3Mul(a: Mat3F32, b: Mat3F32) Mat3F32 {
|
||||
const c_row1: Vec3F32 = .init(
|
||||
a.row1.x() * b.row1.x() + a.row1.y() * b.row2.x() + a.row1.z() * b.row3.x(),
|
||||
a.row1.x() * b.row1.y() + a.row1.y() * b.row2.y() + a.row1.z() * b.row3.y(),
|
||||
a.row1.x() * b.row1.z() + a.row1.y() * b.row2.z() + a.row1.z() * b.row3.z(),
|
||||
);
|
||||
|
||||
const c_row2: Vec3F32 = .init(
|
||||
a.row2.x() * b.row1.x() + a.row2.y() * b.row2.x() + a.row2.z() * b.row3.x(),
|
||||
a.row2.x() * b.row1.y() + a.row2.y() * b.row2.y() + a.row2.z() * b.row3.y(),
|
||||
a.row2.x() * b.row1.z() + a.row2.y() * b.row2.z() + a.row2.z() * b.row3.z(),
|
||||
);
|
||||
|
||||
const c_row3: Vec3F32 = .init(
|
||||
a.row3.x() * b.row1.x() + a.row3.y() * b.row2.x() + a.row3.z() * b.row3.x(),
|
||||
a.row3.x() * b.row1.y() + a.row3.y() * b.row2.y() + a.row3.z() * b.row3.y(),
|
||||
a.row3.x() * b.row1.z() + a.row3.y() * b.row2.z() + a.row3.z() * b.row3.z(),
|
||||
);
|
||||
|
||||
return .{ .row1 = c_row1, .row2 = c_row2, .row3 = c_row3 };
|
||||
}
|
||||
|
||||
pub fn vec3MulMat3(a: Mat3F32, b: Vec3F32) Vec3F32 {
|
||||
return .init(
|
||||
vec3Dot(a.row1, b),
|
||||
vec3Dot(a.row2, b),
|
||||
vec3Dot(a.row3, b),
|
||||
);
|
||||
}
|
||||
|
||||
// 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/
|
||||
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 } };
|
||||
}
|
||||
pub fn x(self: QuatF32) f32 {
|
||||
return self.data[0];
|
||||
}
|
||||
|
||||
pub fn y(self: QuatF32) f32 {
|
||||
return self.data[1];
|
||||
}
|
||||
|
||||
pub fn z(self: QuatF32) f32 {
|
||||
return self.data[2];
|
||||
}
|
||||
pub fn w(self: QuatF32) f32 {
|
||||
return self.data[3];
|
||||
}
|
||||
pub fn mag(q: QuatF32) f32 {
|
||||
return std.math.sqrt(@reduce(.Add, q.data * q.data));
|
||||
}
|
||||
|
||||
pub fn normalized(q: QuatF32) QuatF32 {
|
||||
const magnitude = q.mag();
|
||||
return .{ .data = q.data / @as(@Vector(4, f32), @splat(magnitude)) };
|
||||
}
|
||||
|
||||
pub fn conjugate(q: QuatF32) QuatF32 {
|
||||
return .{ .data = q.data * @as(@Vector(4, f32), .{ -1, -1, -1, 1 }) };
|
||||
}
|
||||
};
|
||||
|
||||
pub fn yawPitchRollToQuat(yaw: f32, pitch: f32, roll: f32) QuatF32 {
|
||||
const cx = @cos(roll / 2);
|
||||
const sx = @sin(roll / 2);
|
||||
const cy = @cos(pitch / 2);
|
||||
const sy = @sin(pitch / 2);
|
||||
const cz = @cos(yaw / 2);
|
||||
const sz = @sin(yaw / 2);
|
||||
|
||||
return .init(
|
||||
sx * cy * cz - cx * sy * sz,
|
||||
cx * sy * cz + sx * cy * sz,
|
||||
cx * cy * sz - sx * sy * cz,
|
||||
cx * cy * cz + sx * sy * sz,
|
||||
);
|
||||
}
|
||||
|
||||
pub fn quatMul(a: QuatF32, b: QuatF32) QuatF32 {
|
||||
return .init(
|
||||
a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
|
||||
a.w() * b.y() - a.x() * b.z() + a.y() * b.w() + a.z() * b.x(),
|
||||
a.w() * b.z() + a.x() * b.y() - a.y() * b.x() + a.z() * b.w(),
|
||||
a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
|
||||
);
|
||||
}
|
||||
|
||||
pub fn quatApply(a: QuatF32, b: Vec3F32) Vec3F32 {
|
||||
const quat_v = quatMul(quatMul(a, .init(b.x(), b.y(), b.z(), 0)), a.conjugate());
|
||||
return .init(quat_v.data[0], quat_v.data[1], quat_v.data[2]);
|
||||
}
|
||||
|
||||
test "vec3Add adds properly" {
|
||||
const a = Vec3F32.init(1, 2, 3);
|
||||
const b = Vec3F32.init(3, 1, 0);
|
||||
const c = vec3Add(a, b);
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), 4, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 3, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7));
|
||||
}
|
||||
test "vec3Sub subs properly" {
|
||||
const a = Vec3F32.init(1, 2, 3);
|
||||
const b = Vec3F32.init(3, 1, 0);
|
||||
const c = vec3Sub(a, b);
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 1, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7));
|
||||
}
|
||||
test "vec3Dot dots properly" {
|
||||
const a = Vec3F32.init(1, 2, 3);
|
||||
const b = Vec3F32.init(3, 1, 0);
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, vec3Dot(a, b), 5, 1e-7));
|
||||
}
|
||||
test "vec3Cross crosses properly" {
|
||||
const a = Vec3F32.init(1, 2, 3);
|
||||
const b = Vec3F32.init(3, 1, 0);
|
||||
|
||||
const c = vec3Cross(a, b);
|
||||
|
||||
try std.testing.expect(c.x() == -3);
|
||||
try std.testing.expect(c.y() == 9);
|
||||
try std.testing.expect(c.z() == -5);
|
||||
}
|
||||
|
||||
test "Mat3 add works" {
|
||||
const a: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(1, 2, 3),
|
||||
.row2 = Vec3F32.init(0, 1, 5),
|
||||
.row3 = Vec3F32.init(0, 0, 9),
|
||||
};
|
||||
const b: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(0, 1, 0),
|
||||
.row2 = Vec3F32.init(0, 1, 1),
|
||||
.row3 = Vec3F32.init(0, 0, 1),
|
||||
};
|
||||
|
||||
const c = mat3Add(a, b);
|
||||
|
||||
try std.testing.expect(c.row1.x() == 1);
|
||||
try std.testing.expect(c.row1.y() == 3);
|
||||
try std.testing.expect(c.row1.z() == 3);
|
||||
try std.testing.expect(c.row2.x() == 0);
|
||||
try std.testing.expect(c.row2.y() == 2);
|
||||
try std.testing.expect(c.row2.z() == 6);
|
||||
try std.testing.expect(c.row3.x() == 0);
|
||||
try std.testing.expect(c.row3.y() == 0);
|
||||
try std.testing.expect(c.row3.z() == 10);
|
||||
}
|
||||
|
||||
test "Mat3 sub works" {
|
||||
const a: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(1, 2, 3),
|
||||
.row2 = Vec3F32.init(0, 1, 5),
|
||||
.row3 = Vec3F32.init(0, 0, 9),
|
||||
};
|
||||
const b: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(0, 1, 0),
|
||||
.row2 = Vec3F32.init(0, 1, 1),
|
||||
.row3 = Vec3F32.init(0, 0, 1),
|
||||
};
|
||||
|
||||
const c = mat3Sub(a, b);
|
||||
|
||||
try std.testing.expect(c.row1.x() == 1);
|
||||
try std.testing.expect(c.row1.y() == 1);
|
||||
try std.testing.expect(c.row1.z() == 3);
|
||||
try std.testing.expect(c.row2.x() == 0);
|
||||
try std.testing.expect(c.row2.y() == 0);
|
||||
try std.testing.expect(c.row2.z() == 4);
|
||||
try std.testing.expect(c.row3.x() == 0);
|
||||
try std.testing.expect(c.row3.y() == 0);
|
||||
try std.testing.expect(c.row3.z() == 8);
|
||||
}
|
||||
|
||||
test "Mat3 mul works" {
|
||||
const a: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(1, 2, 3),
|
||||
.row2 = Vec3F32.init(0, 1, 5),
|
||||
.row3 = Vec3F32.init(0, 0, 9),
|
||||
};
|
||||
const b: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(0, 1, 0),
|
||||
.row2 = Vec3F32.init(0, 1, 1),
|
||||
.row3 = Vec3F32.init(0, 0, 1),
|
||||
};
|
||||
|
||||
const c = mat3Mul(a, b);
|
||||
|
||||
try std.testing.expect(c.row1.x() == 0);
|
||||
try std.testing.expect(c.row1.y() == 3);
|
||||
try std.testing.expect(c.row1.z() == 5);
|
||||
try std.testing.expect(c.row2.x() == 0);
|
||||
try std.testing.expect(c.row2.y() == 1);
|
||||
try std.testing.expect(c.row2.z() == 6);
|
||||
try std.testing.expect(c.row3.x() == 0);
|
||||
try std.testing.expect(c.row3.y() == 0);
|
||||
try std.testing.expect(c.row3.z() == 9);
|
||||
}
|
||||
|
||||
test "vec3 mul mat3 works" {
|
||||
const b: Mat3F32 = .{
|
||||
.row1 = Vec3F32.init(0, 1, 0),
|
||||
.row2 = Vec3F32.init(0, 1, 1),
|
||||
.row3 = Vec3F32.init(0, 0, 1),
|
||||
};
|
||||
const a = Vec3F32.init(1, 2, 3);
|
||||
|
||||
const c = vec3MulMat3(b, a);
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), 2, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 5, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 3, 1e-7));
|
||||
}
|
||||
|
||||
test "quat magnitude" {
|
||||
const q: QuatF32 = .init(1.0, 2.0, 3.0, 4.0);
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, q.mag(), std.math.sqrt(30.0), 1e-7));
|
||||
}
|
||||
|
||||
test "quat normalized" {
|
||||
const q: QuatF32 = .init(1.0, 2.0, 3.0, 4.0);
|
||||
const expected: QuatF32 = .init(
|
||||
1.0 / std.math.sqrt(30.0),
|
||||
2.0 / std.math.sqrt(30.0),
|
||||
3.0 / std.math.sqrt(30.0),
|
||||
4.0 / std.math.sqrt(30.0),
|
||||
);
|
||||
|
||||
const normalized = q.normalized();
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, normalized.x(), expected.x(), 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, normalized.y(), expected.y(), 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, normalized.z(), expected.z(), 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, normalized.w(), expected.w(), 1e-7));
|
||||
}
|
||||
|
||||
test "quat mul" {
|
||||
const a: QuatF32 = .init(2, 3, 4, 1);
|
||||
const b: QuatF32 = .init(0, 1, 0, 1);
|
||||
|
||||
const c = quatMul(a, b);
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.w(), -2, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.x(), -2, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.y(), 4, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, c.z(), 6, 1e-7));
|
||||
}
|
||||
|
||||
test "quat euler" {
|
||||
const quat = yawPitchRollToQuat(0.5, 0.5, 1);
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0.3963648, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0.3252922, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), 0.0954433, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), 0.8532119, 1e-7));
|
||||
}
|
||||
|
||||
test "quat conjugate" {
|
||||
const quat: QuatF32 = .init(1, 1, 1, 1);
|
||||
const conj = quat.conjugate();
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, conj.x(), -1, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, conj.y(), -1, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, conj.z(), -1, 1e-7));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, conj.w(), 1, 1e-7));
|
||||
}
|
||||
|
||||
test "quat apply to vec " {
|
||||
const quat: QuatF32 = yawPitchRollToQuat(std.math.pi / 2.0, 0, 0);
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.x(), 0, 1e-6));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.y(), 0, 1e-6));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.z(), @sin(std.math.pi / 4.0), 1e-6));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, quat.w(), @cos(std.math.pi / 4.0), 1e-6));
|
||||
|
||||
const rotated = quatApply(quat, .init(1, 2, 1));
|
||||
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, rotated.x(), -2, 1e-6));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, rotated.y(), 1, 1e-6));
|
||||
try std.testing.expect(std.math.approxEqAbs(f32, rotated.z(), 1, 1e-6));
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue