NAMD
Matrix4.C
Go to the documentation of this file.
1 /***************************************************************************
2  *cr
3  *cr (C) Copyright 1995-2008 The Board of Trustees of the
4  *cr University of Illinois
6  *cr
7  ***************************************************************************/
8
9 /***************************************************************************
10  * RCS INFORMATION:
11  *
12  * \$RCSfile: Matrix4.C,v \$
13  * \$Author: jim \$ \$Locker: \$ \$State: Exp \$
14  * \$Revision: 1.2 \$ \$Date: 2008/09/17 16:19:54 \$
15  *
16  ***************************************************************************
17  * DESCRIPTION:
18  *
19  * 4 x 4 Matrix, used for a transformation matrix.
20  *
21  ***************************************************************************/
22
23 #include <math.h>
24 #include <string.h>
25 #include <stdio.h>
26 #include "Matrix4.h"
27 // #include "utilities.h"
28
29 #define VMD_PI 3.14159265358979323846
30 #define VMD_TWOPI (2.0 * VMD_PI)
33
34
35 // constructor, for the case when an array of doubleing-point numbers is given
36 Matrix4::Matrix4(const double *m) {
37  memcpy((void *)mat, (const void *)m, 16*sizeof(double));
38 }
39
40 // multiplies a 3D point (first arg) by the Matrix, returns in second arg
41 void Matrix4::multpoint3d(const double opoint[3], double npoint[3]) const {
42 #if 0
43  // should try re-testing this formulation to see if it outperforms
44  // the old one, without introducing doubleing point imprecision
45  double tmp[3];
46  double itmp3 = 1.0 / (opoint[0]*mat[3] + opoint[1]*mat[7] +
47  opoint[2]*mat[11] + mat[15]);
48  npoint[0]=itmp3 * (opoint[0]*mat[0] + opoint[1]*mat[4] + opoint[2]*mat[ 8] + mat[12]);
49  npoint[1]=itmp3 * (opoint[0]*mat[1] + opoint[1]*mat[5] + opoint[2]*mat[ 9] + mat[13]);
50  npoint[2]=itmp3 * (opoint[0]*mat[2] + opoint[1]*mat[6] + opoint[2]*mat[10] + mat[14]);
51 #else
52  double tmp[3];
53  double itmp3 = 1.0 / (opoint[0]*mat[3] + opoint[1]*mat[7] +
54  opoint[2]*mat[11] + mat[15]);
55  tmp[0] = itmp3*opoint[0];
56  tmp[1] = itmp3*opoint[1];
57  tmp[2] = itmp3*opoint[2];
58  npoint[0]=tmp[0]*mat[0] + tmp[1]*mat[4] + tmp[2]*mat[ 8] + itmp3*mat[12];
59  npoint[1]=tmp[0]*mat[1] + tmp[1]*mat[5] + tmp[2]*mat[ 9] + itmp3*mat[13];
60  npoint[2]=tmp[0]*mat[2] + tmp[1]*mat[6] + tmp[2]*mat[10] + itmp3*mat[14];
61 #endif
62 }
63
64
65 // multiplies a 3D norm (first arg) by the Matrix, returns in second arg
66 // This differs from point multiplication in that the translatation operations
67 // are ignored.
68 void Matrix4::multnorm3d(const double onorm[3], double nnorm[3]) const {
69  double tmp[4];
70
71  tmp[0]=onorm[0]*mat[0] + onorm[1]*mat[4] + onorm[2]*mat[8];
72  tmp[1]=onorm[0]*mat[1] + onorm[1]*mat[5] + onorm[2]*mat[9];
73  tmp[2]=onorm[0]*mat[2] + onorm[1]*mat[6] + onorm[2]*mat[10];
74  tmp[3]=onorm[0]*mat[3] + onorm[1]*mat[7] + onorm[2]*mat[11];
75  double itmp = 1.0 / sqrtf(tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2]);
76  nnorm[0]=tmp[0]*itmp;
77  nnorm[1]=tmp[1]*itmp;
78  nnorm[2]=tmp[2]*itmp;
79 }
80
81
82 // multiplies a 4D point (first arg) by the Matrix, returns in second arg
83 void Matrix4::multpoint4d(const double opoint[4], double npoint[4]) const {
84  npoint[0]=opoint[0]*mat[0]+opoint[1]*mat[4]+opoint[2]*mat[8]+opoint[3]*mat[12];
85  npoint[1]=opoint[0]*mat[1]+opoint[1]*mat[5]+opoint[2]*mat[9]+opoint[3]*mat[13];
86  npoint[2]=opoint[0]*mat[2]+opoint[1]*mat[6]+opoint[2]*mat[10]+opoint[3]*mat[14];
87  npoint[3]=opoint[0]*mat[3]+opoint[1]*mat[7]+opoint[2]*mat[11]+opoint[3]*mat[15];
88 }
89
90
91 // clears the matrix (resets it to identity)
92 void Matrix4::identity(void) {
93  memset((void *)mat, 0, 16*sizeof(double));
94  mat[0]=1.0;
95  mat[5]=1.0;
96  mat[10]=1.0;
97  mat[15]=1.0;
98 }
99
100
101 // sets the matrix so all items are the given constant value
102 void Matrix4::constant(double f) {
103  for (int i=0; i<16; mat[i++] = f);
104 }
105
106 // return the inverse of this matrix, that is,
107 // the inverse of the rotation, the inverse of the scaling, and
108 // the opposite of the translation vector.
109 #define MATSWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
110 int Matrix4::inverse(void) {
111
112  double matr[4][4], ident[4][4];
113  int i, j, k, l, ll;
114  int icol=0, irow=0;
115  int indxc[4], indxr[4], ipiv[4];
116  double big, dum, pivinv, temp;
117
118  for (i=0; i<4; i++) {
119  for (j=0; j<4; j++) {
120  matr[i][j] = mat[4*i+j];
121  ident[i][j] = 0.0;
122  }
123  ident[i][i]=1.0;
124  }
125  // Gauss-jordan elimination with full pivoting. Yes, folks, a
126  // GL Matrix4 is inverted like any other, since the identity is
127  // still the identity.
128
129  // from numerical recipies in C second edition, pg 39
130
131  for(j=0;j<=3;j++) ipiv[j] = 0;
132  for(i=0;i<=3;i++) {
133  big=0.0;
134  for (j=0;j<=3;j++) {
135  if(ipiv[j] != 1) {
136  for (k=0;k<=3;k++) {
137  if(ipiv[k] == 0) {
138  if(fabs(matr[j][k]) >= big) {
139  big = (double) fabs(matr[j][k]);
140  irow=j;
141  icol=k;
142  }
143  } else if (ipiv[k] > 1) return 1;
144  }
145  }
146  }
147  ++(ipiv[icol]);
148  if (irow != icol) {
149  for (l=0;l<=3;l++) MATSWAP(matr[irow][l],matr[icol][l]);
150  for (l=0;l<=3;l++) MATSWAP(ident[irow][l],ident[icol][l]);
151  }
152  indxr[i]=irow;
153  indxc[i]=icol;
154  if(matr[icol][icol] == 0.0) return 1;
155  pivinv = 1.0 / matr[icol][icol];
156  matr[icol][icol]=1.0;
157  for (l=0;l<=3;l++) matr[icol][l] *= pivinv;
158  for (l=0;l<=3;l++) ident[icol][l] *= pivinv;
159  for (ll=0;ll<=3;ll++) {
160  if (ll != icol) {
161  dum=matr[ll][icol];
162  matr[ll][icol]=0.0;
163  for (l=0;l<=3;l++) matr[ll][l] -= matr[icol][l]*dum;
164  for (l=0;l<=3;l++) ident[ll][l] -= ident[icol][l]*dum;
165  }
166  }
167  }
168  for (l=3;l>=0;l--) {
169  if (indxr[l] != indxc[l]) {
170  for (k=0;k<=3;k++) {
171  MATSWAP(matr[k][indxr[l]],matr[k][indxc[l]]);
172  }
173  }
174  }
175  for (i=0; i<4; i++)
176  for (j=0; j<4; j++)
177  mat[4*i+j] = matr[i][j];
178  return 0;
179 }
180
182  double tmp[16];
183  int i,j;
184  for(i=0;i<4;i++) {
185  for(j=0;j<4;j++) {
186  tmp[4*i+j] = mat[i+4*j];
187  }
188  }
189  for(i=0;i<16;i++)
190  mat[i] = tmp[i];
191 }
192
193 // replaces this matrix with the given one
194 void Matrix4::loadmatrix(const Matrix4& m) {
195  memcpy((void *)mat, (const void *)m.mat, 16*sizeof(double));
196 }
197
198 // premultiply the matrix by the given matrix
199 void Matrix4::multmatrix(const Matrix4& m) {
200  double tmp[4];
201  for (int j=0; j<4; j++) {
202  tmp[0] = mat[j];
203  tmp[1] = mat[4+j];
204  tmp[2] = mat[8+j];
205  tmp[3] = mat[12+j];
206  for (int i=0; i<4; i++) {
207  mat[4*i+j] = m.mat[4*i]*tmp[0] + m.mat[4*i+1]*tmp[1] +
208  m.mat[4*i+2]*tmp[2] + m.mat[4*i+3]*tmp[3];
209  }
210  }
211 }
212
213 // performs a rotation around an axis (char == 'x', 'y', or 'z')
214 // angle is in degrees
215 void Matrix4::rot(double a, char axis) {
216  Matrix4 m; // create identity matrix
217  double angle;
218
220
221  if (axis == 'x') {
222  m.mat[0]=1.0;
223  m.mat[5]=(double)cos(angle);
224  m.mat[10]=m.mat[5];
225  m.mat[6] = (double)sin(angle);
226  m.mat[9] = -m.mat[6];
227  } else if (axis == 'y') {
228  m.mat[0] = (double)cos(angle);
229  m.mat[5] = 1.0;
230  m.mat[10] = m.mat[0];
231  m.mat[2] = (double) -sin(angle);
232  m.mat[8] = -m.mat[2];
233  } else if (axis == 'z') {
234  m.mat[0] = (double)cos(angle);
235  m.mat[5] = m.mat[0];
236  m.mat[10] = 1.0;
237  m.mat[1] = (double)sin(angle);
238  m.mat[4] = -m.mat[1];
239  }
240  // If there was an error, m is identity so we can multiply anyway.
241  multmatrix(m);
242 }
243
244 // performs rotation around a given vector
245 void Matrix4::rotate_axis(const double axis[3], double angle) {
246  transvec(axis[0], axis[1], axis[2]);
248  transvecinv(axis[0], axis[1], axis[2]);
249 }
250
251 // applies the transformation needed to bring the x axis along the given vector.
252 void Matrix4::transvec(double x, double y, double z) {
253  double theta = atan2(y,x);
254  double length = sqrt(y*y + x*x);
255  double phi = atan2((double) z, length);
258 }
259
260 // applies the transformation needed to bring the given vector to the x axis.
261 void Matrix4::transvecinv(double x, double y, double z) {
262  double theta = atan2(y,x);
263  double length = sqrt(y*y + x*x);
264  double phi = atan2((double) z, length);
267 }
268
269 // performs a translation
270 void Matrix4::translate(double x, double y, double z) {
271  Matrix4 m; // create identity matrix
272  m.mat[12] = x;
273  m.mat[13] = y;
274  m.mat[14] = z;
275  multmatrix(m);
276 }
277
278 // performs scaling
279 void Matrix4::scale(double x, double y, double z) {
280  Matrix4 m; // create identity matrix
281  m.mat[0] = x;
282  m.mat[5] = y;
283  m.mat[10] = z;
284  multmatrix(m);
285 }
286
287 // sets this matrix to represent a window perspective
288 void Matrix4::window(double left, double right, double bottom,
289  double top, double nearval, double farval) {
290
291  constant(0.0); // initialize this matrix to 0
292  mat[0] = (2.0*nearval) / (right-left);
293  mat[5] = (2.0*nearval) / (top-bottom);
294  mat[8] = (right+left) / (right-left);
295  mat[9] = (top+bottom) / (top-bottom);
296  mat[10] = -(farval+nearval) / (farval-nearval);
297  mat[11] = -1.0;
298  mat[14] = -(2.0*farval*nearval) / (farval-nearval);
299 }
300
301
302 // sets this matrix to a 3D orthographic matrix
303 void Matrix4::ortho(double left, double right, double bottom,
304  double top, double nearval, double farval) {
305
306  constant(0.0); // initialize this matrix to 0
307  mat[0] = 2.0 / (right-left);
308  mat[5] = 2.0 / (top-bottom);
309  mat[10] = -2.0 / (farval-nearval);
310  mat[12] = -(right+left) / (right-left);
311  mat[13] = -(top+bottom) / (top-bottom);
312  mat[14] = -(farval+nearval) / (farval-nearval);
313  mat[15] = 1.0;
314 }
315
316
317 // sets this matrix to a 2D orthographic matrix
318 void Matrix4::ortho2(double left, double right, double bottom, double top) {
319
320  constant(0.0); // initialize this matrix to 0
321  mat[0] = 2.0 / (right-left);
322  mat[5] = 2.0 / (top-bottom);
323  mat[10] = -1.0;
324  mat[12] = -(right+left) / (right-left);
325  mat[13] = -(top+bottom) / (top-bottom);
326  mat[15] = 1.0;
327 }
328
329 /* This subroutine defines a viewing transformation with the eye at the point
330  * (vx,vy,vz) looking at the point (px,py,pz). Twist is the right-hand
332  * the top of the transformation stack and then replaces it. Precisely,
333  * lookat does:
334  * lookat = trans(-vx,-vy,-vz)*rotate(theta,y)*rotate(phi,x)*rotate(-twist,z)
335  */
336  void Matrix4::lookat(double vx, double vy, double vz, double px, double py,
337  double pz, short twist) {
338  Matrix4 m(0.0);
339  double tmp;
340
341  /* pre multiply stack by rotate(-twist,z) */
342  rot(-twist / 10.0,'z');
343
344  tmp = sqrtf((px-vx)*(px-vx) + (py-vy)*(py-vy) + (pz-vz)*(pz-vz));
345  m.mat[0] = 1.0;
346  m.mat[5] = sqrtf((px-vx)*(px-vx) + (pz-vz)*(pz-vz)) / tmp;
347  m.mat[6] = (vy-py) / tmp;
348  m.mat[9] = -m.mat[6];
349  m.mat[10] = m.mat[5];
350  m.mat[15] = 1.0;
351  multmatrix(m);
352
353  /* premultiply by rotate(theta,y) */
354  m.constant(0.0);
355  tmp = sqrtf((px-vx)*(px-vx) + (pz-vz)*(pz-vz));
356  m.mat[0] = (vz-pz) / tmp;
357  m.mat[5] = 1.0;
358  m.mat[10] = m.mat[0];
359  m.mat[15] = 1.0;
360  m.mat[2] = -(px-vx) / tmp;
361  m.mat[8] = -m.mat[2];
362  multmatrix(m);
363
364  /* premultiply by trans(-vx,-vy,-vz) */
365  translate(-vx,-vy,-vz);
366 }
367
368  // Transform 3x3 into 4x4 matrix:
369  void trans_from_rotate(const double mat3[9], Matrix4 *mat4) {
370  int i;
371  for (i=0; i<3; i++) {
372  mat4->mat[0+i] = mat3[3*i];
373  mat4->mat[4+i] = mat3[3*i+1];
374  mat4->mat[8+i] = mat3[3*i+2];
375  }
376 }
377
378 // Print a matrix for debugging purpose
379 void print_Matrix4(const Matrix4 *mat4) {
380  int i, j;
381  for (i=0; i<4; i++) {
382  for (j=0; j<4; j++) {
383  printf("%f ", mat4->mat[4*j+i]);
384  }
385  printf("\n");
386  }
387  printf("\n");
388 }
389
void identity(void)
clears the matrix (resets it to identity)
Definition: Matrix4.C:92
Definition: Matrix4.C:31
void transvecinv(double x, double y, double z)
apply a rotation such that the given vector is brought along &#39;x&#39;.
Definition: Matrix4.C:261
void multnorm3d(const double[3], double[3]) const
multiplies a 3D norm (first arg) by the Matrix, returns in second arg
Definition: Matrix4.C:68
Matrix4(void)
identity constructor
Definition: Matrix4.h:28
#define MATSWAP(a, b)
Definition: Matrix4.C:109
Definition: Matrix4.C:32
void multpoint3d(const double[3], double[3]) const
multiplies a 3D point (first arg) by the Matrix, returns in second arg
Definition: Matrix4.C:41
void lookat(double, double, double, double, double, double, short)
Definition: Matrix4.C:336
void window(double, double, double, double, double, double)
sets this matrix to represent a window perspective
Definition: Matrix4.C:288
void scale(double, double, double)
performs scaling
Definition: Matrix4.C:279
void print_Matrix4(const Matrix4 *mat4)
Print formatted matrix.
Definition: Matrix4.C:379
4x4 matrix class with numerous operators, conversions, etc.
Definition: Matrix4.h:26
gridSize z
void multpoint4d(const double[4], double[4]) const
multiplies a 4D point (first arg) by the Matrix, returns in second arg
Definition: Matrix4.C:83
replaces this matrix with the given one
Definition: Matrix4.C:194
int inverse(void)
Definition: Matrix4.C:110
void trans_from_rotate(const double mat3[9], Matrix4 *mat4)
Transform 3x3 into 4x4 matrix:
Definition: Matrix4.C:369
void rotate_axis(const double axis[3], double angle)
apply a rotation around the given vector; angle in radians.
Definition: Matrix4.C:245
void ortho2(double, double, double, double)
sets this matrix to a 2D orthographic matrix
Definition: Matrix4.C:318
double mat[16]
the matrix itself
Definition: Matrix4.h:33
void transvec(double x, double y, double z)
apply a rotation such that &#39;x&#39; is brought along the given vector.
Definition: Matrix4.C:252
void transpose(void)
transposes the matrix
Definition: Matrix4.C:181
void translate(double, double, double)
performs a translation
Definition: Matrix4.C:270
gridSize y
void rot(double, char)
performs a left-handed rotation around an axis (char == &#39;x&#39;, &#39;y&#39;, or &#39;z&#39;)
Definition: Matrix4.C:215
void multmatrix(const Matrix4 &)
premultiply the matrix by the given matrix, this-&gt;other * this
Definition: Matrix4.C:199
void constant(double)
sets the matrix so all items are the given constant value
Definition: Matrix4.C:102
gridSize x
void ortho(double, double, double, double, double, double)
sets this matrix to a 3D orthographic matrix
Definition: Matrix4.C:303