source: GTP/trunk/Lib/Vis/Preprocessing/src/Matrix4x4.cpp @ 1980

Revision 1980, 14.0 KB checked in by mattausch, 18 years ago (diff)
Line 
1#include "Matrix4x4.h"
2#include "Vector3.h"
3
4// standard headers
5#include <iomanip>
6using namespace std;
7
8namespace GtpVisibilityPreprocessor {
9
10
11Matrix4x4::Matrix4x4(const Vector3 &a, const Vector3 &b, const Vector3 &c)
12{
13  // first index is column [x], the second is row [y]
14  x[0][0] = a.x;
15  x[1][0] = b.x;
16  x[2][0] = c.x;
17  x[3][0] = 0.0f;
18
19  x[0][1] = a.y;
20  x[1][1] = b.y;
21  x[2][1] = c.y;
22  x[3][1] = 0.0f;
23
24  x[0][2] = a.z;
25  x[1][2] = b.z;
26  x[2][2] = c.z;
27  x[3][2] = 0.0f;
28
29  x[0][3] = 0.0f;
30  x[1][3] = 0.0f;
31  x[2][3] = 0.0f;
32  x[3][3] = 1.0f;
33
34}
35
36void
37Matrix4x4::SetColumns(const Vector3 &a, const Vector3 &b, const Vector3 &c)
38{
39  // first index is column [x], the second is row [y]
40  x[0][0] = a.x;
41  x[1][0] = a.y;
42  x[2][0] = a.z;
43  x[3][0] = 0.0f;
44
45  x[0][1] = b.x;
46  x[1][1] = b.y;
47  x[2][1] = b.z;
48  x[3][1] = 0.0f;
49
50  x[0][2] = c.x;
51  x[1][2] = c.y;
52  x[2][2] = c.z;
53  x[3][2] = 0.0f;
54
55  x[0][3] = 0.0f;
56  x[1][3] = 0.0f;
57  x[2][3] = 0.0f;
58  x[3][3] = 1.0f;
59}
60
61// full constructor
62Matrix4x4::Matrix4x4(float x11, float x12, float x13, float x14,
63                                         float x21, float x22, float x23, float x24,
64                                         float x31, float x32, float x33, float x34,
65                                         float x41, float x42, float x43, float x44)
66{
67  // first index is column [x], the second is row [y]
68  x[0][0] = x11;
69  x[1][0] = x12;
70  x[2][0] = x13;
71  x[3][0] = x14;
72
73  x[0][1] = x21;
74  x[1][1] = x22;
75  x[2][1] = x23;
76  x[3][1] = x24;
77
78  x[0][2] = x31;
79  x[1][2] = x32;
80  x[2][2] = x33;
81  x[3][2] = x34;
82
83  x[0][3] = x41;
84  x[1][3] = x42;
85  x[2][3] = x43;
86  x[3][3] = x44;
87}
88
89// inverse matrix computation gauss_jacobiho method .. from N.R. in C
90// if matrix is regular = computatation successfull = returns 0
91// in case of singular matrix returns 1
92int
93Matrix4x4::Invert()
94{
95  int indxc[4],indxr[4],ipiv[4];
96  int i,icol,irow,j,k,l,ll,n;
97  float big,dum,pivinv,temp;
98  // satisfy the compiler
99  icol = irow = 0;
100
101  // the size of the matrix
102  n = 4;
103
104  for ( j = 0 ; j < n ; j++) /* zero pivots */
105    ipiv[j] = 0;
106
107  for ( i = 0; i < n; i++)
108  {
109    big = 0.0;
110    for (j = 0 ; j < n ; j++)
111      if (ipiv[j] != 1)
112        for ( k = 0 ; k<n ; k++)
113        {
114          if (ipiv[k] == 0)
115          {
116            if (fabs(x[k][j]) >= big)
117            {
118              big = fabs(x[k][j]);
119              irow = j;
120              icol = k;
121            }
122          }
123          else
124            if (ipiv[k] > 1)
125              return 1; /* singular matrix */
126        }
127    ++(ipiv[icol]);
128    if (irow != icol)
129    {
130      for ( l = 0 ; l<n ; l++)
131      {
132        temp = x[l][icol];
133        x[l][icol] = x[l][irow];
134        x[l][irow] = temp;
135      }
136    }
137    indxr[i] = irow;
138    indxc[i] = icol;
139    if (x[icol][icol] == 0.0)
140      return 1; /* singular matrix */
141
142    pivinv = 1.0 / x[icol][icol];
143    x[icol][icol] = 1.0 ;
144    for ( l = 0 ; l<n ; l++)
145      x[l][icol] = x[l][icol] * pivinv ;
146   
147    for (ll = 0 ; ll < n ; ll++)
148      if (ll != icol)
149      {
150        dum = x[icol][ll];
151        x[icol][ll] = 0.0;
152        for ( l = 0 ; l<n ; l++)
153          x[l][ll] = x[l][ll] - x[l][icol] * dum ;
154      }
155  }
156  for ( l = n; l--; )
157  {
158    if (indxr[l] != indxc[l])
159      for ( k = 0; k<n ; k++)
160      {
161        temp = x[indxr[l]][k];
162        x[indxr[l]][k] = x[indxc[l]][k];
163        x[indxc[l]][k] = temp;
164      }
165  }
166
167  return 0 ; // matrix is regular .. inversion has been succesfull
168}
169
170// Invert the given matrix using the above inversion routine.
171Matrix4x4
172Invert(const Matrix4x4& M)
173{
174  Matrix4x4 InvertMe = M;
175  InvertMe.Invert();
176  return InvertMe;
177}
178
179
180// Transpose the matrix.
181void
182Matrix4x4::Transpose()
183{
184  for (int i = 0; i < 4; i++)
185    for (int j = i; j < 4; j++)
186      if (i != j) {
187        float temp = x[i][j];
188        x[i][j] = x[j][i];
189        x[j][i] = temp;
190      }
191}
192
193// Transpose the given matrix using the transpose routine above.
194Matrix4x4
195Transpose(const Matrix4x4& M)
196{
197  Matrix4x4 TransposeMe = M;
198  TransposeMe.Transpose();
199  return TransposeMe;
200}
201
202// Construct an identity matrix.
203Matrix4x4
204IdentityMatrix()
205{
206  Matrix4x4 M;
207
208  for (int i = 0; i < 4; i++)
209    for (int j = 0; j < 4; j++)
210      M.x[i][j] = (i == j) ? 1.0 : 0.0;
211  return M;
212}
213
214// Construct a zero matrix.
215Matrix4x4
216ZeroMatrix()
217{
218  Matrix4x4 M;
219  for (int i = 0; i < 4; i++)
220    for (int j = 0; j < 4; j++)
221      M.x[i][j] = 0;
222  return M;
223}
224
225// Construct a translation matrix given the location to translate to.
226Matrix4x4
227TranslationMatrix(const Vector3& Location)
228{
229  Matrix4x4 M = IdentityMatrix();
230  M.x[3][0] = Location.x;
231  M.x[3][1] = Location.y;
232  M.x[3][2] = Location.z;
233  return M;
234}
235
236// Construct a rotation matrix.  Rotates Angle radians about the
237// X axis.
238Matrix4x4
239RotationXMatrix(float Angle)
240{
241  Matrix4x4 M = IdentityMatrix();
242  float Cosine = cos(Angle);
243  float Sine = sin(Angle);
244  M.x[1][1] = Cosine;
245  M.x[2][1] = -Sine;
246  M.x[1][2] = Sine;
247  M.x[2][2] = Cosine;
248  return M;
249}
250
251// Construct a rotation matrix.  Rotates Angle radians about the
252// Y axis.
253Matrix4x4
254RotationYMatrix(float Angle)
255{
256  Matrix4x4 M = IdentityMatrix();
257  float Cosine = cos(Angle);
258  float Sine = sin(Angle);
259  M.x[0][0] = Cosine;
260  M.x[2][0] = -Sine;
261  M.x[0][2] = Sine;
262  M.x[2][2] = Cosine;
263  return M;
264}
265
266// Construct a rotation matrix.  Rotates Angle radians about the
267// Z axis.
268Matrix4x4
269RotationZMatrix(float Angle)
270{
271  Matrix4x4 M = IdentityMatrix();
272  float Cosine = cos(Angle);
273  float Sine = sin(Angle);
274  M.x[0][0] = Cosine;
275  M.x[1][0] = -Sine;
276  M.x[0][1] = Sine;
277  M.x[1][1] = Cosine;
278  return M;
279}
280
281// Construct a yaw-pitch-roll rotation matrix.  Rotate Yaw
282// radians about the XY axis, rotate Pitch radians in the
283// plane defined by the Yaw rotation, and rotate Roll radians
284// about the axis defined by the previous two angles.
285Matrix4x4
286RotationYPRMatrix(float Yaw, float Pitch, float Roll)
287{
288  Matrix4x4 M;
289  float ch = cos(Yaw);
290  float sh = sin(Yaw);
291  float cp = cos(Pitch);
292  float sp = sin(Pitch);
293  float cr = cos(Roll);
294  float sr = sin(Roll);
295
296  M.x[0][0] = ch * cr + sh * sp * sr;
297  M.x[1][0] = -ch * sr + sh * sp * cr;
298  M.x[2][0] = sh * cp;
299  M.x[0][1] = sr * cp;
300  M.x[1][1] = cr * cp;
301  M.x[2][1] = -sp;
302  M.x[0][2] = -sh * cr - ch * sp * sr;
303  M.x[1][2] = sr * sh + ch * sp * cr;
304  M.x[2][2] = ch * cp;
305  for (int i = 0; i < 4; i++)
306    M.x[3][i] = M.x[i][3] = 0;
307  M.x[3][3] = 1;
308
309  return M;
310}
311
312// Construct a rotation of a given angle about a given axis.
313// Derived from Eric Haines's SPD (Standard Procedural
314// Database).
315Matrix4x4
316RotationAxisMatrix(const Vector3& axis, float angle)
317{
318  Matrix4x4 M;
319  double cosine = cos(angle);
320  double sine = sin(angle);
321  double one_minus_cosine = 1 - cosine;
322
323  M.x[0][0] = axis.x * axis.x + (1.0 - axis.x * axis.x) * cosine;
324  M.x[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
325  M.x[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
326  M.x[0][3] = 0;
327
328  M.x[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
329  M.x[1][1] = axis.y * axis.y + (1.0 - axis.y * axis.y) * cosine;
330  M.x[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
331  M.x[1][3] = 0;
332
333  M.x[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
334  M.x[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
335  M.x[2][2] = axis.z * axis.z + (1.0 - axis.z * axis.z) * cosine;
336  M.x[2][3] = 0;
337
338  M.x[3][0] = 0;
339  M.x[3][1] = 0;
340  M.x[3][2] = 0;
341  M.x[3][3] = 1;
342
343  return M;
344}
345
346
347// Constructs the rotation matrix that rotates 'vec1' to 'vec2'
348Matrix4x4
349RotationVectorsMatrix(const Vector3 &vecStart,
350                      const Vector3 &vecTo)
351{
352  Vector3 vec = CrossProd(vecStart, vecTo);
353 
354  if (Magnitude(vec) > Limits::Small) {
355    // vector exist, compute angle
356    float angle = acos(DotProd(vecStart, vecTo));
357    // normalize for sure
358    vec.Normalize();
359    return RotationAxisMatrix(vec, angle);
360  }
361
362  // opposite or colinear vectors
363  Matrix4x4 ret = IdentityMatrix();
364  if (DotProd(vecStart, vecTo) < 0.0)
365    ret *= -1.0; // opposite vectors
366
367  return ret;
368}
369
370
371// Construct a scale matrix given the X, Y, and Z parameters
372// to scale by.  To scale uniformly, let X==Y==Z.
373Matrix4x4
374ScaleMatrix(float X, float Y, float Z)
375{
376  Matrix4x4 M = IdentityMatrix();
377
378  M.x[0][0] = X;
379  M.x[1][1] = Y;
380  M.x[2][2] = Z;
381
382  return M;
383}
384
385// Construct a rotation matrix that makes the x, y, z axes
386// correspond to the vectors given.
387Matrix4x4
388GenRotation(const Vector3 &x, const Vector3 &y, const Vector3 &z)
389{
390  Matrix4x4 M = IdentityMatrix();
391
392#if 1
393  //  x  y
394  M.x[0][0] = x.x;
395  M.x[1][0] = x.y;
396  M.x[2][0] = x.z;
397
398  M.x[0][1] = y.x;
399  M.x[1][1] = y.y;
400  M.x[2][1] = y.z;
401
402  M.x[0][2] = z.x;
403  M.x[1][2] = z.y;
404  M.x[2][2] = z.z;
405#else
406  //  x  y -- old version
407  M.x[0][0] = x.x;
408  M.x[0][1] = x.y;
409  M.x[0][2] = x.z;
410
411  M.x[1][0] = y.x;
412  M.x[1][1] = y.y;
413  M.x[1][2] = y.z;
414
415  M.x[2][0] = z.x;
416  M.x[2][1] = z.y;
417  M.x[2][2] = z.z;
418#endif
419
420  return M;
421}
422
423// Construct a quadric matrix.  After Foley et al. pp. 528-529.
424Matrix4x4
425QuadricMatrix(float a, float b, float c, float d, float e,
426              float f, float g, float h, float j, float k)
427{
428  Matrix4x4 M;
429
430  M.x[0][0] = a;  M.x[0][1] = d;  M.x[0][2] = f;  M.x[0][3] = g;
431  M.x[1][0] = d;  M.x[1][1] = b;  M.x[1][2] = e;  M.x[1][3] = h;
432  M.x[2][0] = f;  M.x[2][1] = e;  M.x[2][2] = c;  M.x[2][3] = j;
433  M.x[3][0] = g;  M.x[3][1] = h;  M.x[3][2] = j;  M.x[3][3] = k;
434
435  return M;
436}
437
438// Construct various "mirror" matrices, which flip coordinate
439// signs in the various axes specified.
440Matrix4x4
441MirrorX()
442{
443  Matrix4x4 M = IdentityMatrix();
444  M.x[0][0] = -1;
445  return M;
446}
447
448Matrix4x4
449MirrorY()
450{
451  Matrix4x4 M = IdentityMatrix();
452  M.x[1][1] = -1;
453  return M;
454}
455
456Matrix4x4
457MirrorZ()
458{
459  Matrix4x4 M = IdentityMatrix();
460  M.x[2][2] = -1;
461  return M;
462}
463
464Matrix4x4
465RotationOnly(const Matrix4x4& x)
466{
467  Matrix4x4 M = x;
468  M.x[3][0] = M.x[3][1] = M.x[3][2] = 0;
469  return M;
470}
471
472// Add corresponding elements of the two matrices.
473Matrix4x4&
474Matrix4x4::operator+= (const Matrix4x4& A)
475{
476  for (int i = 0; i < 4; i++)
477    for (int j = 0; j < 4; j++)
478      x[i][j] += A.x[i][j];
479  return *this;
480}
481
482// Subtract corresponding elements of the matrices.
483Matrix4x4&
484Matrix4x4::operator-= (const Matrix4x4& A)
485{
486  for (int i = 0; i < 4; i++)
487    for (int j = 0; j < 4; j++)
488      x[i][j] -= A.x[i][j];
489  return *this;
490}
491
492// Scale each element of the matrix by A.
493Matrix4x4&
494Matrix4x4::operator*= (float A)
495{
496  for (int i = 0; i < 4; i++)
497    for (int j = 0; j < 4; j++)
498      x[i][j] *= A;
499  return *this;
500}
501
502// Multiply two matrices.
503Matrix4x4&
504Matrix4x4::operator*= (const Matrix4x4& A)
505{
506  Matrix4x4 ret = *this;
507
508  for (int i = 0; i < 4; i++)
509    for (int j = 0; j < 4; j++) {
510      float subt = 0;
511      for (int k = 0; k < 4; k++)
512        subt += ret.x[i][k] * A.x[k][j];
513      x[i][j] = subt;
514    }
515  return *this;
516}
517
518// Add corresponding elements of the matrices.
519Matrix4x4
520operator+ (const Matrix4x4& A, const Matrix4x4& B)
521{
522  Matrix4x4 ret;
523
524  for (int i = 0; i < 4; i++)
525    for (int j = 0; j < 4; j++)
526      ret.x[i][j] = A.x[i][j] + B.x[i][j];
527  return ret;
528}
529
530// Subtract corresponding elements of the matrices.
531Matrix4x4
532operator- (const Matrix4x4& A, const Matrix4x4& B)
533{
534  Matrix4x4 ret;
535
536  for (int i = 0; i < 4; i++)
537    for (int j = 0; j < 4; j++)
538      ret.x[i][j] = A.x[i][j] - B.x[i][j];
539  return ret;
540}
541
542// Multiply matrices.
543Matrix4x4
544operator* (const Matrix4x4& A, const Matrix4x4& B)
545{
546  Matrix4x4 ret;
547
548  for (int i = 0; i < 4; i++)
549    for (int j = 0; j < 4; j++) {
550      float subt = 0;
551      for (int k = 0; k < 4; k++)
552        subt += A.x[i][k] * B.x[k][j];
553      ret.x[i][j] = subt;
554    }
555  return ret;
556}
557
558// Transform a vector by a matrix.
559Vector3
560operator* (const Matrix4x4& M, const Vector3& v)
561{
562  Vector3 ret;
563  float denom;
564
565  ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0] + M.x[3][0];
566  ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1] + M.x[3][1];
567  ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2] + M.x[3][2];
568  denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
569  if (denom != 1.0)
570    ret /= denom;
571  return ret;
572}
573
574// Apply the rotation portion of a matrix to a vector.
575Vector3
576RotateOnly(const Matrix4x4& M, const Vector3& v)
577{
578  Vector3 ret;
579  float denom;
580
581  ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0];
582  ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1];
583  ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2];
584  denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
585  if (denom != 1.0)
586    ret /= denom;
587  return ret;
588}
589
590// Scale each element of the matrix by B.
591Matrix4x4
592operator* (const Matrix4x4& A, float B)
593{
594  Matrix4x4 ret;
595
596  for (int i = 0; i < 4; i++)
597    for (int j = 0; j < 4; j++)
598      ret.x[i][j] = A.x[i][j] * B;
599  return ret;
600}
601
602// Overloaded << for C++-style output.
603ostream&
604operator<< (ostream& s, const Matrix4x4& M)
605{
606  for (int i = 0; i < 4; i++) { // y
607    for (int j = 0; j < 4; j++) { // x
608                                          //  x  y
609      s << setprecision(4) << setw(10) << M.x[j][i];
610    }
611    s << '\n';
612  }
613  return s;
614}
615
616// Rotate a direction vector...
617Vector3
618PlaneRotate(const Matrix4x4& tform, const Vector3& p)
619{
620  // I sure hope that matrix is invertible...
621  Matrix4x4 use = Transpose(Invert(tform));
622
623  return RotateOnly(use, p);
624}
625
626// Transform a normal
627Vector3
628TransformNormal(const Matrix4x4& tform, const Vector3& n)
629{
630  Matrix4x4 use = NormalTransformMatrix(tform);
631
632  return RotateOnly(use, n);
633}
634 
635Matrix4x4
636NormalTransformMatrix(const Matrix4x4 &tform)
637{
638  Matrix4x4 m = tform;
639  // for normal translation vector must be zero!
640  m.x[3][0] = m.x[3][1] = m.x[3][2] = 0.0;
641  // I sure hope that matrix is invertible...
642  return Transpose(Invert(m));
643}
644
645Vector3
646GetTranslation(const Matrix4x4 &M)
647{
648  return Vector3(M.x[3][0], M.x[3][1], M.x[3][2]);
649}
650
651}
Note: See TracBrowser for help on using the repository browser.