• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 "Math.hpp"
19 
20 namespace sw
21 {
diag(float m11,float m22,float m33,float m44)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 
operator float*()30 	Matrix::operator float*()
31 	{
32 		return &(*this)(1, 1);
33 	}
34 
operator +() const35 	Matrix Matrix::operator+() const
36 	{
37 		return *this;
38 	}
39 
operator -() const40 	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 
operator !() const50 	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 
operator ~() const104 	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 
operator +=(const Matrix & N)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 
operator -=(const Matrix & N)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 
operator *=(float s)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 
operator *=(const Matrix & M)150 	Matrix &Matrix::operator*=(const Matrix &M)
151 	{
152 		return *this = *this * M;
153 	}
154 
operator /=(float s)155 	Matrix &Matrix::operator/=(float s)
156 	{
157 		float r = 1.0f / s;
158 
159 		return *this *= r;
160 	}
161 
operator ==(const Matrix & M,const Matrix & N)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 
operator !=(const Matrix & M,const Matrix & N)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 
operator +(const Matrix & M,const Matrix & N)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 
operator -(const Matrix & M,const Matrix & N)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 
operator *(float s,const Matrix & M)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 
operator *(const Matrix & M,float s)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 
operator *(const Matrix & M,const Matrix & N)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 
operator /(const Matrix & M,float s)224 	Matrix operator/(const Matrix &M, float s)
225 	{
226 		float r = 1.0f / s;
227 
228 		return M * r;
229 	}
230 
operator *(const float4 & v) const231 	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 
det(const Matrix & M)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 
det(float m11)257 	float Matrix::det(float m11)
258 	{
259 		return m11;
260 	}
261 
det(float m11,float m12,float m21,float m22)262 	float Matrix::det(float m11, float m12,
263 	                  float m21, float m22)
264 	{
265 		return m11 * m22 - m12 * m21;
266 	}
267 
det(float m11,float m12,float m13,float m21,float m22,float m23,float m31,float m32,float m33)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 
det(float m11,float m12,float m13,float m14,float m21,float m22,float m23,float m24,float m31,float m32,float m33,float m34,float m41,float m42,float m43,float m44)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 
det(const Vector & v1,const Vector & v2,const Vector & v3)295 	float Matrix::det(const Vector &v1, const Vector &v2, const Vector &v3)
296 	{
297 		return v1 * (v2 % v3);
298 	}
299 
det3(const Matrix & M)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 
tr(const Matrix & M)307 	float Matrix::tr(const Matrix &M)
308 	{
309 		return M(1, 1) + M(2, 2) + M(3, 3) + M(4, 4);
310 	}
311 
orthogonalise()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 
eulerRotate(const Vector & v)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 
eulerRotate(float x,float y,float z)354 	Matrix Matrix::eulerRotate(float x, float y, float z)
355 	{
356 		return eulerRotate(Vector(x, y, z));
357 	}
358 
translate(const Vector & v)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 
translate(float x,float y,float z)367 	Matrix Matrix::translate(float x, float y, float z)
368 	{
369 		return translate(Vector(x, y, z));
370 	}
371 
scale(const Vector & v)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 
scale(float x,float y,float z)379 	Matrix Matrix::scale(float x, float y, float z)
380 	{
381 		return scale(Vector(x, y, z));
382 	}
383 
lookAt(const Vector & v)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 
lookAt(float x,float y,float z)398 	Matrix Matrix::lookAt(float x, float y, float z)
399 	{
400 		return translate(Vector(x, y, z));
401 	}
402 }
403