Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/lib/newmat/tmtd.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: 5.7 KB
Line 
1
2//#define WANT_STREAM
3
4#include "include.h"
5#include "newmatap.h"
6
7#include "tmt.h"
8
9#ifdef use_namespace
10using namespace NEWMAT;
11#endif
12
13ReturnMatrix Inverter(const CroutMatrix& X)
14{
15   Matrix Y = X.i();
16   Y.Release();
17   return Y.ForReturn();
18}
19
20
21void trymatd()
22{
23   Tracer et("Thirteenth test of Matrix package");
24   Tracer::PrintTrace();
25   Matrix X(5,20);
26   int i,j;
27   for (j=1;j<=20;j++) X(1,j) = j+1;
28   for (i=2;i<=5;i++) for (j=1;j<=20; j++) X(i,j) = (long)X(i-1,j) * j % 1001;
29   SymmetricMatrix S; S << X * X.t();
30   Matrix SM = X * X.t() - S;
31   Print(SM);
32   LowerTriangularMatrix L = Cholesky(S);
33   Matrix Diff = L*L.t()-S; Clean(Diff, 0.000000001);
34   Print(Diff);
35   {
36      Tracer et1("Stage 1");
37      LowerTriangularMatrix L1(5);
38      Matrix Xt = X.t(); Matrix Xt2 = Xt;
39      QRZT(X,L1);
40      Diff = L - L1; Clean(Diff,0.000000001); Print(Diff);
41      UpperTriangularMatrix Ut(5);
42      QRZ(Xt,Ut);
43      Diff = L - Ut.t(); Clean(Diff,0.000000001); Print(Diff);
44      Matrix Y(3,20);
45      for (j=1;j<=20;j++) Y(1,j) = 22-j;
46      for (i=2;i<=3;i++) for (j=1;j<=20; j++)
47         Y(i,j) = (long)Y(i-1,j) * j % 101;
48      Matrix Yt = Y.t(); Matrix M,Mt; Matrix Y2=Y;
49      QRZT(X,Y,M); QRZ(Xt,Yt,Mt);
50      Diff = Xt - X.t(); Clean(Diff,0.000000001); Print(Diff);
51      Diff = Yt - Y.t(); Clean(Diff,0.000000001); Print(Diff);
52      Diff = Mt - M.t(); Clean(Diff,0.000000001); Print(Diff);
53      Diff = Y2 * Xt2 * S.i() - M * L.i();
54      Clean(Diff,0.000000001); Print(Diff);
55   }
56
57   ColumnVector C1(5);
58   {
59      Tracer et1("Stage 2");
60      X.ReSize(5,5);
61      for (j=1;j<=5;j++) X(1,j) = j+1;
62      for (i=2;i<=5;i++) for (j=1;j<=5; j++)
63         X(i,j) = (long)X(i-1,j) * j % 1001;
64      for (i=1;i<=5;i++) C1(i) = i*i;
65      CroutMatrix A = X;
66      ColumnVector C2 = A.i() * C1; C1 = X.i()  * C1;
67      X = C1 - C2; Clean(X,0.000000001); Print(X);
68   }
69
70   {
71      Tracer et1("Stage 3");
72      X.ReSize(7,7);
73      for (j=1;j<=7;j++) X(1,j) = j+1;
74      for (i=2;i<=7;i++) for (j=1;j<=7; j++)
75         X(i,j) = (long)X(i-1,j) * j % 1001;
76      C1.ReSize(7);
77      for (i=1;i<=7;i++) C1(i) = i*i;
78      RowVector R1 = C1.t();
79      Diff = R1 * X.i() - ( X.t().i() * R1.t() ).t(); Clean(Diff,0.000000001);
80      Print(Diff);
81   }
82
83   {
84      Tracer et1("Stage 4");
85      X.ReSize(5,5);
86      for (j=1;j<=5;j++) X(1,j) = j+1;
87      for (i=2;i<=5;i++) for (j=1;j<=5; j++)
88         X(i,j) = (long)X(i-1,j) * j % 1001;
89      C1.ReSize(5);
90      for (i=1;i<=5;i++) C1(i) = i*i;
91      CroutMatrix A1 = X*X;
92      ColumnVector C2 = A1.i() * C1; C1 = X.i()  * C1; C1 = X.i()  * C1;
93      X = C1 - C2; Clean(X,0.000000001); Print(X);
94   }
95
96
97   {
98      Tracer et1("Stage 5");
99      int n = 40;
100      SymmetricBandMatrix B(n,2); B = 0.0;
101      for (i=1; i<=n; i++)
102      {
103         B(i,i) = 6;
104         if (i<=n-1) B(i,i+1) = -4;
105         if (i<=n-2) B(i,i+2) = 1;
106      }
107      B(1,1) = 5; B(n,n) = 5;
108      SymmetricMatrix A = B;
109      ColumnVector X(n);
110      X(1) = 429;
111      for (i=2;i<=n;i++) X(i) = (long)X(i-1) * 31 % 1001;
112      X = X / 100000L;
113      // the matrix B is rather ill-conditioned so the difficulty is getting
114      // good agreement (we have chosen X very small) may not be surprising;
115      // maximum element size in B.i() is around 1400
116      ColumnVector Y1 = A.i() * X;
117      LowerTriangularMatrix C1 = Cholesky(A);
118      ColumnVector Y2 = C1.t().i() * (C1.i() * X) - Y1;
119      Clean(Y2, 0.000000001); Print(Y2);
120      UpperTriangularMatrix CU = C1.t().i();
121      LowerTriangularMatrix CL = C1.i();
122      Y2 = CU * (CL * X) - Y1;
123      Clean(Y2, 0.000000001); Print(Y2);
124      Y2 = B.i() * X - Y1; Clean(Y2, 0.000000001); Print(Y2);
125
126      LowerBandMatrix C2 = Cholesky(B);
127      Matrix M = C2 - C1; Clean(M, 0.000000001); Print(M);
128      ColumnVector Y3 = C2.t().i() * (C2.i() * X) - Y1;
129      Clean(Y3, 0.000000001); Print(Y3);
130      CU = C1.t().i();
131      CL = C1.i();
132      Y3 = CU * (CL * X) - Y1;
133      Clean(Y3, 0.000000001); Print(Y3);
134
135      Y3 = B.i() * X - Y1; Clean(Y3, 0.000000001); Print(Y3);
136
137      SymmetricMatrix AI = A.i();
138      Y2 = AI*X - Y1; Clean(Y2, 0.000000001); Print(Y2);
139      SymmetricMatrix BI = B.i();
140      BandMatrix C = B; Matrix CI = C.i();
141      M = A.i() - CI; Clean(M, 0.000000001); Print(M);
142      M = B.i() - CI; Clean(M, 0.000000001); Print(M);
143      M = AI-BI; Clean(M, 0.000000001); Print(M);
144      M = AI-CI; Clean(M, 0.000000001); Print(M);
145
146      M = A; AI << M; M = AI-A; Clean(M, 0.000000001); Print(M);
147      C = B; BI << C; M = BI-B; Clean(M, 0.000000001); Print(M);
148
149
150   }
151
152   {
153      Tracer et1("Stage 5");
154      SymmetricMatrix A(4), B(4);
155      A << 5
156        << 1 << 4
157        << 2 << 1 << 6
158        << 1 << 0 << 1 << 7;
159      B << 8
160        << 1 << 5
161        << 1 << 0 << 9
162        << 2 << 1 << 0 << 6;
163      LowerTriangularMatrix AB = Cholesky(A) * Cholesky(B);
164      Matrix M = Cholesky(A) * B * Cholesky(A).t() - AB*AB.t();
165      Clean(M, 0.000000001); Print(M);
166      M = A * Cholesky(B); M = M * M.t() - A * B * A;
167      Clean(M, 0.000000001); Print(M);
168   }
169   {
170      Tracer et1("Stage 6");
171      int N=49;
172      int i;
173      SymmetricBandMatrix S(N,1);
174      Matrix B(N,N+1); B=0;
175      for (i=1;i<=N;i++) { S(i,i)=1; B(i,i)=1; B(i,i+1)=-1; }
176      for (i=1;i<N; i++) S(i,i+1)=-.5;
177      DiagonalMatrix D(N+1); D = 1;
178      B = B.t()*S.i()*B - (D-1.0/(N+1))*2.0;
179      Clean(B, 0.000000001); Print(B);
180   }
181   {
182      Tracer et1("Stage 7");
183      // See if you can pass a CroutMatrix to a function
184      Matrix A(4,4);
185      A.Row(1) <<  3 <<  2 << -1 <<  4;
186      A.Row(2) << -8 <<  7 <<  2 <<  0;
187      A.Row(3) <<  2 << -2 <<  3 <<  1;
188      A.Row(4) << -1 <<  5 <<  2 <<  2;
189      CroutMatrix B = A;
190      Matrix C = A * Inverter(B) - IdentityMatrix(4);
191      Clean(C, 0.000000001); Print(C);
192   }
193
194
195//   cout << "\nEnd of Thirteenth test\n";
196}
Note: See TracBrowser for help on using the repository browser.