Matrix4.C

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