Line data Source code
1 0 : /*
2 :
3 : OOMatrix.m
4 :
5 : Oolite
6 : Copyright (C) 2004-2013 Giles C Williams and contributors
7 :
8 : This program is free software; you can redistribute it and/or
9 : modify it under the terms of the GNU General Public License
10 : as published by the Free Software Foundation; either version 2
11 : of the License, or (at your option) any later version.
12 :
13 : This program is distributed in the hope that it will be useful,
14 : but WITHOUT ANY WARRANTY; without even the implied warranty of
15 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 : GNU General Public License for more details.
17 :
18 : You should have received a copy of the GNU General Public License
19 : along with this program; if not, write to the Free Software
20 : Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
21 : MA 02110-1301, USA.
22 :
23 : */
24 :
25 :
26 : #include "OOMaths.h"
27 : #if OOMATHS_OPENGL_INTEGRATION
28 : #import "OOOpenGLExtensionManager.h"
29 : #endif
30 :
31 0 : const OOMatrix kIdentityMatrix =
32 : { .m = {
33 : {1.0f, 0.0f, 0.0f, 0.0f},
34 : {0.0f, 1.0f, 0.0f, 0.0f},
35 : {0.0f, 0.0f, 1.0f, 0.0f},
36 : {0.0f, 0.0f, 0.0f, 1.0f}
37 : }};
38 0 : const OOMatrix kZeroMatrix = { .m = {
39 : {0.0f, 0.0f, 0.0f, 0.0f},
40 : {0.0f, 0.0f, 0.0f, 0.0f},
41 : {0.0f, 0.0f, 0.0f, 0.0f},
42 : {0.0f, 0.0f, 0.0f, 0.0f},
43 : }};
44 :
45 :
46 0 : OOMatrix OOMatrixForRotation(Vector axis, OOScalar angle)
47 : {
48 : axis = vector_normal(axis);
49 :
50 : OOScalar x = axis.x, y = axis.y, z = axis.z;
51 : OOScalar s = sin(angle), c = cos(angle);
52 : OOScalar t = 1.0f - c;
53 :
54 : // Lots of opportunity for common subexpression elimintation here, but I'll leave it to the compiler for now.
55 : return OOMatrixConstruct
56 : (
57 : t * x * x + c, t * x * y + s * z, t * x * z - s * y, 0.0f,
58 : t * x * y - s * z, t * y * y + c, t * y * z + s * x, 0.0f,
59 : t * x * y + s * y, t * y * z - s * x, t * z * z + c, 0.0f,
60 : 0.0f, 0.0f, 0.0f, 1.0f
61 : );
62 : }
63 :
64 :
65 0 : OOMatrix OOMatrixForQuaternionRotation(Quaternion orientation)
66 : {
67 : OOScalar w, wz, wy, wx;
68 : OOScalar x, xz, xy, xx;
69 : OOScalar y, yz, yy;
70 : OOScalar z, zz;
71 :
72 : Quaternion q = orientation;
73 : quaternion_normalize(&q);
74 :
75 : w = q.w;
76 : z = q.z;
77 : y = q.y;
78 : x = q.x;
79 :
80 : xx = 2.0f * x; yy = 2.0f * y; zz = 2.0f * z;
81 : wx = w * xx; wy = w * yy; wz = w * zz;
82 : xx = x * xx; xy = x * yy; xz = x * zz;
83 : yy = y * yy; yz = y * zz;
84 : zz = z * zz;
85 :
86 : return OOMatrixConstruct
87 : (
88 : 1.0f - yy - zz, xy - wz, xz + wy, 0.0f,
89 : xy + wz, 1.0f - xx - zz, yz - wx, 0.0f,
90 : xz - wy, yz + wx, 1.0f - xx - yy, 0.0f,
91 : 0.0f, 0.0f, 0.0f, 1.0f
92 : );
93 : }
94 :
95 :
96 0 : bool OOMatrixEqual(OOMatrix a, OOMatrix b)
97 : {
98 : OOScalar *ma = &a.m[0][0];
99 : OOScalar *mb = &b.m[0][0];
100 :
101 : unsigned i;
102 : for (i = 0; i < 16; i++)
103 : {
104 : if (*ma++ != *mb++) return false;
105 : }
106 :
107 : return true;
108 : }
109 :
110 :
111 0 : OOMatrix OOMatrixMultiply(OOMatrix a, OOMatrix b)
112 : {
113 : unsigned i = 0;
114 : OOMatrix r;
115 :
116 : // This is amenable to significant optimization with Altivec, and presumably also SSE.
117 : for (i = 0; i != 4; ++i)
118 : {
119 : r.m[i][0] = a.m[i][0] * b.m[0][0] + a.m[i][1] * b.m[1][0] + a.m[i][2] * b.m[2][0] + a.m[i][3] * b.m[3][0];
120 : r.m[i][1] = a.m[i][0] * b.m[0][1] + a.m[i][1] * b.m[1][1] + a.m[i][2] * b.m[2][1] + a.m[i][3] * b.m[3][1];
121 : r.m[i][2] = a.m[i][0] * b.m[0][2] + a.m[i][1] * b.m[1][2] + a.m[i][2] * b.m[2][2] + a.m[i][3] * b.m[3][2];
122 : r.m[i][3] = a.m[i][0] * b.m[0][3] + a.m[i][1] * b.m[1][3] + a.m[i][2] * b.m[2][3] + a.m[i][3] * b.m[3][3];
123 : }
124 :
125 : return r;
126 : }
127 :
128 :
129 0 : Vector OOVectorMultiplyMatrix(Vector v, OOMatrix m)
130 : {
131 : OOScalar x, y, z, w;
132 :
133 : x = m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z + m.m[3][0];
134 : y = m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z + m.m[3][1];
135 : z = m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z + m.m[3][2];
136 : w = m.m[0][3] * v.x + m.m[1][3] * v.y + m.m[2][3] * v.z + m.m[3][3];
137 :
138 : w = 1.0f/w;
139 : return make_vector(x * w, y * w, z * w);
140 : }
141 :
142 : // HPVect: this loses precision because the matrix is single-precision
143 : // Best used for rotation matrices only - use HPvector_add for
144 : // translations of vectors if possible
145 0 : HPVector OOHPVectorMultiplyMatrix(HPVector v, OOMatrix m)
146 : {
147 : OOScalar x, y, z, w;
148 :
149 : x = m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z + m.m[3][0];
150 : y = m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z + m.m[3][1];
151 : z = m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z + m.m[3][2];
152 : w = m.m[0][3] * v.x + m.m[1][3] * v.y + m.m[2][3] * v.z + m.m[3][3];
153 :
154 : w = 1.0f/w;
155 : return make_HPvector(x * w, y * w, z * w);
156 : }
157 :
158 :
159 0 : OOMatrix OOMatrixOrthogonalize(OOMatrix m)
160 : {
161 : // Simple orthogonalization: make everything orthogonal to everything else.
162 :
163 : Vector i;// = { m.m[0][0], m.m[1][0], m.m[2][0] }; // Overwritten without being used
164 : Vector j = { m.m[0][1], m.m[1][1], m.m[2][1] };
165 : Vector k = { m.m[0][2], m.m[1][2], m.m[2][2] };
166 :
167 : k = vector_normal(k);
168 : i = vector_normal(cross_product(j, k));
169 : j = cross_product(k, i);
170 :
171 : m.m[0][0] = i.x; m.m[1][0] = i.y; m.m[2][0] = i.z;
172 : m.m[0][1] = j.x; m.m[1][1] = j.y; m.m[2][1] = j.z;
173 : m.m[0][2] = k.x; m.m[1][2] = k.y; m.m[2][2] = k.z;
174 :
175 : return m;
176 : }
177 :
178 :
179 : #if __OBJC__
180 : NSString *OOMatrixDescription(OOMatrix matrix)
181 : {
182 : return [NSString stringWithFormat:@"{{%g, %g, %g, %g}, {%g, %g, %g, %g}, {%g, %g, %g, %g}, {%g, %g, %g, %g}}",
183 : matrix.m[0][0], matrix.m[0][1], matrix.m[0][2], matrix.m[0][3],
184 : matrix.m[1][0], matrix.m[1][1], matrix.m[1][2], matrix.m[1][3],
185 : matrix.m[2][0], matrix.m[2][1], matrix.m[2][2], matrix.m[2][3],
186 : matrix.m[3][0], matrix.m[3][1], matrix.m[3][2], matrix.m[3][3]];
187 : }
188 : #endif
189 :
190 :
191 0 : OOMatrix OOMatrixForBillboard(HPVector bbPos, HPVector eyePos)
192 : {
193 : Vector v0, v1, v2, arbv;
194 :
195 : v0 = HPVectorToVector(HPvector_subtract(bbPos, eyePos));
196 : // once the subtraction is done safe to lose precision since
197 : // we're now dealing with relatively small vectors
198 : v0 = vector_normal_or_fallback(v0, kBasisZVector);
199 :
200 : // arbitrary axis - not aligned with v0
201 : if (EXPECT_NOT(v0.x == 0.0 && v0.y == 0.0)) arbv = kBasisXVector;
202 : else arbv = kBasisZVector;
203 :
204 : v1 = cross_product(v0, arbv); // 90 degrees to (v0 x arb1)
205 : v2 = cross_product(v0, v1); // 90 degrees to (v0 x v1)
206 :
207 : return OOMatrixFromBasisVectors(v1, v2, v0);
208 : }
209 :
210 :
211 0 : void OOMatrixRowSwap(OOMatrix *M, int row1, int row2)
212 : {
213 : if (row1<0 || row1 >= 4 || row2 < 0 || row2 >= 4 || row1 == row2 )
214 : {
215 : return;
216 : }
217 : float x;
218 : int i;
219 : for (i = 0; i < 4; i++)
220 : {
221 : x = M->m[row1][i];
222 : M->m[row1][i] = M->m[row2][i];
223 : M->m[row2][i] = x;
224 : }
225 : return;
226 : }
227 :
228 0 : void OOMatrixRowScale(OOMatrix *M, int row, OOScalar factor)
229 : {
230 : if (row < 0 || row >= 4)
231 : {
232 : return;
233 : }
234 : int i;
235 : for (i = 0; i < 4; i++)
236 : {
237 : M->m[row][i] *= factor;
238 : }
239 : return;
240 : }
241 :
242 0 : void OOMatrixRowOperation(OOMatrix *M, int row1, OOScalar factor1, int row2, OOScalar factor2)
243 : {
244 : if (row1 < 0 || row1 >= 4 || row2 < 0 || row2 >= 4)
245 : {
246 : return;
247 : }
248 : int i;
249 : for (i = 0; i < 4; i++)
250 : {
251 : M->m[row1][i] = factor1 * M->m[row1][i] + factor2 * M->m[row2][i];
252 : }
253 : return;
254 : }
255 :
256 0 : void OOMatrixColumnSwap(OOMatrix *M, int column1, int column2)
257 : {
258 : if (column1<0 || column1 >= 4 || column2 < 0 || column2 >= 4 || column1 == column2 )
259 : {
260 : return;
261 : }
262 : float x;
263 : int i;
264 : for (i = 0; i < 4; i++)
265 : {
266 : x = M->m[i][column1];
267 : M->m[i][column1] = M->m[i][column2];
268 : M->m[i][column2] = x;
269 : }
270 : return;
271 : }
272 :
273 0 : void OOMatrixColumnScale(OOMatrix *M, int column, OOScalar factor)
274 : {
275 : if (column < 0 || column >= 4)
276 : {
277 : return;
278 : }
279 : int i;
280 : for (i = 0; i < 4; i++)
281 : {
282 : M->m[i][column] *= factor;
283 : }
284 : return;
285 : }
286 :
287 0 : void OOMatrixColumnOperation(OOMatrix *M, int column1, OOScalar factor1, int column2, OOScalar factor2)
288 : {
289 : if (column1 < 0 || column1 >= 4 || column2 < 0 || column2 >= 4)
290 : {
291 : return;
292 : }
293 : int i;
294 : for (i = 0; i < 4; i++)
295 : {
296 : M->m[i][column1] = factor1 * M->m[i][column1] + factor2 * M->m[i][column2];
297 : }
298 : return;
299 : }
300 :
301 0 : OOMatrix OOMatrixLeftTransform(OOMatrix A, OOMatrix B)
302 : {
303 : int i, j;
304 : bool found;
305 : for (i = 0; i < 4; i++)
306 : {
307 : if (A.m[i][i] == 0.0)
308 : {
309 : found = false;
310 : for (j = i+1; j < 4; j++)
311 : {
312 : if (A.m[j][i] != 0.0)
313 : {
314 : found = true;
315 : OOMatrixColumnSwap(&A,i,j);
316 : OOMatrixColumnSwap(&B,i,j);
317 : break;
318 : }
319 : }
320 : if (!found)
321 : {
322 : return kZeroMatrix;
323 : }
324 : }
325 : OOMatrixColumnScale(&B, i, 1/A.m[i][i]);
326 : OOMatrixColumnScale(&A, i, 1/A.m[i][i]);
327 : for (j = i+1; j < 4; j++)
328 : {
329 : OOMatrixColumnOperation(&B, j, 1, i, -A.m[i][j]);
330 : OOMatrixColumnOperation(&A, j, 1, i, -A.m[i][j]);
331 : }
332 : }
333 : for (i = 3; i > 0; i--)
334 : {
335 : for (j = 0; j < i; j++)
336 : {
337 : OOMatrixColumnOperation(&B, j, 1, i, -A.m[i][j]);
338 : OOMatrixColumnOperation(&A, j, 1, i, -A.m[i][j]);
339 : }
340 : }
341 : return B;
342 : }
343 :
344 0 : OOMatrix OOMatrixRightTransform(OOMatrix A, OOMatrix B)
345 : {
346 : int i, j;
347 : bool found;
348 : for (i = 0; i < 4; i++)
349 : {
350 : if (A.m[i][i] == 0.0)
351 : {
352 : found = false;
353 : for (j = i+1; j < 4; j++)
354 : {
355 : if (A.m[j][i] != 0.0)
356 : {
357 : found = true;
358 : OOMatrixRowSwap(&A,i,j);
359 : OOMatrixRowSwap(&B,i,j);
360 : break;
361 : }
362 : }
363 : if (!found)
364 : {
365 : return kZeroMatrix;
366 : }
367 : }
368 : OOMatrixRowScale(&B, i, 1/A.m[i][i]);
369 : OOMatrixRowScale(&A, i, 1/A.m[i][i]);
370 : for (j = i+1; j < 4; j++)
371 : {
372 : if (A.m[j][i] != 0.0)
373 : {
374 : OOMatrixRowOperation(&B, j, 1, i, -A.m[j][i]);
375 : OOMatrixRowOperation(&A, j, 1, i, -A.m[j][i]);
376 : }
377 : }
378 : }
379 : for (i = 3; i > 0; i--)
380 : {
381 : for (j = 0; j < i; j++)
382 : {
383 : if (A.m[j][i] != 0.0)
384 : {
385 : OOMatrixRowOperation(&B, j, 1, i, -A.m[j][i]);
386 : OOMatrixRowOperation(&A, j, 1, i, -A.m[j][i]);
387 : }
388 : }
389 : }
390 : return B;
391 : }
392 :
393 0 : OOMatrix OOMatrixInverse(OOMatrix M)
394 : {
395 : return OOMatrixLeftTransform(M, kIdentityMatrix);
396 : }
397 :
398 0 : OOMatrix OOMatrixInverseWithDeterminant(OOMatrix M, OOScalar *d)
399 : {
400 : int i, j;
401 : OOMatrix B = kIdentityMatrix;
402 : int best_row;
403 : float max_value;
404 :
405 : *d = 1.0;
406 : for (i = 0; i < 4; i++)
407 : {
408 : max_value = fabsf(M.m[i][i]);
409 : best_row = i;
410 : for (j = i+1; j < 4; j++)
411 : {
412 : if (fabs(M.m[j][i]) > max_value)
413 : {
414 : best_row = j;
415 : max_value = fabs(M.m[j][i]);
416 : }
417 : }
418 : if (max_value == 0.0)
419 : {
420 : *d = 0.0;
421 : return kZeroMatrix;
422 : }
423 : if (best_row != i)
424 : {
425 : OOMatrixRowSwap(&M,i,best_row);
426 : OOMatrixRowSwap(&B,i,best_row);
427 : *d *= -1;
428 : }
429 : *d /= M.m[i][i];
430 : OOMatrixRowScale(&B, i, 1/M.m[i][i]);
431 : OOMatrixRowScale(&M, i, 1/M.m[i][i]);
432 : for (j = i+1; j < 4; j++)
433 : {
434 : if (M.m[j][i] != 0.0)
435 : {
436 : OOMatrixRowOperation(&B, j, 1, i, -M.m[j][i]);
437 : OOMatrixRowOperation(&M, j, 1, i, -M.m[j][i]);
438 : }
439 : }
440 : }
441 : for (i = 3; i > 0; i--)
442 : {
443 : for (j = 0; j < i; j++)
444 : {
445 : if (M.m[j][i] != 0.0)
446 : {
447 : OOMatrixRowOperation(&B, j, 1, i, -M.m[j][i]);
448 : OOMatrixRowOperation(&M, j, 1, i, -M.m[j][i]);
449 : }
450 : }
451 : }
452 : return B;
453 : }
454 :
455 : #if OOMATHS_OPENGL_INTEGRATION
456 :
457 0 : void GLUniformMatrix3(int location, OOMatrix M)
458 : {
459 : OOScalar m[9];
460 : m[0] = M.m[0][0];
461 : m[1] = M.m[0][1];
462 : m[2] = M.m[0][2];
463 : m[3] = M.m[1][0];
464 : m[4] = M.m[1][1];
465 : m[5] = M.m[1][2];
466 : m[6] = M.m[2][0];
467 : m[7] = M.m[2][1];
468 : m[8] = M.m[2][2];
469 : OOGL(glUniformMatrix3fvARB(location, 1, NO, m));
470 : }
471 :
472 : #endif
473 :
|
