source: trunk/src/math/vector.cpp @ 1257

Last change on this file since 1257 was 1257, checked in by sam, 8 years ago

math: add inversion code for 2×2 and 3×3 matrices, and transposition
code for all matrices.

  • Property svn:keywords set to Id
File size: 12.4 KB
Line 
1//
2// Lol Engine
3//
4// Copyright: (c) 2010-2012 Sam Hocevar <sam@hocevar.net>
5//   This program is free software; you can redistribute it and/or
6//   modify it under the terms of the Do What The Fuck You Want To
7//   Public License, Version 2, as published by Sam Hocevar. See
8//   http://sam.zoy.org/projects/COPYING.WTFPL for more details.
9//
10
11#if defined HAVE_CONFIG_H
12#   include "config.h"
13#endif
14
15#if defined _XBOX
16#   define _USE_MATH_DEFINES /* for M_PI */
17#   include <xtl.h>
18#   undef near /* Fuck Microsoft */
19#   undef far /* Fuck Microsoft again */
20#elif defined _WIN32
21#   define _USE_MATH_DEFINES /* for M_PI */
22#   define WIN32_LEAN_AND_MEAN
23#   include <windows.h>
24#   undef near /* Fuck Microsoft */
25#   undef far /* Fuck Microsoft again */
26#endif
27
28#include <cmath> /* for M_PI */
29#include <cstdlib> /* free() */
30#include <cstring> /* strdup() */
31
32#include "core.h"
33
34using namespace std;
35
36namespace lol
37{
38
39static inline float det2(float a, float b,
40                         float c, float d)
41{
42    return a * d - b * c;
43}
44
45static inline float det3(float a, float b, float c,
46                         float d, float e, float f,
47                         float g, float h, float i)
48{
49    return a * (e * i - h * f)
50         + b * (f * g - i * d)
51         + c * (d * h - g * e);
52}
53
54static inline float cofact(mat2 const &mat, int i, int j)
55{
56    return mat[(i + 1) & 1][(j + 1) & 1] * (((i + j) & 1) ? -1.0f : 1.0f);
57}
58
59static inline float cofact(mat3 const &mat, int i, int j)
60{
61    return det2(mat[(i + 1) % 3][(j + 1) % 3],
62                mat[(i + 2) % 3][(j + 1) % 3],
63                mat[(i + 1) % 3][(j + 2) % 3],
64                mat[(i + 2) % 3][(j + 2) % 3]);
65}
66
67static inline float cofact(mat4 const &mat, int i, int j)
68{
69    return det3(mat[(i + 1) & 3][(j + 1) & 3],
70                mat[(i + 2) & 3][(j + 1) & 3],
71                mat[(i + 3) & 3][(j + 1) & 3],
72                mat[(i + 1) & 3][(j + 2) & 3],
73                mat[(i + 2) & 3][(j + 2) & 3],
74                mat[(i + 3) & 3][(j + 2) & 3],
75                mat[(i + 1) & 3][(j + 3) & 3],
76                mat[(i + 2) & 3][(j + 3) & 3],
77                mat[(i + 3) & 3][(j + 3) & 3]) * (((i + j) & 1) ? -1.0f : 1.0f);
78}
79
80template<> float determinant(mat2 const &mat)
81{
82    return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
83}
84
85template<> mat2 transpose(mat2 const &mat)
86{
87    mat2 ret;
88    for (int j = 0; j < 2; j++)
89        for (int i = 0; i < 2; i++)
90            ret[j][i] = mat[i][j];
91    return ret;
92}
93
94template<> mat2 inverse(mat2 const &mat)
95{
96    mat2 ret;
97    float d = determinant(mat);
98    if (d)
99    {
100        d = 1.0f / d;
101        for (int j = 0; j < 2; j++)
102            for (int i = 0; i < 2; i++)
103                ret[j][i] = cofact(mat, i, j) * d;
104    }
105    return ret;
106}
107
108template<> float determinant(mat3 const &mat)
109{
110    return det3(mat[0][0], mat[0][1], mat[0][2],
111                mat[1][0], mat[1][1], mat[1][2],
112                mat[2][0], mat[2][1], mat[2][2]);
113}
114
115template<> mat3 transpose(mat3 const &mat)
116{
117    mat3 ret;
118    for (int j = 0; j < 3; j++)
119        for (int i = 0; i < 3; i++)
120            ret[j][i] = mat[i][j];
121    return ret;
122}
123
124template<> mat3 inverse(mat3 const &mat)
125{
126    mat3 ret;
127    float d = determinant(mat);
128    if (d)
129    {
130        d = 1.0f / d;
131        for (int j = 0; j < 3; j++)
132            for (int i = 0; i < 3; i++)
133                ret[j][i] = cofact(mat, i, j) * d;
134    }
135    return ret;
136}
137
138template<> float determinant(mat4 const &mat)
139{
140    float ret = 0;
141    for (int n = 0; n < 4; n++)
142        ret += mat[n][0] * cofact(mat, n, 0);
143    return ret;
144}
145
146template<> mat4 transpose(mat4 const &mat)
147{
148    mat4 ret;
149    for (int j = 0; j < 4; j++)
150        for (int i = 0; i < 4; i++)
151            ret[j][i] = mat[i][j];
152    return ret;
153}
154
155template<> mat4 inverse(mat4 const &mat)
156{
157    mat4 ret;
158    float d = determinant(mat);
159    if (d)
160    {
161        d = 1.0f / d;
162        for (int j = 0; j < 4; j++)
163            for (int i = 0; i < 4; i++)
164                ret[j][i] = cofact(mat, i, j) * d;
165    }
166    return ret;
167}
168
169template<> void vec2::printf() const
170{
171    Log::Debug("[ %6.6f %6.6f ]\n", x, y);
172}
173
174template<> void ivec2::printf() const
175{
176    Log::Debug("[ %i %i ]\n", x, y);
177}
178
179template<> void cmplx::printf() const
180{
181    Log::Debug("[ %6.6f %6.6f ]\n", x, y);
182}
183
184template<> void vec3::printf() const
185{
186    Log::Debug("[ %6.6f %6.6f %6.6f ]\n", x, y, z);
187}
188
189template<> void ivec3::printf() const
190{
191    Log::Debug("[ %i %i %i ]\n", x, y, z);
192}
193
194template<> void vec4::printf() const
195{
196    Log::Debug("[ %6.6f %6.6f %6.6f %6.6f ]\n", x, y, z, w);
197}
198
199template<> void ivec4::printf() const
200{
201    Log::Debug("[ %i %i %i %i ]\n", x, y, z, w);
202}
203
204template<> void quat::printf() const
205{
206    Log::Debug("[ %6.6f %6.6f %6.6f %6.6f ]\n", x, y, z, w);
207}
208
209template<> void mat2::printf() const
210{
211    mat2 const &p = *this;
212
213    Log::Debug("[ %6.6f %6.6f\n", p[0][0], p[1][0]);
214    Log::Debug("  %6.6f %6.6f ]\n", p[0][1], p[1][1]);
215}
216
217template<> void mat3::printf() const
218{
219    mat3 const &p = *this;
220
221    Log::Debug("[ %6.6f %6.6f %6.6f\n", p[0][0], p[1][0], p[2][0]);
222    Log::Debug("  %6.6f %6.6f %6.6f\n", p[0][1], p[1][1], p[2][1]);
223    Log::Debug("  %6.6f %6.6f %6.6f ]\n", p[0][2], p[1][2], p[2][2]);
224}
225
226template<> void mat4::printf() const
227{
228    mat4 const &p = *this;
229
230    Log::Debug("[ %6.6f %6.6f %6.6f %6.6f\n",
231               p[0][0], p[1][0], p[2][0], p[3][0]);
232    Log::Debug("  %6.6f %6.6f %6.6f %6.6f\n",
233               p[0][1], p[1][1], p[2][1], p[3][1]);
234    Log::Debug("  %6.6f %6.6f %6.6f %6.6f\n",
235               p[0][2], p[1][2], p[2][2], p[3][2]);
236    Log::Debug("  %6.6f %6.6f %6.6f %6.6f ]\n",
237               p[0][3], p[1][3], p[2][3], p[3][3]);
238}
239
240#if !defined __ANDROID__
241template<> std::ostream &operator<<(std::ostream &stream, ivec2 const &v)
242{
243    return stream << "(" << v.x << ", " << v.y << ")";
244}
245
246template<> std::ostream &operator<<(std::ostream &stream, icmplx const &v)
247{
248    return stream << "(" << v.x << ", " << v.y << ")";
249}
250
251template<> std::ostream &operator<<(std::ostream &stream, ivec3 const &v)
252{
253    return stream << "(" << v.x << ", " << v.y << ", " << v.z << ")";
254}
255
256template<> std::ostream &operator<<(std::ostream &stream, ivec4 const &v)
257{
258    return stream << "(" << v.x << ", " << v.y << ", "
259                         << v.z << ", " << v.w << ")";
260}
261
262template<> std::ostream &operator<<(std::ostream &stream, iquat const &v)
263{
264    return stream << "(" << v.x << ", " << v.y << ", "
265                         << v.z << ", " << v.w << ")";
266}
267
268template<> std::ostream &operator<<(std::ostream &stream, vec2 const &v)
269{
270    return stream << "(" << v.x << ", " << v.y << ")";
271}
272
273template<> std::ostream &operator<<(std::ostream &stream, cmplx const &v)
274{
275    return stream << "(" << v.x << ", " << v.y << ")";
276}
277
278template<> std::ostream &operator<<(std::ostream &stream, vec3 const &v)
279{
280    return stream << "(" << v.x << ", " << v.y << ", " << v.z << ")";
281}
282
283template<> std::ostream &operator<<(std::ostream &stream, vec4 const &v)
284{
285    return stream << "(" << v.x << ", " << v.y << ", "
286                         << v.z << ", " << v.w << ")";
287}
288
289template<> std::ostream &operator<<(std::ostream &stream, quat const &v)
290{
291    return stream << "(" << v.x << ", " << v.y << ", "
292                         << v.z << ", " << v.w << ")";
293}
294
295template<> std::ostream &operator<<(std::ostream &stream, mat4 const &m)
296{
297    stream << "((" << m[0][0] << ", " << m[1][0]
298            << ", " << m[2][0] << ", " << m[3][0] << "), ";
299    stream << "(" << m[0][1] << ", " << m[1][1]
300           << ", " << m[2][1] << ", " << m[3][1] << "), ";
301    stream << "(" << m[0][2] << ", " << m[1][2]
302           << ", " << m[2][2] << ", " << m[3][2] << "), ";
303    stream << "(" << m[0][3] << ", " << m[1][3]
304           << ", " << m[2][3] << ", " << m[3][3] << "))";
305    return stream;
306}
307#endif
308
309template<> mat4 mat4::translate(float x, float y, float z)
310{
311    mat4 ret(1.0f);
312    ret[3][0] = x;
313    ret[3][1] = y;
314    ret[3][2] = z;
315    return ret;
316}
317
318template<> mat4 mat4::translate(vec3 v)
319{
320    return translate(v.x, v.y, v.z);
321}
322
323template<> mat4 mat4::rotate(float angle, float x, float y, float z)
324{
325    angle *= (M_PI / 180.0f);
326
327    float st = sinf(angle);
328    float ct = cosf(angle);
329
330    float len = sqrtf(x * x + y * y + z * z);
331    float invlen = len ? 1.0f / len : 0.0f;
332    x *= invlen;
333    y *= invlen;
334    z *= invlen;
335
336    float mtx = (1.0f - ct) * x;
337    float mty = (1.0f - ct) * y;
338    float mtz = (1.0f - ct) * z;
339
340    mat4 ret(1.0f);
341
342    ret[0][0] = x * mtx + ct;
343    ret[0][1] = x * mty + st * z;
344    ret[0][2] = x * mtz - st * y;
345
346    ret[1][0] = y * mtx - st * z;
347    ret[1][1] = y * mty + ct;
348    ret[1][2] = y * mtz + st * x;
349
350    ret[2][0] = z * mtx + st * y;
351    ret[2][1] = z * mty - st * x;
352    ret[2][2] = z * mtz + ct;
353
354    return ret;
355}
356
357template<> mat4 mat4::rotate(float angle, vec3 v)
358{
359    return rotate(angle, v.x, v.y, v.z);
360}
361
362template<> mat4 mat4::rotate(quat q)
363{
364    mat4 ret(1.0f);
365    float n = norm(q);
366
367    if (!n)
368        return ret;
369
370    float s = 2.0f / n;
371
372    ret[0][0] = 1.0f - s * (q.y * q.y + q.z * q.z);
373    ret[0][1] = s * (q.x * q.y - q.z * q.w);
374    ret[0][2] = s * (q.x * q.z + q.y * q.w);
375
376    ret[1][0] = s * (q.x * q.y + q.z * q.w);
377    ret[1][1] = 1.0f - s * (q.z * q.z + q.x * q.x);
378    ret[1][2] = s * (q.y * q.z - q.x * q.w);
379
380    ret[2][0] = s * (q.x * q.z - q.y * q.w);
381    ret[2][1] = s * (q.y * q.z + q.x * q.w);
382    ret[2][2] = 1.0f - s * (q.x * q.x + q.y * q.y);
383
384    return ret;
385}
386
387template<> quat::Quat(mat4 const &m)
388{
389    /* See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/christian.htm for a version with no branches */
390    float t = m[0][0] + m[1][1] + m[2][2];
391    if (t > 0)
392    {
393        w = 0.5f * sqrtf(1.0f + t);
394        float s = 0.25f / w;
395        x = s * (m[2][1] - m[1][2]);
396        y = s * (m[0][2] - m[2][0]);
397        z = s * (m[1][0] - m[0][1]);
398    }
399    else if (m[0][0] > m[1][1] && m[0][0] > m[2][2])
400    {
401        x = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
402        float s = 0.25f / x;
403        y = s * (m[1][0] + m[0][1]);
404        z = s * (m[0][2] + m[2][0]);
405        w = s * (m[2][1] - m[1][2]);
406    }
407    else if (m[1][1] > m[2][2])
408    {
409        y = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
410        float s = 0.25f / y;
411        x = s * (m[1][0] + m[0][1]);
412        z = s * (m[2][1] + m[1][2]);
413        w = s * (m[0][2] - m[2][0]);
414    }
415    else
416    {
417        z = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
418        float s = 0.25f / z;
419        x = s * (m[0][2] + m[2][0]);
420        y = s * (m[2][1] + m[1][2]);
421        w = s * (m[1][0] - m[0][1]);
422    }
423}
424
425template<> mat4 mat4::lookat(vec3 eye, vec3 center, vec3 up)
426{
427    vec3 v3 = normalize(eye - center);
428    vec3 v2 = normalize(up);
429    vec3 v1 = normalize(cross(v2, v3));
430    v2 = cross(v3, v1);
431
432    mat4 orient(1.0f);
433    orient[0][0] = v1.x;
434    orient[0][1] = v2.x;
435    orient[0][2] = v3.x;
436    orient[1][0] = v1.y;
437    orient[1][1] = v2.y;
438    orient[1][2] = v3.y;
439    orient[2][0] = v1.z;
440    orient[2][1] = v2.z;
441    orient[2][2] = v3.z;
442
443    return orient * mat4::translate(-eye);
444}
445
446template<> mat4 mat4::ortho(float left, float right, float bottom,
447                            float top, float near, float far)
448{
449    float invrl = (right != left) ? 1.0f / (right - left) : 0.0f;
450    float invtb = (top != bottom) ? 1.0f / (top - bottom) : 0.0f;
451    float invfn = (far != near) ? 1.0f / (far - near) : 0.0f;
452
453    mat4 ret(0.0f);
454    ret[0][0] = 2.0f * invrl;
455    ret[1][1] = 2.0f * invtb;
456    ret[2][2] = -2.0f * invfn;
457    ret[3][0] = - (right + left) * invrl;
458    ret[3][1] = - (top + bottom) * invtb;
459    ret[3][2] = - (far + near) * invfn;
460    ret[3][3] = 1.0f;
461    return ret;
462}
463
464template<> mat4 mat4::frustum(float left, float right, float bottom,
465                              float top, float near, float far)
466{
467    float invrl = (right != left) ? 1.0f / (right - left) : 0.0f;
468    float invtb = (top != bottom) ? 1.0f / (top - bottom) : 0.0f;
469    float invfn = (far != near) ? 1.0f / (far - near) : 0.0f;
470
471    mat4 ret(0.0f);
472    ret[0][0] = 2.0f * near * invrl;
473    ret[1][1] = 2.0f * near * invtb;
474    ret[2][0] = (right + left) * invrl;
475    ret[2][1] = (top + bottom) * invtb;
476    ret[2][2] = - (far + near) * invfn;
477    ret[2][3] = -1.0f;
478    ret[3][2] = -2.0f * far * near * invfn;
479    return ret;
480}
481
482template<> mat4 mat4::perspective(float fov_y, float width,
483                                  float height, float near, float far)
484{
485    fov_y *= (M_PI / 180.0f);
486
487    float t2 = tanf(fov_y * 0.5f);
488    float t1 = t2 * width / height;
489
490    return frustum(-near * t1, near * t1, -near * t2, near * t2, near, far);
491}
492
493} /* namespace lol */
494
Note: See TracBrowser for help on using the repository browser.