/* * vvector.h * * FUNCTION: * This file contains a number of utilities useful for handling * 3D vectors * * HISTORY: * Written by Linas Vepstas, August 1991 * Added 2D code, March 1993 * Added Outer products, C++ proofed, Linas Vepstas October 1993 */ #ifndef __GUTIL_VECTOR_H__ #define __GUTIL_VECTOR_H__ #if defined(__cplusplus) || defined(c_plusplus) extern "C" { #endif #include #include "port.h" /* ========================================================== */ /* Zero out a 2D vector */ #define VEC_ZERO_2(a) \ { \ (a)[0] = (a)[1] = 0.0; \ } /* ========================================================== */ /* Zero out a 3D vector */ #define VEC_ZERO(a) \ { \ (a)[0] = (a)[1] = (a)[2] = 0.0; \ } /* ========================================================== */ /* Zero out a 4D vector */ #define VEC_ZERO_4(a) \ { \ (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0; \ } /* ========================================================== */ /* Vector copy */ #define VEC_COPY_2(b,a) \ { \ (b)[0] = (a)[0]; \ (b)[1] = (a)[1]; \ } /* ========================================================== */ /* Copy 3D vector */ #define VEC_COPY(b,a) \ { \ (b)[0] = (a)[0]; \ (b)[1] = (a)[1]; \ (b)[2] = (a)[2]; \ } /* ========================================================== */ /* Copy 4D vector */ #define VEC_COPY_4(b,a) \ { \ (b)[0] = (a)[0]; \ (b)[1] = (a)[1]; \ (b)[2] = (a)[2]; \ (b)[3] = (a)[3]; \ } /* ========================================================== */ /* Vector difference */ #define VEC_DIFF_2(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] - (v1)[0]; \ (v21)[1] = (v2)[1] - (v1)[1]; \ } /* ========================================================== */ /* Vector difference */ #define VEC_DIFF(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] - (v1)[0]; \ (v21)[1] = (v2)[1] - (v1)[1]; \ (v21)[2] = (v2)[2] - (v1)[2]; \ } /* ========================================================== */ /* Vector difference */ #define VEC_DIFF_4(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] - (v1)[0]; \ (v21)[1] = (v2)[1] - (v1)[1]; \ (v21)[2] = (v2)[2] - (v1)[2]; \ (v21)[3] = (v2)[3] - (v1)[3]; \ } /* ========================================================== */ /* Vector sum */ #define VEC_SUM_2(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] + (v1)[0]; \ (v21)[1] = (v2)[1] + (v1)[1]; \ } /* ========================================================== */ /* Vector sum */ #define VEC_SUM(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] + (v1)[0]; \ (v21)[1] = (v2)[1] + (v1)[1]; \ (v21)[2] = (v2)[2] + (v1)[2]; \ } /* ========================================================== */ /* Vector sum */ #define VEC_SUM_4(v21,v2,v1) \ { \ (v21)[0] = (v2)[0] + (v1)[0]; \ (v21)[1] = (v2)[1] + (v1)[1]; \ (v21)[2] = (v2)[2] + (v1)[2]; \ (v21)[3] = (v2)[3] + (v1)[3]; \ } /* ========================================================== */ /* scalar times vector */ #define VEC_SCALE_2(c,a,b) \ { \ (c)[0] = (a)*(b)[0]; \ (c)[1] = (a)*(b)[1]; \ } /* ========================================================== */ /* scalar times vector */ #define VEC_SCALE(c,a,b) \ { \ (c)[0] = (a)*(b)[0]; \ (c)[1] = (a)*(b)[1]; \ (c)[2] = (a)*(b)[2]; \ } /* ========================================================== */ /* scalar times vector */ #define VEC_SCALE_4(c,a,b) \ { \ (c)[0] = (a)*(b)[0]; \ (c)[1] = (a)*(b)[1]; \ (c)[2] = (a)*(b)[2]; \ (c)[3] = (a)*(b)[3]; \ } /* ========================================================== */ /* accumulate scaled vector */ #define VEC_ACCUM_2(c,a,b) \ { \ (c)[0] += (a)*(b)[0]; \ (c)[1] += (a)*(b)[1]; \ } /* ========================================================== */ /* accumulate scaled vector */ #define VEC_ACCUM(c,a,b) \ { \ (c)[0] += (a)*(b)[0]; \ (c)[1] += (a)*(b)[1]; \ (c)[2] += (a)*(b)[2]; \ } /* ========================================================== */ /* accumulate scaled vector */ #define VEC_ACCUM_4(c,a,b) \ { \ (c)[0] += (a)*(b)[0]; \ (c)[1] += (a)*(b)[1]; \ (c)[2] += (a)*(b)[2]; \ (c)[3] += (a)*(b)[3]; \ } /* ========================================================== */ /* Vector dot product */ #define VEC_DOT_PRODUCT_2(c,a,b) \ { \ (c) = (a)[0]*(b)[0] + (a)[1]*(b)[1]; \ } /* ========================================================== */ /* Vector dot product */ #define VEC_DOT_PRODUCT(c,a,b) \ { \ (c) = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]; \ } /* ========================================================== */ /* Vector dot product */ #define VEC_DOT_PRODUCT_4(c,a,b) \ { \ (c) = (a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3] ; \ } /* ========================================================== */ /* vector impact parameter (squared) */ #define VEC_IMPACT_SQ(bsq,direction,position) \ { \ gleDouble len, llel; \ VEC_DOT_PRODUCT (len, (position), (position)); \ VEC_DOT_PRODUCT (llel, (direction), (position)); \ (bsq) = len - llel*llel; \ } /* ========================================================== */ /* vector impact parameter */ #define VEC_IMPACT(bsq,direction,position) \ { \ VEC_IMPACT_SQ((bsq),(direction),(position)); \ (bsq) = sqrt (bsq); \ } /* ========================================================== */ /* Vector length */ #define VEC_LENGTH_2(len,a) \ { \ (len) = (a)[0]*(a)[0] + (a)[1]*(a)[1]; \ (len) = sqrt (len); \ } /* ========================================================== */ /* Vector length */ #define VEC_LENGTH(len,a) \ { \ (len) = (a)[0]*(a)[0] + (a)[1]*(a)[1]; \ (len) += (a)[2]*(a)[2]; \ (len) = sqrt (len); \ } /* ========================================================== */ /* Vector length */ #define VEC_LENGTH_4(len,a) \ { \ (len) = (a)[0]*(a)[0] + (a)[1]*(a)[1]; \ (len) += (a)[2]*(a)[2]; \ (len) += (a)[3] * (a)[3]; \ (len) = sqrt (len); \ } /* ========================================================== */ /* distance between two points */ #define VEC_DISTANCE(len,va,vb) \ { \ gleDouble tmp[4]; \ VEC_DIFF (tmp, (vb), (va)); \ VEC_LENGTH ((len), tmp); \ } /* ========================================================== */ /* Conjugate Length */ #define VEC_CONJUGATE_LENGTH(len,a) \ { \ (len) = 1.0 - (a)[0]*(a)[0] - (a)[1]*(a)[1] - (a)[2]*(a)[2];\ (len) = sqrt (len); \ } /* ========================================================== */ /* Normalize vector length (so that vector is unit length) */ #define VEC_NORMALIZE(a) \ { \ double len; \ VEC_LENGTH (len,a); \ if (len != 0.0) { \ len = 1.0 / len; \ (a)[0] *= len; \ (a)[1] *= len; \ (a)[2] *= len; \ } \ } /* ========================================================== */ /* Change vector length to be newlen */ #define VEC_RENORMALIZE(a,newlen) \ { \ double len; \ VEC_LENGTH (len,a); \ if (len != 0.0) { \ len = (newlen) / len; \ (a)[0] *= len; \ (a)[1] *= len; \ (a)[2] *= len; \ } \ } /* ========================================================== */ /* 3D Vector cross product yeilding vector */ #define VEC_CROSS_PRODUCT(c,a,b) \ { \ (c)[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ (c)[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ (c)[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ } /* ========================================================== */ /* Vector perp -- assumes that n is of unit length * accepts vector v, subtracts out any component parallel to n */ #define VEC_PERP(vp,v,n) \ { \ double dot; \ \ VEC_DOT_PRODUCT (dot, v, n); \ (vp)[0] = (v)[0] - dot * (n)[0]; \ (vp)[1] = (v)[1] - dot * (n)[1]; \ (vp)[2] = (v)[2] - dot * (n)[2]; \ } /* ========================================================== */ /* Vector parallel -- assumes that n is of unit length * Accepts vector v, subtracts out any component perpendicular to n. * That is, it projects vector v to plane defined by n. */ #define VEC_PARALLEL(vp,v,n) \ { \ double dot; \ \ VEC_DOT_PRODUCT (dot, v, n); \ (vp)[0] = dot * (n)[0]; \ (vp)[1] = dot * (n)[1]; \ (vp)[2] = dot * (n)[2]; \ } /* ========================================================== */ /* Vector reflection -- assumes n is of unit length */ /* Takes vector v, reflects it against reflector n, and returns vr. * That is, assumes n defines a plane, and performs a mirror reflection */ #define VEC_REFLECT(vr,v,n) \ { \ double dot; \ \ VEC_DOT_PRODUCT (dot, v, n); \ (vr)[0] = (v)[0] - 2.0 * dot * (n)[0]; \ (vr)[1] = (v)[1] - 2.0 * dot * (n)[1]; \ (vr)[2] = (v)[2] - 2.0 * dot * (n)[2]; \ } /* ========================================================== */ /* Vector blending */ /* Takes two vectors a, b, blends them together */ #define VEC_BLEND(vr,sa,a,sb,b) \ { \ \ (vr)[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ (vr)[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ (vr)[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ } /* ========================================================== */ /* Vector print */ #define VEC_PRINT_2(a) \ { \ double len; \ VEC_LENGTH_2 (len, a); \ printf (#a " is %f %f length of " #a " is %f \n", \ (a)[0], *(a)[1], len); \ } /* ========================================================== */ /* Vector print */ #define VEC_PRINT(a) \ { \ double len; \ VEC_LENGTH (len, (a)); \ printf (#a " is %f %f %f length of " #a " is %f \n", \ (a)[0], (a)[1], (a)[2], len); \ } /* ========================================================== */ /* Vector print */ #define VEC_PRINT_4(a) \ { \ double len; \ VEC_LENGTH_4 (len, (a)); \ printf (#a " is %f %f %f %f length of " #a " is %f \n", \ (a)[0], (a)[1], (a)[2], (a)[3], len); \ } /* ========================================================== */ /* print matrix */ #define MAT_PRINT_4X4(mmm) { \ int i,j; \ printf ("matrix " #mmm " is \n"); \ if ((mmm) == NULL) { \ printf (" Null \n"); \ } else { \ for (i=0; i<4; i++) { \ for (j=0; j<4; j++) { \ printf ("%f ", (mmm)[i][j]); \ } \ printf (" \n"); \ } \ } \ } /* ========================================================== */ /* print matrix */ #define MAT_PRINT_3X3(mmm) { \ int i,j; \ printf ("matrix " #mmm " is \n"); \ if ((mmm) == NULL) { \ printf (" Null \n"); \ } else { \ for (i=0; i<3; i++) { \ for (j=0; j<3; j++) { \ printf ("%f ", (mmm)[i][j]); \ } \ printf (" \n"); \ } \ } \ } /* ========================================================== */ /* print matrix */ #define MAT_PRINT_2X3(mmm) { \ int i,j; \ printf ("matrix " #mmm " is \n"); \ if ((mmm) == NULL) { \ printf (" Null \n"); \ } else { \ for (i=0; i<2; i++) { \ for (j=0; j<3; j++) { \ printf ("%f ", (mmm)[i][j]); \ } \ printf (" \n"); \ } \ } \ } /* ========================================================== */ /* initialize matrix */ #define IDENTIFY_MATRIX_3X3(m) \ { \ (m)[0][0] = 1.0; \ (m)[0][1] = 0.0; \ (m)[0][2] = 0.0; \ \ (m)[1][0] = 0.0; \ (m)[1][1] = 1.0; \ (m)[1][2] = 0.0; \ \ (m)[2][0] = 0.0; \ (m)[2][1] = 0.0; \ (m)[2][2] = 1.0; \ } /* ========================================================== */ /* initialize matrix */ #define IDENTIFY_MATRIX_4X4(m) \ { \ (m)[0][0] = 1.0; \ (m)[0][1] = 0.0; \ (m)[0][2] = 0.0; \ (m)[0][3] = 0.0; \ \ (m)[1][0] = 0.0; \ (m)[1][1] = 1.0; \ (m)[1][2] = 0.0; \ (m)[1][3] = 0.0; \ \ (m)[2][0] = 0.0; \ (m)[2][1] = 0.0; \ (m)[2][2] = 1.0; \ (m)[2][3] = 0.0; \ \ (m)[3][0] = 0.0; \ (m)[3][1] = 0.0; \ (m)[3][2] = 0.0; \ (m)[3][3] = 1.0; \ } /* ========================================================== */ /* matrix copy */ #define COPY_MATRIX_2X2(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[0][1]; \ \ (b)[1][0] = (a)[1][0]; \ (b)[1][1] = (a)[1][1]; \ \ } /* ========================================================== */ /* matrix copy */ #define COPY_MATRIX_2X3(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[0][1]; \ (b)[0][2] = (a)[0][2]; \ \ (b)[1][0] = (a)[1][0]; \ (b)[1][1] = (a)[1][1]; \ (b)[1][2] = (a)[1][2]; \ } /* ========================================================== */ /* matrix copy */ #define COPY_MATRIX_3X3(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[0][1]; \ (b)[0][2] = (a)[0][2]; \ \ (b)[1][0] = (a)[1][0]; \ (b)[1][1] = (a)[1][1]; \ (b)[1][2] = (a)[1][2]; \ \ (b)[2][0] = (a)[2][0]; \ (b)[2][1] = (a)[2][1]; \ (b)[2][2] = (a)[2][2]; \ } /* ========================================================== */ /* matrix copy */ #define COPY_MATRIX_4X4(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[0][1]; \ (b)[0][2] = (a)[0][2]; \ (b)[0][3] = (a)[0][3]; \ \ (b)[1][0] = (a)[1][0]; \ (b)[1][1] = (a)[1][1]; \ (b)[1][2] = (a)[1][2]; \ (b)[1][3] = (a)[1][3]; \ \ (b)[2][0] = (a)[2][0]; \ (b)[2][1] = (a)[2][1]; \ (b)[2][2] = (a)[2][2]; \ (b)[2][3] = (a)[2][3]; \ \ (b)[3][0] = (a)[3][0]; \ (b)[3][1] = (a)[3][1]; \ (b)[3][2] = (a)[3][2]; \ (b)[3][3] = (a)[3][3]; \ } /* ========================================================== */ /* matrix transpose */ #define TRANSPOSE_MATRIX_2X2(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[1][0]; \ \ (b)[1][0] = (a)[0][1]; \ (b)[1][1] = (a)[1][1]; \ } /* ========================================================== */ /* matrix transpose */ #define TRANSPOSE_MATRIX_3X3(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[1][0]; \ (b)[0][2] = (a)[2][0]; \ \ (b)[1][0] = (a)[0][1]; \ (b)[1][1] = (a)[1][1]; \ (b)[1][2] = (a)[2][1]; \ \ (b)[2][0] = (a)[0][2]; \ (b)[2][1] = (a)[1][2]; \ (b)[2][2] = (a)[2][2]; \ } /* ========================================================== */ /* matrix transpose */ #define TRANSPOSE_MATRIX_4X4(b,a) \ { \ (b)[0][0] = (a)[0][0]; \ (b)[0][1] = (a)[1][0]; \ (b)[0][2] = (a)[2][0]; \ (b)[0][3] = (a)[3][0]; \ \ (b)[1][0] = (a)[0][1]; \ (b)[1][1] = (a)[1][1]; \ (b)[1][2] = (a)[2][1]; \ (b)[1][3] = (a)[3][1]; \ \ (b)[2][0] = (a)[0][2]; \ (b)[2][1] = (a)[1][2]; \ (b)[2][2] = (a)[2][2]; \ (b)[2][3] = (a)[3][2]; \ \ (b)[3][0] = (a)[0][3]; \ (b)[3][1] = (a)[1][3]; \ (b)[3][2] = (a)[2][3]; \ (b)[3][3] = (a)[3][3]; \ } /* ========================================================== */ /* multiply matrix by scalar */ #define SCALE_MATRIX_2X2(b,s,a) \ { \ (b)[0][0] = (s) * (a)[0][0]; \ (b)[0][1] = (s) * (a)[0][1]; \ \ (b)[1][0] = (s) * (a)[1][0]; \ (b)[1][1] = (s) * (a)[1][1]; \ } /* ========================================================== */ /* multiply matrix by scalar */ #define SCALE_MATRIX_3X3(b,s,a) \ { \ (b)[0][0] = (s) * (a)[0][0]; \ (b)[0][1] = (s) * (a)[0][1]; \ (b)[0][2] = (s) * (a)[0][2]; \ \ (b)[1][0] = (s) * (a)[1][0]; \ (b)[1][1] = (s) * (a)[1][1]; \ (b)[1][2] = (s) * (a)[1][2]; \ \ (b)[2][0] = (s) * (a)[2][0]; \ (b)[2][1] = (s) * (a)[2][1]; \ (b)[2][2] = (s) * (a)[2][2]; \ } /* ========================================================== */ /* multiply matrix by scalar */ #define SCALE_MATRIX_4X4(b,s,a) \ { \ (b)[0][0] = (s) * (a)[0][0]; \ (b)[0][1] = (s) * (a)[0][1]; \ (b)[0][2] = (s) * (a)[0][2]; \ (b)[0][3] = (s) * (a)[0][3]; \ \ (b)[1][0] = (s) * (a)[1][0]; \ (b)[1][1] = (s) * (a)[1][1]; \ (b)[1][2] = (s) * (a)[1][2]; \ (b)[1][3] = (s) * (a)[1][3]; \ \ (b)[2][0] = (s) * (a)[2][0]; \ (b)[2][1] = (s) * (a)[2][1]; \ (b)[2][2] = (s) * (a)[2][2]; \ (b)[2][3] = (s) * (a)[2][3]; \ \ (b)[3][0] = (s) * (a)[3][0]; \ (b)[3][1] = (s) * (a)[3][1]; \ (b)[3][2] = (s) * (a)[3][2]; \ (b)[3][3] = (s) * (a)[3][3]; \ } /* ========================================================== */ /* multiply matrix by scalar */ #define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ { \ (b)[0][0] += (s) * (a)[0][0]; \ (b)[0][1] += (s) * (a)[0][1]; \ \ (b)[1][0] += (s) * (a)[1][0]; \ (b)[1][1] += (s) * (a)[1][1]; \ } /* +========================================================== */ /* multiply matrix by scalar */ #define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ { \ (b)[0][0] += (s) * (a)[0][0]; \ (b)[0][1] += (s) * (a)[0][1]; \ (b)[0][2] += (s) * (a)[0][2]; \ \ (b)[1][0] += (s) * (a)[1][0]; \ (b)[1][1] += (s) * (a)[1][1]; \ (b)[1][2] += (s) * (a)[1][2]; \ \ (b)[2][0] += (s) * (a)[2][0]; \ (b)[2][1] += (s) * (a)[2][1]; \ (b)[2][2] += (s) * (a)[2][2]; \ } /* +========================================================== */ /* multiply matrix by scalar */ #define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ { \ (b)[0][0] += (s) * (a)[0][0]; \ (b)[0][1] += (s) * (a)[0][1]; \ (b)[0][2] += (s) * (a)[0][2]; \ (b)[0][3] += (s) * (a)[0][3]; \ \ (b)[1][0] += (s) * (a)[1][0]; \ (b)[1][1] += (s) * (a)[1][1]; \ (b)[1][2] += (s) * (a)[1][2]; \ (b)[1][3] += (s) * (a)[1][3]; \ \ (b)[2][0] += (s) * (a)[2][0]; \ (b)[2][1] += (s) * (a)[2][1]; \ (b)[2][2] += (s) * (a)[2][2]; \ (b)[2][3] += (s) * (a)[2][3]; \ \ (b)[3][0] += (s) * (a)[3][0]; \ (b)[3][1] += (s) * (a)[3][1]; \ (b)[3][2] += (s) * (a)[3][2]; \ (b)[3][3] += (s) * (a)[3][3]; \ } /* +========================================================== */ /* matrix product */ /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ #define MATRIX_PRODUCT_2X2(c,a,b) \ { \ (c)[0][0] = (a)[0][0]*(b)[0][0]+(a)[0][1]*(b)[1][0]; \ (c)[0][1] = (a)[0][0]*(b)[0][1]+(a)[0][1]*(b)[1][1]; \ \ (c)[1][0] = (a)[1][0]*(b)[0][0]+(a)[1][1]*(b)[1][0]; \ (c)[1][1] = (a)[1][0]*(b)[0][1]+(a)[1][1]*(b)[1][1]; \ \ } /* ========================================================== */ /* matrix product */ /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ #define MATRIX_PRODUCT_3X3(c,a,b) \ { \ (c)[0][0] = (a)[0][0]*(b)[0][0]+(a)[0][1]*(b)[1][0]+(a)[0][2]*(b)[2][0];\ (c)[0][1] = (a)[0][0]*(b)[0][1]+(a)[0][1]*(b)[1][1]+(a)[0][2]*(b)[2][1];\ (c)[0][2] = (a)[0][0]*(b)[0][2]+(a)[0][1]*(b)[1][2]+(a)[0][2]*(b)[2][2];\ \ (c)[1][0] = (a)[1][0]*(b)[0][0]+(a)[1][1]*(b)[1][0]+(a)[1][2]*(b)[2][0];\ (c)[1][1] = (a)[1][0]*(b)[0][1]+(a)[1][1]*(b)[1][1]+(a)[1][2]*(b)[2][1];\ (c)[1][2] = (a)[1][0]*(b)[0][2]+(a)[1][1]*(b)[1][2]+(a)[1][2]*(b)[2][2];\ \ (c)[2][0] = (a)[2][0]*(b)[0][0]+(a)[2][1]*(b)[1][0]+(a)[2][2]*(b)[2][0];\ (c)[2][1] = (a)[2][0]*(b)[0][1]+(a)[2][1]*(b)[1][1]+(a)[2][2]*(b)[2][1];\ (c)[2][2] = (a)[2][0]*(b)[0][2]+(a)[2][1]*(b)[1][2]+(a)[2][2]*(b)[2][2];\ } /* ========================================================== */ /* matrix product */ /* c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ #define MATRIX_PRODUCT_4X4(c,a,b) \ { \ (c)[0][0] = (a)[0][0]*(b)[0][0]+(a)[0][1]*(b)[1][0]+(a)[0][2]*(b)[2][0]+(a)[0][3]*(b)[3][0];\ (c)[0][1] = (a)[0][0]*(b)[0][1]+(a)[0][1]*(b)[1][1]+(a)[0][2]*(b)[2][1]+(a)[0][3]*(b)[3][1];\ (c)[0][2] = (a)[0][0]*(b)[0][2]+(a)[0][1]*(b)[1][2]+(a)[0][2]*(b)[2][2]+(a)[0][3]*(b)[3][2];\ (c)[0][3] = (a)[0][0]*(b)[0][3]+(a)[0][1]*(b)[1][3]+(a)[0][2]*(b)[2][3]+(a)[0][3]*(b)[3][3];\ \ (c)[1][0] = (a)[1][0]*(b)[0][0]+(a)[1][1]*(b)[1][0]+(a)[1][2]*(b)[2][0]+(a)[1][3]*(b)[3][0];\ (c)[1][1] = (a)[1][0]*(b)[0][1]+(a)[1][1]*(b)[1][1]+(a)[1][2]*(b)[2][1]+(a)[1][3]*(b)[3][1];\ (c)[1][2] = (a)[1][0]*(b)[0][2]+(a)[1][1]*(b)[1][2]+(a)[1][2]*(b)[2][2]+(a)[1][3]*(b)[3][2];\ (c)[1][3] = (a)[1][0]*(b)[0][3]+(a)[1][1]*(b)[1][3]+(a)[1][2]*(b)[2][3]+(a)[1][3]*(b)[3][3];\ \ (c)[2][0] = (a)[2][0]*(b)[0][0]+(a)[2][1]*(b)[1][0]+(a)[2][2]*(b)[2][0]+(a)[2][3]*(b)[3][0];\ (c)[2][1] = (a)[2][0]*(b)[0][1]+(a)[2][1]*(b)[1][1]+(a)[2][2]*(b)[2][1]+(a)[2][3]*(b)[3][1];\ (c)[2][2] = (a)[2][0]*(b)[0][2]+(a)[2][1]*(b)[1][2]+(a)[2][2]*(b)[2][2]+(a)[2][3]*(b)[3][2];\ (c)[2][3] = (a)[2][0]*(b)[0][3]+(a)[2][1]*(b)[1][3]+(a)[2][2]*(b)[2][3]+(a)[2][3]*(b)[3][3];\ \ (c)[3][0] = (a)[3][0]*(b)[0][0]+(a)[3][1]*(b)[1][0]+(a)[3][2]*(b)[2][0]+(a)[3][3]*(b)[3][0];\ (c)[3][1] = (a)[3][0]*(b)[0][1]+(a)[3][1]*(b)[1][1]+(a)[3][2]*(b)[2][1]+(a)[3][3]*(b)[3][1];\ (c)[3][2] = (a)[3][0]*(b)[0][2]+(a)[3][1]*(b)[1][2]+(a)[3][2]*(b)[2][2]+(a)[3][3]*(b)[3][2];\ (c)[3][3] = (a)[3][0]*(b)[0][3]+(a)[3][1]*(b)[1][3]+(a)[3][2]*(b)[2][3]+(a)[3][3]*(b)[3][3];\ } /* ========================================================== */ /* matrix times vector */ #define MAT_DOT_VEC_2X2(p,m,v) \ { \ (p)[0] = (m)[0][0]*(v)[0] + (m)[0][1]*(v)[1]; \ (p)[1] = (m)[1][0]*(v)[0] + (m)[1][1]*(v)[1]; \ } /* ========================================================== */ /* matrix times vector */ #define MAT_DOT_VEC_3X3(p,m,v) \ { \ (p)[0] = (m)[0][0]*(v)[0] + (m)[0][1]*(v)[1] + (m)[0][2]*(v)[2]; \ (p)[1] = (m)[1][0]*(v)[0] + (m)[1][1]*(v)[1] + (m)[1][2]*(v)[2]; \ (p)[2] = (m)[2][0]*(v)[0] + (m)[2][1]*(v)[1] + (m)[2][2]*(v)[2]; \ } /* ========================================================== */ /* matrix times vector */ #define MAT_DOT_VEC_4X4(p,m,v) \ { \ (p)[0] = (m)[0][0]*(v)[0] + (m)[0][1]*(v)[1] + (m)[0][2]*(v)[2] + (m)[0][3]*(v)[3]; \ (p)[1] = (m)[1][0]*(v)[0] + (m)[1][1]*(v)[1] + (m)[1][2]*(v)[2] + (m)[1][3]*(v)[3]; \ (p)[2] = (m)[2][0]*(v)[0] + (m)[2][1]*(v)[1] + (m)[2][2]*(v)[2] + (m)[2][3]*(v)[3]; \ (p)[3] = (m)[3][0]*(v)[0] + (m)[3][1]*(v)[1] + (m)[3][2]*(v)[2] + (m)[3][3]*(v)[3]; \ } /* ========================================================== */ /* vector transpose times matrix */ /* (p)[j] = (v)[0]*(m)[0][j] + (v)[1]*(m)[1][j] + (v)[2]*(m)[2][j]; */ #define VEC_DOT_MAT_3X3(p,v,m) \ { \ (p)[0] = (v)[0]*(m)[0][0] + (v)[1]*(m)[1][0] + (v)[2]*(m)[2][0]; \ (p)[1] = (v)[0]*(m)[0][1] + (v)[1]*(m)[1][1] + (v)[2]*(m)[2][1]; \ (p)[2] = (v)[0]*(m)[0][2] + (v)[1]*(m)[1][2] + (v)[2]*(m)[2][2]; \ } /* ========================================================== */ /* affine matrix times vector */ /* The matrix is assumed to be an affine matrix, with last two * entries representing a translation */ #define MAT_DOT_VEC_2X3(p,m,v) \ { \ (p)[0] = (m)[0][0]*(v)[0] + (m)[0][1]*(v)[1] + (m)[0][2]; \ (p)[1] = (m)[1][0]*(v)[0] + (m)[1][1]*(v)[1] + (m)[1][2]; \ } /* ========================================================== */ /* inverse transpose of matrix times vector * * This macro computes inverse transpose of matrix m, * and multiplies vector v into it, to yeild vector p * * DANGER !!! Do Not use this on normal vectors!!! * It will leave normals the wrong length !!! * See macro below for use on normals. */ #define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ { \ gleDouble det; \ \ det = (m)[0][0]*(m)[1][1] - (m)[0][1]*(m)[1][0]; \ (p)[0] = (m)[1][1]*(v)[0] - (m)[1][0]*(v)[1]; \ (p)[1] = - (m)[0][1]*(v)[0] + (m)[0][0]*(v)[1]; \ \ /* if matrix not singular, and not orthonormal, then renormalize */ \ if ((det!=1.0) && (det != 0.0)) { \ det = 1.0 / det; \ (p)[0] *= det; \ (p)[1] *= det; \ } \ } /* ========================================================== */ /* transform normal vector by inverse transpose of matrix * and then renormalize the vector * * This macro computes inverse transpose of matrix m, * and multiplies vector v into it, to yeild vector p * Vector p is then normalized. */ #define NORM_XFORM_2X2(p,m,v) \ { \ double len; \ \ /* do nothing if off-diagonals are zero and diagonals are \ * equal */ \ if (((m)[0][1] != 0.0) || ((m)[1][0] != 0.0) || ((m)[0][0] != (m)[1][1])) { \ (p)[0] = (m)[1][1]*(v)[0] - (m)[1][0]*(v)[1]; \ (p)[1] = - (m)[0][1]*(v)[0] + (m)[0][0]*(v)[1]; \ \ len = (p)[0]*(p)[0] + (p)[1]*(p)[1]; \ len = 1.0 / sqrt (len); \ (p)[0] *= len; \ (p)[1] *= len; \ } else { \ VEC_COPY_2 ((p), (v)); \ } \ } /* ========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define OUTER_PRODUCT_2X2(m,v,t) \ { \ (m)[0][0] = (v)[0] * (t)[0]; \ (m)[0][1] = (v)[0] * (t)[1]; \ \ (m)[1][0] = (v)[1] * (t)[0]; \ (m)[1][1] = (v)[1] * (t)[1]; \ } /* ========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define OUTER_PRODUCT_3X3(m,v,t) \ { \ (m)[0][0] = (v)[0] * (t)[0]; \ (m)[0][1] = (v)[0] * (t)[1]; \ (m)[0][2] = (v)[0] * (t)[2]; \ \ (m)[1][0] = (v)[1] * (t)[0]; \ (m)[1][1] = (v)[1] * (t)[1]; \ (m)[1][2] = (v)[1] * (t)[2]; \ \ (m)[2][0] = (v)[2] * (t)[0]; \ (m)[2][1] = (v)[2] * (t)[1]; \ (m)[2][2] = (v)[2] * (t)[2]; \ } /* ========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define OUTER_PRODUCT_4X4(m,v,t) \ { \ (m)[0][0] = (v)[0] * (t)[0]; \ (m)[0][1] = (v)[0] * (t)[1]; \ (m)[0][2] = (v)[0] * (t)[2]; \ (m)[0][3] = (v)[0] * (t)[3]; \ \ (m)[1][0] = (v)[1] * (t)[0]; \ (m)[1][1] = (v)[1] * (t)[1]; \ (m)[1][2] = (v)[1] * (t)[2]; \ (m)[1][3] = (v)[1] * (t)[3]; \ \ (m)[2][0] = (v)[2] * (t)[0]; \ (m)[2][1] = (v)[2] * (t)[1]; \ (m)[2][2] = (v)[2] * (t)[2]; \ (m)[2][3] = (v)[2] * (t)[3]; \ \ (m)[3][0] = (v)[3] * (t)[0]; \ (m)[3][1] = (v)[3] * (t)[1]; \ (m)[3][2] = (v)[3] * (t)[2]; \ (m)[3][3] = (v)[3] * (t)[3]; \ } /* +========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ { \ (m)[0][0] += (v)[0] * (t)[0]; \ (m)[0][1] += (v)[0] * (t)[1]; \ \ (m)[1][0] += (v)[1] * (t)[0]; \ (m)[1][1] += (v)[1] * (t)[1]; \ } /* +========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ { \ (m)[0][0] += (v)[0] * (t)[0]; \ (m)[0][1] += (v)[0] * (t)[1]; \ (m)[0][2] += (v)[0] * (t)[2]; \ \ (m)[1][0] += (v)[1] * (t)[0]; \ (m)[1][1] += (v)[1] * (t)[1]; \ (m)[1][2] += (v)[1] * (t)[2]; \ \ (m)[2][0] += (v)[2] * (t)[0]; \ (m)[2][1] += (v)[2] * (t)[1]; \ (m)[2][2] += (v)[2] * (t)[2]; \ } /* +========================================================== */ /* outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ #define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ { \ (m)[0][0] += (v)[0] * (t)[0]; \ (m)[0][1] += (v)[0] * (t)[1]; \ (m)[0][2] += (v)[0] * (t)[2]; \ (m)[0][3] += (v)[0] * (t)[3]; \ \ (m)[1][0] += (v)[1] * (t)[0]; \ (m)[1][1] += (v)[1] * (t)[1]; \ (m)[1][2] += (v)[1] * (t)[2]; \ (m)[1][3] += (v)[1] * (t)[3]; \ \ (m)[2][0] += (v)[2] * (t)[0]; \ (m)[2][1] += (v)[2] * (t)[1]; \ (m)[2][2] += (v)[2] * (t)[2]; \ (m)[2][3] += (v)[2] * (t)[3]; \ \ (m)[3][0] += (v)[3] * (t)[0]; \ (m)[3][1] += (v)[3] * (t)[1]; \ (m)[3][2] += (v)[3] * (t)[2]; \ (m)[3][3] += (v)[3] * (t)[3]; \ } /* +========================================================== */ /* determinant of matrix * * Computes determinant of matrix m, returning d */ #define DETERMINANT_2X2(d,m) \ { \ (d) = (m)[0][0] * (m)[1][1] - (m)[0][1] * (m)[1][0]; \ } /* ========================================================== */ /* determinant of matrix * * Computes determinant of matrix m, returning d */ #define DETERMINANT_3X3(d,m) \ { \ (d) = (m)[0][0] * ((m)[1][1]*(m)[2][2] - (m)[1][2] * (m)[2][1]);\ (d) -= (m)[0][1] * ((m)[1][0]*(m)[2][2] - (m)[1][2] * (m)[2][0]);\ (d) += (m)[0][2] * ((m)[1][0]*(m)[2][1] - (m)[1][1] * (m)[2][0]);\ } /* ========================================================== */ /* i,j,th cofactor of a 4x4 matrix * */ #define COFACTOR_4X4_IJ(fac,m,i,j) \ { \ int ii[4], jj[4], k; \ \ /* compute which row, columnt to skip */ \ for (k=0; k<(i); k++) ii[k] = k; \ for (k=(i); k<3; k++) ii[k] = k+1; \ for (k=0; k<(j); k++) jj[k] = k; \ for (k=(j); k<3; k++) jj[k] = k+1; \ \ (fac) = (m)[ii[0]][jj[0]] * ((m)[ii[1]][jj[1]]*(m)[ii[2]][jj[2]] \ - (m)[ii[1]][jj[2]]*(m)[ii[2]][jj[1]]); \ (fac) -= (m)[ii[0]][jj[1]] * ((m)[ii[1]][jj[0]]*(m)[ii[2]][jj[2]] \ - (m)[ii[1]][jj[2]]*(m)[ii[2]][jj[0]]);\ (fac) += (m)[ii[0]][jj[2]] * ((m)[ii[1]][jj[0]]*(m)[ii[2]][jj[1]] \ - (m)[ii[1]][jj[1]]*(m)[ii[2]][jj[0]]);\ \ /* compute sign */ \ k = (i)+(j); \ if ( k != (k/2)*2) { \ (fac) = -(fac); \ } \ } /* ========================================================== */ /* determinant of matrix * * Computes determinant of matrix m, returning d */ #define DETERMINANT_4X4(d,m) \ { \ double cofac; \ COFACTOR_4X4_IJ (cofac, (m), 0, 0); \ (d) = (m)[0][0] * cofac; \ COFACTOR_4X4_IJ (cofac, (m), 0, 1); \ (d) += (m)[0][1] * cofac; \ COFACTOR_4X4_IJ (cofac, (m), 0, 2); \ (d) += (m)[0][2] * cofac; \ COFACTOR_4X4_IJ (cofac, (m), 0, 3); \ (d) += (m)[0][3] * cofac; \ } /* ========================================================== */ /* cofactor of matrix * * Computes cofactor of matrix m, returning a */ #define COFACTOR_2X2(a,m) \ { \ (a)[0][0] = (m)[1][1]; \ (a)[0][1] = - (m)[1][0]; \ (a)[1][0] = - (m)[0][1]; \ (a)[1][1] = (m)[0][0]; \ } /* ========================================================== */ /* cofactor of matrix * * Computes cofactor of matrix m, returning a */ #define COFACTOR_3X3(a,m) \ { \ (a)[0][0] = (m)[1][1]*(m)[2][2] - (m)[1][2]*(m)[2][1]; \ (a)[0][1] = - ((m)[1][0]*(m)[2][2] - (m)[2][0]*(m)[1][2]); \ (a)[0][2] = (m)[1][0]*(m)[2][1] - (m)[1][1]*(m)[2][0]; \ (a)[1][0] = - ((m)[0][1]*(m)[2][2] - (m)[0][2]*(m)[2][1]); \ (a)[1][1] = (m)[0][0]*(m)[2][2] - (m)[0][2]*(m)[2][0]; \ (a)[1][2] = - ((m)[0][0]*(m)[2][1] - (m)[0][1]*(m)[2][0]); \ (a)[2][0] = (m)[0][1]*(m)[1][2] - (m)[0][2]*(m)[1][1]; \ (a)[2][1] = - ((m)[0][0]*(m)[1][2] - (m)[0][2]*(m)[1][0]); \ (a)[2][2] = (m)[0][0]*(m)[1][1] - (m)[0][1]*(m)[1][0]); \ } /* ========================================================== */ /* cofactor of matrix * * Computes cofactor of matrix m, returning a */ #define COFACTOR_4X4(a,m) \ { \ int i,j; \ \ for (i=0; i<4; i++) { \ for (j=0; j<4; j++) { \ COFACTOR_4X4_IJ ((a)[i][j], (m), i, j); \ } \ } \ } /* ========================================================== */ /* adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ #define ADJOINT_2X2(a,m) \ { \ (a)[0][0] = (m)[1][1]; \ (a)[1][0] = - (m)[1][0]; \ (a)[0][1] = - (m)[0][1]; \ (a)[1][1] = (m)[0][0]; \ } /* ========================================================== */ /* adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ #define ADJOINT_3X3(a,m) \ { \ (a)[0][0] = (m)[1][1]*(m)[2][2] - (m)[1][2]*(m)[2][1]; \ (a)[1][0] = - ((m)[1][0]*(m)[2][2] - (m)[2][0]*(m)[1][2]); \ (a)[2][0] = (m)[1][0]*(m)[2][1] - (m)[1][1]*(m)[2][0]; \ (a)[0][1] = - ((m)[0][1]*(m)[2][2] - (m)[0][2]*(m)[2][1]); \ (a)[1][1] = (m)[0][0]*(m)[2][2] - (m)[0][2]*(m)[2][0]; \ (a)[2][1] = - ((m)[0][0]*(m)[2][1] - (m)[0][1]*(m)[2][0]); \ (a)[0][2] = (m)[0][1]*(m)[1][2] - (m)[0][2]*(m)[1][1]; \ (a)[1][2] = - ((m)[0][0]*(m)[1][2] - (m)[0][2]*(m)[1][0]); \ (a)[2][2] = (m)[0][0]*(m)[1][1] - (m)[0][1]*(m)[1][0]); \ } /* ========================================================== */ /* adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ #define ADJOINT_4X4(a,m) \ { \ int i,j; \ \ for (i=0; i<4; i++) { \ for (j=0; j<4; j++) { \ COFACTOR_4X4_IJ ((a)[j][i], (m), i, j); \ } \ } \ } /* ========================================================== */ /* compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ #define SCALE_ADJOINT_2X2(a,s,m) \ { \ (a)[0][0] = s * (m)[1][1]; \ (a)[1][0] = - s * (m)[1][0]; \ (a)[0][1] = - s * (m)[0][1]; \ (a)[1][1] = s * (m)[0][0]; \ } /* ========================================================== */ /* compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ #define SCALE_ADJOINT_3X3(a,s,m) \ { \ (a)[0][0] = s * ((m)[1][1] * (m)[2][2] - (m)[1][2] * (m)[2][1]); \ (a)[1][0] = s * ((m)[1][2] * (m)[2][0] - (m)[1][0] * (m)[2][2]); \ (a)[2][0] = s * ((m)[1][0] * (m)[2][1] - (m)[1][1] * (m)[2][0]); \ \ (a)[0][1] = s * ((m)[0][2] * (m)[2][1] - (m)[0][1] * (m)[2][2]); \ (a)[1][1] = s * ((m)[0][0] * (m)[2][2] - (m)[0][2] * (m)[2][0]); \ (a)[2][1] = s * ((m)[0][1] * (m)[2][0] - (m)[0][0] * (m)[2][1]); \ \ (a)[0][2] = s * ((m)[0][1] * (m)[1][2] - (m)[0][2] * (m)[1][1]); \ (a)[1][2] = s * ((m)[0][2] * (m)[1][0] - (m)[0][0] * (m)[1][2]); \ (a)[2][2] = s * ((m)[0][0] * (m)[1][1] - (m)[0][1] * (m)[1][0]); \ } /* ========================================================== */ /* compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ #define SCALE_ADJOINT_4X4(a,s,m) \ { \ int i,j; \ \ for (i=0; i<4; i++) { \ for (j=0; j<4; j++) { \ COFACTOR_4X4_IJ ((a)[j][i], (m), i, j); \ (a)[j][i] *= (s); \ } \ } \ } /* ========================================================== */ /* inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ #define INVERT_2X2(b,det,a) \ { \ double tmp; \ DETERMINANT_2X2 ((det), (a)); \ tmp = 1.0 / (det); \ SCALE_ADJOINT_2X2 ((b), tmp, (a)); \ } /* ========================================================== */ /* inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ #define INVERT_3X3(b,det,a) \ { \ double tmp; \ DETERMINANT_3X3 ((det), (a)); \ tmp = 1.0 / (det); \ SCALE_ADJOINT_3X3 ((b), tmp, (a)); \ } /* ========================================================== */ /* inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ #define INVERT_4X4(b,det,a) \ { \ double tmp; \ DETERMINANT_4X4 ((det), (a)); \ tmp = 1.0 / (det); \ SCALE_ADJOINT_4X4 ((b), tmp, (a)); \ } /* ========================================================== */ #if defined(__cplusplus) || defined(c_plusplus) }; #endif #endif /* __GUTIL_VECTOR_H__ */ /* ===================== END OF FILE ======================== */