add MatN invert(), apply()
s-ol
1 year, 11 months ago
| 0 | 0 | const std = @import("std"); |
| 1 | 1 | const math = std.math; |
| 2 | 2 | const testing = std.testing; |
| 3 | const Vector = @import("vector.zig").Vector; | |
| 3 | 4 | |
| 4 | 5 | pub fn Matrix(comptime N: usize) type { |
| 5 | 6 | return packed struct { |
| 59 | 60 | } |
| 60 | 61 | |
| 61 | 62 | return result; |
| 63 | } | |
| 64 | ||
| 65 | pub fn invert(src: Self) ?Self { | |
| 66 | // https://github.com/stackgl/gl-mat4/blob/master/invert.js | |
| 67 | const a = @bitCast([16]f32, src.values); | |
| 68 | ||
| 69 | const a00 = a[0]; | |
| 70 | const a01 = a[1]; | |
| 71 | const a02 = a[2]; | |
| 72 | const a03 = a[3]; | |
| 73 | const a10 = a[4]; | |
| 74 | const a11 = a[5]; | |
| 75 | const a12 = a[6]; | |
| 76 | const a13 = a[7]; | |
| 77 | const a20 = a[8]; | |
| 78 | const a21 = a[9]; | |
| 79 | const a22 = a[10]; | |
| 80 | const a23 = a[11]; | |
| 81 | const a30 = a[12]; | |
| 82 | const a31 = a[13]; | |
| 83 | const a32 = a[14]; | |
| 84 | const a33 = a[15]; | |
| 85 | ||
| 86 | const b00 = a00 * a11 - a01 * a10; | |
| 87 | const b01 = a00 * a12 - a02 * a10; | |
| 88 | const b02 = a00 * a13 - a03 * a10; | |
| 89 | const b03 = a01 * a12 - a02 * a11; | |
| 90 | const b04 = a01 * a13 - a03 * a11; | |
| 91 | const b05 = a02 * a13 - a03 * a12; | |
| 92 | const b06 = a20 * a31 - a21 * a30; | |
| 93 | const b07 = a20 * a32 - a22 * a30; | |
| 94 | const b08 = a20 * a33 - a23 * a30; | |
| 95 | const b09 = a21 * a32 - a22 * a31; | |
| 96 | const b10 = a21 * a33 - a23 * a31; | |
| 97 | const b11 = a22 * a33 - a23 * a32; | |
| 98 | ||
| 99 | // Calculate the determinant | |
| 100 | var det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06; | |
| 101 | ||
| 102 | if (std.math.approxEqAbs(f32, det, 0, 1e-8)) { | |
| 103 | return null; | |
| 104 | } | |
| 105 | det = 1.0 / det; | |
| 106 | ||
| 107 | const out = [16]f32{ | |
| 108 | (a11 * b11 - a12 * b10 + a13 * b09) * det, // 0 | |
| 109 | (a02 * b10 - a01 * b11 - a03 * b09) * det, // 1 | |
| 110 | (a31 * b05 - a32 * b04 + a33 * b03) * det, // 2 | |
| 111 | (a22 * b04 - a21 * b05 - a23 * b03) * det, // 3 | |
| 112 | (a12 * b08 - a10 * b11 - a13 * b07) * det, // 4 | |
| 113 | (a00 * b11 - a02 * b08 + a03 * b07) * det, // 5 | |
| 114 | (a32 * b02 - a30 * b05 - a33 * b01) * det, // 6 | |
| 115 | (a20 * b05 - a22 * b02 + a23 * b01) * det, // 7 | |
| 116 | (a10 * b10 - a11 * b08 + a13 * b06) * det, // 8 | |
| 117 | (a01 * b08 - a00 * b10 - a03 * b06) * det, // 9 | |
| 118 | (a30 * b04 - a31 * b02 + a33 * b00) * det, // 10 | |
| 119 | (a21 * b02 - a20 * b04 - a23 * b00) * det, // 11 | |
| 120 | (a11 * b07 - a10 * b09 - a12 * b06) * det, // 12 | |
| 121 | (a00 * b09 - a01 * b07 + a02 * b06) * det, // 13 | |
| 122 | (a31 * b01 - a30 * b03 - a32 * b00) * det, // 14 | |
| 123 | (a20 * b03 - a21 * b01 + a22 * b00) * det, // 15 | |
| 124 | }; | |
| 125 | return Self{ | |
| 126 | .values = @bitCast([4][4]f32, out), | |
| 127 | }; | |
| 62 | 128 | } |
| 63 | 129 | |
| 64 | 130 | /// Multiplies 2 matrices together. |
| 83 | 149 | } |
| 84 | 150 | |
| 85 | 151 | result.values[i][j] = sum; |
| 152 | } | |
| 153 | } | |
| 154 | ||
| 155 | return result; | |
| 156 | } | |
| 157 | ||
| 158 | /// Multiplies 2 matrices together. | |
| 159 | pub fn mulAssign(self: *Self, other: Self) void { | |
| 160 | const a = self.transpose(); | |
| 161 | const b = other; | |
| 162 | ||
| 163 | comptime var i = 0; | |
| 164 | inline while (i < N) : (i += 1) { | |
| 165 | comptime var j = 0; | |
| 166 | inline while (j < N) : (j += 1) { | |
| 167 | const row: @Vector(N, Scalar) = a.values[j]; | |
| 168 | const column: @Vector(N, Scalar) = b.values[i]; | |
| 169 | const products: [N]Scalar = row * column; | |
| 170 | ||
| 171 | var sum = @floatCast(f32, 0); | |
| 172 | comptime var k = 0; | |
| 173 | inline while (k < N) : (k += 1) { | |
| 174 | sum += products[k]; | |
| 175 | } | |
| 176 | ||
| 177 | self.values[i][j] = sum; | |
| 178 | } | |
| 179 | } | |
| 180 | } | |
| 181 | ||
| 182 | const VecN = Vector(N); | |
| 183 | pub fn apply(self: Self, vec: VecN) VecN { | |
| 184 | var result: VecN = undefined; | |
| 185 | ||
| 186 | comptime var i = 0; | |
| 187 | inline while (i < N) : (i += 1) { | |
| 188 | result.values[i] = 0; | |
| 189 | ||
| 190 | comptime var j = 0; | |
| 191 | inline while (j < N) : (j += 1) { | |
| 192 | result.values[i] += self.values[i][j] * vec.values[j]; | |
| 86 | 193 | } |
| 87 | 194 | } |
| 88 | 195 | |