1 //
2 // Copyright 2014 The ANGLE Project Authors. All rights reserved.
3 // Use of this source code is governed by a BSD-style license that can be
4 // found in the LICENSE file.
5 //
6 // Matrix:
7 // Helper class for doing matrix math.
8 //
9
10 #include "Matrix.h"
11
12 #define _USE_MATH_DEFINES
13 #include <math.h>
14 #include <cstddef>
15
16 using namespace angle;
17
Matrix4()18 Matrix4::Matrix4()
19 {
20 data[0] = 1.0f;
21 data[4] = 0.0f;
22 data[8] = 0.0f;
23 data[12] = 0.0f;
24 data[1] = 0.0f;
25 data[5] = 1.0f;
26 data[9] = 0.0f;
27 data[13] = 0.0f;
28 data[2] = 0.0f;
29 data[6] = 0.0f;
30 data[10] = 1.0f;
31 data[14] = 0.0f;
32 data[3] = 0.0f;
33 data[7] = 0.0f;
34 data[11] = 0.0f;
35 data[15] = 1.0f;
36 }
37
Matrix4(float m00,float m01,float m02,float m03,float m10,float m11,float m12,float m13,float m20,float m21,float m22,float m23,float m30,float m31,float m32,float m33)38 Matrix4::Matrix4(float m00,
39 float m01,
40 float m02,
41 float m03,
42 float m10,
43 float m11,
44 float m12,
45 float m13,
46 float m20,
47 float m21,
48 float m22,
49 float m23,
50 float m30,
51 float m31,
52 float m32,
53 float m33)
54 {
55 data[0] = m00;
56 data[4] = m01;
57 data[8] = m02;
58 data[12] = m03;
59 data[1] = m10;
60 data[5] = m11;
61 data[9] = m12;
62 data[13] = m13;
63 data[2] = m20;
64 data[6] = m21;
65 data[10] = m22;
66 data[14] = m23;
67 data[3] = m30;
68 data[7] = m31;
69 data[11] = m32;
70 data[15] = m33;
71 }
72
identity()73 Matrix4 Matrix4::identity()
74 {
75 return Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
76 0.0f, 0.0f, 1.0f);
77 }
78
rotate(float angle,const Vector3 & p)79 Matrix4 Matrix4::rotate(float angle, const Vector3 &p)
80 {
81 Vector3 u = p.normalized();
82 float theta = static_cast<float>(angle * (M_PI / 180.0f));
83 float cos_t = cosf(theta);
84 float sin_t = sinf(theta);
85
86 return Matrix4(cos_t + (u.x() * u.x() * (1.0f - cos_t)),
87 (u.x() * u.y() * (1.0f - cos_t)) - (u.z() * sin_t),
88 (u.x() * u.z() * (1.0f - cos_t)) + (u.y() * sin_t), 0.0f,
89 (u.y() * u.x() * (1.0f - cos_t)) + (u.z() * sin_t),
90 cos_t + (u.y() * u.y() * (1.0f - cos_t)),
91 (u.y() * u.z() * (1.0f - cos_t)) - (u.x() * sin_t), 0.0f,
92 (u.z() * u.x() * (1.0f - cos_t)) - (u.y() * sin_t),
93 (u.z() * u.y() * (1.0f - cos_t)) + (u.x() * sin_t),
94 cos_t + (u.z() * u.z() * (1.0f - cos_t)), 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
95 }
96
translate(const Vector3 & t)97 Matrix4 Matrix4::translate(const Vector3 &t)
98 {
99 return Matrix4(1.0f, 0.0f, 0.0f, t.x(), 0.0f, 1.0f, 0.0f, t.y(), 0.0f, 0.0f, 1.0f, t.z(), 0.0f,
100 0.0f, 0.0f, 1.0f);
101 }
102
scale(const Vector3 & s)103 Matrix4 Matrix4::scale(const Vector3 &s)
104 {
105 return Matrix4(s.x(), 0.0f, 0.0f, 0.0f, 0.0f, s.y(), 0.0f, 0.0f, 0.0f, 0.0f, s.z(), 0.0f, 0.0f,
106 0.0f, 0.0f, 1.0f);
107 }
108
frustum(float l,float r,float b,float t,float n,float f)109 Matrix4 Matrix4::frustum(float l, float r, float b, float t, float n, float f)
110 {
111 return Matrix4((2.0f * n) / (r - l), 0.0f, (r + l) / (r - l), 0.0f, 0.0f, (2.0f * n) / (t - b),
112 (t + b) / (t - b), 0.0f, 0.0f, 0.0f, -(f + n) / (f - n),
113 -(2.0f * f * n) / (f - n), 0.0f, 0.0f, -1.0f, 0.0f);
114 }
115
perspective(float fovY,float aspectRatio,float nearZ,float farZ)116 Matrix4 Matrix4::perspective(float fovY, float aspectRatio, float nearZ, float farZ)
117 {
118 const float frustumHeight = tanf(static_cast<float>(fovY / 360.0f * M_PI)) * nearZ;
119 const float frustumWidth = frustumHeight * aspectRatio;
120 return frustum(-frustumWidth, frustumWidth, -frustumHeight, frustumHeight, nearZ, farZ);
121 }
122
ortho(float l,float r,float b,float t,float n,float f)123 Matrix4 Matrix4::ortho(float l, float r, float b, float t, float n, float f)
124 {
125 return Matrix4(2.0f / (r - l), 0.0f, 0.0f, -(r + l) / (r - l), 0.0f, 2.0f / (t - b), 0.0f,
126 -(t + b) / (t - b), 0.0f, 0.0f, -2.0f / (f - n), -(f + n) / (f - n), 0.0f, 0.0f,
127 0.0f, 1.0f);
128 }
129
rollPitchYaw(float roll,float pitch,float yaw)130 Matrix4 Matrix4::rollPitchYaw(float roll, float pitch, float yaw)
131 {
132 return rotate(yaw, Vector3(0, 0, 1)) * rotate(pitch, Vector3(0, 1, 0)) *
133 rotate(roll, Vector3(1, 0, 0));
134 }
135
invert(const Matrix4 & mat)136 Matrix4 Matrix4::invert(const Matrix4 &mat)
137 {
138 Matrix4 inverted(
139 mat.data[5] * mat.data[10] * mat.data[15] - mat.data[5] * mat.data[11] * mat.data[14] -
140 mat.data[9] * mat.data[6] * mat.data[15] + mat.data[9] * mat.data[7] * mat.data[14] +
141 mat.data[13] * mat.data[6] * mat.data[11] - mat.data[13] * mat.data[7] * mat.data[10],
142 -mat.data[4] * mat.data[10] * mat.data[15] + mat.data[4] * mat.data[11] * mat.data[14] +
143 mat.data[8] * mat.data[6] * mat.data[15] - mat.data[8] * mat.data[7] * mat.data[14] -
144 mat.data[12] * mat.data[6] * mat.data[11] + mat.data[12] * mat.data[7] * mat.data[10],
145 mat.data[4] * mat.data[9] * mat.data[15] - mat.data[4] * mat.data[11] * mat.data[13] -
146 mat.data[8] * mat.data[5] * mat.data[15] + mat.data[8] * mat.data[7] * mat.data[13] +
147 mat.data[12] * mat.data[5] * mat.data[11] - mat.data[12] * mat.data[7] * mat.data[9],
148 -mat.data[4] * mat.data[9] * mat.data[14] + mat.data[4] * mat.data[10] * mat.data[13] +
149 mat.data[8] * mat.data[5] * mat.data[14] - mat.data[8] * mat.data[6] * mat.data[13] -
150 mat.data[12] * mat.data[5] * mat.data[10] + mat.data[12] * mat.data[6] * mat.data[9],
151 -mat.data[1] * mat.data[10] * mat.data[15] + mat.data[1] * mat.data[11] * mat.data[14] +
152 mat.data[9] * mat.data[2] * mat.data[15] - mat.data[9] * mat.data[3] * mat.data[14] -
153 mat.data[13] * mat.data[2] * mat.data[11] + mat.data[13] * mat.data[3] * mat.data[10],
154 mat.data[0] * mat.data[10] * mat.data[15] - mat.data[0] * mat.data[11] * mat.data[14] -
155 mat.data[8] * mat.data[2] * mat.data[15] + mat.data[8] * mat.data[3] * mat.data[14] +
156 mat.data[12] * mat.data[2] * mat.data[11] - mat.data[12] * mat.data[3] * mat.data[10],
157 -mat.data[0] * mat.data[9] * mat.data[15] + mat.data[0] * mat.data[11] * mat.data[13] +
158 mat.data[8] * mat.data[1] * mat.data[15] - mat.data[8] * mat.data[3] * mat.data[13] -
159 mat.data[12] * mat.data[1] * mat.data[11] + mat.data[12] * mat.data[3] * mat.data[9],
160 mat.data[0] * mat.data[9] * mat.data[14] - mat.data[0] * mat.data[10] * mat.data[13] -
161 mat.data[8] * mat.data[1] * mat.data[14] + mat.data[8] * mat.data[2] * mat.data[13] +
162 mat.data[12] * mat.data[1] * mat.data[10] - mat.data[12] * mat.data[2] * mat.data[9],
163 mat.data[1] * mat.data[6] * mat.data[15] - mat.data[1] * mat.data[7] * mat.data[14] -
164 mat.data[5] * mat.data[2] * mat.data[15] + mat.data[5] * mat.data[3] * mat.data[14] +
165 mat.data[13] * mat.data[2] * mat.data[7] - mat.data[13] * mat.data[3] * mat.data[6],
166 -mat.data[0] * mat.data[6] * mat.data[15] + mat.data[0] * mat.data[7] * mat.data[14] +
167 mat.data[4] * mat.data[2] * mat.data[15] - mat.data[4] * mat.data[3] * mat.data[14] -
168 mat.data[12] * mat.data[2] * mat.data[7] + mat.data[12] * mat.data[3] * mat.data[6],
169 mat.data[0] * mat.data[5] * mat.data[15] - mat.data[0] * mat.data[7] * mat.data[13] -
170 mat.data[4] * mat.data[1] * mat.data[15] + mat.data[4] * mat.data[3] * mat.data[13] +
171 mat.data[12] * mat.data[1] * mat.data[7] - mat.data[12] * mat.data[3] * mat.data[5],
172 -mat.data[0] * mat.data[5] * mat.data[14] + mat.data[0] * mat.data[6] * mat.data[13] +
173 mat.data[4] * mat.data[1] * mat.data[14] - mat.data[4] * mat.data[2] * mat.data[13] -
174 mat.data[12] * mat.data[1] * mat.data[6] + mat.data[12] * mat.data[2] * mat.data[5],
175 -mat.data[1] * mat.data[6] * mat.data[11] + mat.data[1] * mat.data[7] * mat.data[10] +
176 mat.data[5] * mat.data[2] * mat.data[11] - mat.data[5] * mat.data[3] * mat.data[10] -
177 mat.data[9] * mat.data[2] * mat.data[7] + mat.data[9] * mat.data[3] * mat.data[6],
178 mat.data[0] * mat.data[6] * mat.data[11] - mat.data[0] * mat.data[7] * mat.data[10] -
179 mat.data[4] * mat.data[2] * mat.data[11] + mat.data[4] * mat.data[3] * mat.data[10] +
180 mat.data[8] * mat.data[2] * mat.data[7] - mat.data[8] * mat.data[3] * mat.data[6],
181 -mat.data[0] * mat.data[5] * mat.data[11] + mat.data[0] * mat.data[7] * mat.data[9] +
182 mat.data[4] * mat.data[1] * mat.data[11] - mat.data[4] * mat.data[3] * mat.data[9] -
183 mat.data[8] * mat.data[1] * mat.data[7] + mat.data[8] * mat.data[3] * mat.data[5],
184 mat.data[0] * mat.data[5] * mat.data[10] - mat.data[0] * mat.data[6] * mat.data[9] -
185 mat.data[4] * mat.data[1] * mat.data[10] + mat.data[4] * mat.data[2] * mat.data[9] +
186 mat.data[8] * mat.data[1] * mat.data[6] - mat.data[8] * mat.data[2] * mat.data[5]);
187
188 float determinant = mat.data[0] * inverted.data[0] + mat.data[1] * inverted.data[4] +
189 mat.data[2] * inverted.data[8] + mat.data[3] * inverted.data[12];
190
191 if (determinant != 0.0f)
192 {
193 inverted *= 1.0f / determinant;
194 }
195 else
196 {
197 inverted = identity();
198 }
199
200 return inverted;
201 }
202
transpose(const Matrix4 & mat)203 Matrix4 Matrix4::transpose(const Matrix4 &mat)
204 {
205 return Matrix4(mat.data[0], mat.data[1], mat.data[2], mat.data[3], mat.data[4], mat.data[5],
206 mat.data[6], mat.data[7], mat.data[8], mat.data[9], mat.data[10], mat.data[11],
207 mat.data[12], mat.data[13], mat.data[14], mat.data[15]);
208 }
209
transform(const Matrix4 & mat,const Vector3 & pt)210 Vector3 Matrix4::transform(const Matrix4 &mat, const Vector3 &pt)
211 {
212 Vector4 transformed = (mat * Vector4(pt, 1.0f)).normalized();
213 return Vector3(transformed.x(), transformed.y(), transformed.z());
214 }
215
transform(const Matrix4 & mat,const Vector4 & pt)216 Vector3 Matrix4::transform(const Matrix4 &mat, const Vector4 &pt)
217 {
218 Vector4 transformed = (mat * pt).normalized();
219 return Vector3(transformed.x(), transformed.y(), transformed.z());
220 }
221
operator *(const Matrix4 & a,const Matrix4 & b)222 Matrix4 operator*(const Matrix4 &a, const Matrix4 &b)
223 {
224 return Matrix4(a.data[0] * b.data[0] + a.data[4] * b.data[1] + a.data[8] * b.data[2] +
225 a.data[12] * b.data[3],
226 a.data[0] * b.data[4] + a.data[4] * b.data[5] + a.data[8] * b.data[6] +
227 a.data[12] * b.data[7],
228 a.data[0] * b.data[8] + a.data[4] * b.data[9] + a.data[8] * b.data[10] +
229 a.data[12] * b.data[11],
230 a.data[0] * b.data[12] + a.data[4] * b.data[13] + a.data[8] * b.data[14] +
231 a.data[12] * b.data[15],
232 a.data[1] * b.data[0] + a.data[5] * b.data[1] + a.data[9] * b.data[2] +
233 a.data[13] * b.data[3],
234 a.data[1] * b.data[4] + a.data[5] * b.data[5] + a.data[9] * b.data[6] +
235 a.data[13] * b.data[7],
236 a.data[1] * b.data[8] + a.data[5] * b.data[9] + a.data[9] * b.data[10] +
237 a.data[13] * b.data[11],
238 a.data[1] * b.data[12] + a.data[5] * b.data[13] + a.data[9] * b.data[14] +
239 a.data[13] * b.data[15],
240 a.data[2] * b.data[0] + a.data[6] * b.data[1] + a.data[10] * b.data[2] +
241 a.data[14] * b.data[3],
242 a.data[2] * b.data[4] + a.data[6] * b.data[5] + a.data[10] * b.data[6] +
243 a.data[14] * b.data[7],
244 a.data[2] * b.data[8] + a.data[6] * b.data[9] + a.data[10] * b.data[10] +
245 a.data[14] * b.data[11],
246 a.data[2] * b.data[12] + a.data[6] * b.data[13] + a.data[10] * b.data[14] +
247 a.data[14] * b.data[15],
248 a.data[3] * b.data[0] + a.data[7] * b.data[1] + a.data[11] * b.data[2] +
249 a.data[15] * b.data[3],
250 a.data[3] * b.data[4] + a.data[7] * b.data[5] + a.data[11] * b.data[6] +
251 a.data[15] * b.data[7],
252 a.data[3] * b.data[8] + a.data[7] * b.data[9] + a.data[11] * b.data[10] +
253 a.data[15] * b.data[11],
254 a.data[3] * b.data[12] + a.data[7] * b.data[13] + a.data[11] * b.data[14] +
255 a.data[15] * b.data[15]);
256 }
257
operator *=(Matrix4 & a,const Matrix4 & b)258 Matrix4 &operator*=(Matrix4 &a, const Matrix4 &b)
259 {
260 a = a * b;
261 return a;
262 }
263
operator *(const Matrix4 & a,float b)264 Matrix4 operator*(const Matrix4 &a, float b)
265 {
266 Matrix4 ret(a);
267 for (size_t i = 0; i < 16; i++)
268 {
269 ret.data[i] *= b;
270 }
271 return ret;
272 }
273
operator *=(Matrix4 & a,float b)274 Matrix4 &operator*=(Matrix4 &a, float b)
275 {
276 for (size_t i = 0; i < 16; i++)
277 {
278 a.data[i] *= b;
279 }
280 return a;
281 }
282
operator *(const Matrix4 & a,const Vector4 & b)283 Vector4 operator*(const Matrix4 &a, const Vector4 &b)
284 {
285 return Vector4(a.data[0] * b.x() + a.data[4] * b.y() + a.data[8] * b.z() + a.data[12] * b.w(),
286 a.data[1] * b.x() + a.data[5] * b.y() + a.data[9] * b.z() + a.data[13] * b.w(),
287 a.data[2] * b.x() + a.data[6] * b.y() + a.data[10] * b.z() + a.data[14] * b.w(),
288 a.data[3] * b.x() + a.data[7] * b.y() + a.data[11] * b.z() + a.data[15] * b.w());
289 }
290
operator ==(const Matrix4 & a,const Matrix4 & b)291 bool operator==(const Matrix4 &a, const Matrix4 &b)
292 {
293 for (size_t i = 0; i < 16; i++)
294 {
295 if (a.data[i] != b.data[i])
296 {
297 return false;
298 }
299 }
300 return true;
301 }
302
operator !=(const Matrix4 & a,const Matrix4 & b)303 bool operator!=(const Matrix4 &a, const Matrix4 &b)
304 {
305 return !(a == b);
306 }
307