const std = @import("std"); const vm = @import("../root.zig"); pub const Matrix4x4 = extern struct { // zig fmt: off ix: f32, iy: f32, iz: f32, iw: f32, jx: f32, jy: f32, jz: f32, jw: f32, kx: f32, ky: f32, kz: f32, kw: f32, tx: f32, ty: f32, tz: f32, tw: f32, // zig fmt: on pub const Array = [16]f32; pub const identity = init( // zig fmt: off 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, // zig fmt: on ); // --- INIT ---------------------------------------------------------------- pub inline fn init( // zig fmt: off ix: f32, iy: f32, iz: f32, iw: f32, jx: f32, jy: f32, jz: f32, jw: f32, kx: f32, ky: f32, kz: f32, kw: f32, tx: f32, ty: f32, tz: f32, tw: f32, // zig fmt: on ) Matrix4x4 { return .{ // zig fmt: off .ix = ix, .iy = iy, .iz = iz, .iw = iw, .jx = jx, .jy = jy, .jz = jz, .jw = jw, .kx = kx, .ky = ky, .kz = kz, .kw = kw, .tx = tx, .ty = ty, .tz = tz, .tw = tw, // zig fmt: on }; } pub inline fn initVersors(i: vm.Vector4, j: vm.Vector4, k: vm.Vector4, t: vm.Vector4) Matrix4x4 { return .{ // zig fmt: off .ix = i.x, .iy = i.y, .iz = i.z, .iw = i.w, .jx = j.x, .jy = j.y, .jz = j.z, .jw = j.w, .kx = k.x, .ky = k.y, .kz = k.z, .kw = k.w, .tx = t.x, .ty = t.y, .tz = t.z, .tw = t.w, // zig fmt: on }; } pub inline fn initTranslation(t: vm.Vector3) Matrix4x4 { return .{ // zig fmt: off .ix = 1, .iy = 0, .iz = 0, .iw = 0, .jx = 0, .jy = 1, .jz = 0, .jw = 0, .kx = 0, .ky = 0, .kz = 1, .kw = 0, .tx = t.x, .ty = t.y, .tz = t.z, .tw = 1, // zig fmt: on }; } pub inline fn initRotation(q: vm.Quaternion) Matrix4x4 { const xx = q.x * q.x; const xy = q.x * q.y; const xz = q.x * q.z; const xw = q.x * q.w; const yy = q.y * q.y; const yz = q.y * q.z; const yw = q.y * q.w; const zz = q.z * q.z; const zw = q.z * q.w; return .{ // zig fmt: off .ix = 1 - 2 * (yy + zz), .iy = 2 * (xy + zw), .iz = 2 * (xz - yw), .iw = 0, .jx = 2 * (xy - zw), .jy = 1 - 2 * (xx + zz), .jz = 2 * (yz + xw), .jw = 0, .kx = 2 * (xz + yw), .ky = 2 * (yz - xw), .kz = 1 - 2 * (xx + yy), .kw = 0, .tx = 0, .ty = 0, .tz = 0, .tw = 1, // zig fmt: on }; } test initRotation { const q = vm.Quaternion.initRotation(.XY, 0.5); const m = initRotation(q); try std.testing.expectEqual(init( // zig fmt: off -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, // zig fmt: on ), m); } pub inline fn initScale(s: vm.Vector3) Matrix4x4 { return .{ // zig fmt: off .ix = s.x, .iy = 0, .iz = 0, .iw = 0, .jx = 0, .jy = s.y, .jz = 0, .jw = 0, .kx = 0, .ky = 0, .kz = s.z, .kw = 0, .tx = 0, .ty = 0, .tz = 0, .tw = 1, // zig fmt: on }; } pub inline fn initTranslationRotation(t: vm.Vector3, q: vm.Quaternion) Matrix4x4 { const xx = q.x * q.x; const xy = q.x * q.y; const xz = q.x * q.z; const xw = q.x * q.w; const yy = q.y * q.y; const yz = q.y * q.z; const yw = q.y * q.w; const zz = q.z * q.z; const zw = q.z * q.w; return .{ // zig fmt: off .ix = 1 - 2 * (yy + zz), .iy = 2 * (xy + zw), .iz = 2 * (xz - yw), .iw = 0, .jx = 2 * (xy - zw), .jy = 1 - 2 * (xx + zz), .jz = 2 * (yz + xw), .jw = 0, .kx = 2 * (xz + yw), .ky = 2 * (yz - xw), .kz = 1 - 2 * (xx + yy), .kw = 0, .tx = t.x, .ty = t.y, .tz = t.z, .tw = 1, // zig fmt: on }; } pub inline fn initTranslationScale(t: vm.Vector3, s: vm.Vector3) Matrix4x4 { return .{ // zig fmt: off .ix = s.x, .iy = 0, .iz = 0, .iw = 0, .jx = 0, .jy = s.y, .jz = 0, .jw = 0, .kx = 0, .ky = 0, .kz = s.z, .kw = 0, .tx = t.x, .ty = t.y, .tz = t.z, .tw = 1, // zig fmt: on }; } pub inline fn initTranslationRotationScale(t: vm.Vector3, q: vm.Quaternion, s: vm.Vector3) Matrix4x4 { const xx = q.x * q.x; const xy = q.x * q.y; const xz = q.x * q.z; const xw = q.x * q.w; const yy = q.y * q.y; const yz = q.y * q.z; const yw = q.y * q.w; const zz = q.z * q.z; const zw = q.z * q.w; return .{ // zig fmt: off .ix = s.x * (1 - 2 * (yy + zz)), .iy = s.x * 2 * (xy + zw), .iz = s.x * 2 * (xz - yw), .iw = 0, .jx = s.y * 2 * (xy - zw), .jy = s.y * (1 - 2 * (xx + zz)), .jz = s.y * 2 * (yz + xw), .jw = 0, .kx = s.z * 2 * (xz + yw), .ky = s.z * 2 * (yz - xw), .kz = s.z * (1 - 2 * (xx + yy)), .kw = 0, .tx = t.x, .ty = t.y, .tz = t.z, .tw = 1, // zig fmt: on }; } pub inline fn initArray(array: Array) Matrix4x4 { return @bitCast(array); } // --- CONVERSION ---------------------------------------------------------- pub inline fn asArray(self: Matrix4x4) Array { return @bitCast(self); } // --- ACCESSORS ----------------------------------------------------------- pub inline fn getIVersor(self: Matrix4x4) vm.Vector4 { return .{ .x = self.ix, .y = self.iy, .z = self.iz, .w = self.iw }; } pub inline fn getJVersor(self: Matrix4x4) vm.Vector4 { return .{ .x = self.jx, .y = self.jy, .z = self.jz, .w = self.jw }; } pub inline fn getKVersor(self: Matrix4x4) vm.Vector4 { return .{ .x = self.kx, .y = self.ky, .z = self.kz, .w = self.kw }; } pub inline fn getTranslationVector(self: Matrix4x4) vm.Vector4 { return .{ .x = self.tx, .y = self.ty, .z = self.tz, .w = self.tw }; } // --- COMPONENT-WISE ------------------------------------------------------ pub inline fn add(self: Matrix4x4, other: Matrix4x4) Matrix4x4 { return .{ .ix = self.ix + other.ix, .iy = self.iy + other.iy, .iz = self.iz + other.iz, .iw = self.iw + other.iw, .jx = self.jx + other.jx, .jy = self.jy + other.jy, .jz = self.jz + other.jz, .jw = self.jw + other.jw, .kx = self.kx + other.kx, .ky = self.ky + other.ky, .kz = self.kz + other.kz, .kw = self.kw + other.kw, .tx = self.tx + other.tx, .ty = self.ty + other.ty, .tz = self.tz + other.tz, .tw = self.tw + other.tw, }; } pub inline fn sub(self: Matrix4x4, other: Matrix4x4) Matrix4x4 { return .{ .ix = self.ix - other.ix, .iy = self.iy - other.iy, .iz = self.iz - other.iz, .iw = self.iw - other.iw, .jx = self.jx - other.jx, .jy = self.jy - other.jy, .jz = self.jz - other.jz, .jw = self.jw - other.jw, .kx = self.kx - other.kx, .ky = self.ky - other.ky, .kz = self.kz - other.kz, .kw = self.kw - other.kw, .tx = self.tx - other.tx, .ty = self.ty - other.ty, .tz = self.tz - other.tz, .tw = self.tw - other.tw, }; } pub inline fn mulScalar(self: Matrix4x4, scalar: f32) Matrix4x4 { return .{ .ix = self.ix * scalar, .iy = self.iy * scalar, .iz = self.iz * scalar, .iw = self.iw * scalar, .jx = self.jx * scalar, .jy = self.jy * scalar, .jz = self.jz * scalar, .jw = self.jw * scalar, .kx = self.kx * scalar, .ky = self.ky * scalar, .kz = self.kz * scalar, .kw = self.kw * scalar, .tx = self.tx * scalar, .ty = self.ty * scalar, .tz = self.tz * scalar, .tw = self.tw * scalar, }; } pub inline fn divScalar(self: Matrix4x4, scalar: f32) Matrix4x4 { return .{ .ix = self.ix / scalar, .iy = self.iy / scalar, .iz = self.iz / scalar, .iw = self.iw / scalar, .jx = self.jx / scalar, .jy = self.jy / scalar, .jz = self.jz / scalar, .jw = self.jw / scalar, .kx = self.kx / scalar, .ky = self.ky / scalar, .kz = self.kz / scalar, .kw = self.kw / scalar, .tx = self.tx / scalar, .ty = self.ty / scalar, .tz = self.tz / scalar, .tw = self.tw / scalar, }; } // --- COMPOSE ------------------------------------------------------------- /// The caller asserts that W rows of all matrices are equal to [0 0 0 1]. pub fn mulMatrixAffine(self: Matrix4x4, other: Matrix4x4) Matrix4x4 { std.debug.assert(self.iw == 0 and self.jw == 0 and self.kw == 0 and self.tw == 1); std.debug.assert(other.iw == 0 and other.jw == 0 and other.kw == 0 and other.tw == 1); return .{ .ix = other.ix * self.ix + other.iy * self.jx + other.iz * self.kx, .iy = other.ix * self.iy + other.iy * self.jy + other.iz * self.ky, .iz = other.ix * self.iz + other.iy * self.jz + other.iz * self.kz, .iw = 0, .jx = other.jx * self.ix + other.jy * self.jx + other.jz * self.kx, .jy = other.jx * self.iy + other.jy * self.jy + other.jz * self.ky, .jz = other.jx * self.iz + other.jy * self.jz + other.jz * self.kz, .jw = 0, .kx = other.kx * self.ix + other.ky * self.jx + other.kz * self.kx, .ky = other.kx * self.iy + other.ky * self.jy + other.kz * self.ky, .kz = other.kx * self.iz + other.ky * self.jz + other.kz * self.kz, .kw = 0, .tx = other.tx * self.ix + other.ty * self.jx + other.tz * self.kx + self.tx, .ty = other.tx * self.iy + other.ty * self.jy + other.tz * self.ky + self.ty, .tz = other.tx * self.iz + other.ty * self.jz + other.tz * self.kz + self.tz, .tw = 1, }; } pub fn mulMatrixFull(self: Matrix4x4, other: Matrix4x4) Matrix4x4 { return .{ .ix = other.ix * self.ix + other.iy * self.jx + other.iz * self.kx + other.iw * self.tx, .iy = other.ix * self.iy + other.iy * self.jy + other.iz * self.ky + other.iw * self.ty, .iz = other.ix * self.iz + other.iy * self.jz + other.iz * self.kz + other.iw * self.tz, .iw = other.ix * self.iw + other.iy * self.jw + other.iz * self.kw + other.iw * self.tw, .jx = other.jx * self.ix + other.jy * self.jx + other.jz * self.kx + other.jw * self.tx, .jy = other.jx * self.iy + other.jy * self.jy + other.jz * self.ky + other.jw * self.ty, .jz = other.jx * self.iz + other.jy * self.jz + other.jz * self.kz + other.jw * self.tz, .jw = other.jx * self.iw + other.jy * self.jw + other.jz * self.kw + other.jw * self.tw, .kx = other.kx * self.ix + other.ky * self.jx + other.kz * self.kx + other.kw * self.tx, .ky = other.kx * self.iy + other.ky * self.jy + other.kz * self.ky + other.kw * self.ty, .kz = other.kx * self.iz + other.ky * self.jz + other.kz * self.kz + other.kw * self.tz, .kw = other.kx * self.iw + other.ky * self.jw + other.kz * self.kw + other.kw * self.tw, .tx = other.tx * self.ix + other.ty * self.jx + other.tz * self.kx + other.tw * self.tx, .ty = other.tx * self.iy + other.ty * self.jy + other.tz * self.ky + other.tw * self.ty, .tz = other.tx * self.iz + other.ty * self.jz + other.tz * self.kz + other.tw * self.tz, .tw = other.tx * self.iw + other.ty * self.jw + other.tz * self.kw + other.tw * self.tw, }; } // INVERSION DERIVATION (affine and orthonormal case) // // We assume the matrix looks like so: // // ⎡ix jx kx tx⎤ // ⎢iy jy ky ty⎥ // A = ⎢iz jz kz tz⎥ // ⎣ 0 0 0 1⎦ // // Then: // ⎡ix jx kx⎤ // det A = det ⎢iy jy ky⎥ // ⎣iz jz kz⎦ // // ⎡ ⎡jy ky ty⎤ ⎡iy ky ty⎤ ⎡iy jy ty⎤ ⎡iy jy ky⎤⎤T // ⎢ det ⎢jz kz tz⎥ −det ⎢iz kz tz⎥ det ⎢iz jz tz⎥ −det ⎢iz jz kz⎥⎥ // ⎢ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 0⎦⎥ // ⎢ ⎡jx kx tx⎤ ⎡ix kx tx⎤ ⎡ix jx tx⎤ ⎡ix jx kx⎤⎥ // ⎢−det ⎢jz kz tz⎥ det ⎢iz kz tz⎥ −det ⎢iz jz tz⎥ det ⎢iz jz kz⎥⎥ // 1 ⎢ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 0⎦⎥ // inv A = ⎯⎯⎯⎯⎯ ⎢ ⎡jx kx tx⎤ ⎡ix kx tx⎤ ⎡ix jx tx⎤ ⎡ix jx kx⎤⎥ // det A ⎢ det ⎢jy ky ty⎥ −det ⎢iy ky ty⎥ det ⎢iy jy ty⎥ −det ⎢iy jy ky⎥⎥ // ⎢ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 1⎦ ⎣ 0 0 0⎦⎥ // ⎢ ⎡jx kx tx⎤ ⎡ix kx tx⎤ ⎡ix jx tx⎤ ⎡ix jx kx⎤⎥ // ⎢−det ⎢jy ky ty⎥ det ⎢iy ky ty⎥ −det ⎢iy jy ty⎥ det ⎢iy jy ky⎥⎥ // ⎣ ⎣jz kz tz⎦ ⎣iz kz tz⎦ ⎣iz jz tz⎦ ⎣iz jz kz⎦⎦ // // ⎡ ⎡kx jx tx⎤⎤ // ⎢jy · kz − ky · jz kx · jz − jx · kz jx · ky − kx · jy det ⎢ky jy ty⎥⎥ // ⎢ ⎣kz jz tz⎦⎥ // ⎢ ⎡ix kx tx⎤⎥ // 1 ⎢ky · iz − iy · kz ix · kz − kx · iz kx · iy − ix · ky det ⎢iy ky ty⎥⎥ // inv A = ⎯⎯⎯⎯⎯ ⎢ ⎣iz kz tz⎦⎥ // det A ⎢ ⎡jx ix tx⎤⎥ // ⎢iy · jz − jy · iz jx · iz − ix · jz ix · jy − jx · iy det ⎢jy iy ty⎥⎥ // ⎢ ⎣jz iz tz⎦⎥ // ⎣ 0 0 0 det A ⎦ // // After multiplying by 1 / (det A), the fourth row becomes [0 0 0 1]. // When A is orthonormal, we can assume det A = 1. pub fn inverseOrthonormal(self: Matrix4x4) Matrix4x4 { std.debug.assert(self.iw == 0 and self.jw == 0 and self.kw == 0 and self.tw == 1); const ix = self.ix; const iy = self.jx; const iz = self.kx; const jx = self.iy; const jy = self.jy; const jz = self.ky; const kx = self.iz; const ky = self.jz; const kz = self.kz; return .{ .ix = ix, .iy = iy, .iz = iz, .iw = 0, .jx = jx, .jy = jy, .jz = jz, .jw = 0, .kx = kx, .ky = ky, .kz = kz, .kw = 0, .tx = -(self.tx * ix + self.ty * jx + self.tz * kx), .ty = -(self.tx * iy + self.ty * jy + self.tz * ky), .tz = -(self.tx * iz + self.ty * jz + self.tz * kz), .tw = 1, }; } pub fn inverseAffine(self: Matrix4x4) Matrix4x4 { std.debug.assert(self.iw == 0 and self.jw == 0 and self.kw == 0 and self.tw == 1); const inv_det = 1.0 / ( // zig fmt: off self.ix * (self.jy * self.kz - self.ky * self.jz) + self.jx * (self.ky * self.iz - self.iy * self.kz) + self.kx * (self.iy * self.jz - self.iy * self.jz) // zig fmt: on ); const ix = self.jy * self.kz - self.ky * self.jz; const iy = self.ky * self.iz - self.iy * self.kz; const iz = self.iy * self.jz - self.jy * self.iz; const jx = self.kx * self.jz - self.jx * self.kz; const jy = self.ix * self.kz - self.kx * self.iz; const jz = self.jx * self.iz - self.ix * self.jz; const kx = self.jx * self.ky - self.kx * self.jy; const ky = self.kx * self.iy - self.ix * self.ky; const kz = self.ix * self.jy - self.jx * self.iy; return .{ .ix = inv_det * ix, .iy = inv_det * iy, .iz = inv_det * iz, .iw = 0, .jx = inv_det * jx, .jy = inv_det * jy, .jz = inv_det * jz, .jw = 0, .kx = inv_det * kx, .ky = inv_det * ky, .kz = inv_det * kz, .kw = 0, .tx = -inv_det * (self.tx * ix + self.ty * jx + self.tz * kx), .ty = -inv_det * (self.tx * iy + self.ty * jy + self.tz * ky), .tz = -inv_det * (self.tx * iz + self.ty * jz + self.tz * kz), .tw = 1, }; } // DETERMINANT DERIVATION (full case) // // ⎡ix jx kx tx⎤ // ⎢iy jy ky ty⎥ // det A = det ⎢iz jz kz tz⎥ // ⎣iw jw kw tw⎦ // // ⎡jy ky ty⎤ ⎡iy ky ty⎤ ⎡iy jy ty⎤ ⎡iy jy ky⎤ // det A = ix · det ⎢jz kz tz⎥ − jx · det ⎢iz kz tz⎥ + kx · det ⎢iz jz tz⎥ − tx · det ⎢iz jz kz⎥ // ⎣jw kw tw⎦ ⎣iw kw tw⎦ ⎣iw jw tw⎦ ⎣iw jw kw⎦ // // ⎛ ⎡kz tz⎤ ⎡jz tz⎤ ⎡jz kz⎤⎞ // det A = ix · ⎝jy · det ⎣kw tw⎦ − ky · det ⎣jw tw⎦ + ty · det ⎣jw kw⎦⎠ + // // ⎛ ⎡kz tz⎤ ⎡iz tz⎤ ⎡iz kz⎤⎞ // − jx · ⎝iy · det ⎣kw tw⎦ − ky · det ⎣iw tw⎦ + ty · det ⎣iw kw⎦⎠ + // // ⎛ ⎡jz tz⎤ ⎡iz tz⎤ ⎡iz jz⎤⎞ // + kx · ⎝iy · det ⎣jw tw⎦ − jy · det ⎣iw tw⎦ + ty · det ⎣iw jw⎦⎠ + // // ⎛ ⎡jz kz⎤ ⎡iz kz⎤ ⎡iz jz⎤⎞ // − tx · ⎝iy · det ⎣jw kw⎦ − jy · det ⎣iw kw⎦ + ky · det ⎣iw jw⎦⎠ pub fn inverseFull(self: Matrix4x4) Matrix4x4 { const iy_jw = self.iy * self.jw - self.jy * self.iw; const iy_jz = self.iy * self.jz - self.jy * self.iz; const iy_kz = self.iy * self.kz - self.ky * self.iz; const iy_kw = self.iy * self.kw - self.ky * self.iw; const iy_tz = self.iy * self.tz - self.ty * self.iz; const iy_tw = self.iy * self.tw - self.ty * self.iw; const iz_jw = self.iz * self.jw - self.jz * self.iw; const iz_kw = self.iz * self.kw - self.kz * self.iw; const iz_tw = self.iz * self.tw - self.tz * self.iw; const jy_kz = self.jy * self.kz - self.ky * self.jz; const jy_kw = self.jy * self.kw - self.ky * self.jw; const jy_tw = self.jy * self.tw - self.ty * self.jw; const jy_tz = self.jy * self.tz - self.ty * self.jz; const jz_kw = self.jz * self.kw - self.kz * self.jw; const jz_tw = self.jz * self.tw - self.tz * self.jw; const ky_tz = self.ky * self.tz - self.ty * self.kz; const ky_tw = self.ky * self.tw - self.ty * self.kw; const kz_tw = self.kz * self.tw - self.tz * self.kw; const det_ix = self.jy * kz_tw - self.ky * jz_tw + self.ty * jz_kw; const det_jx = self.iy * kz_tw - self.ky * iz_tw + self.ty * iz_kw; const det_kx = self.iy * jz_tw - self.jy * iz_tw + self.ty * iz_jw; const det_tx = self.iy * jz_kw - self.jy * iz_kw + self.ky * iz_jw; const det_iy = self.jx * kz_tw - self.kx * jz_tw + self.tx * jz_kw; const det_jy = self.ix * kz_tw - self.kx * iz_tw + self.tx * iz_kw; const det_ky = self.ix * jz_tw - self.jz * iz_tw + self.tx * iz_jw; const det_ty = self.ix * jz_kw - self.jx * iz_kw + self.kx * iz_jw; const det_iz = self.jx * ky_tw - self.kx * jy_tw + self.tx * jy_kw; const det_jz = self.ix * ky_tw - self.kx * iy_tw + self.tx * iy_kw; const det_kz = self.ix * jy_tw - self.jx * iy_tw + self.tx * iy_jw; const det_tz = self.ix * jy_kw - self.jx * iy_kw + self.kx * iy_jw; const det_iw = self.jx * ky_tz - self.kx * jy_tz + self.tx * jy_kz; const det_jw = self.ix * ky_tz - self.kx * iy_tz + self.tx * iy_kz; const det_kw = self.ix * jy_tz - self.jx * iy_tz + self.tx * iy_jz; const det_tw = self.ix * jy_kz - self.jx * iy_kz + self.kx * iy_jz; const det = self.ix * det_ix - self.jx * det_jx + self.kx * det_kx - self.tx * det_tx; const inv_det = 1.0 / det; return .{ // zig fmt: off .ix = inv_det * det_ix, .iy = -inv_det * det_jx, .iz = inv_det * det_kx, .iw = -inv_det * det_tx, .jx = -inv_det * det_iy, .jy = inv_det * det_jy, .jz = -inv_det * det_ky, .jw = inv_det * det_ty, .kx = inv_det * det_iz, .ky = -inv_det * det_jz, .kz = inv_det * det_kz, .kw = -inv_det * det_tz, .tx = -inv_det * det_iw, .ty = inv_det * det_jw, .tz = -inv_det * det_kw, .tw = inv_det * det_tw, // zig fmt: on }; } test "refAllDecls" { std.testing.refAllDecls(@This()); } };