geometry.zig

//! Zigen Example: 3D Geometry & Robotics Transforms
//! Quaternion rotations, Euler angles, angle-axis, SLERP interpolation.

const std = @import("std");
const math = std.math;
const Zigen = @import("zigen");

const Vec3 = Zigen.Vector3f;
const Quat = Zigen.Quaternionf;

pub fn main() !void {
    std.debug.print(
        \\
        \\  ╔═══════════════════════════════════════════════╗
        \\  ║    Zigen — 3D Geometry & Transforms           ║
        \\  ╚═══════════════════════════════════════════════╝
        \\
        \\
    , .{});

    // ── 1. Quaternion rotation ────────────────────────────────────────
    std.debug.print("━━ 1. Quaternion Rotation ━━━━━━━━━━━━━━━━━━━━━━━━━━━━\n\n", .{});

    const z_axis = Vec3.fromArray(.{ 0.0, 0.0, 1.0 });
    const q90z = Quat.fromAxisAngle(z_axis, math.pi / 2.0);

    const point = Vec3.fromArray(.{ 1.0, 0.0, 0.0 });
    const rotated = q90z.rotate(point);

    std.debug.print("  Rotate [1,0,0] by 90° around Z:\n", .{});
    std.debug.print("  -> [{d:.4}, {d:.4}, {d:.4}]  (expected [0,1,0])\n\n", .{
        rotated.at(0), rotated.at(1), rotated.at(2),
    });

    // ── 2. Composing rotations (robotic arm) ──────────────────────────
    std.debug.print("━━ 2. Robotic Arm — Joint Composition ━━━━━━━━━━━━━━━━\n\n", .{});
    std.debug.print("  Joint 1: 45° around Z (yaw)\n", .{});
    std.debug.print("  Joint 2: 30° around Y (pitch)\n", .{});
    std.debug.print("  Joint 3: -20° around X (roll)\n\n", .{});

    const q_base = Quat.fromAxisAngle(Vec3.fromArray(.{ 0, 0, 1 }), math.pi / 4.0);
    const q_shoulder = Quat.fromAxisAngle(Vec3.fromArray(.{ 0, 1, 0 }), math.pi / 6.0);
    const q_elbow = Quat.fromAxisAngle(Vec3.fromArray(.{ 1, 0, 0 }), -math.pi / 9.0);

    const q_total = q_base.mul(q_shoulder).mul(q_elbow);
    std.debug.print("  Combined: w={d:.4} x={d:.4} y={d:.4} z={d:.4}\n", .{
        q_total.w, q_total.x, q_total.y, q_total.z,
    });

    const tool_tip = Vec3.fromArray(.{ 1.0, 0.0, 0.0 });
    const final_pos = q_total.rotate(tool_tip);
    std.debug.print("  End-effector: [{d:.4}, {d:.4}, {d:.4}]\n\n", .{
        final_pos.at(0), final_pos.at(1), final_pos.at(2),
    });

    // ── 3. Quaternion → Rotation Matrix ───────────────────────────────
    std.debug.print("━━ 3. Quaternion → Rotation Matrix ━━━━━━━━━━━━━━━━━━━\n\n", .{});

    const R = q90z.toRotationMatrix();
    var row: usize = 0;
    while (row < 3) : (row += 1) {
        std.debug.print("  │ {d:>7.4} {d:>7.4} {d:>7.4} │\n", .{ R.at(row, 0), R.at(row, 1), R.at(row, 2) });
    }
    std.debug.print("\n", .{});

    // ── 4. Euler angles ───────────────────────────────────────────────
    std.debug.print("━━ 4. Euler Angles → Rotation ━━━━━━━━━━━━━━━━━━━━━━━━\n\n", .{});

    const roll: f32 = 10.0 * math.pi / 180.0;
    const pitch: f32 = 5.0 * math.pi / 180.0;
    const yaw: f32 = 15.0 * math.pi / 180.0;

    const Re = Zigen.eulerToRotationMatrix(f32, .Z, .Y, .X, yaw, pitch, roll);
    std.debug.print("  Aircraft (yaw=15° pitch=5° roll=10°):\n", .{});
    row = 0;
    while (row < 3) : (row += 1) {
        std.debug.print("  │ {d:>8.5} {d:>8.5} {d:>8.5} │\n", .{ Re.at(row, 0), Re.at(row, 1), Re.at(row, 2) });
    }
    std.debug.print("\n", .{});

    // ── 5. SLERP ──────────────────────────────────────────────────────
    std.debug.print("━━ 5. SLERP Interpolation ━━━━━━━━━━━━━━━━━━━━━━━━━━━━\n\n", .{});

    const q_start = Quat.identity();
    const q_end = q90z;

    const ts = [_]f32{ 0.0, 0.25, 0.5, 0.75, 1.0 };
    for (ts) |t| {
        const q_interp = q_start.slerp(q_end, t);
        const p_interp = q_interp.rotate(point);
        std.debug.print("  t={d:.2}: [{d:>7.4}, {d:>7.4}, {d:>7.4}]\n", .{
            t, p_interp.at(0), p_interp.at(1), p_interp.at(2),
        });
    }
    std.debug.print("\n", .{});

    // ── 6. AngleAxis ──────────────────────────────────────────────────
    std.debug.print("━━ 6. Angle-Axis ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━\n\n", .{});

    const aa = Zigen.AngleAxis(f32).init(math.pi / 3.0, Vec3.fromArray(.{ 1, 1, 1 }).normalized());
    const R_aa = aa.toRotationMatrix();
    std.debug.print("  60° around [1,1,1] normalized:\n", .{});
    row = 0;
    while (row < 3) : (row += 1) {
        std.debug.print("  │ {d:>8.5} {d:>8.5} {d:>8.5} │\n", .{ R_aa.at(row, 0), R_aa.at(row, 1), R_aa.at(row, 2) });
    }
    std.debug.print("\n✅ Example complete!\n", .{});
}