511 lines
22 KiB
Zig
511 lines
22 KiB
Zig
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
|
||
};
|
||
}
|
||
};
|