Jolt quat transform

src/matrix.zig

@@ -273,5 +273,5 @@ test @"4x4" {
     _ = @"4x4"(f32).translate(.{ 1, 2, 3 });
     _ = @"4x4"(f32).scale(.{ 1, 2, 3 });
     _ = @"4x4"(f32).rotate(std.math.degreesToRadians(90), .{ 1, 2, 3 });
-    _ = @"4x4"(f32).identity.toQuaternion();
+    _ = @"4x4"(f32).identity.inverse();
 }

src/quaternion.zig

@@ -2,18 +2,22 @@ const std = @import("std");
 const vec = @import("vector.zig");
 const Mat4x4 = @import("matrix.zig").@"4x4";
 
-/// Quaternion using Hamiltonian (w-first) convention
+/// Quaternion using Jolt (x,y,z,w) convention
 pub fn Hamiltonian(T: type) type {
     return struct {
-        w: T,
         x: T,
         y: T,
         z: T,
+        w: T,
 
         pub const identity: @This() = .{ .x = 0, .y = 0, .z = 0, .w = 1 };
 
-        pub fn new(w: T, x: T, y: T, z: T) @This() {
-            return .{ .w = w, .x = x, .y = y, .z = z };
+        pub fn new(x: T, y: T, z: T, w: T) @This() {
+            return .{ .x = x, .y = y, .z = z, .w = w };
+        }
+
+        pub fn swapXW(q: @This()) @This() {
+            return .{ q.w, q.y, q.z, q.x };
         }
 
         pub fn mul(a: @This(), b: @This()) @This() {
@@ -24,24 +28,83 @@ pub fn Hamiltonian(T: type) type {
                 .w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
             };
         }
+        pub fn normalize(q: @This()) @This() {
+            const magnitude = @sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
+            if (magnitude == 0.0) return .{ .w = 1, .x = 0, .y = 0, .z = 0 };
 
+            const inv_mag = 1.0 / magnitude;
+            return .{
+                .w = q.w * inv_mag,
+                .x = q.x * inv_mag,
+                .y = q.y * inv_mag,
+                .z = q.z * inv_mag,
+            };
+        }
         pub fn conjugate(q: @This()) @This() {
-            return .{ .w = q.w, .x = -q.x, .y = -q.y, .z = -q.z };
+            return .{ .x = -q.x, .y = -q.y, .z = -q.z, .w = q.w };
         }
 
-        pub fn fromVec(v: @Vector(4, T)) @This() {
-            return .{ .w = v[0], .x = v[1], .y = v[2], .z = v[3] };
+        pub fn inverse(q: @This()) @This() {
+            const mag_sq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+            const inv = 1.0 / mag_sq;
+            return .{ .x = -q.x * inv, .y = -q.y * inv, .z = -q.z * inv, .w = q.w * inv };
         }
 
-        pub fn toVec(self: @This()) @Vector(4, T) {
-            return .{ self.w, self.x, self.y, self.z };
+        pub fn dot(a: @This(), b: @This()) T {
+            return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+        }
+
+        pub fn slerp(a: @This(), b_in: @This(), t: T) @This() {
+            var d = dot(a, b_in);
+            var b = b_in;
+            if (d < 0.0) {
+                b = .{ .x = -b.x, .y = -b.y, .z = -b.z, .w = -b.w };
+                d = -d;
+            }
+            if (d > 0.9995) {
+                return nlerp(a, b, t);
+            }
+            const theta = std.math.acos(d);
+            const sin_theta = @sin(theta);
+            const wa = @sin((1.0 - t) * theta) / sin_theta;
+            const wb = @sin(t * theta) / sin_theta;
+            return .{
+                .x = a.x * wa + b.x * wb,
+                .y = a.y * wa + b.y * wb,
+                .z = a.z * wa + b.z * wb,
+                .w = a.w * wa + b.w * wb,
+            };
+        }
+
+        pub fn nlerp(a: @This(), b_in: @This(), t: T) @This() {
+            var b = b_in;
+            if (dot(a, b) < 0.0) {
+                b = .{ .x = -b.x, .y = -b.y, .z = -b.z, .w = -b.w };
+            }
+            const one_minus_t = 1.0 - t;
+            return (@This(){
+                .x = a.x * one_minus_t + b.x * t,
+                .y = a.y * one_minus_t + b.y * t,
+                .z = a.z * one_minus_t + b.z * t,
+                .w = a.w * one_minus_t + b.w * t,
+            }).normalize();
+        }
+
+        pub fn rotateVec(self: @This(), v: @Vector(3, T)) @Vector(3, T) {
+            const qv = @Vector(3, T){ self.x, self.y, self.z };
+            const uv = vec.cross(qv, v);
+            const uuv = vec.cross(qv, uv);
+            return v + (uv * @as(@Vector(3, T), @splat(self.w)) + uuv) * @as(@Vector(3, T), @splat(2));
         }
-        pub fn fromVecReversed(v: @Vector(4, T)) @This() {
-            return .{ .w = v[0], .x = v[1], .y = v[2], .z = v[3] };
+
+        pub fn fromVec(v: @Vector(4, T)) @This() {
+            return .{ .x = v[0], .y = v[1], .z = v[2], .w = v[3] };
         }
-        pub fn toVecReversed(self: @This()) @Vector(4, T) {
+
+        pub fn toVec(self: @This()) @Vector(4, T) {
             return .{ self.x, self.y, self.z, self.w };
         }
+
         pub fn fromEuler(euler: @Vector(3, T)) @This() {
             const pitch, const yaw, const roll = euler;
 
@@ -95,42 +158,99 @@ pub fn Hamiltonian(T: type) type {
             };
         }
 
+        /// Shortest-arc rotation that maps `from_in` onto `to_in`.
+        pub fn fromTo(from_in: @Vector(3, T), to_in: @Vector(3, T)) @This() {
+            const from = vec.normalize(from_in);
+            const to = vec.normalize(to_in);
+            const d = vec.dot(from, to);
+
+            if (d >= 0.999999) return identity;
+            if (d <= -0.999999) {
+                var axis = vec.cross(from, @Vector(3, T){ 1, 0, 0 });
+                if (vec.length(axis) < 1.0e-6) {
+                    axis = vec.cross(from, @Vector(3, T){ 0, 1, 0 });
+                }
+                return angleAxis(std.math.pi, axis);
+            }
+
+            const axis = vec.cross(from, to);
+            const s = @sqrt((d) * 2.0);
+            const inv_s = 1.0 / s;
+            return .{
+                .w = s * 0.5,
+                .x = axis[0] * inv_s,
+                .y = axis[1] * inv_s,
+                .z = axis[2] * inv_s,
+            };
+        }
+
+        /// Orients local forward `(0, 0, -1)` toward `forward_in`, keeping local up
+        /// `(0, 1, 0)` aligned as closely as possible with `up_hint`.
+        pub fn lookAt(forward_in: @Vector(3, T), up_hint: @Vector(3, T)) @This() {
+            const f = vec.normalize(forward_in);
+
+            const proj = vec.dot(up_hint, f);
+            var u_raw = up_hint - f * @as(@Vector(3, T), @splat(proj));
+            if (vec.length(u_raw) < 1.0e-6) {
+                u_raw = if (@abs(f[0]) < 0.9)
+                    vec.cross(f, @Vector(3, T){ 1, 0, 0 })
+                else
+                    vec.cross(f, @Vector(3, T){ 0, 1, 0 });
+            }
+            const u = vec.normalize(u_raw);
+            const r = vec.cross(u, -f);
+            const back = -f;
+
+            const m: Mat4x4(T) = .new(.{
+                r[0],    r[1],    r[2],    0,
+                u[0],    u[1],    u[2],    0,
+                back[0], back[1], back[2], 0,
+                0,       0,       0,       1,
+            });
+            return fromMat4x4(m);
+        }
+
+        /// Extracts a quaternion from a column-major 4x4 rotation matrix.
+        /// Column-major: R[row, col] = d[row + col * 4]
         pub fn fromMat4x4(m: Mat4x4(T)) @This() {
-            const trace = m.d[0 * 4 + 0] + m.d[1 * 4 + 1] + m.d[2 * 4 + 2];
+            // R[0,0] = d[0], R[1,1] = d[5], R[2,2] = d[10]
+            const trace = m.d[0] + m.d[5] + m.d[10];
             var w: T = 0;
             var x: T = 0;
             var y: T = 0;
             var z: T = 0;
 
             if (trace > @as(T, 0)) {
-                const s = @sqrt(trace + @as(T, 1.0)) * @as(T, 2.0); // s = 4 * w
+                const s = @sqrt(trace + @as(T, 1.0)) * @as(T, 2.0);
                 w = 0.25 * s;
-                x = (m.d[2 * 4 + 1] - m.d[1 * 4 + 2]) / s;
-                y = (m.d[0 * 4 + 2] - m.d[2 * 4 + 0]) / s;
-                z = (m.d[1 * 4 + 0] - m.d[0 * 4 + 1]) / s;
-            } else if ((m.d[0 * 4 + 0] > m.d[1 * 4 + 1]) and (m.d[0 * 4 + 0] > m.d[2 * 4 + 2])) {
-                const s = @sqrt(@as(T, 1.0) + m.d[0 * 4 + 0] - m.d[1 * 4 + 1] - m.d[2 * 4 + 2]) * @as(T, 2.0); // s = 4 * x
-                w = (m.d[2 * 4 + 1] - m.d[1 * 4 + 2]) / s;
+                x = (m.d[6] - m.d[9]) / s; // R[2,1] - R[1,2]
+                y = (m.d[8] - m.d[2]) / s; // R[0,2] - R[2,0]
+                z = (m.d[1] - m.d[4]) / s; // R[1,0] - R[0,1]
+            } else if ((m.d[0] > m.d[5]) and (m.d[0] > m.d[10])) {
+                const s = @sqrt(@as(T, 1.0) + m.d[0] - m.d[5] - m.d[10]) * @as(T, 2.0);
+                w = (m.d[6] - m.d[9]) / s;
                 x = 0.25 * s;
-                y = (m.d[0 * 4 + 1] + m.d[1 * 4 + 0]) / s;
-                z = (m.d[0 * 4 + 2] + m.d[2 * 4 + 0]) / s;
-            } else if (m.d[1 * 4 + 1] > m.d[2 * 4 + 2]) {
-                const s = @sqrt(@as(T, 1.0) + m.d[1 * 4 + 1] - m.d[0 * 4 + 0] - m.d[2 * 4 + 2]) * @as(T, 2.0); // s = 4 * y
-                w = (m.d[0 * 4 + 2] - m.d[2 * 4 + 0]) / s;
-                x = (m.d[0 * 4 + 1] + m.d[1 * 4 + 0]) / s;
+                y = (m.d[4] + m.d[1]) / s; // R[0,1] + R[1,0]
+                z = (m.d[8] + m.d[2]) / s; // R[0,2] + R[2,0]
+            } else if (m.d[5] > m.d[10]) {
+                const s = @sqrt(@as(T, 1.0) + m.d[5] - m.d[0] - m.d[10]) * @as(T, 2.0);
+                w = (m.d[8] - m.d[2]) / s;
+                x = (m.d[4] + m.d[1]) / s;
                 y = 0.25 * s;
-                z = (m.d[1 * 4 + 2] + m.d[2 * 4 + 1]) / s;
+                z = (m.d[9] + m.d[6]) / s; // R[1,2] + R[2,1]
             } else {
-                const s = @sqrt(@as(T, 1.0) + m.d[2 * 4 + 2] - m.d[0 * 4 + 0] - m.d[1 * 4 + 1]) * @as(T, 2.0); // s = 4 * z
-                w = (m.d[1 * 4 + 0] - m.d[0 * 4 + 1]) / s;
-                x = (m.d[0 * 4 + 2] + m.d[2 * 4 + 0]) / s;
-                y = (m.d[1 * 4 + 2] + m.d[2 * 4 + 1]) / s;
+                const s = @sqrt(@as(T, 1.0) + m.d[10] - m.d[0] - m.d[5]) * @as(T, 2.0);
+                w = (m.d[1] - m.d[4]) / s;
+                x = (m.d[8] + m.d[2]) / s;
+                y = (m.d[9] + m.d[6]) / s;
                 z = 0.25 * s;
             }
 
-            return .{ .w = w, .x = x, .y = y, .z = z };
+            return .{ .x = x, .y = y, .z = z, .w = w };
         }
 
+        /// Converts quaternion to a column-major 4x4 rotation matrix.
+        /// Each group of 4 values is one column.
         pub fn toMat4x4(self: @This()) Mat4x4(T) {
             const xx = self.x * self.x;
             const yy = self.y * self.y;
@@ -143,9 +263,9 @@ pub fn Hamiltonian(T: type) type {
             const wz = self.w * self.z;
 
             return .new(.{
-                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,
+                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,
             });
         }

src/root.zig

@@ -23,25 +23,38 @@ pub const Rotor = @import("rotors.zig");
 pub fn Transform3D(T: type) type {
     return struct {
         position: Vec3(T) = @splat(0),
-        rotation: Vec3(T) = @splat(0),
+        rotation: Quat(T) = Quat(T).identity,
         scale: Vec3(T) = @splat(1),
 
         pub fn toMat4x4(self: @This()) Mat4x4(T) {
             return Mat4x4(T)
                 .translate(self.position)
-                .mul(.rotate(std.math.degreesToRadians(self.rotation[0]), .{ 1, 0, 0 }))
-                .mul(.rotate(std.math.degreesToRadians(self.rotation[1]), .{ 0, 1, 0 }))
-                .mul(.rotate(std.math.degreesToRadians(self.rotation[2]), .{ 0, 0, 1 }))
+                .mul(self.rotation.toMat4x4())
                 .mul(.scale(self.scale));
         }
 
         pub fn fromMat4x4(m: Mat4x4(T)) @This() {
             return .{
                 .position = m.vecPosition(),
-                .rotation = m.vecRotation(),
+                .rotation = Quat(T).fromMat4x4(m),
                 .scale = m.vecScale(),
             };
         }
+
+        pub fn forward(self: @This()) Vec3(T) {
+            const f: Vec3(T) = .{ 0, 0, -1 };
+            return vec.normalize(self.rotation.rotateVec(f));
+        }
+
+        pub fn right(self: *@This()) Vec3(T) {
+            const r: Vec3(f32) = .{ 1, 0, 0 };
+            return vec.normalize(self.rotation.rotateVec(r));
+        }
+
+        pub fn up(self: *@This()) Vec3(T) {
+            const u: Vec3(f32) = .{ 0, 1, 0 };
+            return vec.normalize(self.rotation.rotateVec(u));
+        }
     };
 }
 
@@ -72,8 +85,15 @@ pub fn Transform2D(T: type) type {
 }
 
 test Transform3D {
-    var transform: Transform3D(f32) = .{ .position = .{ 10.0, 20.0, 30.0 }, .rotation = .{ 180.0, 360, 270 }, .scale = .{ 1.0, 2.0, 3.0 } };
-    try std.testing.expect(std.meta.eql(Transform3D(f32).fromMat4x4(transform.toMat4x4()), transform));
+    const rotation = Quat(f32).angleAxis(std.math.pi / 4.0, .{ 0, 1, 0 });
+    var transform: Transform3D(f32) = .{ .position = .{ 10.0, 20.0, 30.0 }, .rotation = rotation, .scale = .{ 1.0, 2.0, 3.0 } };
+    const reconstructed = Transform3D(f32).fromMat4x4(transform.toMat4x4());
+    try std.testing.expectApproxEqAbs(reconstructed.position[0], transform.position[0], 0.001);
+    try std.testing.expectApproxEqAbs(reconstructed.position[1], transform.position[1], 0.001);
+    try std.testing.expectApproxEqAbs(reconstructed.position[2], transform.position[2], 0.001);
+    try std.testing.expectApproxEqAbs(reconstructed.scale[0], transform.scale[0], 0.001);
+    try std.testing.expectApproxEqAbs(reconstructed.scale[1], transform.scale[1], 0.001);
+    try std.testing.expectApproxEqAbs(reconstructed.scale[2], transform.scale[2], 0.001);
 }
 
 test Transform2D {

src/vector.zig

@@ -115,13 +115,10 @@ pub fn forwardFromEuler(rotation: anytype) @TypeOf(rotation) {
     const len, _ = info(@TypeOf(rotation));
     if (len != 3) @compileError("forwardFromEuler() only supports vec3");
 
-    const pitch = std.math.degreesToRadians(rotation[0]); // rotation around X
-    const yaw = std.math.degreesToRadians(rotation[1]); // rotation around Y
-
     return .{
-        std.math.sin(yaw) * std.math.cos(pitch), // X
-        -std.math.sin(pitch), // Y
-        -std.math.cos(yaw) * std.math.cos(pitch), // Z
+        std.math.sin(rotation[1]) * std.math.cos(rotation[0]), // X
+        std.math.sin(rotation[0]), // Y
+        -std.math.cos(rotation[1]) * std.math.cos(rotation[0]), // Z
     };
 }