Line data Source code
1 : /* Copyright (C) 2019 Wildfire Games.
2 : * This file is part of 0 A.D.
3 : *
4 : * 0 A.D. is free software: you can redistribute it and/or modify
5 : * it under the terms of the GNU General Public License as published by
6 : * the Free Software Foundation, either version 2 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * 0 A.D. is distributed in the hope that it will be useful,
10 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : * GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License
15 : * along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : /*
19 : * A Matrix class used for holding and manipulating transformation
20 : * info.
21 : */
22 :
23 : #include "precompiled.h"
24 :
25 : #include "Matrix3D.h"
26 : #include "Quaternion.h"
27 : #include "Vector4D.h"
28 :
29 : //Sets the identity matrix
30 281 : void CMatrix3D::SetIdentity ()
31 : {
32 281 : _11=1.0f; _12=0.0f; _13=0.0f; _14=0.0f;
33 281 : _21=0.0f; _22=1.0f; _23=0.0f; _24=0.0f;
34 281 : _31=0.0f; _32=0.0f; _33=1.0f; _34=0.0f;
35 281 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
36 281 : }
37 :
38 : //Sets the zero matrix
39 15 : void CMatrix3D::SetZero()
40 : {
41 15 : _11=0.0f; _12=0.0f; _13=0.0f; _14=0.0f;
42 15 : _21=0.0f; _22=0.0f; _23=0.0f; _24=0.0f;
43 15 : _31=0.0f; _32=0.0f; _33=0.0f; _34=0.0f;
44 15 : _41=0.0f; _42=0.0f; _43=0.0f; _44=0.0f;
45 15 : }
46 :
47 6 : void CMatrix3D::SetOrtho(float left, float right, float bottom, float top, float near, float far)
48 : {
49 : // Based on OpenGL spec
50 6 : SetZero();
51 6 : _11 = 2 / (right - left);
52 6 : _22 = 2 / (top - bottom);
53 6 : _33 = -2 / (far - near);
54 6 : _44 = 1;
55 :
56 6 : _14 = -(right + left) / (right - left);
57 6 : _24 = -(top + bottom) / (top - bottom);
58 6 : _34 = -(far + near) / (far - near);
59 6 : }
60 :
61 8 : void CMatrix3D::SetPerspective(float fov, float aspect, float near, float far)
62 : {
63 8 : const float f = 1.f / tanf(fov / 2.f);
64 :
65 8 : SetZero();
66 8 : _11 = f / aspect;
67 8 : _22 = f;
68 8 : _33 = -(far + near) / (near - far);
69 8 : _34 = 2 * far * near / (near - far);
70 8 : _43 = 1;
71 8 : }
72 :
73 0 : void CMatrix3D::SetPerspectiveTile(float fov, float aspect, float near, float far, int tiles, int tile_x, int tile_y)
74 : {
75 0 : const float f = 1.f / tanf(fov / 2.f);
76 :
77 0 : SetPerspective(fov, aspect, near, far);
78 0 : _11 = tiles * f / aspect;
79 0 : _22 = tiles * f;
80 0 : _13 = -(1 - tiles + 2 * tile_x);
81 0 : _23 = -(1 - tiles + 2 * tile_y);
82 0 : }
83 :
84 : //The following clear the matrix and set the
85 : //rotation of each of the 3 axes
86 :
87 16 : void CMatrix3D::SetXRotation (float angle)
88 : {
89 16 : const float Cos = cosf (angle);
90 16 : const float Sin = sinf (angle);
91 :
92 16 : _11=1.0f; _12=0.0f; _13=0.0f; _14=0.0f;
93 16 : _21=0.0f; _22=Cos; _23=-Sin; _24=0.0f;
94 16 : _31=0.0f; _32=Sin; _33=Cos; _34=0.0f;
95 16 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
96 16 : }
97 :
98 17 : void CMatrix3D::SetYRotation (float angle)
99 : {
100 17 : const float Cos = cosf (angle);
101 17 : const float Sin = sinf (angle);
102 :
103 17 : _11=Cos; _12=0.0f; _13=Sin; _14=0.0f;
104 17 : _21=0.0f; _22=1.0f; _23=0.0f; _24=0.0f;
105 17 : _31=-Sin; _32=0.0f; _33=Cos; _34=0.0f;
106 17 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
107 17 : }
108 :
109 3 : void CMatrix3D::SetZRotation (float angle)
110 : {
111 3 : const float Cos = cosf (angle);
112 3 : const float Sin = sinf (angle);
113 :
114 3 : _11=Cos; _12=-Sin; _13=0.0f; _14=0.0f;
115 3 : _21=Sin; _22=Cos; _23=0.0f; _24=0.0f;
116 3 : _31=0.0f; _32=0.0f; _33=1.0f; _34=0.0f;
117 3 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
118 3 : }
119 :
120 : //The following apply a rotation to the matrix
121 : //about each of the axes;
122 :
123 1 : void CMatrix3D::RotateX (float angle)
124 : {
125 1 : const float Cos = cosf (angle);
126 1 : const float Sin = sinf (angle);
127 1 : const float tmp_21 = _21;
128 1 : const float tmp_22 = _22;
129 1 : const float tmp_23 = _23;
130 1 : const float tmp_24 = _24;
131 :
132 1 : _21 = Cos * _21 - Sin * _31;
133 1 : _22 = Cos * _22 - Sin * _32;
134 1 : _23 = Cos * _23 - Sin * _33;
135 1 : _24 = Cos * _24 - Sin * _34;
136 :
137 1 : _31 = Sin * tmp_21 + Cos * _31;
138 1 : _32 = Sin * tmp_22 + Cos * _32;
139 1 : _33 = Sin * tmp_23 + Cos * _33;
140 1 : _34 = Sin * tmp_24 + Cos * _34;
141 1 : }
142 :
143 16 : void CMatrix3D::RotateY (float angle)
144 : {
145 16 : const float Cos = cosf (angle);
146 16 : const float Sin = sinf (angle);
147 16 : const float tmp_11 = _11;
148 16 : const float tmp_12 = _12;
149 16 : const float tmp_13 = _13;
150 16 : const float tmp_14 = _14;
151 :
152 16 : _11 = Cos * _11 + Sin * _31;
153 16 : _12 = Cos * _12 + Sin * _32;
154 16 : _13 = Cos * _13 + Sin * _33;
155 16 : _14 = Cos * _14 + Sin * _34;
156 :
157 16 : _31 = -Sin * tmp_11 + Cos * _31;
158 16 : _32 = -Sin * tmp_12 + Cos * _32;
159 16 : _33 = -Sin * tmp_13 + Cos * _33;
160 16 : _34 = -Sin * tmp_14 + Cos * _34;
161 16 : }
162 :
163 16 : void CMatrix3D::RotateZ (float angle)
164 : {
165 16 : const float Cos = cosf (angle);
166 16 : const float Sin = sinf (angle);
167 16 : const float tmp_11 = _11;
168 16 : const float tmp_12 = _12;
169 16 : const float tmp_13 = _13;
170 16 : const float tmp_14 = _14;
171 :
172 16 : _11 = Cos * _11 - Sin * _21;
173 16 : _12 = Cos * _12 - Sin * _22;
174 16 : _13 = Cos * _13 - Sin * _23;
175 16 : _14 = Cos * _14 - Sin * _24;
176 :
177 16 : _21 = Sin * tmp_11 + Cos * _21;
178 16 : _22 = Sin * tmp_12 + Cos * _22;
179 16 : _23 = Sin * tmp_13 + Cos * _23;
180 16 : _24 = Sin * tmp_14 + Cos * _24;
181 16 : }
182 :
183 : //Sets the translation of the matrix
184 3 : void CMatrix3D::SetTranslation (float x, float y, float z)
185 : {
186 3 : _11=1.0f; _12=0.0f; _13=0.0f; _14=x;
187 3 : _21=0.0f; _22=1.0f; _23=0.0f; _24=y;
188 3 : _31=0.0f; _32=0.0f; _33=1.0f; _34=z;
189 3 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
190 3 : }
191 :
192 1 : void CMatrix3D::SetTranslation(const CVector3D& vector)
193 : {
194 1 : SetTranslation(vector.X, vector.Y, vector.Z);
195 1 : }
196 :
197 : //Applies a translation to the matrix
198 0 : void CMatrix3D::Translate(float x, float y, float z)
199 : {
200 0 : _14 += x;
201 0 : _24 += y;
202 0 : _34 += z;
203 0 : }
204 :
205 18 : void CMatrix3D::Translate(const CVector3D &vector)
206 : {
207 18 : _14 += vector.X;
208 18 : _24 += vector.Y;
209 18 : _34 += vector.Z;
210 18 : }
211 :
212 0 : void CMatrix3D::PostTranslate(float x, float y, float z)
213 : {
214 : // Equivalent to "m.SetTranslation(x, y, z); *this = *this * m;"
215 0 : _14 += _11*x + _12*y + _13*z;
216 0 : _24 += _21*x + _22*y + _23*z;
217 0 : _34 += _31*x + _32*y + _33*z;
218 0 : _44 += _41*x + _42*y + _43*z;
219 0 : }
220 :
221 32 : CVector3D CMatrix3D::GetTranslation() const
222 : {
223 32 : return CVector3D(_14, _24, _34);
224 : }
225 :
226 : //Clears and sets the scaling of the matrix
227 2 : void CMatrix3D::SetScaling (float x_scale, float y_scale, float z_scale)
228 : {
229 2 : _11=x_scale; _12=0.0f; _13=0.0f; _14=0.0f;
230 2 : _21=0.0f; _22=y_scale; _23=0.0f; _24=0.0f;
231 2 : _31=0.0f; _32=0.0f; _33=z_scale; _34=0.0f;
232 2 : _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
233 2 : }
234 :
235 : //Scales the matrix
236 1 : void CMatrix3D::Scale (float x_scale, float y_scale, float z_scale)
237 : {
238 1 : _11 *= x_scale;
239 1 : _12 *= x_scale;
240 1 : _13 *= x_scale;
241 1 : _14 *= x_scale;
242 :
243 1 : _21 *= y_scale;
244 1 : _22 *= y_scale;
245 1 : _23 *= y_scale;
246 1 : _24 *= y_scale;
247 :
248 1 : _31 *= z_scale;
249 1 : _32 *= z_scale;
250 1 : _33 *= z_scale;
251 1 : _34 *= z_scale;
252 1 : }
253 :
254 : //Returns the transpose of the matrix. For orthonormal
255 : //matrices, this is the same is the inverse matrix
256 0 : CMatrix3D CMatrix3D::GetTranspose() const
257 : {
258 : return CMatrix3D(
259 0 : _11, _21, _31, _41,
260 0 : _12, _22, _32, _42,
261 0 : _13, _23, _33, _43,
262 0 : _14, _24, _34, _44);
263 : }
264 :
265 :
266 : //Get a vector which points to the left of the matrix
267 18 : CVector3D CMatrix3D::GetLeft() const
268 : {
269 18 : return CVector3D(-_11, -_21, -_31);
270 : }
271 :
272 : //Get a vector which points up from the matrix
273 0 : CVector3D CMatrix3D::GetUp() const
274 : {
275 0 : return CVector3D(_12, _22, _32);
276 : }
277 :
278 : //Get a vector which points to front of the matrix
279 13 : CVector3D CMatrix3D::GetIn() const
280 : {
281 13 : return CVector3D(_13, _23, _33);
282 : }
283 :
284 : ///////////////////////////////////////////////////////////////////////////////
285 : // RotateTransposed: rotate a vector by the transpose of this matrix
286 0 : CVector3D CMatrix3D::RotateTransposed(const CVector3D& vector) const
287 : {
288 0 : CVector3D result;
289 0 : RotateTransposed(vector,result);
290 0 : return result;
291 : }
292 :
293 : ///////////////////////////////////////////////////////////////////////////////
294 : // RotateTransposed: rotate a vector by the transpose of this matrix
295 0 : void CMatrix3D::RotateTransposed(const CVector3D& vector,CVector3D& result) const
296 : {
297 0 : result.X = _11*vector.X + _21*vector.Y + _31*vector.Z;
298 0 : result.Y = _12*vector.X + _22*vector.Y + _32*vector.Z;
299 0 : result.Z = _13*vector.X + _23*vector.Y + _33*vector.Z;
300 0 : }
301 :
302 :
303 21 : void CMatrix3D::GetInverse(CMatrix3D& dst) const
304 : {
305 : float tmp[12]; // temp array for pairs
306 : float src[16]; // array of transpose source matrix
307 : float det; // determinant
308 :
309 : // transpose matrix
310 105 : for (int i = 0; i < 4; ++i) {
311 84 : src[i] = _data[i*4];
312 84 : src[i + 4] = _data[i*4 + 1];
313 84 : src[i + 8] = _data[i*4 + 2];
314 84 : src[i + 12] = _data[i*4 + 3];
315 : }
316 :
317 : // calculate pairs for first 8 elements (cofactors)
318 21 : tmp[0] = src[10] * src[15];
319 21 : tmp[1] = src[11] * src[14];
320 21 : tmp[2] = src[9] * src[15];
321 21 : tmp[3] = src[11] * src[13];
322 21 : tmp[4] = src[9] * src[14];
323 21 : tmp[5] = src[10] * src[13];
324 21 : tmp[6] = src[8] * src[15];
325 21 : tmp[7] = src[11] * src[12];
326 21 : tmp[8] = src[8] * src[14];
327 21 : tmp[9] = src[10] * src[12];
328 21 : tmp[10] = src[8] * src[13];
329 21 : tmp[11] = src[9] * src[12];
330 :
331 : // calculate first 8 elements (cofactors)
332 21 : dst._data[0] = (tmp[0]-tmp[1])*src[5] + (tmp[3]-tmp[2])*src[6] + (tmp[4]-tmp[5])*src[7];
333 21 : dst._data[1] = (tmp[1]-tmp[0])*src[4] + (tmp[6]-tmp[7])*src[6] + (tmp[9]-tmp[8])*src[7];
334 21 : dst._data[2] = (tmp[2]-tmp[3])*src[4] + (tmp[7]-tmp[6])*src[5] + (tmp[10]-tmp[11])*src[7];
335 21 : dst._data[3] = (tmp[5]-tmp[4])*src[4] + (tmp[8]-tmp[9])*src[5] + (tmp[11]-tmp[10])*src[6];
336 21 : dst._data[4] = (tmp[1]-tmp[0])*src[1] + (tmp[2]-tmp[3])*src[2] + (tmp[5]-tmp[4])*src[3];
337 21 : dst._data[5] = (tmp[0]-tmp[1])*src[0] + (tmp[7]-tmp[6])*src[2] + (tmp[8]-tmp[9])*src[3];
338 21 : dst._data[6] = (tmp[3]-tmp[2])*src[0] + (tmp[6]-tmp[7])*src[1] + (tmp[11]-tmp[10])*src[3];
339 21 : dst._data[7] = (tmp[4]-tmp[5])*src[0] + (tmp[9]-tmp[8])*src[1] + (tmp[10]-tmp[11])*src[2];
340 :
341 : // calculate pairs for second 8 elements (cofactors)
342 21 : tmp[0] = src[2]*src[7];
343 21 : tmp[1] = src[3]*src[6];
344 21 : tmp[2] = src[1]*src[7];
345 21 : tmp[3] = src[3]*src[5];
346 21 : tmp[4] = src[1]*src[6];
347 21 : tmp[5] = src[2]*src[5];
348 21 : tmp[6] = src[0]*src[7];
349 21 : tmp[7] = src[3]*src[4];
350 21 : tmp[8] = src[0]*src[6];
351 21 : tmp[9] = src[2]*src[4];
352 21 : tmp[10] = src[0]*src[5];
353 21 : tmp[11] = src[1]*src[4];
354 :
355 : // calculate second 8 elements (cofactors)
356 21 : dst._data[8] = (tmp[0]-tmp[1])*src[13] + (tmp[3]-tmp[2])*src[14] + (tmp[4]-tmp[5])*src[15];
357 21 : dst._data[9] = (tmp[1]-tmp[0])*src[12] + (tmp[6]-tmp[7])*src[14] + (tmp[9]-tmp[8])*src[15];
358 21 : dst._data[10] = (tmp[2]-tmp[3])*src[12] + (tmp[7]-tmp[6])*src[13] + (tmp[10]-tmp[11])*src[15];
359 21 : dst._data[11] = (tmp[5]-tmp[4])*src[12] + (tmp[8]-tmp[9])*src[13] + (tmp[11]-tmp[10])*src[14];
360 21 : dst._data[12] = (tmp[2]-tmp[3])*src[10] + (tmp[5]-tmp[4])*src[11] + (tmp[1]-tmp[0])*src[9];
361 21 : dst._data[13] = (tmp[7]-tmp[6])*src[10] + (tmp[8]-tmp[9])*src[11] + (tmp[0]-tmp[1])*src[8];
362 21 : dst._data[14] = (tmp[6]-tmp[7])*src[9] + (tmp[11]-tmp[10])*src[11] + (tmp[3]-tmp[2])*src[8];
363 21 : dst._data[15] = (tmp[10]-tmp[11])*src[10] + (tmp[4]-tmp[5])*src[8] + (tmp[9]-tmp[8])*src[9];
364 :
365 : // calculate matrix inverse
366 21 : det=src[0]*dst._data[0]+src[1]*dst._data[1]+src[2]*dst._data[2]+src[3]*dst._data[3];
367 21 : det = 1/det;
368 357 : for ( int j = 0; j < 16; j++) {
369 336 : dst._data[j] *= det;
370 : }
371 21 : }
372 :
373 13 : CMatrix3D CMatrix3D::GetInverse() const
374 : {
375 13 : CMatrix3D r;
376 13 : GetInverse(r);
377 13 : return r;
378 : }
379 :
380 3 : void CMatrix3D::Rotate(const CQuaternion& quat)
381 : {
382 3 : CMatrix3D rotationMatrix=quat.ToMatrix();
383 3 : Concatenate(rotationMatrix);
384 3 : }
385 :
386 4 : CQuaternion CMatrix3D::GetRotation() const
387 : {
388 4 : float tr = _data2d[0][0] + _data2d[1][1] + _data2d[2][2];
389 :
390 4 : int next[] = { 1, 2, 0 };
391 :
392 : float quat[4];
393 :
394 4 : if (tr > 0.f)
395 : {
396 2 : float s = sqrtf(tr + 1.f);
397 2 : quat[3] = s * 0.5f;
398 2 : s = 0.5f / s;
399 2 : quat[0] = (_data2d[1][2] - _data2d[2][1]) * s;
400 2 : quat[1] = (_data2d[2][0] - _data2d[0][2]) * s;
401 2 : quat[2] = (_data2d[0][1] - _data2d[1][0]) * s;
402 : }
403 : else
404 : {
405 2 : int i = 0;
406 2 : if (_data2d[1][1] > _data2d[0][0]) i = 1;
407 2 : if (_data2d[2][2] > _data2d[i][i]) i = 2;
408 2 : int j = next[i];
409 2 : int k = next[j];
410 :
411 2 : float s = sqrtf((_data2d[i][i] - (_data2d[j][j] + _data2d[k][k])) + 1.f);
412 2 : quat[i] = s * 0.5f;
413 :
414 2 : if (s != 0.f) s = 0.5f / s;
415 :
416 2 : quat[3] = (_data2d[j][k] - _data2d[k][j]) * s;
417 2 : quat[j] = (_data2d[i][j] + _data2d[j][i]) * s;
418 2 : quat[k] = (_data2d[i][k] + _data2d[k][i]) * s;
419 : }
420 :
421 4 : return CQuaternion(quat[0], quat[1], quat[2], quat[3]);
422 : }
423 :
424 0 : void CMatrix3D::SetRotation(const CQuaternion& quat)
425 : {
426 0 : quat.ToMatrix(*this);
427 0 : }
428 :
429 18 : float CMatrix3D::GetYRotation() const
430 : {
431 : // Project the X axis vector onto the XZ plane
432 18 : CVector3D axis = -GetLeft();
433 18 : axis.Y = 0;
434 :
435 : // Normalise projected vector
436 :
437 18 : float len = axis.Length();
438 18 : if (len < 0.0001f)
439 1 : return 0.f;
440 17 : axis *= 1.0f/len;
441 :
442 : // Negate the return angle to match the SetYRotation convention
443 17 : return -atan2(axis.Z, axis.X);
444 3 : }
|
