Quaternion rotation!

src/graphics/transform.zig

@@ -1,47 +1,124 @@
 const std = @import("std");
 const Transform = @This();
 
-// TODO: Rotation
+// TODO: Scale
 
-position: @Vector(3, f32) = @splat(0.0),
-rotation: @Vector(3, f32) = @splat(0.0),
-scale: @Vector(3, f32) = @splat(1.0),
+pub const TMatrix = @Vector(16, f32);
 
-pub fn matrix(transform: Transform) @Vector(16, f32) {
+pub const Position = @Vector(3, f32);
+pub const Rotation = @Vector(4, f32);
+pub const Scale = @Vector(3, f32);
+
+position: Position = @splat(0.0),
+rotation: Rotation = .{ 1.0, 0.0, 0.0, 0.0 },
+scale: Scale = @splat(1.0),
+
+pub fn matrix(transform: Transform) TMatrix {
+    const r = rotationMatrix(transform.rotation);
     return .{
-        1.0, 0.0, 0.0, transform.position[0],
-        0.0, 1.0, 0.0, transform.position[1],
-        0.0, 0.0, 1.0, transform.position[2],
-        0.0, 0.0, 0.0, 1.0,
+        r[0], r[1], r[2], transform.position[0],
+        r[3], r[4], r[5], transform.position[1],
+        r[6], r[7], r[8], transform.position[2],
+        0.0,  0.0,  0.0,  1.0,
     };
 }
 
-pub fn inverse(transform: Transform) @Vector(16, f32) {
+pub fn inverse(transform: Transform) TMatrix {
     // TODO: Could we just translate, rotate and scale back instead of relying on matrix math?
     return invertMatrix(transform.matrix());
 }
 
-fn invertMatrix(a: @Vector(16, f32)) @Vector(16, f32) {
+pub fn rotate(transform: *Transform, rotation: Rotation) void {
+    transform.rotation = normalizeRotation(combineRotations(transform.rotation, rotation));
+}
+
+pub fn normalizeRotation(r: Rotation) Rotation {
+    @setFloatMode(.optimized);
+
+    const length = @sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2] + r[3] * r[3]);
+    return r / @as(Rotation, @splat(length));
+}
+
+pub fn combineRotations(a: Rotation, b: Rotation) Rotation {
+    @setFloatMode(.optimized);
+
+    return .{
+        a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
+        a[1] * b[0] + a[0] * b[1] + a[3] * b[2] - a[2] * b[3],
+        a[2] * b[0] + a[0] * b[2] + a[1] * b[3] - a[3] * b[1],
+        a[3] * b[0] + a[0] * b[3] + a[2] * b[1] - a[1] * b[2],
+    };
+}
+
+pub fn rotationByAxis(axis: Position, rotation: f32) Rotation {
+    @setFloatMode(.optimized);
+
+    const cos = std.math.cos(rotation * 0.5);
+    const sin = std.math.sin(rotation * 0.5);
+
+    return .{ cos, sin * axis[0], sin * axis[1], sin * axis[2] };
+}
+
+pub fn extractNormal(vector: Position) struct { Position, f32 } {
+    @setFloatMode(.optimized);
+
+    const length = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2];
+    return .{ vector / @as(Position, @splat(length)), length };
+}
+
+fn rotationMatrix(quaternion: Rotation) @Vector(9, f32) {
+    @setFloatMode(.optimized);
+
+    const a = quaternion[0];
+    const b = quaternion[1];
+    const c = quaternion[2];
+    const d = quaternion[3];
+
+    const s = 2.0 / (a * a + b * b + c * c + d * d);
+    const bs = b * s;
+    const cs = c * s;
+    const ds = d * s;
+
+    const ab = a * bs;
+    const ac = a * cs;
+    const ad = a * ds;
+    const bb = b * bs;
+    const bc = b * cs;
+    const bd = b * ds;
+    const cc = c * cs;
+    const cd = c * ds;
+    const dd = d * ds;
+
+    return .{
+        1 - cc - dd, bc - ad,     bd + ac,
+        bc + ad,     1 - bb - dd, cd - ab,
+        bd - ac,     cd + ab,     1 - bb - cc,
+    };
+}
+
+fn invertMatrix(a: TMatrix) TMatrix {
+    @setFloatMode(.optimized);
+
     const MOD: f32 = 1.0 / 16.0;
-    const ID = @Vector(16, f32){
+    const ID = TMatrix{
         1, 0, 0, 0,
         0, 1, 0, 0,
         0, 0, 1, 0,
         0, 0, 0, 1,
     };
-    var p = ID - @as(@Vector(16, f32), @splat(MOD)) * a;
+    var p = ID - @as(TMatrix, @splat(MOD)) * a;
     var output = ID + p;
     inline for (0..8) |_| {
         p = multiplyMatrix(p, p);
         output = multiplyMatrix(output, ID + p);
     }
-    return output * @as(@Vector(16, f32), @splat(MOD));
+    return output * @as(TMatrix, @splat(MOD));
 }
 
-pub fn multiplyMatrix(a: @Vector(16, f32), b: @Vector(16, f32)) @Vector(16, f32) {
-    var output: @Vector(16, f32) = [1]f32{0.0} ** 16;
-
+pub fn multiplyMatrix(a: TMatrix, b: TMatrix) TMatrix {
     @setFloatMode(.optimized);
+
+    var output: TMatrix = [1]f32{0.0} ** 16;
     for (0..4) |row| {
         for (0..4) |col| {
             for (0..4) |i| {