diff --git a/README.md b/README.md index 841e236..e2f37fb 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,8 @@ Coord System: Vulkan // --- GENERICS --- pub const Vec = @import("vec.zig").Vec; pub const Mat = @import("mat.zig").Mat; +pub const Quat = @import("quat.zig").Quat; +pub const Euler = @import("euler.zig").Euler; pub const Color = @import("color.zig").Color; // --- VEC2 --- diff --git a/src/euler.zig b/src/euler.zig new file mode 100644 index 0000000..2ee9a1b --- /dev/null +++ b/src/euler.zig @@ -0,0 +1,98 @@ +//! ---------------------------------------------------- +//! ---------------------------------------------------- + +const std = @import("std"); +const Mat = @import("mat.zig").Mat; + +/// ---------------------------------------------------- +/// Euler angles (pitch, yaw, roll) in radians +/// Order: XYZ - applied as R = Rz(roll) * Ry(yaw) * Rx(pitch) +/// ---------------------------------------------------- +pub const Euler = struct { + const Self = @This(); + + // + // FIELDS + // + + x: f32 = 0, + y: f32 = 0, + z: f32 = 0, + + // + // CONSTRUCTORS + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn init(pitch: f32, yaw: f32, roll: f32) Self { + return .{ .x = pitch, .y = yaw, .z = roll }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn zero() Self { + return .{ .x = 0, .y = 0, .z = 0 }; + } + + // + // ARITHMETIC + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn add(self: Self, other: Self) Self { + return .{ .x = self.x + other.x, .y = self.y + other.y, .z = self.z + other.z }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn sub(self: Self, other: Self) Self { + return .{ .x = self.x - other.x, .y = self.y - other.y, .z = self.z - other.z }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn scale(self: Self, scalar: f32) Self { + return .{ .x = self.x * scalar, .y = self.y * scalar, .z = self.z * scalar }; + } + + // + // MATRIX CONVERSIONS + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toMat3(self: Self) Mat(f32, 3, 3) { + const sx = @sin(self.x); + const cx = @cos(self.x); + const sy = @sin(self.y); + const cy = @cos(self.y); + const sz = @sin(self.z); + const cz = @cos(self.z); + + return Mat(f32, 3, 3).fromRows(.{ + .{ cy * cz, cz * sx * sy - cx * sz, cz * cx * sy + sx * sz }, + .{ cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz }, + .{ -sy, cy * sx, cx * cy }, + }); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toMat4(self: Self) Mat(f32, 4, 4) { + const sx = @sin(self.x); + const cx = @cos(self.x); + const sy = @sin(self.y); + const cy = @cos(self.y); + const sz = @sin(self.z); + const cz = @cos(self.z); + + return Mat(f32, 4, 4).fromRows(.{ + .{ cy * cz, cz * sx * sy - cx * sz, cz * cx * sy + sx * sz, 0 }, + .{ cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz, 0 }, + .{ -sy, cy * sx, cx * cy, 0 }, + .{ 0, 0, 0, 1 }, + }); + } +}; diff --git a/src/quat.zig b/src/quat.zig new file mode 100644 index 0000000..33d29c4 --- /dev/null +++ b/src/quat.zig @@ -0,0 +1,410 @@ +//! ---------------------------------------------------- +//! ---------------------------------------------------- + +const std = @import("std"); +const Vec = @import("vec.zig").Vec; +const Mat = @import("mat.zig").Mat; +const Euler = @import("euler.zig").Euler; + +/// ---------------------------------------------------- +/// Quaternion +/// ---------------------------------------------------- +pub const Quat = extern struct { + const Self = @This(); + + // + // FIELDS + // + + x: f32 = 0, + y: f32 = 0, + z: f32 = 0, + w: f32 = 1, + + // + // CONSTRUCTORS + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn identity() Self { + return .{ .x = 0, .y = 0, .z = 0, .w = 1 }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn fromAxisAngle(axis: Vec(f32, 3), angle: f32) Self { + const half = angle * 0.5; + const s = @sin(half); + return .{ + .x = axis.value[0] * s, + .y = axis.value[1] * s, + .z = axis.value[2] * s, + .w = @cos(half), + }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn fromEuler(euler: Euler) Self { + const cp = @cos(euler.x * 0.5); + const sp = @sin(euler.x * 0.5); + const cy = @cos(euler.y * 0.5); + const sy = @sin(euler.y * 0.5); + const cr = @cos(euler.z * 0.5); + const sr = @sin(euler.z * 0.5); + + return .{ + .x = cr * cy * sp - sr * sy * cp, + .y = cr * sy * cp + sr * cy * sp, + .z = sr * cy * cp - cr * sy * sp, + .w = cr * cy * cp + sr * sy * sp, + }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn fromMat4(m: Mat(f32, 4, 4)) Self { + return fromMatrixElements( + m.value[0][0], + m.value[0][1], + m.value[0][2], + m.value[1][0], + m.value[1][1], + m.value[1][2], + m.value[2][0], + m.value[2][1], + m.value[2][2], + ); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn fromMat3(m: Mat(f32, 3, 3)) Self { + return fromMatrixElements( + m.value[0][0], + m.value[0][1], + m.value[0][2], + m.value[1][0], + m.value[1][1], + m.value[1][2], + m.value[2][0], + m.value[2][1], + m.value[2][2], + ); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn fromTwoVectors(from: Vec(f32, 3), to: Vec(f32, 3)) Self { + const f = from.normalize(); + const t = to.normalize(); + const cos_angle = @min(@max(f.dot(t), -1), 1); + + if (cos_angle > 0.9999) return identity(); + + if (cos_angle < -0.9999) { + var axis: Vec(f32, 3) = undefined; + const ref: Vec(f32, 3) = .new(.{ 1, 0, 0 }); + axis = f.cross(ref); + if (axis.length() < 0.0001) { + const ref2: Vec(f32, 3) = .new(.{ 0, 1, 0 }); + axis = f.cross(ref2); + } + axis = axis.normalize(); + return fromAxisAngle(axis, std.math.pi); + } + + const angle = std.math.acos(cos_angle); + const axis = f.cross(t).normalize(); + return fromAxisAngle(axis, angle); + } + + // + // OPERATIONS + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn add(self: Self, other: Self) Self { + return .{ .x = self.x + other.x, .y = self.y + other.y, .z = self.z + other.z, .w = self.w + other.w }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn sub(self: Self, other: Self) Self { + return .{ .x = self.x - other.x, .y = self.y - other.y, .z = self.z - other.z, .w = self.w - other.w }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn mul(self: Self, other: Self) Self { + return .{ + .x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y, + .y = self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x, + .z = self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w, + .w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z, + }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn scale(self: Self, scalar: f32) Self { + return .{ .x = self.x * scalar, .y = self.y * scalar, .z = self.z * scalar, .w = self.w * scalar }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn negate(self: Self) Self { + return .{ .x = -self.x, .y = -self.y, .z = -self.z, .w = -self.w }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn conjugate(self: Self) Self { + return .{ .x = -self.x, .y = -self.y, .z = -self.z, .w = self.w }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn inverse(self: Self) Self { + const lensq = self.lengthSquared(); + const c = self.conjugate(); + return .{ .x = c.x / lensq, .y = c.y / lensq, .z = c.z / lensq, .w = c.w / lensq }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn normalize(self: Self) Self { + const len = self.length(); + if (len == 0) return identity(); + return .{ .x = self.x / len, .y = self.y / len, .z = self.z / len, .w = self.w / len }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn length(self: Self) f32 { + return @sqrt(self.lengthSquared()); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn lengthSquared(self: Self) f32 { + return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn dot(self: Self, other: Self) f32 { + return self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w; + } + + // + // CONVERSIONS + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toMat4(self: Self) Mat(f32, 4, 4) { + const xx = self.x * self.x; + const yy = self.y * self.y; + const zz = self.z * self.z; + const xy = self.x * self.y; + const xz = self.x * self.z; + const yz = self.y * self.z; + const wx = self.w * self.x; + const wy = self.w * self.y; + const wz = self.w * self.z; + + return Mat(f32, 4, 4).fromRows(.{ + .{ 1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy), 0 }, + .{ 2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx), 0 }, + .{ 2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy), 0 }, + .{ 0, 0, 0, 1 }, + }); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toMat3(self: Self) Mat(f32, 3, 3) { + const xx = self.x * self.x; + const yy = self.y * self.y; + const zz = self.z * self.z; + const xy = self.x * self.y; + const xz = self.x * self.z; + const yz = self.y * self.z; + const wx = self.w * self.x; + const wy = self.w * self.y; + const wz = self.w * self.z; + + return Mat(f32, 3, 3).fromRows(.{ + .{ 1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy) }, + .{ 2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx) }, + .{ 2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy) }, + }); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toEuler(self: Self) Euler { + const m21 = 2 * (self.y * self.z + self.w * self.x); + const m22 = 1 - 2 * (self.x * self.x + self.y * self.y); + const m20 = 2 * (self.x * self.z - self.w * self.y); + const m10 = 2 * (self.x * self.y + self.w * self.z); + const m00 = 1 - 2 * (self.y * self.y + self.z * self.z); + const m01 = 2 * (self.x * self.y - self.w * self.z); + const m02 = 2 * (self.x * self.z + self.w * self.y); + + const sy = -m20; + + if (@abs(sy) >= 1) { + const yaw = std.math.copysign(@as(f32, std.math.pi) / 2.0, sy); + const roll: f32 = 0; + const pitch = if (sy >= 0) + std.math.atan2(m01, m02) + else + std.math.atan2(-m01, -m02); + return Euler{ .x = pitch, .y = yaw, .z = roll }; + } + + return Euler{ + .x = std.math.atan2(m21, m22), + .y = std.math.asin(sy), + .z = std.math.atan2(m10, m00), + }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn toAxisAngle(self: Self) struct { axis: Vec(f32, 3), angle: f32 } { + const half_angle = std.math.acos(@min(@max(self.w, -1), 1)); + const angle = 2 * half_angle; + + const s = @sin(half_angle); + if (s < 0.0001) { + return .{ .axis = Vec(f32, 3).new(.{ 0, 1, 0 }), .angle = 0 }; + } + + return .{ + .axis = Vec(f32, 3).new(.{ self.x / s, self.y / s, self.z / s }), + .angle = angle, + }; + } + + // + // VECTOR ROTATION + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn rotate(self: Self, v: Vec(f32, 3)) Vec(f32, 3) { + const qv = Vec(f32, 3).new(.{ self.x, self.y, self.z }); + const t = qv.cross(v).scale(2); + return v.add(t.scale(self.w)).add(qv.cross(t)); + } + + // + // INTERPOLATION + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn lerp(self: Self, other: Self, t: f32) Self { + return .{ + .x = self.x + (other.x - self.x) * t, + .y = self.y + (other.y - self.y) * t, + .z = self.z + (other.z - self.z) * t, + .w = self.w + (other.w - self.w) * t, + }; + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn nlerp(self: Self, other: Self, t: f32) Self { + return self.lerp(other, t).normalize(); + } + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + pub inline fn slerp(self: Self, other: Self, t: f32) Self { + var cos_half = self.dot(other); + var q2 = other; + + if (cos_half < 0) { + q2 = q2.negate(); + cos_half = -cos_half; + } + + if (cos_half >= 1) return self.nlerp(q2, t); + + const half_theta = std.math.acos(cos_half); + const sin_half = @sqrt(1 - cos_half * cos_half); + + if (sin_half < 0.001) return self.nlerp(q2, t); + + const a = @sin((1 - t) * half_theta) / sin_half; + const b = @sin(t * half_theta) / sin_half; + + return self.scale(a).add(q2.scale(b)); + } + + // + // PRIVATE + // + + /// ---------------------------------------------------- + /// ---------------------------------------------------- + inline fn fromMatrixElements( + m00: f32, + m01: f32, + m02: f32, + m10: f32, + m11: f32, + m12: f32, + m20: f32, + m21: f32, + m22: f32, + ) Self { + const trace = m00 + m11 + m22; + + if (trace > 0) { + const s = @sqrt(trace + 1) * 2; + return .{ + .w = 0.25 * s, + .x = (m21 - m12) / s, + .y = (m02 - m20) / s, + .z = (m10 - m01) / s, + }; + } + + if (m00 > m11 and m00 > m22) { + const s = @sqrt(1 + m00 - m11 - m22) * 2; + return .{ + .w = (m21 - m12) / s, + .x = 0.25 * s, + .y = (m01 + m10) / s, + .z = (m02 + m20) / s, + }; + } + + if (m11 > m22) { + const s = @sqrt(1 + m11 - m00 - m22) * 2; + return .{ + .w = (m02 - m20) / s, + .x = (m01 + m10) / s, + .y = 0.25 * s, + .z = (m12 + m21) / s, + }; + } + + { + const s = @sqrt(1 + m22 - m00 - m11) * 2; + return .{ + .w = (m10 - m01) / s, + .x = (m02 + m20) / s, + .y = (m12 + m21) / s, + .z = 0.25 * s, + }; + } + } +}; diff --git a/src/root.zig b/src/root.zig index d19c7a0..f426368 100644 --- a/src/root.zig +++ b/src/root.zig @@ -14,6 +14,8 @@ const std = @import("std"); // --- GENERICS --- pub const Vec = @import("vec.zig").Vec; pub const Mat = @import("mat.zig").Mat; +pub const Quat = @import("quat.zig").Quat; +pub const Euler = @import("euler.zig").Euler; pub const Color = @import("color.zig").Color; // --- VEC2 --- @@ -167,3 +169,70 @@ test "color hsv" { const hsv = c.toHSV(); std.debug.print("fromHSV(120,1,1): r={d:.4} g={d:.4} b={d:.4} -> h={d:.1} s={d:.1} v={d:.1}\n", .{ c.r, c.g, c.b, hsv.h, hsv.s, hsv.v }); } + +test "quat identity" { + std.debug.print("\x1b[34m--- quat identity ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const q: Quat = .identity(); + std.debug.print("x={d:.1} y={d:.1} z={d:.1} w={d:.1}\n", .{ q.x, q.y, q.z, q.w }); +} + +test "quat rotate" { + std.debug.print("\x1b[35m--- quat rotate ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const axis: Vec3f = .new(.{ 0, 0, 1 }); + const q: Quat = .fromAxisAngle(axis, @as(f32, std.math.pi) / 2.0); + const v: Vec3f = .new(.{ 1, 0, 0 }); + const r = q.rotate(v); + + std.debug.print("rotate (1,0,0) by 90deg around Z: {d:.4} {d:.4} {d:.4}\n", .{ r.value[0], r.value[1], r.value[2] }); +} + +test "quat euler" { + std.debug.print("\x1b[34m--- quat euler ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const e: Euler = .init(@as(f32, std.math.pi) / 4.0, 0, 0); + const q: Quat = .fromEuler(e); + const back = q.toEuler(); + + std.debug.print("pitch: {d:.4} -> {d:.4}\n", .{ e.x, back.x }); +} + +test "quat mat4 roundtrip" { + std.debug.print("\x1b[35m--- quat mat4 roundtrip ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const q: Quat = .fromAxisAngle(.new(.{ 0, 1, 0 }), @as(f32, std.math.pi) / 3.0); + const m = q.toMat4(); + const q2: Quat = .fromMat4(m); + + std.debug.print("orig: {d:.4} {d:.4} {d:.4} {d:.4}\n", .{ q.x, q.y, q.z, q.w }); + std.debug.print("back: {d:.4} {d:.4} {d:.4} {d:.4}\n", .{ q2.x, q2.y, q2.z, q2.w }); +} + +test "quat slerp" { + std.debug.print("\x1b[34m--- quat slerp ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const q1: Quat = .identity(); + const q2: Quat = .fromAxisAngle(.new(.{ 0, 0, 1 }), @as(f32, std.math.pi) / 2.0); + const mid = q1.slerp(q2, 0.5); + + std.debug.print("halfway: {d:.4} {d:.4} {d:.4} {d:.4}\n", .{ mid.x, mid.y, mid.z, mid.w }); +} + +test "euler toMat4" { + std.debug.print("\x1b[35m--- euler toMat4 ---\n", .{}); + defer std.debug.print("\x1b[0m\n", .{}); + + const e: Euler = .init(@as(f32, std.math.pi) / 4.0, 0, 0); + const m = e.toMat4(); + + std.debug.print("pitch 45deg rotation:\n", .{}); + for (m.value, 0..) |row, i| { + std.debug.print("row {d}: {d:.4} {d:.4} {d:.4} {d:.4}\n", .{ i, row[0], row[1], row[2], row[3] }); + } +}