goat3d
view libs/vmath/matrix_c.c @ 55:af1310ed212b
not done yet
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sun, 19 Jan 2014 14:56:44 +0200 |
| 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_set_translation(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
58 {
59 m4_identity(m);
60 m[0][3] = x;
61 m[1][3] = y;
62 m[2][3] = z;
63 }
65 void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
66 {
67 mat4_t tm;
68 m4_set_translation(tm, x, y, z);
69 m4_mult(m, m, tm);
70 }
72 void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
73 {
74 m4_rotate_x(m, x);
75 m4_rotate_y(m, y);
76 m4_rotate_z(m, z);
77 }
79 void m4_set_rotation_x(mat4_t m, scalar_t angle)
80 {
81 m4_identity(m);
82 m[1][1] = cos(angle); m[1][2] = -sin(angle);
83 m[2][1] = sin(angle); m[2][2] = cos(angle);
84 }
86 void m4_rotate_x(mat4_t m, scalar_t angle)
87 {
88 mat4_t rm;
89 m4_set_rotation_x(m, angle);
90 m4_mult(m, m, rm);
91 }
93 void m4_set_rotation_y(mat4_t m, scalar_t angle)
94 {
95 m4_identity(m);
96 m[0][0] = cos(angle); m[0][2] = sin(angle);
97 m[2][0] = -sin(angle); m[2][2] = cos(angle);
98 }
100 void m4_rotate_y(mat4_t m, scalar_t angle)
101 {
102 mat4_t rm;
103 m4_set_rotation_y(rm, angle);
104 m4_mult(m, m, rm);
105 }
107 void m4_set_rotation_z(mat4_t m, scalar_t angle)
108 {
109 m4_identity(m);
110 m[0][0] = cos(angle); m[0][1] = -sin(angle);
111 m[1][0] = sin(angle); m[1][1] = cos(angle);
112 }
114 void m4_rotate_z(mat4_t m, scalar_t angle)
115 {
116 mat4_t rm;
117 m4_set_rotation_z(rm, angle);
118 m4_mult(m, m, rm);
119 }
121 void m4_set_rotation_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
122 {
123 scalar_t sina = sin(angle);
124 scalar_t cosa = cos(angle);
125 scalar_t one_minus_cosa = 1.0 - cosa;
126 scalar_t nxsq = x * x;
127 scalar_t nysq = y * y;
128 scalar_t nzsq = z * z;
130 m[0][0] = nxsq + (1.0 - nxsq) * cosa;
131 m[0][1] = x * y * one_minus_cosa - z * sina;
132 m[0][2] = x * z * one_minus_cosa + y * sina;
133 m[1][0] = x * y * one_minus_cosa + z * sina;
134 m[1][1] = nysq + (1.0 - nysq) * cosa;
135 m[1][2] = y * z * one_minus_cosa - x * sina;
136 m[2][0] = x * z * one_minus_cosa - y * sina;
137 m[2][1] = y * z * one_minus_cosa + x * sina;
138 m[2][2] = nzsq + (1.0 - nzsq) * cosa;
140 /* the rest are identity */
141 m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0;
142 m[3][3] = 1.0;
143 }
145 void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
146 {
147 mat4_t xform;
148 m4_set_rotation_axis(xform, angle, x, y, z);
149 m4_mult(m, m, xform);
150 }
152 void m4_rotate_quat(mat4_t m, quat_t q)
153 {
154 mat4_t rm;
155 quat_to_mat4(rm, q);
156 m4_mult(m, m, rm);
157 }
159 void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
160 {
161 mat4_t sm;
162 m4_identity(sm);
163 sm[0][0] = x;
164 sm[1][1] = y;
165 sm[2][2] = z;
166 m4_mult(m, m, sm);
167 }
169 void m4_transpose(mat4_t res, mat4_t m)
170 {
171 int i, j;
172 mat4_t tmp;
173 m4_copy(tmp, m);
175 for(i=0; i<4; i++) {
176 for(j=0; j<4; j++) {
177 res[i][j] = tmp[j][i];
178 }
179 }
180 }
182 scalar_t m4_determinant(mat4_t m)
183 {
184 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
185 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
186 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
188 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
189 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
190 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
192 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
193 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
194 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
196 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
197 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
198 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
200 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
201 }
203 void m4_adjoint(mat4_t res, mat4_t m)
204 {
205 int i, j;
206 mat4_t coef;
208 coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
209 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
210 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
211 coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
212 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
213 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
214 coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
215 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
216 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
217 coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
218 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
219 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
221 coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
222 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
223 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
224 coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
225 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
226 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
227 coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
228 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
229 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
230 coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
231 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
232 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
234 coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
235 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
236 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
237 coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
238 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
239 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
240 coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
241 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
242 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
243 coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
244 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
245 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
247 coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
248 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
249 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
250 coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
251 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
252 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
253 coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
254 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
255 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
256 coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
257 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
258 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
260 m4_transpose(res, coef);
262 for(i=0; i<4; i++) {
263 for(j=0; j<4; j++) {
264 res[i][j] = j % 2 ? -res[i][j] : res[i][j];
265 if(i % 2) res[i][j] = -res[i][j];
266 }
267 }
268 }
270 void m4_inverse(mat4_t res, mat4_t m)
271 {
272 int i, j;
273 mat4_t adj;
274 scalar_t det;
276 m4_adjoint(adj, m);
277 det = m4_determinant(m);
279 for(i=0; i<4; i++) {
280 for(j=0; j<4; j++) {
281 res[i][j] = adj[i][j] / det;
282 }
283 }
284 }
286 void m4_print(FILE *fp, mat4_t m)
287 {
288 int i;
289 for(i=0; i<4; i++) {
290 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]);
291 }
292 }