istereo2
view libs/vmath/matrix_c.c @ 2:81d35769f546
added the tunnel effect source
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sat, 19 Sep 2015 05:51:51 +0300 |
| parents | |
| children |
line source
1 /*
2 libvmath - a vector math library
3 Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as published
7 by the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
15 You should have received a copy of the GNU Lesser General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 */
20 #include <stdio.h>
21 #include "matrix.h"
22 #include "vector.h"
23 #include "quat.h"
25 void m3_to_m4(mat4_t dest, mat3_t src)
26 {
27 int i, j;
29 memset(dest, 0, sizeof(mat4_t));
30 for(i=0; i<3; i++) {
31 for(j=0; j<3; j++) {
32 dest[i][j] = src[i][j];
33 }
34 }
35 dest[3][3] = 1.0;
36 }
38 void m3_print(FILE *fp, mat3_t m)
39 {
40 int i;
41 for(i=0; i<3; i++) {
42 fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
43 }
44 }
46 /* C matrix 4x4 functions */
47 void m4_to_m3(mat3_t dest, mat4_t src)
48 {
49 int i, j;
50 for(i=0; i<3; i++) {
51 for(j=0; j<3; j++) {
52 dest[i][j] = src[i][j];
53 }
54 }
55 }
57 void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
58 {
59 mat4_t tm;
60 m4_identity(tm);
61 tm[0][3] = x;
62 tm[1][3] = y;
63 tm[2][3] = z;
64 m4_mult(m, m, tm);
65 }
67 void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
68 {
69 m4_rotate_x(m, x);
70 m4_rotate_y(m, y);
71 m4_rotate_z(m, z);
72 }
74 void m4_rotate_x(mat4_t m, scalar_t angle)
75 {
76 mat4_t rm;
77 m4_identity(rm);
78 rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
79 rm[2][1] = sin(angle); rm[2][2] = cos(angle);
80 m4_mult(m, m, rm);
81 }
83 void m4_rotate_y(mat4_t m, scalar_t angle)
84 {
85 mat4_t rm;
86 m4_identity(rm);
87 rm[0][0] = cos(angle); rm[0][2] = sin(angle);
88 rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
89 m4_mult(m, m, rm);
90 }
92 void m4_rotate_z(mat4_t m, scalar_t angle)
93 {
94 mat4_t rm;
95 m4_identity(rm);
96 rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
97 rm[1][0] = sin(angle); rm[1][1] = cos(angle);
98 m4_mult(m, m, rm);
99 }
101 void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
102 {
103 mat4_t xform;
104 scalar_t sina = sin(angle);
105 scalar_t cosa = cos(angle);
106 scalar_t one_minus_cosa = 1.0 - cosa;
107 scalar_t nxsq = x * x;
108 scalar_t nysq = y * y;
109 scalar_t nzsq = z * z;
111 m4_identity(xform);
112 xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
113 xform[0][1] = x * y * one_minus_cosa - z * sina;
114 xform[0][2] = x * z * one_minus_cosa + y * sina;
115 xform[1][0] = x * y * one_minus_cosa + z * sina;
116 xform[1][1] = nysq + (1.0 - nysq) * cosa;
117 xform[1][2] = y * z * one_minus_cosa - x * sina;
118 xform[2][0] = x * z * one_minus_cosa - y * sina;
119 xform[2][1] = y * z * one_minus_cosa + x * sina;
120 xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
122 m4_mult(m, m, xform);
123 }
125 void m4_rotate_quat(mat4_t m, quat_t q)
126 {
127 mat4_t rm;
128 quat_to_mat4(rm, q);
129 m4_mult(m, m, rm);
130 }
132 void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
133 {
134 mat4_t sm;
135 m4_identity(sm);
136 sm[0][0] = x;
137 sm[1][1] = y;
138 sm[2][2] = z;
139 m4_mult(m, m, sm);
140 }
142 void m4_transpose(mat4_t res, mat4_t m)
143 {
144 int i, j;
145 mat4_t tmp;
146 m4_copy(tmp, m);
148 for(i=0; i<4; i++) {
149 for(j=0; j<4; j++) {
150 res[i][j] = tmp[j][i];
151 }
152 }
153 }
155 scalar_t m4_determinant(mat4_t m)
156 {
157 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
158 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
159 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
161 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
162 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
163 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
165 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
166 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
167 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
169 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
170 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
171 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
173 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
174 }
176 void m4_adjoint(mat4_t res, mat4_t m)
177 {
178 int i, j;
179 mat4_t coef;
181 coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
182 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
183 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
184 coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
185 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
186 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
187 coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
188 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
189 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
190 coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
191 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
192 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
194 coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
195 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
196 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
197 coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
198 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
199 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
200 coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
201 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
202 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
203 coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
204 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
205 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
207 coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
208 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
209 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
210 coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
211 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
212 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
213 coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
214 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
215 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
216 coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
217 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
218 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
220 coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
221 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
222 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
223 coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
224 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
225 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
226 coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
227 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
228 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
229 coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
230 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
231 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
233 m4_transpose(res, coef);
235 for(i=0; i<4; i++) {
236 for(j=0; j<4; j++) {
237 res[i][j] = j % 2 ? -res[i][j] : res[i][j];
238 if(i % 2) res[i][j] = -res[i][j];
239 }
240 }
241 }
243 void m4_inverse(mat4_t res, mat4_t m)
244 {
245 int i, j;
246 mat4_t adj;
247 scalar_t det;
249 m4_adjoint(adj, m);
250 det = m4_determinant(m);
252 for(i=0; i<4; i++) {
253 for(j=0; j<4; j++) {
254 res[i][j] = adj[i][j] / det;
255 }
256 }
257 }
259 void m4_print(FILE *fp, mat4_t m)
260 {
261 int i;
262 for(i=0; i<4; i++) {
263 fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]);
264 }
265 }