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

Revision 2678, 14.0 KB checked in by mattausch, 16 years ago (diff)

started working on dynamic objects

RevLine 
[162]1#include "Matrix4x4.h"
2#include "Vector3.h"
3
4// standard headers
5#include <iomanip>
6using namespace std;
7
[863]8namespace GtpVisibilityPreprocessor {
[162]9
[860]10
[209]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;
[245]15  x[1][0] = b.x;
16  x[2][0] = c.x;
[209]17  x[3][0] = 0.0f;
[162]18
[245]19  x[0][1] = a.y;
[209]20  x[1][1] = b.y;
[245]21  x[2][1] = c.y;
[209]22  x[3][1] = 0.0f;
23
[245]24  x[0][2] = a.z;
25  x[1][2] = b.z;
[209]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;
[245]33
[209]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;
[245]41  x[1][0] = a.y;
42  x[2][0] = a.z;
[209]43  x[3][0] = 0.0f;
44
[245]45  x[0][1] = b.x;
[209]46  x[1][1] = b.y;
[245]47  x[2][1] = b.z;
[209]48  x[3][1] = 0.0f;
49
[245]50  x[0][2] = c.x;
51  x[1][2] = c.y;
[209]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
[162]61// full constructor
62Matrix4x4::Matrix4x4(float x11, float x12, float x13, float x14,
[1980]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)
[162]66{
[209]67  // first index is column [x], the second is row [y]
[162]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;
[2678]139    if (x[icol][icol] == 0.0f)
[162]140      return 1; /* singular matrix */
141
[2678]142    pivinv = 1.0f / x[icol][icol];
143    x[icol][icol] = 1.0f ;
[162]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];
[2678]151        x[icol][ll] = 0.0f;
[162]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++)
[2678]210      M.x[i][j] = (i == j) ? 1.0f : 0.0f;
[162]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
[2678]323  M.x[0][0] = axis.x * axis.x + (1.0f - axis.x * axis.x) * cosine;
[162]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;
[2678]329  M.x[1][1] = axis.y * axis.y + (1.0f - axis.y * axis.y) * cosine;
[162]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;
[2678]335  M.x[2][2] = axis.z * axis.z + (1.0f - axis.z * axis.z) * cosine;
[162]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);
[176]353 
[162]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}
[860]650
651}
Note: See TracBrowser for help on using the repository browser.