1// Copyright 2016 The SwiftShader Authors. All Rights Reserved.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#include "Matrix.hpp"
16
17#include "Point.hpp"
18#include "System/Math.hpp"
19
20namespace sw
21{
22 Matrix Matrix::diag(float m11, float m22, float m33, float m44)
23 {
24 return Matrix(m11, 0, 0, 0,
25 0, m22, 0, 0,
26 0, 0, m33, 0,
27 0, 0, 0, m44);
28 }
29
30 Matrix::operator float*()
31 {
32 return &(*this)(1, 1);
33 }
34
35 Matrix Matrix::operator+() const
36 {
37 return *this;
38 }
39
40 Matrix Matrix::operator-() const
41 {
42 const Matrix &M = *this;
43
44 return Matrix(-M(1, 1), -M(1, 2), -M(1, 3), -M(1, 4),
45 -M(2, 1), -M(2, 2), -M(2, 3), -M(2, 4),
46 -M(3, 1), -M(3, 2), -M(3, 3), -M(3, 4),
47 -M(4, 1), -M(4, 2), -M(4, 3), -M(4, 4));
48 }
49
50 Matrix Matrix::operator!() const
51 {
52 const Matrix &M = *this;
53 Matrix I;
54
55 float M3344 = M(3, 3) * M(4, 4) - M(4, 3) * M(3, 4);
56 float M2344 = M(2, 3) * M(4, 4) - M(4, 3) * M(2, 4);
57 float M2334 = M(2, 3) * M(3, 4) - M(3, 3) * M(2, 4);
58 float M3244 = M(3, 2) * M(4, 4) - M(4, 2) * M(3, 4);
59 float M2244 = M(2, 2) * M(4, 4) - M(4, 2) * M(2, 4);
60 float M2234 = M(2, 2) * M(3, 4) - M(3, 2) * M(2, 4);
61 float M3243 = M(3, 2) * M(4, 3) - M(4, 2) * M(3, 3);
62 float M2243 = M(2, 2) * M(4, 3) - M(4, 2) * M(2, 3);
63 float M2233 = M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3);
64 float M1344 = M(1, 3) * M(4, 4) - M(4, 3) * M(1, 4);
65 float M1334 = M(1, 3) * M(3, 4) - M(3, 3) * M(1, 4);
66 float M1244 = M(1, 2) * M(4, 4) - M(4, 2) * M(1, 4);
67 float M1234 = M(1, 2) * M(3, 4) - M(3, 2) * M(1, 4);
68 float M1243 = M(1, 2) * M(4, 3) - M(4, 2) * M(1, 3);
69 float M1233 = M(1, 2) * M(3, 3) - M(3, 2) * M(1, 3);
70 float M1324 = M(1, 3) * M(2, 4) - M(2, 3) * M(1, 4);
71 float M1224 = M(1, 2) * M(2, 4) - M(2, 2) * M(1, 4);
72 float M1223 = M(1, 2) * M(2, 3) - M(2, 2) * M(1, 3);
73
74 // Adjoint Matrix
75 I(1, 1) = M(2, 2) * M3344 - M(3, 2) * M2344 + M(4, 2) * M2334;
76 I(2, 1) = -M(2, 1) * M3344 + M(3, 1) * M2344 - M(4, 1) * M2334;
77 I(3, 1) = M(2, 1) * M3244 - M(3, 1) * M2244 + M(4, 1) * M2234;
78 I(4, 1) = -M(2, 1) * M3243 + M(3, 1) * M2243 - M(4, 1) * M2233;
79
80 I(1, 2) = -M(1, 2) * M3344 + M(3, 2) * M1344 - M(4, 2) * M1334;
81 I(2, 2) = M(1, 1) * M3344 - M(3, 1) * M1344 + M(4, 1) * M1334;
82 I(3, 2) = -M(1, 1) * M3244 + M(3, 1) * M1244 - M(4, 1) * M1234;
83 I(4, 2) = M(1, 1) * M3243 - M(3, 1) * M1243 + M(4, 1) * M1233;
84
85 I(1, 3) = M(1, 2) * M2344 - M(2, 2) * M1344 + M(4, 2) * M1324;
86 I(2, 3) = -M(1, 1) * M2344 + M(2, 1) * M1344 - M(4, 1) * M1324;
87 I(3, 3) = M(1, 1) * M2244 - M(2, 1) * M1244 + M(4, 1) * M1224;
88 I(4, 3) = -M(1, 1) * M2243 + M(2, 1) * M1243 - M(4, 1) * M1223;
89
90 I(1, 4) = -M(1, 2) * M2334 + M(2, 2) * M1334 - M(3, 2) * M1324;
91 I(2, 4) = M(1, 1) * M2334 - M(2, 1) * M1334 + M(3, 1) * M1324;
92 I(3, 4) = -M(1, 1) * M2234 + M(2, 1) * M1234 - M(3, 1) * M1224;
93 I(4, 4) = M(1, 1) * M2233 - M(2, 1) * M1233 + M(3, 1) * M1223;
94
95 // Division by determinant
96 I /= M(1, 1) * I(1, 1) +
97 M(2, 1) * I(1, 2) +
98 M(3, 1) * I(1, 3) +
99 M(4, 1) * I(1, 4);
100
101 return I;
102 }
103
104 Matrix Matrix::operator~() const
105 {
106 const Matrix &M = *this;
107
108 return Matrix(M(1, 1), M(2, 1), M(3, 1), M(4, 1),
109 M(1, 2), M(2, 2), M(3, 2), M(4, 2),
110 M(1, 3), M(2, 3), M(3, 3), M(4, 3),
111 M(1, 4), M(2, 4), M(3, 4), M(4, 4));
112 }
113
114 Matrix &Matrix::operator+=(const Matrix &N)
115 {
116 Matrix &M = *this;
117
118 M(1, 1) += N(1, 1); M(1, 2) += N(1, 2); M(1, 3) += N(1, 3); M(1, 4) += N(1, 4);
119 M(2, 1) += N(2, 1); M(2, 2) += N(2, 2); M(2, 3) += N(2, 3); M(2, 4) += N(2, 4);
120 M(3, 1) += N(3, 1); M(3, 2) += N(3, 2); M(3, 3) += N(3, 3); M(3, 4) += N(3, 4);
121 M(4, 1) += N(4, 1); M(4, 2) += N(4, 2); M(4, 3) += N(4, 3); M(4, 4) += N(4, 4);
122
123 return M;
124 }
125
126 Matrix &Matrix::operator-=(const Matrix &N)
127 {
128 Matrix &M = *this;
129
130 M(1, 1) -= N(1, 1); M(1, 2) -= N(1, 2); M(1, 3) -= N(1, 3); M(1, 4) -= N(1, 4);
131 M(2, 1) -= N(2, 1); M(2, 2) -= N(2, 2); M(2, 3) -= N(2, 3); M(2, 4) -= N(2, 4);
132 M(3, 1) -= N(3, 1); M(3, 2) -= N(3, 2); M(3, 3) -= N(3, 3); M(3, 4) -= N(3, 4);
133 M(4, 1) -= N(4, 1); M(4, 2) -= N(4, 2); M(4, 3) -= N(4, 3); M(4, 4) -= N(4, 4);
134
135 return M;
136 }
137
138 Matrix &Matrix::operator*=(float s)
139 {
140 Matrix &M = *this;
141
142 M(1, 1) *= s; M(1, 2) *= s; M(1, 3) *= s; M(1, 4) *= s;
143 M(2, 1) *= s; M(2, 2) *= s; M(2, 3) *= s; M(2, 4) *= s;
144 M(3, 1) *= s; M(3, 2) *= s; M(3, 3) *= s; M(3, 4) *= s;
145 M(4, 1) *= s; M(4, 2) *= s; M(4, 3) *= s; M(4, 4) *= s;
146
147 return M;
148 }
149
150 Matrix &Matrix::operator*=(const Matrix &M)
151 {
152 return *this = *this * M;
153 }
154
155 Matrix &Matrix::operator/=(float s)
156 {
157 float r = 1.0f / s;
158
159 return *this *= r;
160 }
161
162 bool operator==(const Matrix &M, const Matrix &N)
163 {
164 if(M(1, 1) == N(1, 1) && M(1, 2) == N(1, 2) && M(1, 3) == N(1, 3) && M(1, 4) == N(1, 4) &&
165 M(2, 1) == N(2, 1) && M(2, 2) == N(2, 2) && M(2, 3) == N(2, 3) && M(2, 4) == N(2, 4) &&
166 M(3, 1) == N(3, 1) && M(3, 2) == N(3, 2) && M(3, 3) == N(3, 3) && M(3, 4) == N(3, 4) &&
167 M(4, 1) == N(4, 1) && M(4, 2) == N(4, 2) && M(4, 3) == N(4, 3) && M(4, 4) == N(4, 4))
168 return true;
169 else
170 return false;
171 }
172
173 bool operator!=(const Matrix &M, const Matrix &N)
174 {
175 if(M(1, 1) != N(1, 1) || M(1, 2) != N(1, 2) || M(1, 3) != N(1, 3) || M(1, 4) != N(1, 4) ||
176 M(2, 1) != N(2, 1) || M(2, 2) != N(2, 2) || M(2, 3) != N(2, 3) || M(2, 4) != N(2, 4) ||
177 M(3, 1) != N(3, 1) || M(3, 2) != N(3, 2) || M(3, 3) != N(3, 3) || M(3, 4) != N(3, 4) ||
178 M(4, 1) != N(4, 1) || M(4, 2) != N(4, 2) || M(4, 3) != N(4, 3) || M(4, 4) != N(4, 4))
179 return true;
180 else
181 return false;
182 }
183
184 Matrix operator+(const Matrix &M, const Matrix &N)
185 {
186 return Matrix(M(1, 1) + N(1, 1), M(1, 2) + N(1, 2), M(1, 3) + N(1, 3), M(1, 4) + N(1, 4),
187 M(2, 1) + N(2, 1), M(2, 2) + N(2, 2), M(2, 3) + N(2, 3), M(2, 4) + N(2, 4),
188 M(3, 1) + N(3, 1), M(3, 2) + N(3, 2), M(3, 3) + N(3, 3), M(3, 4) + N(3, 4),
189 M(4, 1) + N(4, 1), M(4, 2) + N(4, 2), M(4, 3) + N(4, 3), M(4, 4) + N(4, 4));
190 }
191
192 Matrix operator-(const Matrix &M, const Matrix &N)
193 {
194 return Matrix(M(1, 1) - N(1, 1), M(1, 2) - N(1, 2), M(1, 3) - N(1, 3), M(1, 4) - N(1, 4),
195 M(2, 1) - N(2, 1), M(2, 2) - N(2, 2), M(2, 3) - N(2, 3), M(2, 4) - N(2, 4),
196 M(3, 1) - N(3, 1), M(3, 2) - N(3, 2), M(3, 3) - N(3, 3), M(3, 4) - N(3, 4),
197 M(4, 1) - N(4, 1), M(4, 2) - N(4, 2), M(4, 3) - N(4, 3), M(4, 4) - N(4, 4));
198 }
199
200 Matrix operator*(float s, const Matrix &M)
201 {
202 return Matrix(s * M(1, 1), s * M(1, 2), s * M(1, 3), s * M(1, 4),
203 s * M(2, 1), s * M(2, 2), s * M(2, 3), s * M(2, 4),
204 s * M(3, 1), s * M(3, 2), s * M(3, 3), s * M(3, 4),
205 s * M(4, 1), s * M(4, 2), s * M(4, 3), s * M(4, 4));
206 }
207
208 Matrix operator*(const Matrix &M, float s)
209 {
210 return Matrix(M(1, 1) * s, M(1, 2) * s, M(1, 3) * s, M(1, 4) * s,
211 M(2, 1) * s, M(2, 2) * s, M(2, 3) * s, M(2, 4) * s,
212 M(3, 1) * s, M(3, 2) * s, M(3, 3) * s, M(3, 4) * s,
213 M(4, 1) * s, M(4, 2) * s, M(4, 3) * s, M(4, 4) * s);
214 }
215
216 Matrix operator*(const Matrix &M, const Matrix &N)
217 {
218 return Matrix(M(1, 1) * N(1, 1) + M(1, 2) * N(2, 1) + M(1, 3) * N(3, 1) + M(1, 4) * N(4, 1), M(1, 1) * N(1, 2) + M(1, 2) * N(2, 2) + M(1, 3) * N(3, 2) + M(1, 4) * N(4, 2), M(1, 1) * N(1, 3) + M(1, 2) * N(2, 3) + M(1, 3) * N(3, 3) + M(1, 4) * N(4, 3), M(1, 1) * N(1, 4) + M(1, 2) * N(2, 4) + M(1, 3) * N(3, 4) + M(1, 4) * N(4, 4),
219 M(2, 1) * N(1, 1) + M(2, 2) * N(2, 1) + M(2, 3) * N(3, 1) + M(2, 4) * N(4, 1), M(2, 1) * N(1, 2) + M(2, 2) * N(2, 2) + M(2, 3) * N(3, 2) + M(2, 4) * N(4, 2), M(2, 1) * N(1, 3) + M(2, 2) * N(2, 3) + M(2, 3) * N(3, 3) + M(2, 4) * N(4, 3), M(2, 1) * N(1, 4) + M(2, 2) * N(2, 4) + M(2, 3) * N(3, 4) + M(2, 4) * N(4, 4),
220 M(3, 1) * N(1, 1) + M(3, 2) * N(2, 1) + M(3, 3) * N(3, 1) + M(3, 4) * N(4, 1), M(3, 1) * N(1, 2) + M(3, 2) * N(2, 2) + M(3, 3) * N(3, 2) + M(3, 4) * N(4, 2), M(3, 1) * N(1, 3) + M(3, 2) * N(2, 3) + M(3, 3) * N(3, 3) + M(3, 4) * N(4, 3), M(3, 1) * N(1, 4) + M(3, 2) * N(2, 4) + M(3, 3) * N(3, 4) + M(3, 4) * N(4, 4),
221 M(4, 1) * N(1, 1) + M(4, 2) * N(2, 1) + M(4, 3) * N(3, 1) + M(4, 4) * N(4, 1), M(4, 1) * N(1, 2) + M(4, 2) * N(2, 2) + M(4, 3) * N(3, 2) + M(4, 4) * N(4, 2), M(4, 1) * N(1, 3) + M(4, 2) * N(2, 3) + M(4, 3) * N(3, 3) + M(4, 4) * N(4, 3), M(4, 1) * N(1, 4) + M(4, 2) * N(2, 4) + M(4, 3) * N(3, 4) + M(4, 4) * N(4, 4));
222 }
223
224 Matrix operator/(const Matrix &M, float s)
225 {
226 float r = 1.0f / s;
227
228 return M * r;
229 }
230
231 float4 Matrix::operator*(const float4 &v) const
232 {
233 const Matrix &M = *this;
234 float Mx = M(1, 1) * v.x + M(1, 2) * v.y + M(1, 3) * v.z + M(1, 4) * v.w;
235 float My = M(2, 1) * v.x + M(2, 2) * v.y + M(2, 3) * v.z + M(2, 4) * v.w;
236 float Mz = M(3, 1) * v.x + M(3, 2) * v.y + M(3, 3) * v.z + M(3, 4) * v.w;
237 float Mw = M(4, 1) * v.x + M(4, 2) * v.y + M(4, 3) * v.z + M(4, 4) * v.w;
238
239 return {Mx, My, Mz, Mw};
240 }
241
242 float Matrix::det(const Matrix &M)
243 {
244 float M3344 = M(3, 3) * M(4, 4) - M(4, 3) * M(3, 4);
245 float M2344 = M(2, 3) * M(4, 4) - M(4, 3) * M(2, 4);
246 float M2334 = M(2, 3) * M(3, 4) - M(3, 3) * M(2, 4);
247 float M1344 = M(1, 3) * M(4, 4) - M(4, 3) * M(1, 4);
248 float M1334 = M(1, 3) * M(3, 4) - M(3, 3) * M(1, 4);
249 float M1324 = M(1, 3) * M(2, 4) - M(2, 3) * M(1, 4);
250
251 return M(1, 1) * (M(2, 2) * M3344 - M(3, 2) * M2344 + M(4, 2) * M2334) -
252 M(2, 1) * (M(1, 2) * M3344 - M(3, 2) * M1344 + M(4, 2) * M1334) +
253 M(3, 1) * (M(1, 2) * M2344 - M(2, 2) * M1344 + M(4, 2) * M1324) -
254 M(4, 1) * (M(1, 2) * M2334 - M(2, 2) * M1334 + M(3, 2) * M1324);
255 }
256
257 float Matrix::det(float m11)
258 {
259 return m11;
260 }
261
262 float Matrix::det(float m11, float m12,
263 float m21, float m22)
264 {
265 return m11 * m22 - m12 * m21;
266 }
267
268 float Matrix::det(float m11, float m12, float m13,
269 float m21, float m22, float m23,
270 float m31, float m32, float m33)
271 {
272 return m11 * (m22 * m33 - m32 * m23) -
273 m21 * (m12 * m33 - m32 * m13) +
274 m31 * (m12 * m23 - m22 * m13);
275 }
276
277 float Matrix::det(float m11, float m12, float m13, float m14,
278 float m21, float m22, float m23, float m24,
279 float m31, float m32, float m33, float m34,
280 float m41, float m42, float m43, float m44)
281 {
282 float M3344 = m33 * m44 - m43 * m34;
283 float M2344 = m23 * m44 - m43 * m24;
284 float M2334 = m23 * m34 - m33 * m24;
285 float M1344 = m13 * m44 - m43 * m14;
286 float M1334 = m13 * m34 - m33 * m14;
287 float M1324 = m13 * m24 - m23 * m14;
288
289 return m11 * (m22 * M3344 - m32 * M2344 + m42 * M2334) -
290 m21 * (m12 * M3344 - m32 * M1344 + m42 * M1334) +
291 m31 * (m12 * M2344 - m22 * M1344 + m42 * M1324) -
292 m41 * (m12 * M2334 - m22 * M1334 + m32 * M1324);
293 }
294
295 float Matrix::det(const Vector &v1, const Vector &v2, const Vector &v3)
296 {
297 return v1 * (v2 % v3);
298 }
299
300 float Matrix::det3(const Matrix &M)
301 {
302 return M(1, 1) * (M(2, 2) * M(3, 3) - M(3, 2) * M(2, 3)) -
303 M(2, 1) * (M(1, 2) * M(3, 3) - M(3, 2) * M(1, 3)) +
304 M(3, 1) * (M(1, 2) * M(2, 3) - M(2, 2) * M(1, 3));
305 }
306
307 float Matrix::tr(const Matrix &M)
308 {
309 return M(1, 1) + M(2, 2) + M(3, 3) + M(4, 4);
310 }
311
312 Matrix &Matrix::orthogonalise()
313 {
314 // NOTE: Numnerically instable, won't return exact the same result when already orhtogonal
315
316 Matrix &M = *this;
317
318 Vector v1(M(1, 1), M(2, 1), M(3, 1));
319 Vector v2(M(1, 2), M(2, 2), M(3, 2));
320 Vector v3(M(1, 3), M(2, 3), M(3, 3));
321
322 v2 -= v1 * (v1 * v2) / (v1 * v1);
323 v3 -= v1 * (v1 * v3) / (v1 * v1);
324 v3 -= v2 * (v2 * v3) / (v2 * v2);
325
326 v1 /= Vector::N(v1);
327 v2 /= Vector::N(v2);
328 v3 /= Vector::N(v3);
329
330 M(1, 1) = v1.x; M(1, 2) = v2.x; M(1, 3) = v3.x;
331 M(2, 1) = v1.y; M(2, 2) = v2.y; M(2, 3) = v3.y;
332 M(3, 1) = v1.z; M(3, 2) = v2.z; M(3, 3) = v3.z;
333
334 return *this;
335 }
336
337 Matrix Matrix::eulerRotate(const Vector &v)
338 {
339 float cz = cos(v.z);
340 float sz = sin(v.z);
341 float cx = cos(v.x);
342 float sx = sin(v.x);
343 float cy = cos(v.y);
344 float sy = sin(v.y);
345
346 float sxsy = sx * sy;
347 float sxcy = sx * cy;
348
349 return Matrix(cy * cz - sxsy * sz, -cy * sz - sxsy * cz, -sy * cx,
350 cx * sz, cx * cz, -sx,
351 sy * cz + sxcy * sz, -sy * sz + sxcy * cz, cy * cx);
352 }
353
354 Matrix Matrix::eulerRotate(float x, float y, float z)
355 {
356 return eulerRotate(Vector(x, y, z));
357 }
358
359 Matrix Matrix::translate(const Vector &v)
360 {
361 return Matrix(1, 0, 0, v.x,
362 0, 1, 0, v.y,
363 0, 0, 1, v.z,
364 0, 0, 0, 1);
365 }
366
367 Matrix Matrix::translate(float x, float y, float z)
368 {
369 return translate(Vector(x, y, z));
370 }
371
372 Matrix Matrix::scale(const Vector &v)
373 {
374 return Matrix(v.x, 0, 0,
375 0, v.y, 0,
376 0, 0, v.z);
377 }
378
379 Matrix Matrix::scale(float x, float y, float z)
380 {
381 return scale(Vector(x, y, z));
382 }
383
384 Matrix Matrix::lookAt(const Vector &v)
385 {
386 Vector y = v;
387 y /= Vector::N(y);
388
389 Vector x = y % Vector(0, 0, 1);
390 x /= Vector::N(x);
391
392 Vector z = x % y;
393 z /= Vector::N(z);
394
395 return ~Matrix(x, y, z);
396 }
397
398 Matrix Matrix::lookAt(float x, float y, float z)
399 {
400 return translate(Vector(x, y, z));
401 }
402}
403