Debian Codegraph @

src/math/aslMatrices.cxx

raw
1 /*
2 * Advanced Simulation Library <http://asl.org.il>
3 *
4 * Copyright 2015 Avtech Scientific <http://avtechscientific.com>
5 *
6 *
7 * This file is part of Advanced Simulation Library (ASL).
8 *
9 * ASL is free software: you can redistribute it and/or modify it
10 * under the terms of the GNU Affero General Public License as
11 * published by the Free Software Foundation, version 3 of the License.
12 *
13 * ASL is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Affero General Public License for more details.
17 *
18 * You should have received a copy of the GNU Affero General Public License
19 * along with ASL. If not, see <http://www.gnu.org/licenses/>.
20 *
21 */
22
23
24 #include"aslMatrices.h"
25
26 using namespace std;
27
28 namespace asl
29 {
30
31 template <typename T> const AMatr<T> & AMatr<T>::operator=(const AMatr<T> & a)
32 {
33 if(nCol != a.nCol || nRow != a.nRow )
34 resize(a.nRow, a.nCol);
35 for(unsigned int i(0); i < nRow; ++i)
36 for(unsigned int j(0); j < nCol; ++j)
37 (*this)(i,j) = a(i,j);
38 return *this;
39 }
40
41 template const AMatr<double> & AMatr<double>::operator=(const AMatr<double> & a);
42 template const AMatr<float> & AMatr<float>::operator=(const AMatr<float> & a);
43
44 template <typename T> void AMatr<T>::setRow(unsigned int r, const AVec<T> & b)
45 {
46 if(nCol != b.getSize())
47 errorMessage("Error: AMatr<T>::setRow: size of b does not match number of columns");
48 if(nRow < r)
49 errorMessage("Error: AMatr<T>::setRow: r larger than number of rows");
50 for(unsigned int i(0); i < b.getSize(); ++i)
51 (*this)(r,i) = b[i];
52 }
53
54 template void AMatr<double>::setRow(unsigned int r, const AVec<double> & b);
55 template void AMatr<float>::setRow(unsigned int r, const AVec<float> & b);
56
57 template <typename T> void AMatr<T>::setColumn(unsigned int c, const AVec<T> & b)
58 {
59 if(nRow != b.getSize())
60 errorMessage("Error: AMatr<T>::setColumn: size of b does not match number of rows");
61 if(nCol < c)
62 errorMessage("Error: AMatr<T>::setColumn: c larger than number of columns");
63 for(unsigned int i(0); i < b.getSize(); ++i)
64 (*this)(i,c) = b[i];
65 }
66
67 template void AMatr<double>::setColumn(unsigned int c, const AVec<double> & b);
68 template void AMatr<float>::setColumn(unsigned int c, const AVec<float> & b);
69
70 template <typename T> ostream& operator<<(ostream &f, const AMatr<T> & a)
71 {
72 for (unsigned int i(0); i < a.getNRow(); ++i){
73 for (unsigned int j(0); j < a.getNCol() - 1; ++j)
74 f << a(i, j) << ' ';
75 f << a(i, a.getNCol()-1) << endl;
76 };
77 return f;
78 }
79
80 template ostream& operator<<(ostream &f,const AMatr<float> & a);
81 template ostream& operator<<(ostream &f,const AMatr<double> & a);
82
83 template <typename T> const AMatr<T> operator* (const AMatr<T> &a, const AMatr<T> & b)
84 {
85 AMatr<T> c(a.getNRow(), b.getNCol());
86 T s(0);
87 for (unsigned int i(0); i < a.getNRow(); ++i)
88 for (unsigned int j(0); j < b.getNCol(); ++j)
89 {
90 for (unsigned int k(0); k<a.getNCol(); ++k)
91 s+=a(i, k) * b(k, j);
92 c(i, j)=s;
93 s=0;
94 }
95 return c;
96 }
97
98 template const AMatr<double> operator* (const AMatr<double> &a, const AMatr<double> & b);
99 template const AMatr<float> operator* (const AMatr<float> &a, const AMatr<float> & b);
100
101 template <typename T> const AVec<T> operator* (const AMatr<T> &a, const AVec<T> & b)
102 {
103 AVec<T> c(a.getNRow());
104 T s(0);
105 for (unsigned int i(0); i < a.getNRow(); ++i){
106 for (unsigned int k(0); k<a.getNCol(); ++k)
107 s+=a(i, k) * b[k];
108 c[i]=s;
109 s=0;
110 }
111 return c;
112 }
113
114 template const AVec<double> operator* (const AMatr<double> &a, const AVec<double> & b);
115 template const AVec<float> operator* (const AMatr<float> &a, const AVec<float> & b);
116
117 template <typename T> const AVec<T> operator* (const AVec<T> &a, const AMatr<T> & b)
118 {
119 AVec<T> c(b.getNCol());
120 T s(0);
121 for (unsigned int j(0); j < b.getNCol(); ++j)
122 {
123 for (unsigned int k(0); k<a.getSize(); ++k)
124 s+=a[k] * b(k, j);
125 c[j]=s;
126 s=0;
127 }
128 return c;
129 }
130
131 template const AVec<double> operator* (const AVec<double> &a, const AMatr<double> & b);
132 template const AVec<float> operator* (const AVec<float> &a, const AMatr<float> & b);
133
134 template <typename T> const AMatr<T> operator* (const AMatr<T> &a, const T & b)
135 {
136 AMatr<T> c(a.getNRow(), a.getNCol());
137 c.getInternalVec() = a.getInternalVec() * b;
138 return c;
139 }
140
141 template const AMatr<double> operator* (const AMatr<double> &a, const double & b);
142 template const AMatr<float> operator* (const AMatr<float> &a, const float & b);
143
144 template <typename T> const AMatr<T> operator* (const T &a, const AMatr<T> & b)
145 {
146 AMatr<T> c(b.getNRow(), b.getNCol());
147 c.getInternalVec() = b.getInternalVec() * a;
148 return c;
149 }
150
151 template const AMatr<double> operator* (const double &a, const AMatr<double> & b);
152 template const AMatr<float> operator* (const float &a, const AMatr<float> & b);
153
154 template <typename T> AMatr<T> elementProduct(const AVec<T> & a, const AVec<T> & b)
155 {
156 AMatr<T> m(a.getSize(),b.getSize());
157 for (unsigned int i(0); i < a.getSize(); ++i)
158 for (unsigned int j(0);j<b.getSize();++j)
159 m(i,j)= a[i]*b[j];
160 return m;
161 }
162
163 template AMatr<double> elementProduct(const AVec<double> & a, const AVec<double> & b);
164 template AMatr<float> elementProduct(const AVec<float> & a, const AVec<float> & b);
165
166 template <typename T> const T trace(const AMatr<T> &a)
167 {
168 T c(0);
169 for (unsigned int i(0); i < a.getNRow(); ++i)
170 c += a(i, i);
171 return c;
172 }
173
174 template const double trace(const AMatr<double> &a);
175 template const float trace(const AMatr<float> &a);
176
177 template <typename T>
178 const T trace(const AMatr<T> & a, const AMatr<T> & b)
179 {
180 T c(0);
181 for (unsigned int i(0); i < a.getNRow(); ++i)
182 for (unsigned int j(0); j < a.getNCol(); ++j)
183 c += a(i,j) * b(j,i);
184 return c;
185 }
186
187 template const double trace(const AMatr<double> & a, const AMatr<double> & b);
188 template const float trace(const AMatr<float> & a, const AMatr<float> & b);
189
190 template <typename T> AMatr<T> makeAMatr(const AVec<T> & a)
191 {
192 return {1,a.getSize(),a};
193 }
194
195 template AMatr<double> makeAMatr(const AVec<double> & a);
196 template AMatr<float> makeAMatr(const AVec<float> & a);
197
198 template <typename T> AMatr<T> makeAMatr(const AVec<T> & a, const AVec<T> & b)
199 {
200 if(a.getSize() != b.getSize())
201 errorMessage("makeMatr: vectors have different sizes" );
202 AMatr<T> m(2, a.getSize());
203 m.setRow(0,a);
204 m.setRow(1,b);
205 return m;
206 }
207
208 template AMatr<double> makeAMatr<double>(const AVec<double> & a, const AVec<double> & b);
209 template AMatr<float> makeAMatr<float>(const AVec<float> & a, const AVec<float> & b);
210
211 template <typename T> AMatr<T> makeAMatr(const AVec<T> & a,
212 const AVec<T> & b,
213 const AVec<T> & c)
214 {
215 if((a.getSize() != b.getSize()) || (a.getSize() != c.getSize()))
216 errorMessage("makeMatr: vectors have different sizes" );
217 AMatr<T> m(3, a.getSize());
218 m.setRow(0,a);
219 m.setRow(1,b);
220 m.setRow(2,c);
221 return m;
222 }
223
224 template AMatr<double> makeAMatr(const AVec<double> & a, const AVec<double> & b, const AVec<double> & c);
225 template AMatr<float> makeAMatr(const AVec<float> & a, const AVec<float> & b, const AVec<float> & c);
226
227 template <typename T> AMatr<T> makeAMatr(AVec<T> *a, unsigned int n)
228 {
229 for(unsigned int i(1); i<n; ++i)
230 if((a[0].getSize() != a[i].getSize()))
231 errorMessage("makeMatr: some vectors have different sizes");
232
233 AMatr<T> m(n, a[0].getSize());
234 for(unsigned int i(0); i<n; ++i)
235 m.setRow(i,a[i]);
236 return m;
237 }
238
239 template AMatr<double> makeAMatr(AVec<double> *a, unsigned int n);
240 template AMatr<float> makeAMatr(AVec<float> *a, unsigned int n);
241
242 template <typename T> AMatr<T> makeAMatrUnit(unsigned int n)
243 {
244 AMatr<T> m(n,n);
245
246 for(unsigned int i(0); i<n; ++i)
247 for(unsigned int j(0); j<n; ++j)
248 m(i,j)=0;
249 for(unsigned int i(0); i<n; ++i)
250 m(i,i) = 1;
251 return m;
252 }
253
254 template AMatr<double> makeAMatrUnit<double>(unsigned int n);
255 template AMatr<float> makeAMatrUnit<float>(unsigned int n);
256
257 template <typename T> AVec<T> getDiagonal(const AMatr<T> & a)
258 {
259 if(a.getNCol()!=a.getNRow())
260 errorMessage("Error: getDiagonal: the matrix is not square one");
261
262 unsigned int n(a.getNCol());
263 AVec<T> v(n);
264
265 for(unsigned int i(0); i<n; ++i)
266 v[i]=a(i,i);
267
268 return v;
269 }
270
271 template AVec<double> getDiagonal(const AMatr<double> & a);
272 template AVec<float> getDiagonal(const AMatr<float> & a);
273
274 template <typename T> AVec<T> getOffDiagonalUp(const AMatr<T> & a)
275 {
276 if(a.getNCol()!=a.getNRow())
277 errorMessage("Error: getOffDiagonalUp: the matrix is not square one");
278
279 unsigned int n(a.getNCol());
280 AVec<T> v((n*n-n)/2);
281
282 unsigned int k(0);
283 for(unsigned int i(1); i<n; ++i)
284 for(unsigned int j(1); j<=i; ++j)
285 {
286 v[k]=a.getElement(i,j);
287 ++k;
288 }
289 return v;
290 }
291
292 template <typename T> T det(const AMatr<T> & m)
293 {
294 if(m.getNCol()!=m.getNRow())
295 errorMessage("Error: det: the matrix is not square one");
296 if(m.getNCol()>3)
297 errorMessage("Error: det: the matrix size is larger than 3");
298
299 T v(0);
300 if(m.getNCol()==1)
301 v=m(0,0);
302 if(m.getNCol()==2)
303 v=m(0,0) * m(1,1) - m(0,1) * m(1,0);
304 if(m.getNCol()==3)
305 v=m(0,0) * m(1,1) * m(2,2) +
306 m(1,0) * m(2,1) * m(0,2) +
307 m(0,1) * m(1,2) * m(2,0) -
308 m(0,2) * m(1,1) * m(2,0) -
309 m(1,0) * m(0,1) * m(2,2) -
310 m(2,1) * m(1,2) * m(0,0);
311 return v;
312 }
313
314 template double det(const AMatr<double> & m);
315 template float det(const AMatr<float> & m);
316
317 template <typename T>
318 AVec<T> solveSystem(const AMatr<T> & a, const AVec<T> & b)
319 {
320 if(a.getNCol()!=a.getNRow())
321 errorMessage("Error: solveSystem: the matrix is not square one");
322 if(a.getNCol()>3)
323 errorMessage("Error: solveSystem: the matrix size is larger than 3");
324 if(a.getNCol()!=b.getSize())
325 errorMessage("Error: solveSystem: size of b does not match the size of matrix");
326
327 AVec<T> v(b.getSize());
328
329 T d(det(a));
330 AVec<T> db(b.getSize());
331 for(unsigned int i(0); i < b.getSize(); ++i)
332 db[i] = det(replaceColumn(a,b,i));
333
334 return db/d;
335 }
336
337 template AVec<double> solveSystem(const AMatr<double> & a, const AVec<double> & b);
338 template AVec<float> solveSystem(const AMatr<float> & a, const AVec<float> & b);
339
340
341 template <typename T> AMatr<T> replaceRow(const AMatr<T> & a,
342 const AVec<T> & b,
343 unsigned int r)
344 {
345 AMatr<T> m(a);
346 m.setRow(r,b);
347 return m;
348 }
349
350 template <typename T> AMatr<T> replaceColumn(const AMatr<T> & a,
351 const AVec<T> & b,
352 unsigned int c)
353 {
354 AMatr<T> m(a);
355 m.setColumn(c,b);
356 return m;
357 }
358
359
360 template <typename T>
361 T matrixCofactor(const AMatr<T> & a, unsigned int r, unsigned int c)
362 {
363 AMatr<T> m(a.getNRow()-1, a.getNCol()-1);
364 for(unsigned int i(0), im(0); im < m.getNRow(); ++i, ++im)
365 {
366 i += (i == r) ? 1 : 0;
367 for(unsigned int j(0), jm(0); jm < m.getNCol(); ++j, ++jm)
368 {
369 j += (j == c) ? 1 : 0;
370 m(im, jm) = a(i,j);
371 }
372 }
373 return det(m);
374 }
375
376
377 template <typename T> AMatr<T> generateMatrixCofactors(const AMatr<T> & a)
378 {
379 if(a.getNRow() != a.getNCol())
380 errorMessage("Error: generateMatrixCofactors: the matrix is not rectangular one");
381 if(a.getNRow() > 3)
382 errorMessage("Error: generateMatrixCofactors: the matrix size is more than 3");
383 AMatr<T> m(a.getNRow(), a.getNCol());
384 int rfactor(-1);
385 for(unsigned int i(0); i < a.getNRow(); ++i){
386 rfactor*=-1;
387 int factor(-rfactor);
388 for(unsigned int j(0); j < a.getNCol(); ++j)
389 {
390 factor*=-1;
391 m(i,j) = (factor * matrixCofactor(a,i,j));
392 }
393 }
394 return m;
395 }
396
397 template <typename T> AMatr<T> inverseMatrix(const AMatr<T> & a)
398 {
399 AMatr<T> mc(generateMatrixCofactors(a));
400 T d(det(a));
401 return mc/d;
402 }
403
404 template AMatr<double> inverseMatrix(const AMatr<double> & a);
405 template AMatr<float> inverseMatrix(const AMatr<float> & a);
406
407
408
409 double detSymMat(double a,double b, double c)
410 {
411 return a*b-c*c;
412 }
413
414 void getEValEVecMatSym2x2(double a,double b, double c, double & l1, double & l2, double & v1x, double & v1y, double & v2x, double & v2y)
415 {
416 double tr2((a+b)*.5);
417 double det(detSymMat(a,b,c));
418 l1= tr2 + sqrt(tr2*tr2-det);
419 l2= tr2 - sqrt(tr2*tr2-det);
420 v1x=l1-b;
421 v1y=c;
422 v2x=l2-b;
423 v2y=c;
424 }
425
426 double detSymMat(double a, double b, double c, double d, double e, double f)
427 {
428 return a*b*c+2.*d*e*f-b*e*e-a*f*f-c*d*d;
429 }
430
431 void getEValEVecMatSym3x3(double a, double b, double c, double d, double e, double f,
432 double & l1, double & l2, double & l3,
433 double & v1x, double & v1y, double & v1z,
434 double & v2x, double & v2y, double & v2z,
435 double & v3x, double & v3y, double & v3z)
436 {
437 // double tr(a+b+c);
438 // double det(detSymMat(a,b,c,d,e,f))
439
440 }
441
442 }