Matrix4.C

Go to the documentation of this file.
00001 /***************************************************************************
00002  *cr                                                                       
00003  *cr            (C) Copyright 1995-2008 The Board of Trustees of the           
00004  *cr                        University of Illinois                       
00005  *cr                         All Rights Reserved                        
00006  *cr                                                                   
00007  ***************************************************************************/
00008 
00009 /***************************************************************************
00010  * RCS INFORMATION:
00011  *
00012  *      $RCSfile: Matrix4.C,v $
00013  *      $Author: jim $  $Locker:  $             $State: Exp $
00014  *      $Revision: 1.2 $        $Date: 2008/09/17 16:19:54 $
00015  *
00016  ***************************************************************************
00017  * DESCRIPTION:
00018  *
00019  * 4 x 4 Matrix, used for a transformation matrix.
00020  *
00021  ***************************************************************************/
00022 
00023 #include <math.h>
00024 #include <string.h>
00025 #include <stdio.h>
00026 #include "Matrix4.h"
00027 // #include "utilities.h"
00028 
00029 #define VMD_PI      3.14159265358979323846
00030 #define VMD_TWOPI   (2.0 * VMD_PI)
00031 #define DEGTORAD(a)     (a*VMD_PI/180.0)
00032 #define RADTODEG(a)     (a*180.0/VMD_PI)
00033 
00034 
00035 // constructor, for the case when an array of doubleing-point numbers is given
00036 Matrix4::Matrix4(const double *m) {
00037   memcpy((void *)mat, (const void *)m, 16*sizeof(double));
00038 }
00039 
00040 // multiplies a 3D point (first arg) by the Matrix, returns in second arg
00041 void Matrix4::multpoint3d(const double opoint[3], double npoint[3]) const {
00042 #if 0
00043     // should try re-testing this formulation to see if it outperforms
00044     // the old one, without introducing doubleing point imprecision
00045     double tmp[3];
00046     double itmp3 = 1.0 / (opoint[0]*mat[3] + opoint[1]*mat[7] + 
00047                           opoint[2]*mat[11] + mat[15]);
00048     npoint[0]=itmp3 * (opoint[0]*mat[0] + opoint[1]*mat[4] + opoint[2]*mat[ 8] + mat[12]);
00049     npoint[1]=itmp3 * (opoint[0]*mat[1] + opoint[1]*mat[5] + opoint[2]*mat[ 9] + mat[13]);
00050     npoint[2]=itmp3 * (opoint[0]*mat[2] + opoint[1]*mat[6] + opoint[2]*mat[10] + mat[14]);
00051 #else
00052     double tmp[3];
00053     double itmp3 = 1.0 / (opoint[0]*mat[3] + opoint[1]*mat[7] +
00054                           opoint[2]*mat[11] + mat[15]);
00055     tmp[0] = itmp3*opoint[0];
00056     tmp[1] = itmp3*opoint[1];
00057     tmp[2] = itmp3*opoint[2];
00058     npoint[0]=tmp[0]*mat[0] + tmp[1]*mat[4] + tmp[2]*mat[ 8] + itmp3*mat[12];
00059     npoint[1]=tmp[0]*mat[1] + tmp[1]*mat[5] + tmp[2]*mat[ 9] + itmp3*mat[13];
00060     npoint[2]=tmp[0]*mat[2] + tmp[1]*mat[6] + tmp[2]*mat[10] + itmp3*mat[14];
00061 #endif
00062 }
00063 
00064 
00065 // multiplies a 3D norm (first arg) by the Matrix, returns in second arg
00066 // This differs from point multiplication in that the translatation operations
00067 // are ignored.
00068 void Matrix4::multnorm3d(const double onorm[3], double nnorm[3]) const {
00069   double tmp[4];
00070 
00071   tmp[0]=onorm[0]*mat[0] + onorm[1]*mat[4] + onorm[2]*mat[8];
00072   tmp[1]=onorm[0]*mat[1] + onorm[1]*mat[5] + onorm[2]*mat[9];
00073   tmp[2]=onorm[0]*mat[2] + onorm[1]*mat[6] + onorm[2]*mat[10];
00074   tmp[3]=onorm[0]*mat[3] + onorm[1]*mat[7] + onorm[2]*mat[11];
00075   double itmp = 1.0 / sqrtf(tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2]);
00076   nnorm[0]=tmp[0]*itmp;
00077   nnorm[1]=tmp[1]*itmp;
00078   nnorm[2]=tmp[2]*itmp;
00079 }
00080 
00081 
00082 // multiplies a 4D point (first arg) by the Matrix, returns in second arg
00083 void Matrix4::multpoint4d(const double opoint[4], double npoint[4]) const {
00084   npoint[0]=opoint[0]*mat[0]+opoint[1]*mat[4]+opoint[2]*mat[8]+opoint[3]*mat[12];
00085   npoint[1]=opoint[0]*mat[1]+opoint[1]*mat[5]+opoint[2]*mat[9]+opoint[3]*mat[13];
00086   npoint[2]=opoint[0]*mat[2]+opoint[1]*mat[6]+opoint[2]*mat[10]+opoint[3]*mat[14];
00087   npoint[3]=opoint[0]*mat[3]+opoint[1]*mat[7]+opoint[2]*mat[11]+opoint[3]*mat[15];
00088 }
00089 
00090 
00091 // clears the matrix (resets it to identity)
00092 void Matrix4::identity(void) {
00093   memset((void *)mat, 0, 16*sizeof(double));
00094   mat[0]=1.0;
00095   mat[5]=1.0;
00096   mat[10]=1.0;
00097   mat[15]=1.0;
00098 }
00099 
00100 
00101 // sets the matrix so all items are the given constant value
00102 void Matrix4::constant(double f) {
00103   for (int i=0; i<16; mat[i++] = f); 
00104 }
00105 
00106 // return the inverse of this matrix, that is, 
00107 // the inverse of the rotation, the inverse of the scaling, and 
00108 // the opposite of the translation vector.
00109 #define MATSWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
00110 int Matrix4::inverse(void) {
00111 
00112   double matr[4][4], ident[4][4];
00113   int i, j, k, l, ll;
00114   int icol=0, irow=0;
00115   int indxc[4], indxr[4], ipiv[4];
00116   double big, dum, pivinv, temp;
00117  
00118   for (i=0; i<4; i++) {
00119     for (j=0; j<4; j++) {
00120       matr[i][j] = mat[4*i+j];
00121       ident[i][j] = 0.0;
00122     }
00123     ident[i][i]=1.0;
00124   } 
00125   // Gauss-jordan elimination with full pivoting.  Yes, folks, a 
00126   // GL Matrix4 is inverted like any other, since the identity is 
00127   // still the identity.
00128   
00129   // from numerical recipies in C second edition, pg 39
00130 
00131   for(j=0;j<=3;j++) ipiv[j] = 0;
00132   for(i=0;i<=3;i++) {
00133     big=0.0;
00134     for (j=0;j<=3;j++) {
00135       if(ipiv[j] != 1) {
00136         for (k=0;k<=3;k++) {
00137           if(ipiv[k] == 0) {
00138             if(fabs(matr[j][k]) >= big) {
00139               big = (double) fabs(matr[j][k]);
00140               irow=j;
00141               icol=k;
00142             }
00143           } else if (ipiv[k] > 1) return 1;
00144         } 
00145       }
00146     }
00147     ++(ipiv[icol]);
00148     if (irow != icol) {
00149       for (l=0;l<=3;l++) MATSWAP(matr[irow][l],matr[icol][l]);
00150       for (l=0;l<=3;l++) MATSWAP(ident[irow][l],ident[icol][l]);
00151     }
00152     indxr[i]=irow;
00153     indxc[i]=icol;
00154     if(matr[icol][icol] == 0.0) return 1; 
00155     pivinv = 1.0 / matr[icol][icol];
00156     matr[icol][icol]=1.0;
00157     for (l=0;l<=3;l++) matr[icol][l] *= pivinv;
00158     for (l=0;l<=3;l++) ident[icol][l] *= pivinv;
00159     for (ll=0;ll<=3;ll++) {
00160       if (ll != icol) {
00161         dum=matr[ll][icol];
00162         matr[ll][icol]=0.0;
00163         for (l=0;l<=3;l++) matr[ll][l] -= matr[icol][l]*dum;
00164         for (l=0;l<=3;l++) ident[ll][l] -= ident[icol][l]*dum;
00165       }
00166     }
00167   }
00168   for (l=3;l>=0;l--) {
00169     if (indxr[l] != indxc[l]) {
00170       for (k=0;k<=3;k++) {
00171         MATSWAP(matr[k][indxr[l]],matr[k][indxc[l]]);
00172       }
00173     }
00174   }
00175   for (i=0; i<4; i++) 
00176     for (j=0; j<4; j++)
00177       mat[4*i+j] = matr[i][j];
00178   return 0;
00179 }
00180       
00181 void Matrix4::transpose() {
00182   double tmp[16];
00183   int i,j;
00184   for(i=0;i<4;i++) {
00185     for(j=0;j<4;j++) {
00186       tmp[4*i+j] = mat[i+4*j];
00187     }
00188   }
00189   for(i=0;i<16;i++)
00190     mat[i] = tmp[i];
00191 }
00192 
00193 // replaces this matrix with the given one
00194 void Matrix4::loadmatrix(const Matrix4& m) {
00195   memcpy((void *)mat, (const void *)m.mat, 16*sizeof(double));
00196 }
00197 
00198 // premultiply the matrix by the given matrix
00199 void Matrix4::multmatrix(const Matrix4& m) {
00200   double tmp[4];
00201   for (int j=0; j<4; j++) {
00202     tmp[0] = mat[j];
00203     tmp[1] = mat[4+j];
00204     tmp[2] = mat[8+j]; 
00205     tmp[3] = mat[12+j];
00206     for (int i=0; i<4; i++) {
00207       mat[4*i+j] = m.mat[4*i]*tmp[0] + m.mat[4*i+1]*tmp[1] +
00208         m.mat[4*i+2]*tmp[2] + m.mat[4*i+3]*tmp[3]; 
00209     }
00210   } 
00211 }
00212 
00213 // performs a rotation around an axis (char == 'x', 'y', or 'z')
00214 // angle is in degrees
00215 void Matrix4::rot(double a, char axis) {
00216   Matrix4 m;                    // create identity matrix
00217   double angle;
00218 
00219   angle = (double)DEGTORAD(a);
00220 
00221   if (axis == 'x') {
00222     m.mat[0]=1.0;
00223     m.mat[5]=(double)cos(angle);
00224     m.mat[10]=m.mat[5];
00225     m.mat[6] = (double)sin(angle);
00226     m.mat[9] = -m.mat[6];
00227   } else if (axis == 'y') {
00228     m.mat[0] = (double)cos(angle);
00229     m.mat[5] = 1.0;
00230     m.mat[10] = m.mat[0];
00231     m.mat[2] = (double) -sin(angle);
00232     m.mat[8] = -m.mat[2];
00233   } else if (axis == 'z') {
00234     m.mat[0] = (double)cos(angle);
00235     m.mat[5] = m.mat[0];
00236     m.mat[10] = 1.0;
00237     m.mat[1] = (double)sin(angle);
00238     m.mat[4] = -m.mat[1];
00239   }
00240   // If there was an error, m is identity so we can multiply anyway.
00241   multmatrix(m);
00242 }
00243 
00244 // performs rotation around a given vector
00245 void Matrix4::rotate_axis(const double axis[3], double angle) {
00246   transvec(axis[0], axis[1], axis[2]);
00247   rot((double) (RADTODEG(angle)), 'x');
00248   transvecinv(axis[0], axis[1], axis[2]);
00249 }
00250 
00251 // applies the transformation needed to bring the x axis along the given vector. 
00252 void Matrix4::transvec(double x, double y, double z) {
00253   double theta = atan2(y,x);
00254   double length = sqrt(y*y + x*x);
00255   double phi = atan2((double) z, length);
00256   rot((double) RADTODEG(theta), 'z');
00257   rot((double) RADTODEG(-phi), 'y');
00258 }
00259 
00260 // applies the transformation needed to bring the given vector to the x axis.
00261 void Matrix4::transvecinv(double x, double y, double z) {
00262   double theta = atan2(y,x);
00263   double length = sqrt(y*y + x*x);
00264   double phi = atan2((double) z, length);
00265   rot((double) RADTODEG(phi), 'y');
00266   rot((double) RADTODEG(-theta), 'z');
00267 }
00268 
00269 // performs a translation
00270 void Matrix4::translate(double x, double y, double z) {
00271   Matrix4 m;            // create identity matrix
00272   m.mat[12] = x;
00273   m.mat[13] = y;
00274   m.mat[14] = z;
00275   multmatrix(m);
00276 }
00277 
00278 // performs scaling
00279 void Matrix4::scale(double x, double y, double z) {
00280   Matrix4 m;            // create identity matrix
00281   m.mat[0] = x;
00282   m.mat[5] = y;
00283   m.mat[10] = z;
00284   multmatrix(m);
00285 }
00286 
00287 // sets this matrix to represent a window perspective
00288 void Matrix4::window(double left, double right, double bottom, 
00289                          double top, double nearval, double farval) {
00290 
00291   constant(0.0);                // initialize this matrix to 0
00292   mat[0] = (2.0*nearval) / (right-left);
00293   mat[5] = (2.0*nearval) / (top-bottom);
00294   mat[8] = (right+left) / (right-left);
00295   mat[9] = (top+bottom) / (top-bottom);
00296   mat[10] = -(farval+nearval) / (farval-nearval);
00297   mat[11] = -1.0;
00298   mat[14] = -(2.0*farval*nearval) / (farval-nearval);
00299 }
00300 
00301 
00302 // sets this matrix to a 3D orthographic matrix
00303 void Matrix4::ortho(double left, double right, double bottom,
00304                         double top, double nearval, double farval) {
00305 
00306   constant(0.0);                // initialize this matrix to 0
00307   mat[0] =  2.0 / (right-left);
00308   mat[5] =  2.0 / (top-bottom);
00309   mat[10] = -2.0 / (farval-nearval);
00310   mat[12] = -(right+left) / (right-left);
00311   mat[13] = -(top+bottom) / (top-bottom);
00312   mat[14] = -(farval+nearval) / (farval-nearval);
00313   mat[15] = 1.0;
00314 }
00315 
00316 
00317 // sets this matrix to a 2D orthographic matrix
00318 void Matrix4::ortho2(double left, double right, double bottom, double top) {
00319 
00320   constant(0.0);                // initialize this matrix to 0
00321   mat[0] =  2.0 / (right-left);
00322   mat[5] =  2.0 / (top-bottom);
00323   mat[10] = -1.0;
00324   mat[12] = -(right+left) / (right-left);
00325   mat[13] = -(top+bottom) / (top-bottom);
00326   mat[15] =  1.0;
00327 }
00328 
00329 /* This subroutine defines a viewing transformation with the eye at the point
00330  * (vx,vy,vz) looking at the point (px,py,pz).  Twist is the right-hand
00331  * rotation about this line.  The resultant matrix is multiplied with
00332  * the top of the transformation stack and then replaces it.  Precisely,
00333  * lookat does:
00334  * lookat = trans(-vx,-vy,-vz)*rotate(theta,y)*rotate(phi,x)*rotate(-twist,z)
00335  */
00336  void Matrix4::lookat(double vx, double vy, double vz, double px, double py,
00337                          double pz, short twist) {
00338   Matrix4 m(0.0);
00339   double tmp;
00340 
00341   /* pre multiply stack by rotate(-twist,z) */
00342   rot(-twist / 10.0,'z');
00343 
00344   tmp = sqrtf((px-vx)*(px-vx) + (py-vy)*(py-vy) + (pz-vz)*(pz-vz));
00345   m.mat[0] = 1.0;
00346   m.mat[5] = sqrtf((px-vx)*(px-vx) + (pz-vz)*(pz-vz)) / tmp;
00347   m.mat[6] = (vy-py) / tmp;
00348   m.mat[9] = -m.mat[6];
00349   m.mat[10] = m.mat[5];
00350   m.mat[15] = 1.0;
00351   multmatrix(m);
00352 
00353   /* premultiply by rotate(theta,y) */
00354   m.constant(0.0);
00355   tmp = sqrtf((px-vx)*(px-vx) + (pz-vz)*(pz-vz));
00356   m.mat[0] = (vz-pz) / tmp;
00357   m.mat[5] = 1.0;
00358   m.mat[10] = m.mat[0];
00359   m.mat[15] = 1.0;
00360   m.mat[2] = -(px-vx) / tmp;
00361   m.mat[8] = -m.mat[2];
00362   multmatrix(m);
00363 
00364   /* premultiply by trans(-vx,-vy,-vz) */
00365   translate(-vx,-vy,-vz);
00366 }
00367 
00368  // Transform 3x3 into 4x4 matrix:
00369  void trans_from_rotate(const double mat3[9], Matrix4 *mat4) {
00370   int i;
00371   for (i=0; i<3; i++) {
00372     mat4->mat[0+i] = mat3[3*i];
00373     mat4->mat[4+i] = mat3[3*i+1];
00374     mat4->mat[8+i] = mat3[3*i+2];
00375   }
00376 }
00377 
00378 // Print a matrix for debugging purpose
00379 void print_Matrix4(const Matrix4 *mat4) {
00380   int i, j;
00381   for (i=0; i<4; i++) {
00382     for (j=0; j<4; j++) {
00383       printf("%f ", mat4->mat[4*j+i]);
00384     }
00385     printf("\n");
00386   }
00387   printf("\n");
00388 }
00389 

Generated on Mon Nov 20 01:17:12 2017 for NAMD by  doxygen 1.4.7