Back to index

radiance  4R0+20100331
vect.c
Go to the documentation of this file.
00001 #ifndef lint
00002 static const char    RCSid[] = "$Id: vect.c,v 1.3 2003/02/28 20:11:30 greg Exp $";
00003 #endif
00004 #include <math.h>
00005 #include <string.h>
00006 #include "vect.h"
00007 
00008 #ifndef M_PI
00009 #define M_PI  3.14159265358979323846
00010 #endif
00011 #define PI    ((double)M_PI)
00012 
00013 #define EPSILON 1e-6
00014 
00015 void   adjoint (Matrix mat);
00016 double det4x4 (Matrix mat);
00017 double det3x3 (double a1, double a2, double a3, double b1, double b2,
00018               double b3, double c1, double c2, double c3);
00019 double det2x2 (double a, double b, double c, double d);
00020 
00021 
00022 void vect_init (Vector v, float  x, float  y, float  z)
00023 {
00024     v[X] = x;
00025     v[Y] = y;
00026     v[Z] = z;
00027 }
00028 
00029 
00030 void vect_copy (Vector v1, Vector v2)
00031 {
00032     v1[X] = v2[X];
00033     v1[Y] = v2[Y];
00034     v1[Z] = v2[Z];
00035 }
00036 
00037 
00038 int vect_equal (Vector v1, Vector v2)
00039 {
00040     if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
00041        return 1;
00042     else
00043        return 0;
00044 }
00045 
00046 
00047 void vect_add (Vector v1, Vector v2, Vector v3)
00048 {
00049     v1[X] = v2[X] + v3[X];
00050     v1[Y] = v2[Y] + v3[Y];
00051     v1[Z] = v2[Z] + v3[Z];
00052 }
00053 
00054 
00055 void vect_sub (Vector v1, Vector v2, Vector v3)
00056 {
00057     v1[X] = v2[X] - v3[X];
00058     v1[Y] = v2[Y] - v3[Y];
00059     v1[Z] = v2[Z] - v3[Z];
00060 }
00061 
00062 
00063 void vect_scale (Vector v1, Vector v2, float  k)
00064 {
00065     v1[X] = k * v2[X];
00066     v1[Y] = k * v2[Y];
00067     v1[Z] = k * v2[Z];
00068 }
00069 
00070 
00071 float vect_mag (Vector v)
00072 {
00073     float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
00074 
00075     return mag;
00076 }
00077 
00078 
00079 void vect_normalize (Vector v)
00080 {
00081     float mag = vect_mag (v);
00082 
00083     if (mag > 0.0)
00084        vect_scale (v, v, 1.0/mag);
00085 }
00086 
00087 
00088 float vect_dot (Vector v1, Vector v2)
00089 {
00090     return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
00091 }
00092 
00093 
00094 void vect_cross (Vector v1, Vector v2, Vector v3)
00095 {
00096     v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
00097     v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
00098     v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
00099 }
00100 
00101 void vect_min (Vector v1, Vector v2, Vector v3)
00102 {
00103     v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
00104     v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
00105     v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
00106 }
00107 
00108 
00109 void vect_max (Vector v1, Vector v2, Vector v3)
00110 {
00111     v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
00112     v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
00113     v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
00114 }
00115 
00116 
00117 /* Return the angle between two vectors */
00118 float vect_angle (Vector v1, Vector v2)
00119 {
00120     float  mag1, mag2, angle, cos_theta;
00121 
00122     mag1 = vect_mag(v1);
00123     mag2 = vect_mag(v2);
00124 
00125     if (mag1 * mag2 == 0.0)
00126        angle = 0.0;
00127     else {
00128        cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
00129 
00130        if (cos_theta <= -1.0)
00131            angle = 180.0;
00132        else if (cos_theta >= +1.0)
00133            angle = 0.0;
00134        else
00135            angle = (180.0/PI) * acos(cos_theta);
00136     }
00137 
00138     return angle;
00139 }
00140 
00141 
00142 void vect_print (FILE *f, Vector v, int dec, char sep)
00143 {
00144     char fstr[] = "%.4f, %.4f, %.4f";
00145 
00146     if (dec < 0) dec = 0;
00147     if (dec > 9) dec = 9;
00148 
00149     fstr[2]  = '0' + dec;
00150     fstr[8]  = '0' + dec;
00151     fstr[14] = '0' + dec;
00152 
00153     fstr[4]  = sep;
00154     fstr[10] = sep;
00155 
00156     fprintf (f, fstr, v[X], v[Y], v[Z]);
00157 }
00158 
00159 
00160 /* Rotate a vector about the X, Y or Z axis */
00161 void vect_rotate (Vector v1, Vector v2, int axis, float angle)
00162 {
00163     float  cosa, sina;
00164 
00165     cosa = cos ((PI/180.0) * angle);
00166     sina = sin ((PI/180.0) * angle);
00167 
00168     switch (axis) {
00169        case X:
00170            v1[X] =  v2[X];
00171            v1[Y] =  v2[Y] * cosa + v2[Z] * sina;
00172            v1[Z] =  v2[Z] * cosa - v2[Y] * sina;
00173            break;
00174 
00175        case Y:
00176            v1[X] = v2[X] * cosa - v2[Z] * sina;
00177            v1[Y] = v2[Y];
00178            v1[Z] = v2[Z] * cosa + v2[X] * sina;
00179            break;
00180 
00181        case Z:
00182            v1[X] = v2[X] * cosa + v2[Y] * sina;
00183            v1[Y] = v2[Y] * cosa - v2[X] * sina;
00184            v1[Z] = v2[Z];
00185            break;
00186     }
00187 }
00188 
00189 
00190 /* Rotate a vector about a specific axis */
00191 void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
00192 {
00193     float  cosa, sina;
00194     Matrix mat;
00195 
00196     cosa = cos ((PI/180.0) * angle);
00197     sina = sin ((PI/180.0) * angle);
00198 
00199     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
00200     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
00201     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
00202     mat[0][3] = 0.0;
00203 
00204     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
00205     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
00206     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
00207     mat[1][3] = 0.0;
00208 
00209     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
00210     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
00211     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
00212     mat[2][3] = 0.0;
00213 
00214     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
00215 
00216     vect_transform (v1, v2, mat);
00217 }
00218 
00219 
00220 /* Transform the given vector */
00221 void vect_transform (Vector v1, Vector v2, Matrix mat)
00222 {
00223     Vector tmp;
00224 
00225     tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0];
00226     tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1];
00227     tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2];
00228 
00229     vect_copy (v1, tmp);
00230 }
00231 
00232 
00233 /* Create an identity matrix */
00234 void mat_identity (Matrix mat)
00235 {
00236     int i, j;
00237 
00238     for (i = 0; i < 4; i++)
00239        for (j = 0; j < 4; j++)
00240            mat[i][j] = 0.0;
00241 
00242     for (i = 0; i < 4; i++)
00243        mat[i][i] = 1.0;
00244 }
00245 
00246 
00247 void mat_copy (Matrix mat1, Matrix mat2)
00248 {
00249     int i, j;
00250 
00251     for (i = 0; i < 4; i++)
00252        for (j = 0; j < 4; j++)
00253            mat1[i][j] = mat2[i][j];
00254 }
00255 
00256 
00257 /* Rotate a matrix about the X, Y or Z axis */
00258 void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
00259 {
00260     Matrix mat;
00261     float  cosa, sina;
00262 
00263     cosa = cos ((PI/180.0) * angle);
00264     sina = sin ((PI/180.0) * angle);
00265 
00266     mat_identity (mat);
00267 
00268     switch (axis) {
00269        case X:
00270            mat[1][1] = cosa;
00271            mat[1][2] = sina;
00272            mat[2][1] = -sina;
00273            mat[2][2] = cosa;
00274            break;
00275 
00276        case Y:
00277            mat[0][0] = cosa;
00278            mat[0][2] = -sina;
00279            mat[2][0] = sina;
00280            mat[2][2] = cosa;
00281            break;
00282 
00283        case Z:
00284            mat[0][0] = cosa;
00285            mat[0][1] = sina;
00286            mat[1][0] = -sina;
00287            mat[1][1] = cosa;
00288            break;
00289     }
00290 
00291     mat_mult (mat1, mat2, mat);
00292 }
00293 
00294 
00295 void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
00296 {
00297     float  cosa, sina;
00298     Matrix mat;
00299 
00300     cosa = cos ((PI/180.0) * angle);
00301     sina = sin ((PI/180.0) * angle);
00302 
00303     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
00304     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
00305     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
00306     mat[0][3] = 0.0;
00307 
00308     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
00309     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
00310     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
00311     mat[1][3] = 0.0;
00312 
00313     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
00314     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
00315     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
00316     mat[2][3] = 0.0;
00317 
00318     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
00319 
00320     mat_mult (mat1, mat2, mat);
00321 }
00322 
00323 
00324 /*  mat1 <-- mat2 * mat3 */
00325 void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
00326 {
00327     float sum;
00328     int   i, j, k;
00329     Matrix result;
00330 
00331     for (i = 0; i < 4; i++) {
00332        for (j = 0; j < 4; j++) {
00333            sum = 0.0;
00334 
00335            for (k = 0; k < 4; k++)
00336               sum = sum + mat2[i][k] * mat3[k][j];
00337 
00338            result[i][j] = sum;
00339        }
00340     }
00341 
00342     for (i = 0; i < 4; i++)
00343        for (j = 0; j < 4; j++)
00344            mat1[i][j] = result[i][j];
00345 }
00346 
00347 
00348 /*
00349    Decodes a 3x4 transformation matrix into separate scale, rotation,
00350    translation, and shear vectors. Based on a program by Spencer W.
00351    Thomas (Graphics Gems II)
00352 */
00353 void mat_decode (Matrix mat, Vector scale,  Vector shear, Vector rotate,
00354               Vector transl)
00355 {
00356     int i;
00357     Vector row[3], temp;
00358 
00359     for (i = 0; i < 3; i++)
00360        transl[i] = mat[3][i];
00361 
00362     for (i = 0; i < 3; i++) {
00363        row[i][X] = mat[i][0];
00364        row[i][Y] = mat[i][1];
00365        row[i][Z] = mat[i][2];
00366     }
00367 
00368     scale[X] = vect_mag (row[0]);
00369     vect_normalize (row[0]);
00370 
00371     shear[X] = vect_dot (row[0], row[1]);
00372     row[1][X] = row[1][X] - shear[X]*row[0][X];
00373     row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
00374     row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
00375 
00376     scale[Y] = vect_mag (row[1]);
00377     vect_normalize (row[1]);
00378 
00379     if (scale[Y] != 0.0)
00380        shear[X] /= scale[Y];
00381 
00382     shear[Y] = vect_dot (row[0], row[2]);
00383     row[2][X] = row[2][X] - shear[Y]*row[0][X];
00384     row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
00385     row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
00386 
00387     shear[Z] = vect_dot (row[1], row[2]);
00388     row[2][X] = row[2][X] - shear[Z]*row[1][X];
00389     row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
00390     row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
00391 
00392     scale[Z] = vect_mag (row[2]);
00393     vect_normalize (row[2]);
00394 
00395     if (scale[Z] != 0.0) {
00396        shear[Y] /= scale[Z];
00397        shear[Z] /= scale[Z];
00398     }
00399 
00400     vect_cross (temp, row[1], row[2]);
00401     if (vect_dot (row[0], temp) < 0.0) {
00402        for (i = 0; i < 3; i++) {
00403            scale[i]  *= -1.0;
00404            row[i][X] *= -1.0;
00405            row[i][Y] *= -1.0;
00406            row[i][Z] *= -1.0;
00407        }
00408     }
00409 
00410     if (row[0][Z] < -1.0) row[0][Z] = -1.0;
00411     if (row[0][Z] > +1.0) row[0][Z] = +1.0;
00412 
00413     rotate[Y] = asin(-row[0][Z]);
00414 
00415     if (fabs(cos(rotate[Y])) > EPSILON) {
00416        rotate[X] = atan2 (row[1][Z], row[2][Z]);
00417        rotate[Z] = atan2 (row[0][Y], row[0][X]);
00418     }
00419     else {
00420        rotate[X] = atan2 (row[1][X], row[1][Y]);
00421        rotate[Z] = 0.0;
00422     }
00423 
00424     /* Convert the rotations to degrees */
00425     rotate[X] = (180.0/PI)*rotate[X];
00426     rotate[Y] = (180.0/PI)*rotate[Y];
00427     rotate[Z] = (180.0/PI)*rotate[Z];
00428 }
00429 
00430 
00431 /* Matrix inversion code from Graphics Gems */
00432 
00433 /* mat1 <-- mat2^-1 */
00434 float mat_inv (Matrix mat1, Matrix mat2)
00435 {
00436     int i, j;
00437     float det;
00438 
00439     if (mat1 != mat2) {
00440        for (i = 0; i < 4; i++)
00441            for (j = 0; j < 4; j++)
00442               mat1[i][j] = mat2[i][j];
00443     }
00444 
00445     det = det4x4 (mat1);
00446 
00447     if (fabs (det) < EPSILON)
00448        return 0.0;
00449 
00450     adjoint (mat1);
00451 
00452     for (i = 0; i < 4; i++)
00453        for(j = 0; j < 4; j++)
00454            mat1[i][j] = mat1[i][j] / det;
00455 
00456     return det;
00457 }
00458 
00459 
00460 void adjoint (Matrix mat)
00461 {
00462     double a1, a2, a3, a4, b1, b2, b3, b4;
00463     double c1, c2, c3, c4, d1, d2, d3, d4;
00464 
00465     a1 = mat[0][0]; b1 = mat[0][1];
00466     c1 = mat[0][2]; d1 = mat[0][3];
00467 
00468     a2 = mat[1][0]; b2 = mat[1][1];
00469     c2 = mat[1][2]; d2 = mat[1][3];
00470 
00471     a3 = mat[2][0]; b3 = mat[2][1];
00472     c3 = mat[2][2]; d3 = mat[2][3];
00473 
00474     a4 = mat[3][0]; b4 = mat[3][1];
00475     c4 = mat[3][2]; d4 = mat[3][3];
00476 
00477     /* row column labeling reversed since we transpose rows & columns */
00478     mat[0][0]  =  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
00479     mat[1][0]  = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
00480     mat[2][0]  =  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
00481     mat[3][0]  = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
00482 
00483     mat[0][1]  = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
00484     mat[1][1]  =  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
00485     mat[2][1]  = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
00486     mat[3][1]  =  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
00487 
00488     mat[0][2]  =  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
00489     mat[1][2]  = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
00490     mat[2][2]  =  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
00491     mat[3][2]  = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
00492 
00493     mat[0][3]  = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
00494     mat[1][3]  =  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
00495     mat[2][3]  = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
00496     mat[3][3]  =  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
00497 }
00498 
00499 
00500 double det4x4 (Matrix mat)
00501 {
00502     double ans;
00503     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,                     d4;
00504 
00505     a1 = mat[0][0]; b1 = mat[0][1];
00506     c1 = mat[0][2]; d1 = mat[0][3];
00507 
00508     a2 = mat[1][0]; b2 = mat[1][1];
00509     c2 = mat[1][2]; d2 = mat[1][3];
00510 
00511     a3 = mat[2][0]; b3 = mat[2][1];
00512     c3 = mat[2][2]; d3 = mat[2][3];
00513 
00514     a4 = mat[3][0]; b4 = mat[3][1];
00515     c4 = mat[3][2]; d4 = mat[3][3];
00516 
00517     ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
00518          b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
00519          c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
00520          d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
00521 
00522     return ans;
00523 }
00524 
00525 
00526 double det3x3 (double a1, double a2, double a3, double b1, double b2,
00527               double b3, double c1, double c2, double c3)
00528 {
00529     double ans;
00530 
00531     ans = a1 * det2x2 (b2, b3, c2, c3)
00532        - b1 * det2x2 (a2, a3, c2, c3)
00533        + c1 * det2x2 (a2, a3, b2, b3);
00534 
00535     return ans;
00536 }
00537 
00538 
00539 double det2x2 (double a, double b, double c, double d)
00540 {
00541     double ans;
00542     ans = a * d - b * c;
00543     return ans;
00544 }
00545