Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/matrix.cc @ 3372

Last change on this file since 3372 was 3365, checked in by bensch, 20 years ago

orxonox/trunk: merged branches/parenting back to the.
merged with command:
svn merge branches/parenting trunk -r 3247:HEAD
resolved all conflicts in favor of parenting.

File size: 15.2 KB
Line 
1
2/*
3   orxonox - the future of 3D-vertical-scrollers
4
5   Copyright (C) 2004 orx
6
7   This program is free software; you can redistribute it and/or modify
8   it under the terms of the GNU General Public License as published by
9   the Free Software Foundation; either version 2, or (at your option)
10   any later version.
11
12   ### File Specific:
13   main-programmer: Benjamin Grauer
14   co-programmer: ...
15
16   \todo Null-Parent => center of the coord system - singleton
17   \todo Smooth-Parent: delay, speed
18   \todo destroy the stuff again, delete...
19*/
20
21#include "matrix.h"
22
23Matrix::Matrix (size_t row, size_t col)
24{
25  _m = new base_mat( row, col, 0);
26}
27
28// copy constructor
29Matrix::Matrix (const Matrix& m)
30{
31    _m = m._m;
32    _m->Refcnt++;
33}
34
35// Internal copy constructor
36void Matrix::clone ()
37{
38    _m->Refcnt--;
39    _m = new base_mat( _m->Row, _m->Col, _m->Val);
40}
41
42// destructor
43Matrix::~Matrix ()
44{
45   if (--_m->Refcnt == 0) delete _m;
46}
47
48// assignment operator
49Matrix& Matrix::operator = (const Matrix& m) 
50{
51    m._m->Refcnt++;
52    if (--_m->Refcnt == 0) delete _m;
53    _m = m._m;
54    return *this;
55}
56
57//  reallocation method
58void Matrix::realloc (size_t row, size_t col)
59{
60   if (row == _m->RowSiz && col == _m->ColSiz)
61   {
62      _m->Row = _m->RowSiz;
63      _m->Col = _m->ColSiz;
64      return;
65   }
66
67   base_mat *m1 = new base_mat( row, col, NULL);
68   size_t colSize = min(_m->Col,col) * sizeof(float);
69   size_t minRow = min(_m->Row,row);
70
71   for (size_t i=0; i < minRow; i++)
72      memcpy( m1->Val[i], _m->Val[i], colSize);
73
74   if (--_m->Refcnt == 0) 
75       delete _m;
76   _m = m1;
77
78   return;
79}
80
81// public method for resizing Matrix
82void Matrix::SetSize (size_t row, size_t col) 
83{
84   size_t i,j;
85   size_t oldRow = _m->Row;
86   size_t oldCol = _m->Col;
87
88   if (row != _m->RowSiz || col != _m->ColSiz)
89      realloc( row, col);
90
91   for (i=oldRow; i < row; i++)
92      for (j=0; j < col; j++)
93         _m->Val[i][j] = float(0);
94
95   for (i=0; i < row; i++)                     
96      for (j=oldCol; j < col; j++)
97         _m->Val[i][j] = float(0);
98
99   return;
100}
101
102// subscript operator to get/set individual elements
103float& Matrix::operator () (size_t row, size_t col) 
104{
105   if (row >= _m->Row || col >= _m->Col)
106      printf( "Matrix::operator(): Index out of range!\n");
107   if (_m->Refcnt > 1) clone();
108   return _m->Val[row][col];
109}
110
111// subscript operator to get/set individual elements
112float Matrix::operator () (size_t row, size_t col) const 
113{
114   if (row >= _m->Row || col >= _m->Col)
115      printf( "Matrix::operator(): Index out of range!\n");
116   return _m->Val[row][col];
117}
118
119// input stream function
120istream& operator >> (istream& istrm, Matrix& m)
121{
122   for (size_t i=0; i < m.RowNo(); i++)
123      for (size_t j=0; j < m.ColNo(); j++)
124      {
125         float x;
126         istrm >> x;
127         m(i,j) = x;
128      }
129   return istrm;
130}
131
132// output stream function
133ostream& operator << (ostream& ostrm, const Matrix& m)
134{
135   for (size_t i=0; i < m.RowNo(); i++)
136   {
137      for (size_t j=0; j < m.ColNo(); j++)
138      {
139         float x = m(i,j);
140         ostrm << x << '\t';
141      }
142      ostrm << endl;
143   }
144   return ostrm;
145}
146
147
148// logical equal-to operator
149bool operator == (const Matrix& m1, const Matrix& m2) 
150{
151   if (m1.RowNo() != m2.RowNo() || m1.ColNo() != m2.ColNo())
152      return false;
153
154   for (size_t i=0; i < m1.RowNo(); i++)
155      for (size_t j=0; j < m1.ColNo(); j++)
156              if (m1(i,j) != m2(i,j))
157                 return false;
158
159   return true;
160}
161
162// logical no-equal-to operator
163bool operator != (const Matrix& m1, const Matrix& m2) 
164{
165    return (m1 == m2) ? false : true;
166}
167
168// combined addition and assignment operator
169Matrix& Matrix::operator += (const Matrix& m) 
170{
171   if (_m->Row != m._m->Row || _m->Col != m._m->Col)
172     printf("Matrix::operator+= : Inconsistent Matrix sizes in addition!\n");
173   if (_m->Refcnt > 1) clone();
174   for (size_t i=0; i < m._m->Row; i++)
175      for (size_t j=0; j < m._m->Col; j++)
176         _m->Val[i][j] += m._m->Val[i][j];
177   return *this;
178}
179
180// combined subtraction and assignment operator
181Matrix& Matrix::operator -= (const Matrix& m) 
182{
183   if (_m->Row != m._m->Row || _m->Col != m._m->Col)
184      printf( "Matrix::operator-= : Inconsistent Matrix sizes in subtraction!\n");
185   if (_m->Refcnt > 1) clone();
186   for (size_t i=0; i < m._m->Row; i++)
187      for (size_t j=0; j < m._m->Col; j++)
188         _m->Val[i][j] -= m._m->Val[i][j];
189   return *this;
190}
191
192// combined scalar multiplication and assignment operator
193 Matrix&
194Matrix::operator *= (const float& c) 
195{
196    if (_m->Refcnt > 1) clone();
197    for (size_t i=0; i < _m->Row; i++)
198        for (size_t j=0; j < _m->Col; j++)
199            _m->Val[i][j] *= c;
200    return *this;
201}
202
203// combined Matrix multiplication and assignment operator
204 Matrix&
205Matrix::operator *= (const Matrix& m) 
206{
207   if (_m->Col != m._m->Row)
208      printf( "Matrix::operator*= : Inconsistent Matrix sizes in multiplication!\n");
209
210   Matrix temp(_m->Row,m._m->Col);
211
212   for (size_t i=0; i < _m->Row; i++)
213      for (size_t j=0; j < m._m->Col; j++)
214      {
215         temp._m->Val[i][j] = float(0);
216         for (size_t k=0; k < _m->Col; k++)
217            temp._m->Val[i][j] += _m->Val[i][k] * m._m->Val[k][j];
218      }
219   *this = temp;
220
221   return *this;
222}
223
224// combined scalar division and assignment operator
225 Matrix&
226Matrix::operator /= (const float& c) 
227{
228    if (_m->Refcnt > 1) clone();
229    for (size_t i=0; i < _m->Row; i++)
230        for (size_t j=0; j < _m->Col; j++)
231            _m->Val[i][j] /= c;
232
233    return *this;
234}
235
236// combined power and assignment operator
237 Matrix&
238Matrix::operator ^= (const size_t& pow) 
239{
240  Matrix temp(*this);
241
242  for (size_t i=2; i <= pow; i++)
243    *this *=  temp; // changed from *this = *this * temp;
244
245  return *this;
246}
247
248// unary negation operator
249 Matrix
250Matrix::operator - () 
251{
252   Matrix temp(_m->Row,_m->Col);
253
254   for (size_t i=0; i < _m->Row; i++)
255      for (size_t j=0; j < _m->Col; j++)
256         temp._m->Val[i][j] = - _m->Val[i][j];
257
258   return temp;
259}
260
261// binary addition operator
262 Matrix
263operator + (const Matrix& m1, const Matrix& m2) 
264{
265   Matrix temp = m1;
266   temp += m2;
267   return temp;
268}
269
270// binary subtraction operator
271 Matrix
272operator - (const Matrix& m1, const Matrix& m2) 
273{
274   Matrix temp = m1;
275   temp -= m2;
276   return temp;
277}
278
279// binary scalar multiplication operator
280 Matrix
281operator * (const Matrix& m, const float& no) 
282{
283   Matrix temp = m;
284   temp *= no;
285   return temp;
286}
287
288
289// binary scalar multiplication operator
290 Matrix
291operator * (const float& no, const Matrix& m) 
292{
293   return (m * no);
294}
295
296// binary Matrix multiplication operator
297 Matrix
298operator * (const Matrix& m1, const Matrix& m2) 
299{
300   Matrix temp = m1;
301   temp *= m2;
302   return temp;
303}
304
305// binary scalar division operator
306 Matrix
307operator / (const Matrix& m, const float& no) 
308{
309    return (m * (float(1) / no));
310}
311
312
313// binary scalar division operator
314 Matrix
315operator / (const float& no, const Matrix& m) 
316{
317    return (!m * no);
318}
319
320// binary Matrix division operator
321 Matrix
322operator / (const Matrix& m1, const Matrix& m2) 
323{
324    return (m1 * !m2);
325}
326
327// binary power operator
328 Matrix
329operator ^ (const Matrix& m, const size_t& pow) 
330{
331   Matrix temp = m;
332   temp ^= pow;
333   return temp;
334}
335
336// unary transpose operator
337 Matrix
338operator ~ (const Matrix& m) 
339{
340   Matrix temp(m.ColNo(),m.RowNo());
341
342   for (size_t i=0; i < m.RowNo(); i++)
343      for (size_t j=0; j < m.ColNo(); j++)
344      {
345         float x = m(i,j);
346              temp(j,i) = x;
347      }
348   return temp;
349}
350
351// unary inversion operator
352 Matrix
353operator ! (const Matrix m) 
354{
355   Matrix temp = m;
356   return temp.Inv();
357}
358
359// inversion function
360 Matrix
361Matrix::Inv () 
362{
363   size_t i,j,k;
364   float a1,a2,*rowptr;
365
366   if (_m->Row != _m->Col)
367      printf( "Matrix::operator!: Inversion of a non-square Matrix\n");
368
369   Matrix temp(_m->Row,_m->Col);
370   if (_m->Refcnt > 1) clone();
371
372
373   temp.Unit();
374   for (k=0; k < _m->Row; k++)
375   {
376      int indx = pivot(k);
377      if (indx == -1)
378              printf( "Matrix::operator!: Inversion of a singular Matrix\n");
379
380      if (indx != 0)
381      {
382              rowptr = temp._m->Val[k];
383              temp._m->Val[k] = temp._m->Val[indx];
384              temp._m->Val[indx] = rowptr;
385      }
386      a1 = _m->Val[k][k];
387      for (j=0; j < _m->Row; j++)
388      {
389              _m->Val[k][j] /= a1;
390              temp._m->Val[k][j] /= a1;
391      }
392      for (i=0; i < _m->Row; i++)
393           if (i != k)
394           {
395              a2 = _m->Val[i][k];
396              for (j=0; j < _m->Row; j++)
397              {
398                 _m->Val[i][j] -= a2 * _m->Val[k][j];
399                 temp._m->Val[i][j] -= a2 * temp._m->Val[k][j];
400              }
401           }
402   }
403   return temp;
404}
405
406// solve simultaneous equation
407 Matrix
408Matrix::Solve (const Matrix& v) const 
409{
410   size_t i,j,k;
411   float a1;
412
413   if (!(_m->Row == _m->Col && _m->Col == v._m->Row))
414      printf( "Matrix::Solve():Inconsistent matrices!\n");
415
416   Matrix temp(_m->Row,_m->Col+v._m->Col);
417   for (i=0; i < _m->Row; i++)
418   {
419      for (j=0; j < _m->Col; j++)
420         temp._m->Val[i][j] = _m->Val[i][j];
421      for (k=0; k < v._m->Col; k++)
422         temp._m->Val[i][_m->Col+k] = v._m->Val[i][k];
423   }
424   for (k=0; k < _m->Row; k++)
425   {
426      int indx = temp.pivot(k);
427      if (indx == -1)
428         printf( "Matrix::Solve(): Singular Matrix!\n");
429
430      a1 = temp._m->Val[k][k];
431      for (j=k; j < temp._m->Col; j++)
432         temp._m->Val[k][j] /= a1;
433
434      for (i=k+1; i < _m->Row; i++)
435      {
436         a1 = temp._m->Val[i][k];
437         for (j=k; j < temp._m->Col; j++)
438           temp._m->Val[i][j] -= a1 * temp._m->Val[k][j];
439      }
440   }
441   Matrix s(v._m->Row,v._m->Col);
442   for (k=0; k < v._m->Col; k++)
443      for (int m=int(_m->Row)-1; m >= 0; m--)
444      {
445         s._m->Val[m][k] = temp._m->Val[m][_m->Col+k];
446         for (j=m+1; j < _m->Col; j++)
447            s._m->Val[m][k] -= temp._m->Val[m][j] * s._m->Val[j][k];
448      }
449   return s;
450}
451
452// set zero to all elements of this Matrix
453 void
454Matrix::Null (const size_t& row, const size_t& col) 
455{
456    if (row != _m->Row || col != _m->Col)
457        realloc( row,col);
458
459    if (_m->Refcnt > 1) 
460        clone();
461
462    for (size_t i=0; i < _m->Row; i++)
463        for (size_t j=0; j < _m->Col; j++)
464            _m->Val[i][j] = float(0);
465    return;
466}
467
468// set zero to all elements of this Matrix
469 void
470Matrix::Null() 
471{
472    if (_m->Refcnt > 1) clone();   
473    for (size_t i=0; i < _m->Row; i++)
474        for (size_t j=0; j < _m->Col; j++)
475                _m->Val[i][j] = float(0);
476    return;
477}
478
479// set this Matrix to unity
480 void
481Matrix::Unit (const size_t& row) 
482{
483    if (row != _m->Row || row != _m->Col)
484        realloc( row, row);
485       
486    if (_m->Refcnt > 1) 
487        clone();
488
489    for (size_t i=0; i < _m->Row; i++)
490        for (size_t j=0; j < _m->Col; j++)
491            _m->Val[i][j] = i == j ? float(1) : float(0);
492    return;
493}
494
495// set this Matrix to unity
496 void
497Matrix::Unit () 
498{
499    if (_m->Refcnt > 1) clone();   
500    size_t row = min(_m->Row,_m->Col);
501    _m->Row = _m->Col = row;
502
503    for (size_t i=0; i < _m->Row; i++)
504        for (size_t j=0; j < _m->Col; j++)
505            _m->Val[i][j] = i == j ? float(1) : float(0);
506    return;
507}
508
509// private partial pivoting method
510 int
511Matrix::pivot (size_t row)
512{
513  int k = int(row);
514  double amax,temp;
515
516  amax = -1;
517  for (size_t i=row; i < _m->Row; i++)
518    if ( (temp = abs( _m->Val[i][row])) > amax && temp != 0.0)
519     {
520       amax = temp;
521       k = i;
522     }
523  if (_m->Val[k][row] == float(0))
524     return -1;
525  if (k != int(row))
526  {
527     float* rowptr = _m->Val[k];
528     _m->Val[k] = _m->Val[row];
529     _m->Val[row] = rowptr;
530     return k;
531  }
532  return 0;
533}
534
535// calculate the determinant of a Matrix
536 float
537Matrix::Det () const 
538{
539   size_t i,j,k;
540   float piv,detVal = float(1);
541
542   if (_m->Row != _m->Col)
543      printf( "Matrix::Det(): Determinant a non-squareMatrix!\n");
544   
545   Matrix temp(*this);
546   if (temp._m->Refcnt > 1) temp.clone();
547
548   for (k=0; k < _m->Row; k++)
549   {
550      int indx = temp.pivot(k);
551      if (indx == -1)
552         return 0;
553      if (indx != 0)
554         detVal = - detVal;
555      detVal = detVal * temp._m->Val[k][k];
556      for (i=k+1; i < _m->Row; i++)
557      {
558         piv = temp._m->Val[i][k] / temp._m->Val[k][k];
559         for (j=k+1; j < _m->Row; j++)
560            temp._m->Val[i][j] -= piv * temp._m->Val[k][j];
561      }
562   }
563   return detVal;
564}
565
566// calculate the norm of a Matrix
567 float
568Matrix::Norm () 
569{
570   float retVal = float(0);
571
572   for (size_t i=0; i < _m->Row; i++)
573      for (size_t j=0; j < _m->Col; j++)
574         retVal += _m->Val[i][j] * _m->Val[i][j];
575   retVal = sqrt( retVal);
576
577   return retVal;
578}
579
580// calculate the condition number of a Matrix
581 float
582Matrix::Cond () 
583{
584   Matrix inv = ! (*this);
585   return (Norm() * inv.Norm());
586}
587
588// calculate the cofactor of a Matrix for a given element
589 float
590Matrix::Cofact (size_t row, size_t col) 
591{
592   size_t i,i1,j,j1;
593
594   if (_m->Row != _m->Col)
595      printf( "Matrix::Cofact(): Cofactor of a non-square Matrix!\n");
596
597   if (row > _m->Row || col > _m->Col)
598      printf( "Matrix::Cofact(): Index out of range!\n");
599
600   Matrix temp (_m->Row-1,_m->Col-1);
601
602   for (i=i1=0; i < _m->Row; i++)
603   {
604      if (i == row)
605        continue;
606      for (j=j1=0; j < _m->Col; j++)
607      {
608         if (j == col)
609            continue;
610         temp._m->Val[i1][j1] = _m->Val[i][j];
611         j1++;
612      }
613      i1++;
614   }
615   float  cof = temp.Det();
616   if ((row+col)%2 == 1)
617      cof = -cof;
618
619   return cof;
620}
621
622
623// calculate adjoin of a Matrix
624 Matrix
625Matrix::Adj () 
626{
627   if (_m->Row != _m->Col)
628      printf( "Matrix::Adj(): Adjoin of a non-square Matrix.\n");
629
630   Matrix temp(_m->Row,_m->Col);
631
632   for (size_t i=0; i < _m->Row; i++)
633      for (size_t j=0; j < _m->Col; j++)
634          temp._m->Val[j][i] = Cofact(i,j);
635   return temp;
636}
637
638// Determine if the Matrix is singular
639 bool
640Matrix::IsSingular () 
641{
642   if (_m->Row != _m->Col)
643      return false;
644   return (Det() == float(0));
645}
646
647// Determine if the Matrix is diagonal
648 bool
649Matrix::IsDiagonal () 
650{
651   if (_m->Row != _m->Col)
652      return false;
653   for (size_t i=0; i < _m->Row; i++)
654     for (size_t j=0; j < _m->Col; j++)
655        if (i != j && _m->Val[i][j] != float(0))
656          return false;
657   return true;
658}
659
660// Determine if the Matrix is scalar
661 bool
662Matrix::IsScalar () 
663{
664   if (!IsDiagonal())
665     return false;
666   float v = _m->Val[0][0];
667   for (size_t i=1; i < _m->Row; i++)
668     if (_m->Val[i][i] != v)
669        return false;
670   return true;
671}
672
673// Determine if the Matrix is a unit Matrix
674 bool
675Matrix::IsUnit () 
676{
677   if (IsScalar() && _m->Val[0][0] == float(1))
678     return true;
679   return false;
680}
681
682// Determine if this is a null Matrix
683 bool
684Matrix::IsNull () 
685{
686   for (size_t i=0; i < _m->Row; i++)
687      for (size_t j=0; j < _m->Col; j++)
688         if (_m->Val[i][j] != float(0))
689            return false;
690   return true;
691}
692
693// Determine if the Matrix is symmetric
694 bool
695Matrix::IsSymmetric () 
696{
697   if (_m->Row != _m->Col)
698      return false;
699   for (size_t i=0; i < _m->Row; i++)
700      for (size_t j=0; j < _m->Col; j++)
701         if (_m->Val[i][j] != _m->Val[j][i])
702            return false;
703   return true;
704}
705           
706// Determine if the Matrix is skew-symmetric
707 bool
708Matrix::IsSkewSymmetric () 
709{
710   if (_m->Row != _m->Col)
711      return false;
712   for (size_t i=0; i < _m->Row; i++)
713      for (size_t j=0; j < _m->Col; j++)
714         if (_m->Val[i][j] != -_m->Val[j][i])
715            return false;
716   return true;
717}
718   
719// Determine if the Matrix is upper triangular
720 bool
721Matrix::IsUpperTriangular () 
722{
723   if (_m->Row != _m->Col)
724      return false;
725   for (size_t i=1; i < _m->Row; i++)
726      for (size_t j=0; j < i-1; j++)
727         if (_m->Val[i][j] != float(0))
728            return false;
729   return true;
730}
731
732// Determine if the Matrix is lower triangular
733 bool
734Matrix::IsLowerTriangular () 
735{
736   if (_m->Row != _m->Col)
737      return false;
738
739   for (size_t j=1; j < _m->Col; j++)
740      for (size_t i=0; i < j-1; i++)
741         if (_m->Val[i][j] != float(0))
742            return false;
743
744   return true;
745}
746
Note: See TracBrowser for help on using the repository browser.