Added [Euler, Quat]

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 ---

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 },
+        });
+    }
+};

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,
+            };
+        }
+    }
+};

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] });
+    }
+}