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 |
#define PI ((double)M_PI) |
13 |
|
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 |
angle = (180.0/PI) * acos(cos_theta); |
137 |
} |
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 |
cosa = cos ((PI/180.0) * angle); |
167 |
sina = sin ((PI/180.0) * angle); |
168 |
|
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 |
cosa = cos ((PI/180.0) * angle); |
198 |
sina = sin ((PI/180.0) * angle); |
199 |
|
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 |
cosa = cos ((PI/180.0) * angle); |
265 |
sina = sin ((PI/180.0) * angle); |
266 |
|
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 |
cosa = cos ((PI/180.0) * angle); |
302 |
sina = sin ((PI/180.0) * angle); |
303 |
|
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 |
rotate[X] = (180.0/PI)*rotate[X]; |
427 |
rotate[Y] = (180.0/PI)*rotate[Y]; |
428 |
rotate[Z] = (180.0/PI)*rotate[Z]; |
429 |
} |
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 |
|