Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/lib/newmat/newmat5.cpp @ 4607

Last change on this file since 4607 was 4565, checked in by patrick, 19 years ago

orxonox/trunk: added the newmat library to the project. needs some translation in directory, temp under util/newmat. is needed by the collision detection engine to perform lin alg operations such as eigenvector decomposition. perhaps we will make our own library to do that later.

File size: 14.2 KB
Line 
1//$$ newmat5.cpp         Transpose, evaluate etc
2
3// Copyright (C) 1991,2,3,4: R B Davies
4
5//#define WANT_STREAM
6
7#include "include.h"
8
9#include "newmat.h"
10#include "newmatrc.h"
11
12#ifdef use_namespace
13namespace NEWMAT {
14#endif
15
16
17#ifdef DO_REPORT
18#define REPORT { static ExeCounter ExeCount(__LINE__,5); ++ExeCount; }
19#else
20#define REPORT {}
21#endif
22
23
24/************************ carry out operations ******************************/
25
26
27GeneralMatrix* GeneralMatrix::Transpose(TransposedMatrix* tm, MatrixType mt)
28{
29   GeneralMatrix* gm1;
30
31   if (Compare(Type().t(),mt))
32   {
33      REPORT
34      gm1 = mt.New(ncols,nrows,tm);
35      for (int i=0; i<ncols; i++)
36      {
37         MatrixRow mr(gm1, StoreOnExit+DirectPart, i);
38         MatrixCol mc(this, mr.Data(), LoadOnEntry, i);
39      }
40   }
41   else
42   {
43      REPORT
44      gm1 = mt.New(ncols,nrows,tm);
45      MatrixRow mr(this, LoadOnEntry);
46      MatrixCol mc(gm1, StoreOnExit+DirectPart);
47      int i = nrows;
48      while (i--) { mc.Copy(mr); mr.Next(); mc.Next(); }
49   }
50   tDelete(); gm1->ReleaseAndDelete(); return gm1;
51}
52
53GeneralMatrix* SymmetricMatrix::Transpose(TransposedMatrix*, MatrixType mt)
54{ REPORT  return Evaluate(mt); }
55
56
57GeneralMatrix* DiagonalMatrix::Transpose(TransposedMatrix*, MatrixType mt)
58{ REPORT return Evaluate(mt); }
59
60GeneralMatrix* ColumnVector::Transpose(TransposedMatrix*, MatrixType mt)
61{
62   REPORT
63   GeneralMatrix* gmx = new RowVector; MatrixErrorNoSpace(gmx);
64   gmx->nrows = 1; gmx->ncols = gmx->storage = storage;
65   return BorrowStore(gmx,mt);
66}
67
68GeneralMatrix* RowVector::Transpose(TransposedMatrix*, MatrixType mt)
69{
70   REPORT
71   GeneralMatrix* gmx = new ColumnVector; MatrixErrorNoSpace(gmx);
72   gmx->ncols = 1; gmx->nrows = gmx->storage = storage;
73   return BorrowStore(gmx,mt);
74}
75
76GeneralMatrix* IdentityMatrix::Transpose(TransposedMatrix*, MatrixType mt)
77{ REPORT return Evaluate(mt); }
78
79GeneralMatrix* GeneralMatrix::Evaluate(MatrixType mt)
80{
81   if (Compare(this->Type(),mt)) { REPORT return this; }
82   REPORT
83   GeneralMatrix* gmx = mt.New(nrows,ncols,this);
84   MatrixRow mr(this, LoadOnEntry);
85   MatrixRow mrx(gmx, StoreOnExit+DirectPart);
86   int i=nrows;
87   while (i--) { mrx.Copy(mr); mrx.Next(); mr.Next(); }
88   tDelete(); gmx->ReleaseAndDelete(); return gmx;
89}
90
91GeneralMatrix* GenericMatrix::Evaluate(MatrixType mt)
92   { REPORT  return gm->Evaluate(mt); }
93
94GeneralMatrix* ShiftedMatrix::Evaluate(MatrixType mt)
95{
96   gm=((BaseMatrix*&)bm)->Evaluate();
97   int nr=gm->Nrows(); int nc=gm->Ncols();
98   Compare(gm->Type().AddEqualEl(),mt);
99   if (!(mt==gm->Type()))
100   {
101      REPORT
102      GeneralMatrix* gmx = mt.New(nr,nc,this);
103      MatrixRow mr(gm, LoadOnEntry);
104      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
105      while (nr--) { mrx.Add(mr,f); mrx.Next(); mr.Next(); }
106      gmx->ReleaseAndDelete(); gm->tDelete();
107#ifdef TEMPS_DESTROYED_QUICKLY
108      delete this;
109#endif
110      return gmx;
111   }
112   else if (gm->reuse())
113   {
114      REPORT gm->Add(f);
115#ifdef TEMPS_DESTROYED_QUICKLY
116      GeneralMatrix* gmx = gm; delete this; return gmx;
117#else
118      return gm;
119#endif
120   }
121   else
122   {
123      REPORT GeneralMatrix* gmy = gm->Type().New(nr,nc,this);
124      gmy->ReleaseAndDelete(); gmy->Add(gm,f);
125#ifdef TEMPS_DESTROYED_QUICKLY
126      delete this;
127#endif
128      return gmy;
129   }
130}
131
132GeneralMatrix* NegShiftedMatrix::Evaluate(MatrixType mt)
133{
134   gm=((BaseMatrix*&)bm)->Evaluate();
135   int nr=gm->Nrows(); int nc=gm->Ncols();
136   Compare(gm->Type().AddEqualEl(),mt);
137   if (!(mt==gm->Type()))
138   {
139      REPORT
140      GeneralMatrix* gmx = mt.New(nr,nc,this);
141      MatrixRow mr(gm, LoadOnEntry);
142      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
143      while (nr--) { mrx.NegAdd(mr,f); mrx.Next(); mr.Next(); }
144      gmx->ReleaseAndDelete(); gm->tDelete();
145#ifdef TEMPS_DESTROYED_QUICKLY
146      delete this;
147#endif
148      return gmx;
149   }
150   else if (gm->reuse())
151   {
152      REPORT gm->NegAdd(f);
153#ifdef TEMPS_DESTROYED_QUICKLY
154      GeneralMatrix* gmx = gm; delete this; return gmx;
155#else
156      return gm;
157#endif
158   }
159   else
160   {
161      REPORT GeneralMatrix* gmy = gm->Type().New(nr,nc,this);
162      gmy->ReleaseAndDelete(); gmy->NegAdd(gm,f);
163#ifdef TEMPS_DESTROYED_QUICKLY
164      delete this;
165#endif
166      return gmy;
167   }
168}
169
170GeneralMatrix* ScaledMatrix::Evaluate(MatrixType mt)
171{
172   gm=((BaseMatrix*&)bm)->Evaluate();
173   int nr=gm->Nrows(); int nc=gm->Ncols();
174   if (Compare(gm->Type(),mt))
175   {
176      if (gm->reuse())
177      {
178         REPORT gm->Multiply(f);
179#ifdef TEMPS_DESTROYED_QUICKLY
180         GeneralMatrix* gmx = gm; delete this; return gmx;
181#else
182         return gm;
183#endif
184      }
185      else
186      {
187         REPORT GeneralMatrix* gmx = gm->Type().New(nr,nc,this);
188         gmx->ReleaseAndDelete(); gmx->Multiply(gm,f);
189#ifdef TEMPS_DESTROYED_QUICKLY
190         delete this;
191#endif
192         return gmx;
193      }
194   }
195   else
196   {
197      REPORT
198      GeneralMatrix* gmx = mt.New(nr,nc,this);
199      MatrixRow mr(gm, LoadOnEntry);
200      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
201      while (nr--) { mrx.Multiply(mr,f); mrx.Next(); mr.Next(); }
202      gmx->ReleaseAndDelete(); gm->tDelete();
203#ifdef TEMPS_DESTROYED_QUICKLY
204      delete this;
205#endif
206      return gmx;
207   }
208}
209
210GeneralMatrix* NegatedMatrix::Evaluate(MatrixType mt)
211{
212   gm=((BaseMatrix*&)bm)->Evaluate();
213   int nr=gm->Nrows(); int nc=gm->Ncols();
214   if (Compare(gm->Type(),mt))
215   {
216      if (gm->reuse())
217      {
218         REPORT gm->Negate();
219#ifdef TEMPS_DESTROYED_QUICKLY
220         GeneralMatrix* gmx = gm; delete this; return gmx;
221#else
222         return gm;
223#endif
224      }
225      else
226      {
227         REPORT
228         GeneralMatrix* gmx = gm->Type().New(nr,nc,this);
229         gmx->ReleaseAndDelete(); gmx->Negate(gm);
230#ifdef TEMPS_DESTROYED_QUICKLY
231         delete this;
232#endif
233         return gmx;
234      }
235   }
236   else
237   {
238      REPORT
239      GeneralMatrix* gmx = mt.New(nr,nc,this);
240      MatrixRow mr(gm, LoadOnEntry);
241      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
242      while (nr--) { mrx.Negate(mr); mrx.Next(); mr.Next(); }
243      gmx->ReleaseAndDelete(); gm->tDelete();
244#ifdef TEMPS_DESTROYED_QUICKLY
245      delete this;
246#endif
247      return gmx;
248   }
249}
250
251GeneralMatrix* ReversedMatrix::Evaluate(MatrixType mt)
252{
253   gm=((BaseMatrix*&)bm)->Evaluate(); GeneralMatrix* gmx;
254
255   if ((gm->Type()).IsBand() && ! (gm->Type()).IsDiagonal())
256   {
257      gm->tDelete();
258#ifdef TEMPS_DESTROYED_QUICKLY
259      delete this;
260#endif
261      Throw(NotDefinedException("Reverse", "band matrices"));
262   }
263
264   if (gm->reuse()) { REPORT gm->ReverseElements(); gmx = gm; }
265   else
266   {
267      REPORT
268      gmx = gm->Type().New(gm->Nrows(), gm->Ncols(), this);
269      gmx->ReverseElements(gm); gmx->ReleaseAndDelete();
270   }
271#ifdef TEMPS_DESTROYED_QUICKLY
272   delete this;
273#endif
274   return gmx->Evaluate(mt); // target matrix is different type?
275
276}
277
278GeneralMatrix* TransposedMatrix::Evaluate(MatrixType mt)
279{
280   REPORT
281   gm=((BaseMatrix*&)bm)->Evaluate();
282   Compare(gm->Type().t(),mt);
283   GeneralMatrix* gmx=gm->Transpose(this, mt);
284#ifdef TEMPS_DESTROYED_QUICKLY
285   delete this;
286#endif
287   return gmx;
288}
289
290GeneralMatrix* RowedMatrix::Evaluate(MatrixType mt)
291{
292   gm = ((BaseMatrix*&)bm)->Evaluate();
293   GeneralMatrix* gmx = new RowVector; MatrixErrorNoSpace(gmx);
294   gmx->nrows = 1; gmx->ncols = gmx->storage = gm->storage;
295#ifdef TEMPS_DESTROYED_QUICKLY
296   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
297#else
298   return gm->BorrowStore(gmx,mt);
299#endif
300}
301
302GeneralMatrix* ColedMatrix::Evaluate(MatrixType mt)
303{
304   gm = ((BaseMatrix*&)bm)->Evaluate();
305   GeneralMatrix* gmx = new ColumnVector; MatrixErrorNoSpace(gmx);
306   gmx->ncols = 1; gmx->nrows = gmx->storage = gm->storage;
307#ifdef TEMPS_DESTROYED_QUICKLY
308   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
309#else
310   return gm->BorrowStore(gmx,mt);
311#endif
312}
313
314GeneralMatrix* DiagedMatrix::Evaluate(MatrixType mt)
315{
316   gm = ((BaseMatrix*&)bm)->Evaluate();
317   GeneralMatrix* gmx = new DiagonalMatrix; MatrixErrorNoSpace(gmx);
318   gmx->nrows = gmx->ncols = gmx->storage = gm->storage;
319#ifdef TEMPS_DESTROYED_QUICKLY
320   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
321#else
322   return gm->BorrowStore(gmx,mt);
323#endif
324}
325
326GeneralMatrix* MatedMatrix::Evaluate(MatrixType mt)
327{
328   Tracer tr("MatedMatrix::Evaluate");
329   gm = ((BaseMatrix*&)bm)->Evaluate();
330   GeneralMatrix* gmx = new Matrix; MatrixErrorNoSpace(gmx);
331   gmx->nrows = nr; gmx->ncols = nc; gmx->storage = gm->storage;
332   if (nr*nc != gmx->storage)
333      Throw(IncompatibleDimensionsException());
334#ifdef TEMPS_DESTROYED_QUICKLY
335   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
336#else
337   return gm->BorrowStore(gmx,mt);
338#endif
339}
340
341GeneralMatrix* GetSubMatrix::Evaluate(MatrixType mt)
342{
343   REPORT
344   Tracer tr("SubMatrix(evaluate)");
345   gm = ((BaseMatrix*&)bm)->Evaluate();
346   if (row_number < 0) row_number = gm->Nrows();
347   if (col_number < 0) col_number = gm->Ncols();
348   if (row_skip+row_number > gm->Nrows() || col_skip+col_number > gm->Ncols())
349   {
350      gm->tDelete();
351#ifdef TEMPS_DESTROYED_QUICKLY
352      delete this;
353#endif
354      Throw(SubMatrixDimensionException());
355   }
356   if (IsSym) Compare(gm->Type().ssub(), mt);
357   else Compare(gm->Type().sub(), mt);
358   GeneralMatrix* gmx = mt.New(row_number, col_number, this);
359   int i = row_number;
360   MatrixRow mr(gm, LoadOnEntry, row_skip); 
361   MatrixRow mrx(gmx, StoreOnExit+DirectPart);
362   MatrixRowCol sub;
363   while (i--)
364   {
365      mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
366      mrx.Copy(sub); mrx.Next(); mr.Next();
367   }
368   gmx->ReleaseAndDelete(); gm->tDelete();
369#ifdef TEMPS_DESTROYED_QUICKLY
370   delete this;
371#endif
372   return gmx;
373}   
374
375
376GeneralMatrix* ReturnMatrixX::Evaluate(MatrixType mt)
377{
378#ifdef TEMPS_DESTROYED_QUICKLY_R
379   GeneralMatrix* gmx = gm; delete this; return gmx->Evaluate(mt);
380#else
381   return gm->Evaluate(mt);
382#endif
383}
384
385
386void GeneralMatrix::Add(GeneralMatrix* gm1, Real f)
387{
388   REPORT
389   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
390   while (i--)
391   { *s++ = *s1++ + f; *s++ = *s1++ + f; *s++ = *s1++ + f; *s++ = *s1++ + f; }
392   i = storage & 3; while (i--) *s++ = *s1++ + f;
393}
394   
395void GeneralMatrix::Add(Real f)
396{
397   REPORT
398   Real* s=store; int i=(storage >> 2);
399   while (i--) { *s++ += f; *s++ += f; *s++ += f; *s++ += f; }
400   i = storage & 3; while (i--) *s++ += f;
401}
402   
403void GeneralMatrix::NegAdd(GeneralMatrix* gm1, Real f)
404{
405   REPORT
406   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
407   while (i--)
408   { *s++ = f - *s1++; *s++ = f - *s1++; *s++ = f - *s1++; *s++ = f - *s1++; }
409   i = storage & 3; while (i--) *s++ = f - *s1++;
410}
411   
412void GeneralMatrix::NegAdd(Real f)
413{
414   REPORT
415   Real* s=store; int i=(storage >> 2);
416   while (i--)
417   {
418      *s = f - *s; s++; *s = f - *s; s++;
419      *s = f - *s; s++; *s = f - *s; s++;
420   }
421   i = storage & 3; while (i--)  { *s = f - *s; s++; }
422}
423   
424void GeneralMatrix::Negate(GeneralMatrix* gm1)
425{
426   // change sign of elements
427   REPORT
428   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
429   while (i--)
430   { *s++ = -(*s1++); *s++ = -(*s1++); *s++ = -(*s1++); *s++ = -(*s1++); }
431   i = storage & 3; while(i--) *s++ = -(*s1++);
432}
433   
434void GeneralMatrix::Negate()
435{
436   REPORT
437   Real* s=store; int i=(storage >> 2);
438   while (i--)
439   { *s = -(*s); s++; *s = -(*s); s++; *s = -(*s); s++; *s = -(*s); s++; }
440   i = storage & 3; while(i--) { *s = -(*s); s++; }
441}
442   
443void GeneralMatrix::Multiply(GeneralMatrix* gm1, Real f)
444{
445   REPORT
446   Real* s1=gm1->store; Real* s=store;  int i=(storage >> 2);
447   while (i--)
448   { *s++ = *s1++ * f; *s++ = *s1++ * f; *s++ = *s1++ * f; *s++ = *s1++ * f; }
449   i = storage & 3; while (i--) *s++ = *s1++ * f;
450}
451   
452void GeneralMatrix::Multiply(Real f)
453{
454   REPORT
455   Real* s=store; int i=(storage >> 2);
456   while (i--) { *s++ *= f; *s++ *= f; *s++ *= f; *s++ *= f; }
457   i = storage & 3; while (i--) *s++ *= f;
458}
459   
460
461/************************ MatrixInput routines ****************************/
462
463// int MatrixInput::n;          // number values still to be read
464// Real* MatrixInput::r;        // pointer to next location to be read to
465
466MatrixInput MatrixInput::operator<<(Real f)
467{
468   REPORT
469   Tracer et("MatrixInput");
470   if (n<=0) Throw(ProgramException("List of values too long"));
471   *r = f; int n1 = n-1; n=0;   // n=0 so we won't trigger exception
472   return MatrixInput(n1, r+1);
473}
474
475
476MatrixInput GeneralMatrix::operator<<(Real f)
477{
478   REPORT
479   Tracer et("MatrixInput");
480   int n = Storage();
481   if (n<=0) Throw(ProgramException("Loading data to zero length matrix"));
482   Real* r; r = Store(); *r = f; n--;
483   return MatrixInput(n, r+1);
484}
485
486MatrixInput GetSubMatrix::operator<<(Real f)
487{
488   REPORT
489   Tracer et("MatrixInput (GetSubMatrix)");
490   SetUpLHS();
491   if (row_number != 1 || col_skip != 0 || col_number != gm->Ncols())
492   {
493#ifdef TEMPS_DESTROYED_QUICKLY
494      delete this;
495#endif
496      Throw(ProgramException("MatrixInput requires complete rows"));
497   }
498   MatrixRow mr(gm, DirectPart, row_skip);  // to pick up location and length
499   int n = mr.Storage();
500   if (n<=0)
501   {
502#ifdef TEMPS_DESTROYED_QUICKLY
503      delete this;
504#endif
505      Throw(ProgramException("Loading data to zero length row"));
506   }
507   Real* r; r = mr.Data(); *r = f; n--;
508   if (+(mr.cw*HaveStore))
509   {
510#ifdef TEMPS_DESTROYED_QUICKLY
511      delete this;
512#endif
513      Throw(ProgramException("Fails with this matrix type"));
514   }
515#ifdef TEMPS_DESTROYED_QUICKLY
516   delete this;
517#endif
518   return MatrixInput(n, r+1);
519}
520
521MatrixInput::~MatrixInput()
522{
523   REPORT
524   Tracer et("MatrixInput");
525   if (n!=0) Throw(ProgramException("A list of values was too short"));
526}
527
528MatrixInput BandMatrix::operator<<(Real)
529{
530   Tracer et("MatrixInput");
531   bool dummy = true;
532   if (dummy)                                   // get rid of warning message
533      Throw(ProgramException("Cannot use list read with a BandMatrix"));
534   return MatrixInput(0, 0);
535}
536
537void BandMatrix::operator<<(const Real*)
538{ Throw(ProgramException("Cannot use array read with a BandMatrix")); }
539
540// ************************* Reverse order of elements ***********************
541
542void GeneralMatrix::ReverseElements(GeneralMatrix* gm)
543{
544   // reversing into a new matrix
545   REPORT
546   int n = Storage(); Real* rx = Store() + n; Real* x = gm->Store();
547   while (n--) *(--rx) = *(x++);
548}
549
550void GeneralMatrix::ReverseElements()
551{
552   // reversing in place
553   REPORT
554   int n = Storage(); Real* x = Store(); Real* rx = x + n;
555   n /= 2;
556   while (n--) { Real t = *(--rx); *rx = *x; *(x++) = t; }
557}
558
559
560#ifdef use_namespace
561}
562#endif
563
Note: See TracBrowser for help on using the repository browser.