Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/oggPlayer/src/lib/newmat/tmt8.cpp @ 4629

Last change on this file since 4629 was 4565, checked in by patrick, 20 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: 8.6 KB
Line 
1
2//#define WANT_STREAM
3
4#include "include.h"
5
6#include "newmatap.h"
7
8#include "tmt.h"
9
10#ifdef use_namespace
11using namespace NEWMAT;
12#endif
13
14
15
16// **************************** test program ******************************
17
18
19void Transposer(const GenericMatrix& GM1, GenericMatrix&GM2)
20   { GM2 = GM1.t(); }
21
22// this is a routine in "Numerical Recipes in C" format
23// if R is a row vector, C a column vector and D diagonal
24// make matrix DCR
25
26static void DCR(Real d[], Real c[], int m, Real r[], int n, Real **dcr)
27{
28   int i, j;
29   for (i = 1; i <= m; i++) for (j = 1; j <= n; j++)
30   dcr[i][j] = d[i] * c[i] * r[j];
31}
32
33ReturnMatrix TestReturn(const GeneralMatrix& gm) { return gm; }
34
35void trymat8()
36{
37//   cout << "\nEighth test of Matrix package\n";
38   Tracer et("Eighth test of Matrix package");
39   Tracer::PrintTrace();
40
41   int i;
42
43
44   DiagonalMatrix D(6);
45   for (i=1;i<=6;i++)  D(i,i)=i*i+i-10;
46   DiagonalMatrix D2=D;
47   Matrix MD=D;
48
49   DiagonalMatrix D1(6); for (i=1;i<=6;i++) D1(i,i)=-100+i*i*i;
50   Matrix MD1=D1;
51   Print(Matrix(D*D1-MD*MD1));
52   Print(Matrix((-D)*D1+MD*MD1));
53   Print(Matrix(D*(-D1)+MD*MD1));
54   DiagonalMatrix DX=D;
55   {
56      Tracer et1("Stage 1");
57      DX=(DX+D1)*DX; Print(Matrix(DX-(MD+MD1)*MD));
58      DX=D;
59      DX=-DX*DX+(DX-(-D1))*((-D1)+DX);
60      // Matrix MX = Matrix(MD1);
61      // MD1=DX+(MX.t())*(MX.t()); Print(MD1);
62      MD1=DX+(Matrix(MD1).t())*(Matrix(MD1).t()); Print(MD1);
63      DX=D; DX=DX; DX=D2-DX; Print(DiagonalMatrix(DX));
64      DX=D;
65   }
66   {
67      Tracer et1("Stage 2");
68      D.Release(2);
69      D1=D; D2=D;
70      Print(DiagonalMatrix(D1-DX));
71      Print(DiagonalMatrix(D2-DX));
72      MD1=1.0;
73      Print(Matrix(MD1-1.0));
74   }
75   {
76      Tracer et1("Stage 3");
77      //GenericMatrix
78      LowerTriangularMatrix LT(4);
79      LT << 1 << 2 << 3 << 4 << 5 << 6  << 7 << 8 << 9 << 10;
80      UpperTriangularMatrix UT = LT.t() * 2.0;
81      GenericMatrix GM1 = LT;
82      LowerTriangularMatrix LT1 = GM1-LT; Print(LT1);
83      GenericMatrix GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
84      GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
85      GM2 = GM1*2; LT1 = GM2; LT1 = LT1-LT*2; Print(LT1);
86      GM1.Release();
87      GM1=GM1; LT1=GM1-LT; Print(LT1); LT1=GM1-LT; Print(LT1);
88      GM1.Release();
89      GM1=GM1*4; LT1=GM1-LT*4; Print(LT1);
90      LT1=GM1-LT*4; Print(LT1); GM1.CleanUp();
91      GM1=LT; GM2=UT; GM1=GM1*GM2; Matrix M=GM1; M=M-LT*UT; Print(M);
92      Transposer(LT,GM2); LT1 = LT - GM2.t(); Print(LT1);
93      GM1=LT; Transposer(GM1,GM2); LT1 = LT - GM2.t(); Print(LT1);
94      GM1 = LT; GM1 = GM1 + GM1; LT1 = LT*2-GM1; Print(LT1);
95      DiagonalMatrix D; D << LT; GM1 = D; LT1 = GM1; LT1 -= D; Print(LT1);
96      UpperTriangularMatrix UT1 = GM1; UT1 -= D; Print(UT1);
97   }
98   {
99      Tracer et1("Stage 4");
100      // Another test of SVD
101      Matrix M(12,12); M = 0;
102      M(1,1) = M(2,2) = M(4,4) = M(6,6) =
103         M(7,7) = M(8,8) = M(10,10) = M(12,12) = -1;
104      M(1,6) = M(1,12) = -5.601594;
105      M(3,6) = M(3,12) = -0.000165;
106      M(7,6) = M(7,12) = -0.008294;
107      DiagonalMatrix D;
108      SVD(M,D);
109      SortDescending(D);
110      // answer given by matlab
111      DiagonalMatrix DX(12);
112      DX(1) = 8.0461;
113      DX(2) = DX(3) = DX(4) = DX(5) = DX(6) = DX(7) = 1;
114      DX(8) = 0.1243;
115      DX(9) = DX(10) = DX(11) = DX(12) = 0;
116      D -= DX; Clean(D,0.0001); Print(D);
117   }
118#ifndef DONT_DO_NRIC
119   {
120      Tracer et1("Stage 5");
121      // test numerical recipes in C interface
122      DiagonalMatrix D(10);
123      D << 1 << 4 << 6 << 2 << 1 << 6 << 4 << 7 << 3 << 1;
124      ColumnVector C(10);
125      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
126      RowVector R(6);
127      R << 2 << 3 << 5 << 7 << 11 << 13;
128      nricMatrix M(10, 6);
129      DCR( D.nric(), C.nric(), 10, R.nric(), 6, M.nric() );
130      M -= D * C * R;  Print(M);
131
132      D.ReSize(5);
133      D << 1.25 << 4.75 << 9.5 << 1.25 << 3.75;
134      C.ReSize(5);
135      C << 1.5 << 7.5 << 4.25 << 0.0 << 7.25;
136      R.ReSize(9);
137      R << 2.5 << 3.25 << 5.5 << 7 << 11.25 << 13.5 << 0.0 << 1.5 << 3.5;
138      Matrix MX = D * C * R;
139      M.ReSize(MX);
140      DCR( D.nric(), C.nric(), 5, R.nric(), 9, M.nric() );
141      M -= MX;  Print(M);
142   }
143#endif
144   {
145      Tracer et1("Stage 6");
146      // test dotproduct
147      DiagonalMatrix test(5); test = 1;
148      ColumnVector C(10);
149      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
150      RowVector R(10);
151      R << 2 << 3 << 5 << 7 << 11 << 13 << -3 << -4 << 2 << 4;
152      test(1) = (R * C).AsScalar() - DotProduct(C, R);
153      test(2) = C.SumSquare() - DotProduct(C, C);
154      test(3) = 6.0 * (C.t() * R.t()).AsScalar() - DotProduct(2.0 * C, 3.0 * R);
155      Matrix MC = C.AsMatrix(2,5), MR = R.AsMatrix(5,2);
156      test(4) = DotProduct(MC, MR) - (R * C).AsScalar();
157      UpperTriangularMatrix UT(5);
158      UT << 3 << 5 << 2 << 1 << 7
159              << 1 << 1 << 8 << 2
160                   << 7 << 0 << 1
161                        << 3 << 5
162                             << 6;
163      LowerTriangularMatrix LT(5);
164      LT << 5
165         << 2 << 3
166         << 1 << 0 << 7
167         << 9 << 8 << 1 << 2
168         << 0 << 2 << 1 << 9 << 2;
169      test(5) = DotProduct(UT, LT) - Sum(SP(UT, LT));
170      Print(test);
171      // check row-wise load;
172      LowerTriangularMatrix LT1(5);
173      LT1.Row(1) << 5;
174      LT1.Row(2) << 2   << 3;
175      LT1.Row(3) << 1   << 0   << 7;
176      LT1.Row(4) << 9   << 8   << 1   << 2;
177      LT1.Row(5) << 0   << 2   << 1   << 9   << 2;
178      Matrix M = LT1 - LT; Print(M);
179      // check solution with identity matrix
180      IdentityMatrix IM(5); IM *= 2;
181      LinearEquationSolver LES1(IM);
182      LowerTriangularMatrix LTX = LES1.i() * LT;
183      M = LTX * 2 - LT; Print(M);
184      DiagonalMatrix D = IM;
185      LinearEquationSolver LES2(IM);
186      LTX = LES2.i() * LT;
187      M = LTX * 2 - LT; Print(M);
188      UpperTriangularMatrix UTX = LES1.i() * UT;
189      M = UTX * 2 - UT; Print(M);
190      UTX = LES2.i() * UT;
191      M = UTX * 2 - UT; Print(M);
192   }
193
194   {
195      Tracer et1("Stage 7");
196      // Some more GenericMatrix stuff with *= |= &=
197      // but don't any additional checks
198      BandMatrix BM1(6,2,3);
199      BM1.Row(1) << 3 << 8 << 4 << 1;
200      BM1.Row(2) << 5 << 1 << 9 << 7 << 2;
201      BM1.Row(3) << 1 << 0 << 6 << 3 << 1 << 3;
202      BM1.Row(4)      << 4 << 2 << 5 << 2 << 4;
203      BM1.Row(5)           << 3 << 3 << 9 << 1;
204      BM1.Row(6)                << 4 << 2 << 9;
205      BandMatrix BM2(6,1,1);
206      BM2.Row(1) << 2.5 << 7.5;
207      BM2.Row(2) << 1.5 << 3.0 << 8.5;
208      BM2.Row(3)        << 6.0 << 6.5 << 7.0;
209      BM2.Row(4)               << 2.5 << 2.0 << 8.0;
210      BM2.Row(5)                      << 0.5 << 4.5 << 3.5;
211      BM2.Row(6)                             << 9.5 << 7.5;
212      Matrix RM1 = BM1, RM2 = BM2;
213      Matrix X;
214      GenericMatrix GRM1 = RM1, GBM1 = BM1, GRM2 = RM2, GBM2 = BM2;
215      Matrix Z(6,0); Z = 5; Print(Z);
216      GRM1 |= Z; GBM1 |= Z; GRM2 &= Z.t(); GBM2 &= Z.t();
217      X = GRM1 - BM1; Print(X); X = GBM1 - BM1; Print(X);
218      X = GRM2 - BM2; Print(X); X = GBM2 - BM2; Print(X);
219
220      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
221      GRM1 *= GRM2; GBM1 *= GBM2;
222      X = GRM1 - BM1 * BM2; Print(X);
223      X = RM1 * RM2 - GBM1; Print(X);
224
225      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
226      GRM1 *= GBM2; GBM1 *= GRM2;          // Bs and Rs swapped on LHS
227      X = GRM1 - BM1 * BM2; Print(X);
228      X = RM1 * RM2 - GBM1; Print(X);
229
230      X = BM1.t(); BandMatrix BM1X = BM1.t();
231      GRM1 = RM1; X -= GRM1.t(); Print(X); X = BM1X - BM1.t(); Print(X);
232
233      // check that linear equation solver works with Identity Matrix
234      IdentityMatrix IM(6); IM *= 2;
235      GBM1 = BM1; GBM1 *= 4; GRM1 = RM1; GRM1 *= 4;
236      DiagonalMatrix D = IM;
237      LinearEquationSolver LES1(D);
238      BandMatrix BX;
239      BX = LES1.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
240      LinearEquationSolver LES2(IM);
241      BX = LES2.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
242      BX = D.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
243      BX = IM.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
244      BX = IM.i(); BX *= GBM1; BX -= BM1 * 2; X = BX; Print(X);
245
246      // try symmetric band matrices
247      SymmetricBandMatrix SBM; SBM << SP(BM1, BM1.t());
248      SBM << IM.i() * SBM;
249      X = 2 * SBM - SP(RM1, RM1.t()); Print(X);
250
251      // Do this again with more general D
252      D << 2.5 << 7.5 << 2 << 5 << 4.5 << 7.5;
253      BX = D.i() * BM1; X = BX - D.i() * RM1;
254      Clean(X,0.00000001); Print(X);
255      BX = D.i(); BX *= BM1; X = BX - D.i() * RM1;
256      Clean(X,0.00000001); Print(X);
257      SBM << SP(BM1, BM1.t());
258      BX = D.i() * SBM; X = BX - D.i() * SP(RM1, RM1.t());
259      Clean(X,0.00000001); Print(X);
260
261      // test return
262      BX = TestReturn(BM1); X = BX - BM1;
263      if (BX.BandWidth() != BM1.BandWidth()) X = 5;
264      Print(X);
265   }
266
267//   cout << "\nEnd of eighth test\n";
268}
Note: See TracBrowser for help on using the repository browser.