1 |
greg |
1.1 |
#ifndef lint |
2 |
|
|
static char SCCSid[] = "$SunId$ LBL"; |
3 |
|
|
#endif |
4 |
|
|
|
5 |
|
|
#include <math.h> |
6 |
|
|
#include <string.h> |
7 |
|
|
#include "vect.h" |
8 |
|
|
|
9 |
|
|
#ifndef M_PI |
10 |
|
|
#define M_PI 3.14159265358979323846 |
11 |
|
|
#endif |
12 |
gregl |
1.2 |
#define PI ((double)M_PI) |
13 |
greg |
1.1 |
|
14 |
|
|
#define EPSILON 1e-6 |
15 |
|
|
|
16 |
|
|
void adjoint (Matrix mat); |
17 |
|
|
double det4x4 (Matrix mat); |
18 |
|
|
double det3x3 (double a1, double a2, double a3, double b1, double b2, |
19 |
|
|
double b3, double c1, double c2, double c3); |
20 |
|
|
double det2x2 (double a, double b, double c, double d); |
21 |
|
|
|
22 |
|
|
|
23 |
|
|
void vect_init (Vector v, float x, float y, float z) |
24 |
|
|
{ |
25 |
|
|
v[X] = x; |
26 |
|
|
v[Y] = y; |
27 |
|
|
v[Z] = z; |
28 |
|
|
} |
29 |
|
|
|
30 |
|
|
|
31 |
|
|
void vect_copy (Vector v1, Vector v2) |
32 |
|
|
{ |
33 |
|
|
v1[X] = v2[X]; |
34 |
|
|
v1[Y] = v2[Y]; |
35 |
|
|
v1[Z] = v2[Z]; |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
|
39 |
|
|
int vect_equal (Vector v1, Vector v2) |
40 |
|
|
{ |
41 |
|
|
if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z]) |
42 |
|
|
return 1; |
43 |
|
|
else |
44 |
|
|
return 0; |
45 |
|
|
} |
46 |
|
|
|
47 |
|
|
|
48 |
|
|
void vect_add (Vector v1, Vector v2, Vector v3) |
49 |
|
|
{ |
50 |
|
|
v1[X] = v2[X] + v3[X]; |
51 |
|
|
v1[Y] = v2[Y] + v3[Y]; |
52 |
|
|
v1[Z] = v2[Z] + v3[Z]; |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
|
56 |
|
|
void vect_sub (Vector v1, Vector v2, Vector v3) |
57 |
|
|
{ |
58 |
|
|
v1[X] = v2[X] - v3[X]; |
59 |
|
|
v1[Y] = v2[Y] - v3[Y]; |
60 |
|
|
v1[Z] = v2[Z] - v3[Z]; |
61 |
|
|
} |
62 |
|
|
|
63 |
|
|
|
64 |
|
|
void vect_scale (Vector v1, Vector v2, float k) |
65 |
|
|
{ |
66 |
|
|
v1[X] = k * v2[X]; |
67 |
|
|
v1[Y] = k * v2[Y]; |
68 |
|
|
v1[Z] = k * v2[Z]; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
|
72 |
|
|
float vect_mag (Vector v) |
73 |
|
|
{ |
74 |
|
|
float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]); |
75 |
|
|
|
76 |
|
|
return mag; |
77 |
|
|
} |
78 |
|
|
|
79 |
|
|
|
80 |
|
|
void vect_normalize (Vector v) |
81 |
|
|
{ |
82 |
|
|
float mag = vect_mag (v); |
83 |
|
|
|
84 |
|
|
if (mag > 0.0) |
85 |
|
|
vect_scale (v, v, 1.0/mag); |
86 |
|
|
} |
87 |
|
|
|
88 |
|
|
|
89 |
|
|
float vect_dot (Vector v1, Vector v2) |
90 |
|
|
{ |
91 |
|
|
return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]); |
92 |
|
|
} |
93 |
|
|
|
94 |
|
|
|
95 |
|
|
void vect_cross (Vector v1, Vector v2, Vector v3) |
96 |
|
|
{ |
97 |
|
|
v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]); |
98 |
|
|
v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]); |
99 |
|
|
v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
void vect_min (Vector v1, Vector v2, Vector v3) |
103 |
|
|
{ |
104 |
|
|
v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X]; |
105 |
|
|
v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y]; |
106 |
|
|
v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z]; |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
|
110 |
|
|
void vect_max (Vector v1, Vector v2, Vector v3) |
111 |
|
|
{ |
112 |
|
|
v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X]; |
113 |
|
|
v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y]; |
114 |
|
|
v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z]; |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
|
118 |
|
|
/* Return the angle between two vectors */ |
119 |
|
|
float vect_angle (Vector v1, Vector v2) |
120 |
|
|
{ |
121 |
|
|
float mag1, mag2, angle, cos_theta; |
122 |
|
|
|
123 |
|
|
mag1 = vect_mag(v1); |
124 |
|
|
mag2 = vect_mag(v2); |
125 |
|
|
|
126 |
|
|
if (mag1 * mag2 == 0.0) |
127 |
|
|
angle = 0.0; |
128 |
|
|
else { |
129 |
|
|
cos_theta = vect_dot(v1,v2) / (mag1 * mag2); |
130 |
|
|
|
131 |
|
|
if (cos_theta <= -1.0) |
132 |
|
|
angle = 180.0; |
133 |
|
|
else if (cos_theta >= +1.0) |
134 |
|
|
angle = 0.0; |
135 |
|
|
else |
136 |
gregl |
1.2 |
angle = (180.0/PI) * acos(cos_theta); |
137 |
greg |
1.1 |
} |
138 |
|
|
|
139 |
|
|
return angle; |
140 |
|
|
} |
141 |
|
|
|
142 |
|
|
|
143 |
|
|
void vect_print (FILE *f, Vector v, int dec, char sep) |
144 |
|
|
{ |
145 |
|
|
char fstr[] = "%.4f, %.4f, %.4f"; |
146 |
|
|
|
147 |
|
|
if (dec < 0) dec = 0; |
148 |
|
|
if (dec > 9) dec = 9; |
149 |
|
|
|
150 |
|
|
fstr[2] = '0' + dec; |
151 |
|
|
fstr[8] = '0' + dec; |
152 |
|
|
fstr[14] = '0' + dec; |
153 |
|
|
|
154 |
|
|
fstr[4] = sep; |
155 |
|
|
fstr[10] = sep; |
156 |
|
|
|
157 |
|
|
fprintf (f, fstr, v[X], v[Y], v[Z]); |
158 |
|
|
} |
159 |
|
|
|
160 |
|
|
|
161 |
|
|
/* Rotate a vector about the X, Y or Z axis */ |
162 |
|
|
void vect_rotate (Vector v1, Vector v2, int axis, float angle) |
163 |
|
|
{ |
164 |
|
|
float cosa, sina; |
165 |
|
|
|
166 |
gregl |
1.2 |
cosa = cos ((PI/180.0) * angle); |
167 |
|
|
sina = sin ((PI/180.0) * angle); |
168 |
greg |
1.1 |
|
169 |
|
|
switch (axis) { |
170 |
|
|
case X: |
171 |
|
|
v1[X] = v2[X]; |
172 |
|
|
v1[Y] = v2[Y] * cosa + v2[Z] * sina; |
173 |
|
|
v1[Z] = v2[Z] * cosa - v2[Y] * sina; |
174 |
|
|
break; |
175 |
|
|
|
176 |
|
|
case Y: |
177 |
|
|
v1[X] = v2[X] * cosa - v2[Z] * sina; |
178 |
|
|
v1[Y] = v2[Y]; |
179 |
|
|
v1[Z] = v2[Z] * cosa + v2[X] * sina; |
180 |
|
|
break; |
181 |
|
|
|
182 |
|
|
case Z: |
183 |
|
|
v1[X] = v2[X] * cosa + v2[Y] * sina; |
184 |
|
|
v1[Y] = v2[Y] * cosa - v2[X] * sina; |
185 |
|
|
v1[Z] = v2[Z]; |
186 |
|
|
break; |
187 |
|
|
} |
188 |
|
|
} |
189 |
|
|
|
190 |
|
|
|
191 |
|
|
/* Rotate a vector about a specific axis */ |
192 |
|
|
void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle) |
193 |
|
|
{ |
194 |
|
|
float cosa, sina; |
195 |
|
|
Matrix mat; |
196 |
|
|
|
197 |
gregl |
1.2 |
cosa = cos ((PI/180.0) * angle); |
198 |
|
|
sina = sin ((PI/180.0) * angle); |
199 |
greg |
1.1 |
|
200 |
|
|
mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa); |
201 |
|
|
mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina); |
202 |
|
|
mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina); |
203 |
|
|
mat[0][3] = 0.0; |
204 |
|
|
|
205 |
|
|
mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina); |
206 |
|
|
mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa); |
207 |
|
|
mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina); |
208 |
|
|
mat[1][3] = 0.0; |
209 |
|
|
|
210 |
|
|
mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina); |
211 |
|
|
mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina); |
212 |
|
|
mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa); |
213 |
|
|
mat[2][3] = 0.0; |
214 |
|
|
|
215 |
|
|
mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0; |
216 |
|
|
|
217 |
|
|
vect_transform (v1, v2, mat); |
218 |
|
|
} |
219 |
|
|
|
220 |
|
|
|
221 |
|
|
/* Transform the given vector */ |
222 |
|
|
void vect_transform (Vector v1, Vector v2, Matrix mat) |
223 |
|
|
{ |
224 |
|
|
Vector tmp; |
225 |
|
|
|
226 |
|
|
tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0]; |
227 |
|
|
tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1]; |
228 |
|
|
tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2]; |
229 |
|
|
|
230 |
|
|
vect_copy (v1, tmp); |
231 |
|
|
} |
232 |
|
|
|
233 |
|
|
|
234 |
|
|
/* Create an identity matrix */ |
235 |
|
|
void mat_identity (Matrix mat) |
236 |
|
|
{ |
237 |
|
|
int i, j; |
238 |
|
|
|
239 |
|
|
for (i = 0; i < 4; i++) |
240 |
|
|
for (j = 0; j < 4; j++) |
241 |
|
|
mat[i][j] = 0.0; |
242 |
|
|
|
243 |
|
|
for (i = 0; i < 4; i++) |
244 |
|
|
mat[i][i] = 1.0; |
245 |
|
|
} |
246 |
|
|
|
247 |
|
|
|
248 |
|
|
void mat_copy (Matrix mat1, Matrix mat2) |
249 |
|
|
{ |
250 |
|
|
int i, j; |
251 |
|
|
|
252 |
|
|
for (i = 0; i < 4; i++) |
253 |
|
|
for (j = 0; j < 4; j++) |
254 |
|
|
mat1[i][j] = mat2[i][j]; |
255 |
|
|
} |
256 |
|
|
|
257 |
|
|
|
258 |
|
|
/* Rotate a matrix about the X, Y or Z axis */ |
259 |
|
|
void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle) |
260 |
|
|
{ |
261 |
|
|
Matrix mat; |
262 |
|
|
float cosa, sina; |
263 |
|
|
|
264 |
gregl |
1.2 |
cosa = cos ((PI/180.0) * angle); |
265 |
|
|
sina = sin ((PI/180.0) * angle); |
266 |
greg |
1.1 |
|
267 |
|
|
mat_identity (mat); |
268 |
|
|
|
269 |
|
|
switch (axis) { |
270 |
|
|
case X: |
271 |
|
|
mat[1][1] = cosa; |
272 |
|
|
mat[1][2] = sina; |
273 |
|
|
mat[2][1] = -sina; |
274 |
|
|
mat[2][2] = cosa; |
275 |
|
|
break; |
276 |
|
|
|
277 |
|
|
case Y: |
278 |
|
|
mat[0][0] = cosa; |
279 |
|
|
mat[0][2] = -sina; |
280 |
|
|
mat[2][0] = sina; |
281 |
|
|
mat[2][2] = cosa; |
282 |
|
|
break; |
283 |
|
|
|
284 |
|
|
case Z: |
285 |
|
|
mat[0][0] = cosa; |
286 |
|
|
mat[0][1] = sina; |
287 |
|
|
mat[1][0] = -sina; |
288 |
|
|
mat[1][1] = cosa; |
289 |
|
|
break; |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
mat_mult (mat1, mat2, mat); |
293 |
|
|
} |
294 |
|
|
|
295 |
|
|
|
296 |
|
|
void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle) |
297 |
|
|
{ |
298 |
|
|
float cosa, sina; |
299 |
|
|
Matrix mat; |
300 |
|
|
|
301 |
gregl |
1.2 |
cosa = cos ((PI/180.0) * angle); |
302 |
|
|
sina = sin ((PI/180.0) * angle); |
303 |
greg |
1.1 |
|
304 |
|
|
mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa); |
305 |
|
|
mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina); |
306 |
|
|
mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina); |
307 |
|
|
mat[0][3] = 0.0; |
308 |
|
|
|
309 |
|
|
mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina); |
310 |
|
|
mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa); |
311 |
|
|
mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina); |
312 |
|
|
mat[1][3] = 0.0; |
313 |
|
|
|
314 |
|
|
mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina); |
315 |
|
|
mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina); |
316 |
|
|
mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa); |
317 |
|
|
mat[2][3] = 0.0; |
318 |
|
|
|
319 |
|
|
mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0; |
320 |
|
|
|
321 |
|
|
mat_mult (mat1, mat2, mat); |
322 |
|
|
} |
323 |
|
|
|
324 |
|
|
|
325 |
|
|
/* mat1 <-- mat2 * mat3 */ |
326 |
|
|
void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3) |
327 |
|
|
{ |
328 |
|
|
float sum; |
329 |
|
|
int i, j, k; |
330 |
|
|
Matrix result; |
331 |
|
|
|
332 |
|
|
for (i = 0; i < 4; i++) { |
333 |
|
|
for (j = 0; j < 4; j++) { |
334 |
|
|
sum = 0.0; |
335 |
|
|
|
336 |
|
|
for (k = 0; k < 4; k++) |
337 |
|
|
sum = sum + mat2[i][k] * mat3[k][j]; |
338 |
|
|
|
339 |
|
|
result[i][j] = sum; |
340 |
|
|
} |
341 |
|
|
} |
342 |
|
|
|
343 |
|
|
for (i = 0; i < 4; i++) |
344 |
|
|
for (j = 0; j < 4; j++) |
345 |
|
|
mat1[i][j] = result[i][j]; |
346 |
|
|
} |
347 |
|
|
|
348 |
|
|
|
349 |
|
|
/* |
350 |
|
|
Decodes a 3x4 transformation matrix into separate scale, rotation, |
351 |
|
|
translation, and shear vectors. Based on a program by Spencer W. |
352 |
|
|
Thomas (Graphics Gems II) |
353 |
|
|
*/ |
354 |
|
|
void mat_decode (Matrix mat, Vector scale, Vector shear, Vector rotate, |
355 |
|
|
Vector transl) |
356 |
|
|
{ |
357 |
|
|
int i; |
358 |
|
|
Vector row[3], temp; |
359 |
|
|
|
360 |
|
|
for (i = 0; i < 3; i++) |
361 |
|
|
transl[i] = mat[3][i]; |
362 |
|
|
|
363 |
|
|
for (i = 0; i < 3; i++) { |
364 |
|
|
row[i][X] = mat[i][0]; |
365 |
|
|
row[i][Y] = mat[i][1]; |
366 |
|
|
row[i][Z] = mat[i][2]; |
367 |
|
|
} |
368 |
|
|
|
369 |
|
|
scale[X] = vect_mag (row[0]); |
370 |
|
|
vect_normalize (row[0]); |
371 |
|
|
|
372 |
|
|
shear[X] = vect_dot (row[0], row[1]); |
373 |
|
|
row[1][X] = row[1][X] - shear[X]*row[0][X]; |
374 |
|
|
row[1][Y] = row[1][Y] - shear[X]*row[0][Y]; |
375 |
|
|
row[1][Z] = row[1][Z] - shear[X]*row[0][Z]; |
376 |
|
|
|
377 |
|
|
scale[Y] = vect_mag (row[1]); |
378 |
|
|
vect_normalize (row[1]); |
379 |
|
|
|
380 |
|
|
if (scale[Y] != 0.0) |
381 |
|
|
shear[X] /= scale[Y]; |
382 |
|
|
|
383 |
|
|
shear[Y] = vect_dot (row[0], row[2]); |
384 |
|
|
row[2][X] = row[2][X] - shear[Y]*row[0][X]; |
385 |
|
|
row[2][Y] = row[2][Y] - shear[Y]*row[0][Y]; |
386 |
|
|
row[2][Z] = row[2][Z] - shear[Y]*row[0][Z]; |
387 |
|
|
|
388 |
|
|
shear[Z] = vect_dot (row[1], row[2]); |
389 |
|
|
row[2][X] = row[2][X] - shear[Z]*row[1][X]; |
390 |
|
|
row[2][Y] = row[2][Y] - shear[Z]*row[1][Y]; |
391 |
|
|
row[2][Z] = row[2][Z] - shear[Z]*row[1][Z]; |
392 |
|
|
|
393 |
|
|
scale[Z] = vect_mag (row[2]); |
394 |
|
|
vect_normalize (row[2]); |
395 |
|
|
|
396 |
|
|
if (scale[Z] != 0.0) { |
397 |
|
|
shear[Y] /= scale[Z]; |
398 |
|
|
shear[Z] /= scale[Z]; |
399 |
|
|
} |
400 |
|
|
|
401 |
|
|
vect_cross (temp, row[1], row[2]); |
402 |
|
|
if (vect_dot (row[0], temp) < 0.0) { |
403 |
|
|
for (i = 0; i < 3; i++) { |
404 |
|
|
scale[i] *= -1.0; |
405 |
|
|
row[i][X] *= -1.0; |
406 |
|
|
row[i][Y] *= -1.0; |
407 |
|
|
row[i][Z] *= -1.0; |
408 |
|
|
} |
409 |
|
|
} |
410 |
|
|
|
411 |
|
|
if (row[0][Z] < -1.0) row[0][Z] = -1.0; |
412 |
|
|
if (row[0][Z] > +1.0) row[0][Z] = +1.0; |
413 |
|
|
|
414 |
|
|
rotate[Y] = asin(-row[0][Z]); |
415 |
|
|
|
416 |
|
|
if (fabs(cos(rotate[Y])) > EPSILON) { |
417 |
|
|
rotate[X] = atan2 (row[1][Z], row[2][Z]); |
418 |
|
|
rotate[Z] = atan2 (row[0][Y], row[0][X]); |
419 |
|
|
} |
420 |
|
|
else { |
421 |
|
|
rotate[X] = atan2 (row[1][X], row[1][Y]); |
422 |
|
|
rotate[Z] = 0.0; |
423 |
|
|
} |
424 |
|
|
|
425 |
|
|
/* Convert the rotations to degrees */ |
426 |
gregl |
1.2 |
rotate[X] = (180.0/PI)*rotate[X]; |
427 |
|
|
rotate[Y] = (180.0/PI)*rotate[Y]; |
428 |
|
|
rotate[Z] = (180.0/PI)*rotate[Z]; |
429 |
greg |
1.1 |
} |
430 |
|
|
|
431 |
|
|
|
432 |
|
|
/* Matrix inversion code from Graphics Gems */ |
433 |
|
|
|
434 |
|
|
/* mat1 <-- mat2^-1 */ |
435 |
|
|
float mat_inv (Matrix mat1, Matrix mat2) |
436 |
|
|
{ |
437 |
|
|
int i, j; |
438 |
|
|
float det; |
439 |
|
|
|
440 |
|
|
if (mat1 != mat2) { |
441 |
|
|
for (i = 0; i < 4; i++) |
442 |
|
|
for (j = 0; j < 4; j++) |
443 |
|
|
mat1[i][j] = mat2[i][j]; |
444 |
|
|
} |
445 |
|
|
|
446 |
|
|
det = det4x4 (mat1); |
447 |
|
|
|
448 |
|
|
if (fabs (det) < EPSILON) |
449 |
|
|
return 0.0; |
450 |
|
|
|
451 |
|
|
adjoint (mat1); |
452 |
|
|
|
453 |
|
|
for (i = 0; i < 4; i++) |
454 |
|
|
for(j = 0; j < 4; j++) |
455 |
|
|
mat1[i][j] = mat1[i][j] / det; |
456 |
|
|
|
457 |
|
|
return det; |
458 |
|
|
} |
459 |
|
|
|
460 |
|
|
|
461 |
|
|
void adjoint (Matrix mat) |
462 |
|
|
{ |
463 |
|
|
double a1, a2, a3, a4, b1, b2, b3, b4; |
464 |
|
|
double c1, c2, c3, c4, d1, d2, d3, d4; |
465 |
|
|
|
466 |
|
|
a1 = mat[0][0]; b1 = mat[0][1]; |
467 |
|
|
c1 = mat[0][2]; d1 = mat[0][3]; |
468 |
|
|
|
469 |
|
|
a2 = mat[1][0]; b2 = mat[1][1]; |
470 |
|
|
c2 = mat[1][2]; d2 = mat[1][3]; |
471 |
|
|
|
472 |
|
|
a3 = mat[2][0]; b3 = mat[2][1]; |
473 |
|
|
c3 = mat[2][2]; d3 = mat[2][3]; |
474 |
|
|
|
475 |
|
|
a4 = mat[3][0]; b4 = mat[3][1]; |
476 |
|
|
c4 = mat[3][2]; d4 = mat[3][3]; |
477 |
|
|
|
478 |
|
|
/* row column labeling reversed since we transpose rows & columns */ |
479 |
|
|
mat[0][0] = det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4); |
480 |
|
|
mat[1][0] = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4); |
481 |
|
|
mat[2][0] = det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4); |
482 |
|
|
mat[3][0] = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4); |
483 |
|
|
|
484 |
|
|
mat[0][1] = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4); |
485 |
|
|
mat[1][1] = det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4); |
486 |
|
|
mat[2][1] = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4); |
487 |
|
|
mat[3][1] = det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4); |
488 |
|
|
|
489 |
|
|
mat[0][2] = det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4); |
490 |
|
|
mat[1][2] = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4); |
491 |
|
|
mat[2][2] = det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4); |
492 |
|
|
mat[3][2] = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4); |
493 |
|
|
|
494 |
|
|
mat[0][3] = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3); |
495 |
|
|
mat[1][3] = det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3); |
496 |
|
|
mat[2][3] = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3); |
497 |
|
|
mat[3][3] = det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3); |
498 |
|
|
} |
499 |
|
|
|
500 |
|
|
|
501 |
|
|
double det4x4 (Matrix mat) |
502 |
|
|
{ |
503 |
|
|
double ans; |
504 |
|
|
double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; |
505 |
|
|
|
506 |
|
|
a1 = mat[0][0]; b1 = mat[0][1]; |
507 |
|
|
c1 = mat[0][2]; d1 = mat[0][3]; |
508 |
|
|
|
509 |
|
|
a2 = mat[1][0]; b2 = mat[1][1]; |
510 |
|
|
c2 = mat[1][2]; d2 = mat[1][3]; |
511 |
|
|
|
512 |
|
|
a3 = mat[2][0]; b3 = mat[2][1]; |
513 |
|
|
c3 = mat[2][2]; d3 = mat[2][3]; |
514 |
|
|
|
515 |
|
|
a4 = mat[3][0]; b4 = mat[3][1]; |
516 |
|
|
c4 = mat[3][2]; d4 = mat[3][3]; |
517 |
|
|
|
518 |
|
|
ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) - |
519 |
|
|
b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) + |
520 |
|
|
c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) - |
521 |
|
|
d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4); |
522 |
|
|
|
523 |
|
|
return ans; |
524 |
|
|
} |
525 |
|
|
|
526 |
|
|
|
527 |
|
|
double det3x3 (double a1, double a2, double a3, double b1, double b2, |
528 |
|
|
double b3, double c1, double c2, double c3) |
529 |
|
|
{ |
530 |
|
|
double ans; |
531 |
|
|
|
532 |
|
|
ans = a1 * det2x2 (b2, b3, c2, c3) |
533 |
|
|
- b1 * det2x2 (a2, a3, c2, c3) |
534 |
|
|
+ c1 * det2x2 (a2, a3, b2, b3); |
535 |
|
|
|
536 |
|
|
return ans; |
537 |
|
|
} |
538 |
|
|
|
539 |
|
|
|
540 |
|
|
double det2x2 (double a, double b, double c, double d) |
541 |
|
|
{ |
542 |
|
|
double ans; |
543 |
|
|
ans = a * d - b * c; |
544 |
|
|
return ans; |
545 |
|
|
} |
546 |
|
|
|