1 //#**************************************************************
2 //#
3 //# filename: linalg.cpp
4 //#
5 //# author: Gerstmayr Johannes
6 //#
7 //# generated: 15.05.97
8 //# description: Classes for linear and nonlinear algebra which is
9 //# thought to be used in finite element or similar applications
10 //# There are 2D and 3D and arbitrary size Vectors (Vector2D, Vector3D, Vector),
11 //# arbitrary size matrices (Matrix), and a nonlinear system (NumNLSys)
12 //# and a nonlinear solver (NumNLSolver)
13 //# remarks: Indizes run from 1 to n except in Vector3D/2D
14 //#
15 //# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian
16 //# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the
17 //# Johannes Kepler Universitaet Linz, Austria. All rights reserved.
18 //#
19 //# This file is part of HotInt.
20 //# HotInt is free software: you can redistribute it and/or modify it under the terms of
21 //# the HOTINT license. See folder 'licenses' for more details.
22 //#
23 //# bug reports are welcome!!!
24 //# WWW: www.hotint.org
25 //# email: bug_reports@hotint.org or support@hotint.org
26 //#**************************************************************
27 #include "mbs_interface.h"
28
29 #include "../WorkingModule/stdafx.h"
30 #include "ioincludes.h"
31
32 #include "release_assert.h"
33 #include <memory.h>
34
35 #include <math.h>
36
37 #include "tarray.h"
38 #include "mystring.h"
39 #include "femathhelperfunctions.h"
40
41 #include "lapack_routines.h"
42 #include "pardiso.h"
43
44 #include "..\superlu\superlumain.h"
45
46 //for output possibility within femath
47 //#include "..\workingmodule\WorkingModuleBaseClass.h"
48 extern UserOutputInterface * global_uo;
49
50 //count news in sparse matrices!!!
51 int gensparsemat = 0;
GenSparsemat()52 void GenSparsemat() {gensparsemat++;}
GetGenSparsemat()53 int GetGenSparsemat() {return gensparsemat;}
54
55 #ifdef gencnt
56 extern int *genvec;
57 extern int *genmat;
58 #endif
59
60
61
62 //-----------------------------------------------------------------
63 //-------------- CLASS VECTOR -------------------------------
64 //-----------------------------------------------------------------
65 //Generation of user-defiened Vector
66 //Operations: +,-,*,<< (also Matrices)
67
68
Vector(const Vector & veci,int noownmem)69 Vector::Vector(const Vector& veci, int noownmem)
70 {
71 if (noownmem)
72 {
73 l = veci.l;
74 lalloc = 0;
75 ownmemory = 0;
76 if (l==0) vec = NULL;
77 else vec = veci.vec;
78 }
79 else
80 {
81 Vector::Vector(veci);
82 }
83 }
84
Vector(int leni,int nofill,double fill)85 Vector::Vector(int leni, int nofill, double fill)
86 {
87 vec=NULL;
88 GenVector(leni);
89 if (!nofill && l!=0)
90 {
91 for (int i=0; i<l; i++) {vec[i]=fill; }
92 }
93 };
94
Vector(const Vector & veci)95 Vector::Vector(const Vector& veci)
96 {
97 l=veci.l;
98 lalloc = l;
99 if (l==0)
100 {
101 vec=NULL;
102 }
103 else
104 {
105 #ifdef gencnt
106 (*genvec)++;
107 #endif
108 ownmemory = 1;
109 vec=new double[l];
110 memcpy(vec,veci.vec,l*sizeof(double));
111 }
112 };
113
Vector(const char * vector,char osbc,char csbc,char commac)114 Vector::Vector(const char* vector, char osbc, char csbc, char commac) // (RL)
115 {
116 //mystr must be formatted like following examples: [1 2 4] or [1,2,3]
117 mystr el = "\n"; //end of line
118 char elc = 'n';
119 mystr osb = osbc; //open square bracket, for vector
120 mystr csb = csbc; //closing square bracket
121 mystr semicolon = ";"; //double stop
122 mystr comma = commac; //comma
123 int endstr = 0;
124 int pos = 0;
125 mystr dataname;
126 char ch;
127
128 mystr dummycomment; //dummy comment string
129 mystr str(vector);
130 mystr text = str.GetStringInBrackets(pos, osbc, csbc);
131
132 TArray<double> ta_vec(0);
133 if(text.Find(el)==-1 && text.Find(semicolon)==-1)
134 {
135 // vector has no semicolon and is written in one single line
136 mystr s_value;
137 int posvec = 0;
138 int possval = 0;
139 int comma_found = 0;
140 while(posvec != -1)
141 {
142 comma_found = text.GetUntil(posvec, commac, s_value); // 'value,'
143 s_value.ReadLeadingSpaces(possval);
144 ta_vec.Add(s_value.MakeDouble());
145
146 if(comma_found)posvec++;
147 }
148 }
149 SetVector(ta_vec);
150 }
151
IsValid(double maxval)152 int Vector :: IsValid(double maxval)
153 {
154 for (int i=0; i<l; i++)
155 {
156 if (IsNaN(vec[i]) || fabs(vec[i])>=maxval) {return 0;}
157 }
158 return 1;
159 }
160
161
GenVector(int leni)162 void Vector::GenVector(int leni)
163 {
164 l=leni;
165 lalloc = leni;
166 if (l!=0)
167 {
168 #ifdef gencnt
169 (*genvec)++;
170 #endif
171 ownmemory = 1;
172 vec=new double[l];
173 }
174 else
175 {
176 vec = NULL;
177 }
178 };
179
operator =(const Vector & veci)180 Vector& Vector::operator= (const Vector& veci)
181 {
182 if (this == &veci) {return *this;}
183 if (veci.l!=0)
184 {
185 if ((((lalloc < veci.l && ownmemory) || (l < veci.l && !ownmemory)) && lalloc_act) || (l != veci.l && !lalloc_act))
186 {
187 #ifdef gencnt
188 (*genvec)++;
189 #endif
190 l=veci.l;
191 if (vec!=NULL && ownmemory && lalloc) {delete [] vec; }
192 ownmemory = 1;
193 vec=new double[l];
194 lalloc = l;
195 }
196 else
197 {
198 l=veci.l;
199 }
200 memcpy(vec,veci.vec,l*sizeof(double));
201 }
202 else
203 {
204 l=0;
205 //lalloc = 0; vec = NULL;
206 }
207 return *this;
208 };
209
operator =(const Vector3D & veci)210 Vector& Vector::operator= (const Vector3D& veci)
211 {
212 if ((lalloc < 3 && lalloc_act) || (l != 3 && !lalloc_act))
213 {
214 #ifdef gencnt
215 (*genvec)++;
216 #endif
217 l=3;
218 if (vec!=NULL && ownmemory && lalloc) {delete [] vec; }
219 ownmemory = 1;
220 vec=new double[l];
221 lalloc = l;
222 }
223 else {l=3; }
224 vec[0] = veci.X(); vec[1] = veci.Y(); vec[2] = veci.Z();
225 return *this;
226 };
227
operator ==(const Vector & vec1)228 int Vector :: operator == (const Vector& vec1)
229 {
230 if (vec1.GetLen() != GetLen())
231 {cout << "Warning: vectors compared with different length!" << endl;}
232
233 int i;
234 for (i = 1; i <= vec1.GetLen(); i++)
235 {
236 if (vec1.Get(i) != Get(i)) return 0;
237 }
238
239 return 1;
240 }
241
SetLen(int leni)242 void Vector::SetLen(int leni)
243 {
244 if ((((lalloc < leni && ownmemory) || (!ownmemory && leni > l)) && lalloc_act) || (l != leni && !lalloc_act))
245 {
246 if (vec!=NULL && ownmemory && lalloc) {delete [] vec;}
247 GenVector(leni);
248 }
249 l = leni;
250 }
251
GetNorm() const252 double Vector::GetNorm() const
253 {
254 return sqrt((*this)*(*this));
255 }
256
MaxNorm() const257 double Vector::MaxNorm() const
258 {
259 double res=-1e100;
260 for(int i=0; i<l; i++)
261 {
262 res=Maximum(fabs(vec[i]),res);
263 }
264 if (res==-1E100) {res=0;}
265 return res;
266 }
267
MinNorm() const268 double Vector::MinNorm() const
269 {
270 double res=1E100;
271 for(int i=0; i<l; i++)
272 {
273 if (fabs(vec[i])<res && vec[i]!=0) {res=fabs(vec[i]);}
274 }
275 if (res==1E100) {res=0;}
276 return res;
277 }
GetNormed2DNormalVec() const278 Vector Vector::GetNormed2DNormalVec() const
279 {
280 Vector v(-(*this)(2),(*this)(1));
281 double len=v.GetNorm();
282 if (len!=0) {v*=1/len;}
283 return v;
284 }
GetNormed2DVec() const285 Vector Vector::GetNormed2DVec() const
286 {
287 Vector v((*this));
288 double len=v.GetNorm();
289 if (len!=0) {v*=1/len;}
290 return v;
291 }
292
Sum() const293 double Vector::Sum() const
294 {
295 int i;
296 double s = 0;
297 for (i = 0; i < l; i++)
298 {
299 s += vec[i];
300 }
301 return s;
302 }
303
Sum(int n) const304 double Vector::Sum(int n) const
305 {
306 int i;
307 double s = 0;
308 for (i = 0; i < n && i < l; i++)
309 {
310 s += vec[i];
311 }
312 return s;
313 }
314
315
FillWithZeros()316 void Vector::FillWithZeros()
317 {
318 for (int i=0; i<l; i++) {vec[i]=0; }
319 }
320 /*
321 void Vector::FillInVector(const Vector& v, const IVector& ref)
322 {
323 for(int i=1; i<=ref.Length(); i++)
324 {
325 (*this)[ref[i]]+=v[i];
326 }
327 }
328 */
Append(const Vector & v) const329 Vector Vector::Append(const Vector& v) const
330 {
331 if (v.l == 0) {return *this;}
332 int i;
333 Vector res(l+v.l);
334 for (i=1;i<=l;i++)
335 {
336 res(i)=(*this)(i);
337 }
338 for (i=1;i<=v.l;i++)
339 {
340 res(i+l)=v(i);
341 }
342 return res;
343 }
344
Insert(const Vector & v,int pos)345 void Vector::Insert(const Vector& v, int pos)
346 {
347 if (v.l == 0) {return;}
348 int i;
349 for (i=1;i<=v.l;i++)
350 {
351 Elem(i+pos-1) = v(i);
352 }
353 }
354
SubVector(int from,int to) const355 Vector Vector::SubVector(int from, int to) const
356 {
357 int i;
358 Vector res(to-from+1);
359 for (i=from;i<=to;i++)
360 {
361 res(i-from+1)=(*this)(i);
362 }
363 return res;
364 }
365
MultAdd(const double & k,const Vector & v)366 void Vector::MultAdd(const double& k, const Vector& v)
367 {
368 #ifndef __QUICKMATH
369 release_assert(v.l==l);
370 #endif
371 for (int i=0; i<l; i++)
372 {
373 vec[i]+=k*v.vec[i];
374 }
375 }
376
operator +=(const Vector & v1)377 Vector& Vector::operator+= (const Vector& v1)
378 {
379 #ifndef __QUICKMATH
380 release_assert(v1.l==l);
381 #endif
382 for (int i=0;i<l;i++)
383 {
384 vec[i]+=v1.vec[i];
385 }
386 return *this;
387 }
388
operator -=(const Vector & v1)389 Vector& Vector::operator-= (const Vector& v1)
390 {
391 #ifndef __QUICKMATH
392 release_assert(v1.l==l);
393 #endif
394 for (int i=0;i<l;i++)
395 {
396 vec[i]-=v1.vec[i];
397 }
398 return *this;
399 }
400
operator *=(const double & v)401 Vector& Vector::operator*= (const double& v)
402 {
403 for (int i=0;i<l;i++)
404 {
405 vec[i]*=v;
406 }
407 return *this;
408 }
409
operator <<(ostream & os,const Vector & v)410 ostream& operator<<(ostream& os, const Vector& v)
411 {
412 if (v.GetLen()<=70)
413 {
414 os << "[";
415 for (int i=1; i<v.GetLen(); i++)
416 {
417 os << v(i) << ", ";
418 }
419 if (v.GetLen()!=0)
420 {
421 os << v(v.GetLen());
422 }
423 os << "]";
424 } else
425 {
426 os << "[\n";
427 for (int i=1; i<=v.GetLen(); i++)
428 {
429 os << v(i);
430 if (i!=v.GetLen()) {os << ", ";}
431 if (i%8==0 && i!=v.GetLen()) {os << "\n col(" << i << "): ";}
432 }
433 os << "]\n";
434 }
435 return os;
436 }
437
operator +(const Vector & v1,const Vector & v2)438 Vector operator+ (const Vector& v1, const Vector& v2)
439 {
440 Vector result=v1;
441 result+=v2;
442 return result;
443 }
444
operator -(const Vector & v1,const Vector & v2)445 Vector operator- (const Vector& v1, const Vector& v2)
446 {
447 Vector result=v1;
448 result-=v2;
449 return result;
450 }
451
operator *(const Vector & vec,const double & val)452 Vector operator* (const Vector& vec, const double& val)
453 {
454 Vector result=vec;
455 result*=val;
456 return result;
457 }
458
operator *(const double & val,const Vector & vec)459 Vector operator* (const double& val, const Vector& vec)
460 {
461 Vector result=vec;
462 result*=val;
463 return result;
464 }
465
operator *(const Vector & vec1,const Vector & vec2)466 double operator* (const Vector& vec1, const Vector& vec2)
467 {
468 #ifndef __QUICKMATH
469 release_assert(vec1.l==vec2.l);
470 #endif
471 double result=0;
472 for (int i=0;i<vec1.l;i++)
473 {
474 result+=vec1.vec[i]*vec2.vec[i];
475 }
476 return result;
477 }
478
479 // Vector entries are equally distributed values of piecewise linear function on [-1, 1]
480 // Interpolate returns value of function in point ploc � [-1, 1]
481 // Vector entries specify function values, ordered as
482 // [ 1 2 3 ... width ] --> [x=-1 x=1]
Interpolate(const double ploc) const483 double Vector::Interpolate(const double ploc) const
484 {
485 int imin;
486 double hx = 2./(l-1.);
487 // lower indices of cell containing ploc
488 imin = int((1.-ploc) / hx ) +1;
489 if (imin >= l) imin = l-1;
490 // "mesh sizes"
491 double xmin = hx * (imin-1)-1;
492 double val = ( (*this)(imin+1)*(hx-ploc+xmin) +
493 (*this)(imin)*(ploc-xmin) ) / hx;
494 return val;
495
496 }
497
MakeString(mystr intro)498 mystr& Vector::MakeString(mystr intro)
499 {
500 mystr* buffer = new mystr(intro);
501 *buffer +=("["); *buffer += mystr(l); *buffer +=("]");
502 *buffer +=(" = { ");
503 for (int i=0; i < this->l; i++)
504 {
505 if( i ) *buffer += ", ";
506 *buffer += mystr(Get0(i));
507 }
508 *buffer += mystr(" };");
509 return *buffer;
510 }
511
512
513
514 //-----------------------------------------------------------------
515 //-------------- CLASS MATRIX -------------------------------
516 //-----------------------------------------------------------------
517 //Generation of Matrices with user-definied size
518 //Operations: +,-,*,<<
519
Matrix(double x)520 Matrix::Matrix(double x)
521 {
522 rows=1; cols=1;
523 #ifdef gencnt
524 (*genmat)++;
525 #endif
526 mat=new double[1];
527 lalloc = 1;
528 mat[0]=x;
529 }
Matrix(double x11,double x12,double x21,double x22)530 Matrix::Matrix(double x11, double x12, double x21, double x22)
531 {
532 rows=2; cols=2;
533 #ifdef gencnt
534 (*genmat)++;
535 #endif
536 mat=new double[4];
537 lalloc = 4;
538 mat[0]=x11; mat[1]=x12;
539 mat[2]=x21; mat[3]=x22;
540 }
Matrix(double x11,double x12,double x13,double x21,double x22,double x23,double x31,double x32,double x33)541 Matrix::Matrix(double x11, double x12, double x13,
542 double x21, double x22, double x23,
543 double x31, double x32, double x33)
544 {
545 rows=3; cols=3;
546 #ifdef gencnt
547 (*genmat)++;
548 #endif
549 mat=new double[9];
550 lalloc = 9;
551 mat[0]=x11; mat[1]=x12; mat[2]=x13;
552 mat[3]=x21; mat[4]=x22; mat[5]=x23;
553 mat[6]=x31; mat[7]=x32; mat[8]=x33;
554 }
555
Matrix(const Matrix3D mat3d)556 Matrix::Matrix(const Matrix3D mat3d)
557 {
558 rows=3; cols=3;
559 #ifdef gencnt
560 (*genmat)++;
561 #endif
562 mat=new double[9];
563 lalloc = 9;
564 mat[0]=mat3d(1,1); mat[1]=mat3d(1,2); mat[2]=mat3d(1,3);
565 mat[3]=mat3d(2,1); mat[4]=mat3d(2,2); mat[5]=mat3d(2,3);
566 mat[6]=mat3d(3,1); mat[7]=mat3d(3,2); mat[8]=mat3d(3,3);
567 }
568
569 int bigmats = 0;
570 int bigmatval = 10000;
Matrix(int rowsi,int colsi,int nofill,double fill)571 Matrix::Matrix(int rowsi, int colsi, int nofill, double fill)
572 {
573 rows=rowsi;
574 cols=colsi;
575 if (rows*cols!=0)
576 {
577 #ifdef gencnt
578 (*genmat)++;
579 #endif
580 mat=new double[rows*cols];
581 lalloc = rows*cols;
582 /* if (lalloc > bigmatval)
583 {
584 bigmats++;
585 (*global_uo) << "BigMat=" << lalloc*8/1000 << "K\n";
586 }*/
587 if (!nofill)
588 {
589 int size=rows*cols;
590 for (int i=0; i<size; i++) {mat[i]=fill; }
591 }
592 }
593 else
594 {
595 mat = NULL;
596 lalloc=0;
597 }
598 };
599
600
Matrix(const Matrix & mati)601 Matrix::Matrix(const Matrix& mati)
602 {
603 rows=mati.rows;
604 cols=mati.cols;
605 if (mati.mat==NULL)
606 {
607 mat = NULL;
608 lalloc = 0;
609 }
610 else
611 {
612 #ifdef gencnt
613 (*genmat)++;
614 #endif
615 mat=new double[rows*cols];
616 lalloc = rows*cols;
617 /* if (lalloc > bigmatval)
618 {
619 bigmats++;
620 (*global_uo) << "BigMat=" << lalloc*8/1000 << "K\n";
621 }*/
622 memcpy(mat,mati.mat,rows*cols*sizeof(double));
623 }
624 };
625
CopyFrom(const Matrix & m,int row1,int col1,int row2,int col2)626 void Matrix::CopyFrom(const Matrix& m, int row1, int col1, int row2, int col2)
627 {
628 Resize(row2-row1+1, col2-col1+1, 1);
629 int i=0;
630 for (int row = row1; row <= row2; row++)
631 {
632 for (int col = col1; col <= col2; col++)
633 {
634 mat[i++] = m(row,col);
635 }
636 }
637 }
638
CopyFrom(const Matrix & m,int row1,int col1,int row2,int col2,const IVector & r)639 void Matrix::CopyFrom(const Matrix& m, int row1, int col1, int row2, int col2, const IVector& r)
640 {
641 Resize(row2-row1+1, col2-col1+1, 1);
642 int i=0;
643 for (int row = row1; row <= row2; row++)
644 {
645 for (int col = col1; col <= col2; col++)
646 {
647 mat[i++] = m(r(row),r(col));
648 }
649 }
650 }
651
CopyFrom(const SparseMatrix & m,int row1,int col1,int row2,int col2)652 void Matrix::CopyFrom(const SparseMatrix& m, int row1, int col1, int row2, int col2)
653 {
654 Resize(row2-row1+1, col2-col1+1, 1);
655
656 for (int i = row1; i <= row2; i++)
657 {
658 int lastsparsecol = 1;
659 for (int j = col1; j <= col2; j++)
660 {
661 Elem0(i-row1,j-col1) = m.Get(i,j,lastsparsecol);
662 }
663 }
664 }
665
CopyFrom(const SparseMatrix & m,int row1,int col1,int row2,int col2,const IVector & r)666 void Matrix::CopyFrom(const SparseMatrix& m, int row1, int col1, int row2, int col2, const IVector& r)
667 {
668 Resize(row2-row1+1, col2-col1+1, 1);
669 for (int i = row1; i <= row2; i++)
670 {
671 for (int j = col1; j <= col2; j++)
672 {
673 Elem0(i-row1,j-col1) = m(r(i),r(j));
674 }
675 }
676 }
677
operator =(const Matrix & mati)678 Matrix& Matrix::operator= (const Matrix& mati)
679 {
680 if (this == &mati) {return *this;}
681 if (rows!=mati.rows || cols!=mati.cols)
682 {
683 if ((mati.rows*mati.cols > lalloc && lalloc_act) || (mati.rows*mati.cols != rows*cols && !lalloc_act))
684 {
685 if (mat!=NULL && lalloc != 0) {delete [] mat; mat=NULL; }
686 rows=mati.rows;
687 cols=mati.cols;
688
689 if (rows*cols!=0)
690 {
691 #ifdef gencnt
692 (*genmat)++;
693 #endif
694 mat=new double[rows*cols];
695 lalloc = rows*cols;
696 /* if (lalloc > bigmatval)
697 {
698 bigmats++;
699 (*global_uo) << "BigMat=" << lalloc*8/1000 << "K\n";
700 }*/
701 }
702 }
703 else
704 {
705 rows=mati.rows;
706 cols=mati.cols;
707 }
708 }
709 if (mati.mat==NULL || rows*cols==0)
710 {
711 mat = NULL;
712 }
713 else
714 {
715 memcpy(mat,mati.mat,rows*cols*sizeof(double));
716 }
717 return *this;
718 };
719
Resize(int rowsi,int colsi,int nofill,double fill)720 void Matrix::Resize(int rowsi, int colsi, int nofill, double fill)
721 {
722 if ((rowsi*colsi > MaxAllocatedSize() && lalloc_act) || (rowsi*colsi != rows*cols && !lalloc_act) || mat==NULL)
723 {
724 #ifndef __QUICKMATH
725 release_assert(IsConstSizeMatrix() == 0);
726 #endif
727 if (mat!=NULL && lalloc != 0) {delete [] mat; mat=NULL; }
728 rows=rowsi;
729 cols=colsi;
730 if (rows*cols!=0)
731 {
732 #ifdef gencnt
733 (*genmat)++;
734 #endif
735 lalloc = rows*cols;
736 mat = new double[rows*cols];
737 if (mat == NULL)
738 {
739 (*global_uo) << "ERROR: memory allocation failed: Matrix::Resize(...) !!!!\n";
740 (*global_uo) << "MEM=" << (8*rows*cols)/(1024*1024) << "MB, mat=[" << rows << "x" << cols << "]\n";
741 release_assert(0);
742 }
743 }
744 }
745 else
746 {
747 rows=rowsi;
748 cols=colsi;
749 }
750 if (!nofill)
751 {
752 int size=rows*cols;
753 for (int i=0; i<size; i++) {mat[i]=fill; }
754 }
755 }
756
MaxNorm() const757 double Matrix::MaxNorm() const
758 {
759 double res=0;
760 for(int i=0; i<rows*cols; i++)
761 {
762 res=Maximum(fabs(mat[i]),res);
763 }
764 return res;
765 }
766
MinNorm() const767 double Matrix::MinNorm() const
768 {
769 double res=10E100;
770 for(int i=0; i<rows*cols; i++)
771 {
772 if (fabs(mat[i])<res && mat[i]!=0) {res=fabs(mat[i]);}
773 }
774 if (res==10E100) {res=1;}
775 return res;
776 }
777
Norm2() const778 double Matrix::Norm2() const
779 {
780 double res=0;
781 for(int i=0; i<rows*cols; i++)
782 {
783 res+=mat[i]*mat[i];
784 }
785 return sqrt(res);
786 }
787
FillWithZeros()788 void Matrix::FillWithZeros()
789 {
790 for(int i=0; i<rows*cols; i++)
791 {
792 mat[i]=0;
793 }
794 }
795
SetAll(double x)796 void Matrix::SetAll(double x)
797 {
798 for(int i=0; i<rows*cols; i++)
799 {
800 mat[i]=x;
801 }
802 }
803
FillWithZeros(int row,int col,int nrows,int ncols)804 void Matrix::FillWithZeros(int row, int col, int nrows, int ncols)
805 {
806 for (int i=row; i<row+nrows; i++)
807 {
808 for (int j=col; j<col+ncols; j++)
809 {
810 Elem(i,j) = 0;
811 }
812 }
813 }
814
815 /*
816 void Matrix::FillInMatrix(const Matrix& K, const IVector& ref1, const IVector& ref2)
817 {
818 for(int i=1; i<=K.Getrows(); i++)
819 {
820 for(int j=1; j<=K.Getcols(); j++)
821 {
822 (*this)(ref1[i],ref2[j])+=K(i,j);
823 }
824 }
825 }
826
827 void Matrix::FillInBMatrix(const Matrix& K, const IVector& ref1, const IVector& ref2)
828 {
829 for(int i=1; i<=K.Getrows(); i++)
830 {
831 for(int j=1; j<=K.Getcols(); j++)
832 {
833 if (ref1[i]<=ref2[j]) {Bset(ref1[i],ref2[j],BGet(ref1[i],ref2[j])+K(i,j));}
834 }
835 }
836 }
837 */
BToMatrix()838 Matrix Matrix::BToMatrix()
839 {
840 Matrix m(rows,rows);
841 for (int i=1; i<=rows; i++)
842 {
843 for (int j=1; j<=rows; j++)
844 {
845 m(i,j)=BGet(i,j);
846 }
847 }
848 return m;
849 }
850
MatrixToB()851 Matrix Matrix::MatrixToB()
852 {
853 int bw = GetBandWidth()+1;
854 Matrix m(rows,bw);
855
856 for (int i=1; i<=rows; i++)
857 {
858 for (int j=1; j<=bw; j++)
859 {
860 if (j+i-1 <= cols)
861 m.Bset(i,j+i-1,(*this)(i,j+i-1));
862 }
863 }
864 return m;
865 }
866
GetColVec(int col) const867 Vector Matrix::GetColVec(int col) const
868 {
869 #ifndef __QUICKMATH
870 release_assert((col>0) && (col<=cols));
871 #endif
872 Vector v(rows);
873 int end=(rows-1)*(cols)+col-1;
874 int j=1;
875 for (int i=col-1;i<=end;i+=cols)
876 {
877 v(j++)=mat[i];
878 }
879 return v;
880 }
881
GetRowVec(int row) const882 Vector Matrix::GetRowVec(int row) const
883 {
884 #ifndef __QUICKMATH
885 release_assert((row>0) && (row<=rows));
886 #endif
887 Vector v(cols);
888 int offset=(row-1)*cols-1;
889 for (int i=1;i<=cols;i++)
890 {
891 v(i)=mat[offset+i];
892 }
893 return v;
894 }
895
GetColVec(int col,Vector & v) const896 void Matrix::GetColVec(int col, Vector& v) const
897 {
898 #ifndef __QUICKMATH
899 release_assert((col>0) && (col<=cols));
900 #endif
901 v.SetLen(rows);
902 int end=(rows-1)*(cols)+col-1;
903 int j=1;
904 for (int i=col-1;i<=end;i+=cols)
905 {
906 v(j++)=mat[i];
907 }
908 }
909
GetRowVec(int row,Vector & v) const910 void Matrix::GetRowVec(int row, Vector& v) const
911 {
912 #ifndef __QUICKMATH
913 release_assert((row>0) && (row<=rows));
914 #endif
915 v.SetLen(cols);
916 int offset=(row-1)*cols-1;
917 for (int i=1;i<=cols;i++)
918 {
919 v(i)=mat[offset+i];
920 }
921 }
922
Det() const923 double Matrix::Det() const
924 {
925 #ifndef __QUICKMATH
926 release_assert(IsSquare());
927 #endif
928 if (cols==1) {return mat[0];}
929 if (cols==2) {return mat[0]*mat[3]-mat[1]*mat[2];}
930 if (cols==3) {return mat[0]*mat[4]*mat[8]-mat[0]*mat[5]*mat[7]-mat[3]*mat[1]*mat[8]
931 +mat[3]*mat[2]*mat[7]+mat[6]*mat[1]*mat[5]-mat[6]*mat[2]*mat[4];}
932 #ifndef __QUICKMATH
933 release_assert(cols>3);
934 #endif
935 return 0;
936 }
937
938 //Returns the diagonal values
GetDiag() const939 Vector Matrix::GetDiag() const
940 {
941 #ifndef __QUICKMATH
942 release_assert(IsSquare());
943 #endif
944
945 Vector ev(cols);
946
947 for (int i=1; i<= cols; i++)
948 {
949 ev(i) = Get(i,i);
950 }
951 return ev;
952 }
953
Trace() const954 double Matrix::Trace() const
955 {
956 #ifndef __QUICKMATH
957 release_assert(IsSquare());
958 #endif
959
960 double sum=0;
961
962 for (int i=1; i<= cols; i++)
963 {
964 sum += Get(i,i);
965 }
966 return sum;
967 }
968
SetColVec(const Vector & x,int col)969 void Matrix::SetColVec(const Vector& x, int col)
970 {
971 #ifndef __QUICKMATH
972 release_assert((col>0) && (col<=cols));
973 #endif
974 int end=(rows-1)*(cols)+col-1;
975 int j=1;
976 for (int i = col-1; i <= end; i += cols)
977 {
978 mat[i]=x(j++);
979 }
980 }
981
982 //Sets the column-vector at clomn col, taken from Matrix m, column mcol
SetColVec(const Matrix & m,int mcol,int col)983 void Matrix::SetColVec(const Matrix& m, int mcol, int col)
984 {
985 #ifndef __QUICKMATH
986 release_assert((col>0) && (col<=cols) && (mcol <= m.cols) && (cols == m.cols));
987 #endif
988
989 int end=(rows-1)*(cols)+col-1;
990 int diff = mcol-col;
991 for (int i = col-1; i <= end; i += cols)
992 {
993 mat[i]=m.mat[i+diff];
994 }
995 }
996
SetRowVec(const Vector & x,int row)997 void Matrix::SetRowVec(const Vector& x, int row)
998 {
999 #ifndef __QUICKMATH
1000 release_assert((row>0) && (row <= rows));
1001 #endif
1002 int offset = (row-1)*cols-1;
1003 for (int i = 1;i <= cols; i++)
1004 {
1005 mat[offset+i] = x(i);
1006 }
1007 }
1008
MultCol(const Matrix & m,int mcol,int col)1009 double Matrix::MultCol(const Matrix& m, int mcol, int col)
1010 {
1011 #ifndef __QUICKMATH
1012 release_assert((col>0) && (col<=cols) && (mcol <= m.cols) && (cols == m.cols));
1013 #endif
1014
1015 double val = 0;
1016 int end=(rows-1)*(cols)+col-1;
1017 int diff = mcol-col;
1018
1019 for (int i = col-1; i <= end; i += cols)
1020 {
1021 val += mat[i]*m.mat[i+diff];
1022 }
1023 return val;
1024 }
1025
GetBandWidth() const1026 int Matrix::GetBandWidth() const
1027 {
1028 if (!IsSquare()) {return rows;}
1029 int bw=0; //minimal, wenn Nullmatrix
1030 for (int i=1;i<=rows;i++)
1031 {
1032 for (int j=0;j<=rows-i;j++)
1033 {
1034 if ((*this)(j+1,i+j)!=0 ||
1035 (*this)(i+j,j+1)!=0) {bw=i;}
1036 }
1037 }
1038 return bw;
1039 }
1040
IsSymmetric()1041 int Matrix::IsSymmetric()
1042 {
1043 #ifndef __QUICKMATH
1044 release_assert(IsSquare());
1045 #endif
1046
1047 for (int i=0; i<rows; i++)
1048 {
1049 for (int j=0; j<i; j++)
1050 {
1051 if (mat[i*cols+j]!=mat[j*cols+i]) {return 0;}
1052 }
1053 }
1054 return 1;
1055 }
1056
MakeSymmetric()1057 void Matrix::MakeSymmetric()
1058 {
1059 #ifndef __QUICKMATH
1060 release_assert(IsSquare());
1061 #endif
1062
1063 double x;
1064 for (int i=0; i<rows; i++)
1065 {
1066 for (int j=0; j<i; j++)
1067 {
1068 x = 0.5*(mat[i*cols+j] + mat[j*cols+i]);
1069 mat[i*cols+j] = x;
1070 mat[j*cols+i] = x;
1071 }
1072 }
1073 }
1074
IsSymmetric(double eps)1075 int Matrix::IsSymmetric(double eps)
1076 {
1077 #ifndef __QUICKMATH
1078 release_assert(IsSquare());
1079 #endif
1080
1081 for (int i=0; i<rows; i++)
1082 {
1083 for (int j=0; j<i; j++)
1084 {
1085 if (fabs(mat[i*cols+j] - mat[j*cols+i]) > eps) {return 0;}
1086 }
1087 }
1088 return 1;
1089 }
1090
1091
TpYs()1092 void Matrix::TpYs()
1093 {
1094 if (IsSquare())
1095 {
1096 for (int i=0; i<rows; i++)
1097 {
1098 for (int j=0; j<i; j++)
1099 {
1100 Swap(mat[i*cols+j],mat[j*cols+i]);
1101 }
1102 }
1103 }
1104 else
1105 {
1106 Matrix mt(cols,rows);
1107
1108 for (int i=0; i<rows; i++)
1109 {
1110 for (int j=0; j<cols; j++)
1111 {
1112 mt.Elem0(j,i) = Get0(i,j);
1113 //mt.mat[j*rows+i]=mat[i*cols+j];
1114 }
1115 }
1116 //CopyFrom(mt,1,1,cols,rows);
1117 *this = mt;
1118 }
1119 }
1120
GetTp() const1121 Matrix Matrix::GetTp() const
1122 {
1123 Matrix mt(cols,rows);
1124
1125 for (int i=0; i<rows; i++)
1126 {
1127 for (int j=0; j<cols; j++)
1128 {
1129 mt.mat[j*rows+i]=mat[i*cols+j];
1130 }
1131 }
1132 return mt;
1133 }
1134
1135 //$ SW 2013-10-9: added
1136 //calculates *= A where A is a square matrix (e.g. a rotation matrix) of dimension 3x3, 2x2 or 1x1
ApplySqrMat(const MatrixXD & A)1137 void Matrix::ApplySqrMat(const MatrixXD &A)
1138 {
1139 #ifndef __QUICKMATH
1140 release_assert("ERROR: ApplySqrMat dimensions wrong" && (cols==A.Getrows() && A.Getrows()==A.Getcols()));
1141 #endif
1142 if (A.Getrows()==3)
1143 {
1144 double tmp1,tmp2,tmp3;
1145 for (int i=0; i<rows; i++)
1146 {
1147 tmp1= mat[i*3]*A(1,1)+mat[i*3+1]*A(2,1)+mat[i*3+2]*A(3,1);
1148 tmp2= mat[i*3]*A(1,2)+mat[i*3+1]*A(2,2)+mat[i*3+2]*A(3,2);
1149 tmp3= mat[i*3]*A(1,3)+mat[i*3+1]*A(2,3)+mat[i*3+2]*A(3,3);
1150 mat[i*3]= tmp1;
1151 mat[i*3+1]= tmp2;
1152 mat[i*3+2]= tmp3;
1153 }
1154 return;
1155 }
1156 if (A.Getrows()==2)
1157 {
1158 double tmp1,tmp2;
1159 for (int i=0; i<rows; i++)
1160 {
1161 tmp1= mat[i*2]*A(1,1)+mat[i*2+1]*A(2,1);
1162 tmp2= mat[i*2]*A(1,2)+mat[i*2+1]*A(2,2);
1163 mat[i*2]= tmp1;
1164 mat[i*2+1]= tmp2;
1165 }
1166 return;
1167 }
1168 if (A.Getrows()==1)
1169 {
1170 operator*=(A(1,1));
1171 return;
1172 }
1173 #ifndef __QUICKMATH
1174 release_assert("ERROR: ApplySqrMat works only for matrices of dimension 1, 2 or 3" && false);
1175 #endif
1176 }
1177
operator +=(const Matrix & m)1178 Matrix& Matrix::operator+= (const Matrix& m)
1179 {
1180 #ifndef __QUICKMATH
1181 release_assert(m.rows==rows && m.cols==cols);
1182 #endif
1183 int size=cols*rows;
1184 for (int i=0;i<size;i++)
1185 {
1186 mat[i]+=m.mat[i];
1187 }
1188 return *this;
1189 }
1190
operator -=(const Matrix & m)1191 Matrix& Matrix::operator-= (const Matrix& m)
1192 {
1193 #ifndef __QUICKMATH
1194 release_assert(m.rows==rows && m.cols==cols);
1195 #endif
1196 int size=cols*rows;
1197 for (int i=0;i<size;i++)
1198 {
1199 mat[i]-=m.mat[i];
1200 }
1201 return *this;
1202 }
1203
operator *=(const double & val)1204 Matrix& Matrix::operator*= (const double& val)
1205 {
1206 int size=cols*rows;
1207 for (int i=0;i<size;i++)
1208 {
1209 mat[i]*=val;
1210 }
1211 return *this;
1212 }
1213
AddSubmatrix(const Matrix & sm,int sr,int sc,int r,int c,int nr,int nc,double x)1214 void Matrix::AddSubmatrix(const Matrix& sm, int sr, int sc, int r, int c, int nr, int nc, double x)
1215 {
1216 int off = -cols+c-1;
1217 for(int i=0; i<nr; i++)
1218 {
1219 for(int j=0; j<nc; j++)
1220 {
1221 mat[(i+r)*cols+j+off]+=x*sm(i+sr,j+sc);
1222 //Elem(i+r,j+c)+=x*sm(i+sr,j+sc);
1223 }
1224 }
1225 }
1226
SetSubmatrix(const Matrix & sm,int r,int c)1227 void Matrix::SetSubmatrix(const Matrix& sm, int r, int c)
1228 {
1229 int off = -cols+c-1;
1230 for(int i=0; i<sm.rows; i++)
1231 {
1232 for(int j=0; j<sm.cols; j++)
1233 {
1234 mat[(i+r)*cols+j+off] = sm.Get0(i,j);
1235 }
1236 }
1237 }
1238
SetSubmatrix(const Matrix & sm,int r,int c,double x)1239 void Matrix::SetSubmatrix(const Matrix& sm, int r, int c, double x)
1240 {
1241 int off = -cols+c-1;
1242 for(int i=0; i<sm.rows; i++)
1243 {
1244 for(int j=0; j<sm.cols; j++)
1245 {
1246 mat[(i+r)*cols+j+off] = x*sm.Get0(i,j);
1247 }
1248 }
1249 }
1250
SetSubmatrix(const Matrix3D & sm,int r,int c)1251 void Matrix::SetSubmatrix(const Matrix3D& sm, int r, int c)
1252 {
1253 int off = -cols+c-1;
1254 for(int i=0; i<sm.Getrows(); i++)
1255 {
1256 for(int j=0; j<sm.Getcols(); j++)
1257 {
1258 mat[(i+r)*cols+j+off] = sm.Get0(i,j);
1259 //=>mat[(i+r-1)*cols+c+j-1]+=x*sm(i,j);
1260 //=>Elem(i+r,j+c)+=x*sm(i+sr,j+sc);
1261 }
1262 }
1263 }
1264
MulCol(int col,double val)1265 void Matrix::MulCol(int col, double val)
1266 {
1267 #ifndef __QUICKMATH
1268 release_assert((col>0) && (col<=cols));
1269 #endif
1270
1271 int end=(rows-1)*(cols)+col-1;
1272 for (int i=col-1;i<=end;i+=cols) {mat[i]*=val; }
1273 }
1274
MulRow(int row,double val)1275 void Matrix::MulRow(int row, double val)
1276 {
1277 #ifndef __QUICKMATH
1278 release_assert((row>0) && (row<=rows));
1279 #endif
1280 for (int i=(row-1)*cols;i<=row*cols-1;i++) {mat[i]*=val; }
1281 }
1282
InsertMatrix(int row,int col,const Matrix & m)1283 void Matrix::InsertMatrix(int row, int col, const Matrix& m)
1284 {
1285 #ifndef __QUICKMATH
1286 release_assert((row>0) && (col>0) && (row+m.rows-1<=rows) && (col+m.cols-1<=cols));
1287 #endif
1288 int i,j;
1289 for (i = 1; i <= m.rows; i++)
1290 {
1291 for (j = 1; j <= m.cols; j++)
1292 {
1293 Elem(row+i-1,col+j-1) = m(i,j);
1294 }
1295 }
1296 }
1297
InsertMatrix(int row,int col,const Matrix & m,int fromrow,int fromcol,int mrows,int mcols)1298 void Matrix::InsertMatrix(int row, int col, const Matrix& m, int fromrow, int fromcol, int mrows, int mcols)
1299 {
1300 #ifndef __QUICKMATH
1301 release_assert((row>0) && (col>0) && (row+mrows-1<=rows) && (col+mcols-1<=cols) && (mrows<=m.rows) && (mcols<=m.cols));
1302 #endif
1303 int i,j;
1304 for (i = 1; i <= mrows; i++)
1305 {
1306 for (j = 1; j <= mcols; j++)
1307 {
1308 Elem(row+i-1,col+j-1) = m(fromrow-1+i,fromcol-1+j);
1309 }
1310 }
1311 }
1312
1313
AddMatrix(int row,int col,const Matrix & m)1314 void Matrix::AddMatrix(int row, int col, const Matrix& m)
1315 {
1316 #ifndef __QUICKMATH
1317 release_assert((row>0) && (col>0) && (row+m.rows-1<=rows) && (col+m.cols-1<=cols));
1318 #endif
1319 int i,j;
1320 for (i = 1; i <= m.rows; i++)
1321 {
1322 for (j = 1; j <= m.cols; j++)
1323 {
1324 Elem(row+i-1,col+j-1) += m(i,j);
1325 }
1326 }
1327 }
1328
AddMatrix(const TArray<int> & rowref,const TArray<int> & colref,const Matrix & m)1329 void Matrix::AddMatrix(const TArray<int>& rowref, const TArray<int>& colref, const Matrix& m)
1330 {
1331 int i,j;
1332 int k;
1333 for (i = 0; i < m.rows; i++)
1334 {
1335 k = (rowref.Get0(i)-1)*cols;
1336 for (j = 0; j < m.cols; j++)
1337 {
1338 mat[k+colref.Get0(j)-1] += m.Get0(i,j);
1339 }
1340 }
1341 }
1342
AddRowVec(int row,const Vector & vec)1343 void Matrix::AddRowVec(int row, const Vector& vec)
1344 {
1345 #ifndef __QUICKMATH
1346 release_assert((row>0) && (row<=rows) && vec.GetLen()==cols);
1347 #endif
1348 int j=1;
1349 for (int i=(row-1)*cols;i<=row*cols-1;i++)
1350 {
1351 mat[i]+=vec(j++);
1352 }
1353 }
1354
AddColVec(int col,const Vector & vec)1355 void Matrix::AddColVec(int col, const Vector& vec)
1356 {
1357 #ifndef __QUICKMATH
1358 release_assert((col>0) && (col<=cols) && vec.GetLen()==rows);
1359 #endif
1360
1361 int end=(rows-1)*(cols)+col-1;
1362 int j=1;
1363 for (int i = col-1; i <= end; i += cols)
1364 {
1365 mat[i] += vec(j++);
1366 }
1367 }
1368
1369 //Adds fromcol multiplied with fact to column toCol
AddColVec(int fromCol,int toCol,double fact)1370 void Matrix::AddColVec(int fromCol, int toCol, double fact)
1371 {
1372 #ifndef __QUICKMATH
1373 release_assert((fromCol>0) && (fromCol<=cols) && (toCol > 0) && (toCol <= cols));
1374 #endif
1375
1376 int end=(rows-1)*(cols)+toCol-1;
1377 int diff = fromCol-toCol;
1378
1379 for (int i = toCol-1; i <= end; i += cols)
1380 {
1381 mat[i] += fact*mat[i+diff];
1382 }
1383 }
1384
AddRowVec(int fromRow,int toRow,double Fact)1385 void Matrix::AddRowVec(int fromRow, int toRow, double Fact)
1386 {
1387 #ifndef __QUICKMATH
1388 release_assert((toRow>0) && (toRow<=rows) &&
1389 (fromRow>0) && (fromRow<=rows));
1390 #endif
1391 int j=(fromRow-1)*cols-(toRow-1)*cols;
1392 int end=toRow*cols-1;
1393 for (int i=(toRow-1)*cols;i<=end;i++)
1394 {
1395 mat[i]+=Fact*mat[j+i];
1396 }
1397 }
1398
AddRowVec(int fromRow,int toRow,double Fact,int fromCol,int toCol)1399 void Matrix::AddRowVec(int fromRow, int toRow, double Fact, int fromCol, int toCol)
1400 {
1401 #ifndef __QUICKMATH
1402 release_assert((toRow>0) && (toRow<=rows) &&
1403 (fromRow>0) && (fromRow<=rows));
1404 #endif
1405
1406 int j=(fromRow-1)*cols-(toRow-1)*cols;
1407 int end=(toRow-1)*cols+toCol-1;
1408 for (int i=(toRow-1)*cols+fromCol-1;i<=end;i++)
1409 {
1410 mat[i]+=Fact*mat[j+i];
1411 }
1412 }
1413
AddRowVecB(int fromRow,int toRow,const double & Fact,int bw)1414 void Matrix::AddRowVecB(int fromRow, int toRow, const double& Fact, int bw)
1415 {
1416 #ifndef __QUICKMATH
1417 release_assert((toRow>0) && (toRow<=rows) &&
1418 (fromRow>0) && (fromRow<=rows));
1419 #endif
1420 int j=(fromRow-1)*cols-(toRow-1)*cols;
1421 int minRow = Minimum(fromRow, toRow);
1422 int maxRow = Maximum(fromRow, toRow);
1423 int start = (toRow-1)*cols + Maximum(minRow-bw,0);
1424 int end = toRow*cols-1 - Maximum(cols-maxRow-bw,0);
1425 //cout << "save=" << Maximum(minRow-bw,0)+Maximum(cols-maxRow-bw,0) << endl;
1426 for (int i = start; i <= end; i++)
1427 {
1428 mat[i]+=Fact*mat[j+i];
1429 }
1430 }
1431
SwapRows(int r1,int r2)1432 void Matrix::SwapRows(int r1, int r2)
1433 {
1434 if (r1==r2) {return;}
1435 #ifndef __QUICKMATH
1436 release_assert((r1>0) && (r1<=rows) && (r2>0) && (r2<=rows));
1437 #endif
1438 for (int i=1; i<=cols; i++) {Swap((*this)(r1,i),(*this)(r2,i));}
1439 }
1440
SwapCols(int c1,int c2)1441 void Matrix::SwapCols(int c1, int c2)
1442 {
1443 if (c1==c2) {return;}
1444 #ifndef __QUICKMATH
1445 release_assert((c1>0) && (c1<=cols) && (c2>0) && (c2<=cols));
1446 #endif
1447 for (int i=1; i<=rows; i++) {Swap((*this)(i,c1),(*this)(i,c2));}
1448 }
1449
1450 //slow convert doubles matrix ....
1451 //returns 1, if sucsess
Invert()1452 int Matrix::Invert()
1453 {
1454 //f�r 2*2 Matrix optimierter L�sungsalgorithmus
1455 if (cols==1 && rows==1)
1456 {
1457 if (mat[0] == 0)
1458 {
1459 return 0;
1460 }
1461 mat[0] = 1/mat[0];
1462 return 1;
1463 }
1464
1465 if (cols==2 && rows==2)
1466 {
1467 double det=mat[0]*mat[3]-mat[1]*mat[2];
1468 if (det==0) {return 0;}
1469 det=1/det;
1470 Swap(mat[0],mat[3]);
1471 mat[0]*=det;
1472 mat[1]*=-det;
1473 mat[2]*=-det;
1474 mat[3]*=det;
1475 return 1;
1476 }
1477 //TMStartTimer(9);
1478
1479 #ifndef __QUICKMATH
1480 release_assert(cols==rows && cols*rows!=0 && mat!=NULL);
1481 #endif
1482
1483 //Insert identity-matrix on left-hand-side
1484 mystatic Matrix m;
1485 m.SetSize(rows,cols*2);
1486
1487 int i,j,k;
1488 for (j=1; j<=rows; j++)
1489 {
1490 for (i=1; i<=cols; i++) {m(j,i)=(*this)(j,i);}
1491 for (i=cols+1; i<=2*cols; i++)
1492 {
1493 if (i-cols==j) {m(j,i)=1;}
1494 else {m(j,i)=0;};
1495 }
1496 }
1497
1498 // Solve lower triangular Matrix
1499
1500 for (j=1; j<=cols; j++)
1501 {
1502 double pivot=fabs(m(j,j));
1503 int pivotpos=j;
1504 for (k=j+1; k<=rows; k++)
1505 {
1506 if (fabs(m(k,j)) > pivot) {pivotpos=k; pivot=fabs(m(k,j));}
1507 }
1508 //cout << "Pivot = " << pivot << endl;
1509 if (pivot == 0)
1510 {
1511 //(*global_uo) << "Invert: problems with column " << j << "\n";
1512 //TMStopTimer(9);
1513 return 0;
1514 }
1515
1516 m.SwapRows(pivotpos,j);
1517 m.MulRow(j,1/m(j,j));
1518 double mij;
1519 for (i=j+1; i<=rows; i++)
1520 {
1521 mij = m(i,j);
1522 if (mij !=0)
1523 {
1524 // m.AddRowVec(j,i,-mij);
1525 m.AddRowVec(j,i,-mij);
1526 }
1527 }
1528 //#ifdef __sgi
1529 if (cols >= 500 && j%50==0)
1530 {
1531 if (j%500==0) cout << j << flush;
1532 else cout << "." << flush;
1533 }
1534 //#endif
1535
1536 }
1537 for (j=cols-1; j>=1; j--)
1538 {
1539 for (i=1; i<=j; i++)
1540 {
1541 if (m(i,j+1)!=0)
1542 {
1543 m.AddRowVec(j+1,i,-m(i,j+1));
1544 }
1545 }
1546 }
1547
1548 // remove unity-matrix on the right-hand-side
1549 for (j=1; j<=rows; j++)
1550 {
1551 for (i=1; i<=cols; i++)
1552 {
1553 (*this)(j,i)=m(j,i+cols);
1554 }
1555 }
1556
1557 //cout << "Invertierte-Matrix: " << m << endl;
1558
1559 //TMStopTimer(9);
1560
1561 return 1;
1562 }
1563
1564 //returns 1, if sucsess
Invert2()1565 int Matrix::Invert2()
1566 {
1567 if (rows*cols == 0) return 1;
1568 // TMStartTimer(9);
1569 #ifndef __QUICKMATH
1570 release_assert(cols==rows && mat!=NULL);
1571 #endif
1572
1573 //Insert identity-matrix on left-hand-side
1574 mystatic Matrix m;
1575 m.SetSize(rows,cols);
1576 m.FillWithZeros();
1577
1578 double mij;
1579 int i,j,k;
1580 int n=rows;
1581 int maxj=0;
1582
1583
1584 for (j=1; j<=n; j++)
1585 {
1586 m(j,j) = 1;
1587 }
1588
1589 // Solve lower triangular Matrix
1590
1591 for (j=1; j<=n; j++)
1592 {
1593 double pivot=fabs(Get(j,j));
1594 int pivotpos=j;
1595 for (k=j+1; k<=n; k++)
1596 {
1597 if (fabs(Get(k,j)) > pivot) {pivotpos=k; pivot=fabs(Get(k,j));}
1598 }
1599 if (pivot == 0)
1600 {
1601 //(*global_uo) << "invert:problems with pivot in col " << j << "\n";
1602 //(*global_uo) << "m=\n" << *this << "\n";
1603 //TMStopTimer(9);
1604 return 0;
1605 }
1606
1607 maxj=Maximum(pivotpos,maxj);
1608
1609 m.SwapRows(pivotpos,j);
1610 SwapRows(pivotpos,j);
1611 m.MulRow(j,1/Get(j,j));
1612 MulRow(j,1/Get(j,j));
1613
1614 for (i=j+1; i<=n; i++)
1615 {
1616 mij = Get(i,j);
1617 if (mij !=0)
1618 {
1619 AddRowVec(j,i,-mij,j,n); //j..n
1620 m.AddRowVec(j,i,-mij,1,maxj); //1..j
1621 }
1622 }
1623
1624 }
1625 //(*global_uo) << "m1=\n"; PrintMatrix01(*this);
1626
1627 //backsubstitution, unfortunately this takes most of the time!!!
1628 for (j=n-1; j>=1; j--)
1629 {
1630 for (i=1; i<=j; i++)
1631 {
1632 mij = Get(i,j+1);
1633 if (mij!=0)
1634 {
1635 m.AddRowVec(j+1,i,-mij); //1..n
1636 }
1637 }
1638 }
1639 //(*global_uo) << "m2=\n"; PrintMatrix01(*this);
1640
1641 *this = m;
1642
1643 //(*global_uo) << "Jac_inv=\n"; PrintMatrix01(m);
1644
1645 //TMStopTimer(9);
1646 return 1;
1647 }
1648
Solve(const Vector & fv,Vector & q)1649 int Matrix::Solve(const Vector& fv, Vector& q)
1650 {
1651 #ifndef __QUICKMATH
1652 release_assert(fv.GetLen()==rows && cols==rows && cols*rows!=0 && mat!=NULL);
1653 #endif
1654 mystatic Matrix m;
1655 mystatic Vector f;
1656
1657 m=*this;
1658 //Matrix mstore = *this;
1659 f=fv;
1660
1661 if (f.GetLen() == 1)
1662 {
1663 if (mat[0] == 0) {return 0;}
1664 q(1) = f(1)/mat[0];
1665 return 1;
1666 }
1667 TMStartTimer(11);
1668
1669 int i,j,k;
1670 // Solve lower triangular matrix
1671 for (j=1; j<=cols; j++)
1672 {
1673 double pivot=fabs(m(j,j));
1674 int pivotpos=j;
1675 for (k=j+1; k<=rows; k++)
1676 {
1677 if (fabs(m(k,j)) > pivot) {pivotpos=k; pivot=fabs(m(k,j));}
1678 }
1679 if (pivot == 0)
1680 {
1681 (*global_uo) << "Invert: problems with column " << j << "\n";
1682 //(*global_uo) << *this << "\n";
1683 TMStopTimer(11);
1684 return 0;
1685 }
1686
1687 m.SwapRows(pivotpos,j); Swap(f(pivotpos),f(j));
1688
1689 f(j)*=1/m(j,j); //diese Zeile mu� vor MulRow(j,1/m(j,j)) stehen!!!
1690 m.MulRow(j,1/m(j,j));
1691
1692 for (i=j+1; i<=rows; i++)
1693 {
1694 if (m(i,j)!=0)
1695 {
1696 f(i)+=-m(i,j)*f(j);
1697 m.AddRowVec(j,i,-m(i,j));
1698 }
1699 }
1700 // if (j>50 && j%50==1) {lout << "Gauss-Pivot-Solve: " << j << " Zeilen von " << rows << "\n" << flush;}
1701 }
1702
1703 //q.SetAll(0); //initialized with zeros
1704 for (j=rows; j>=1; j--)
1705 {
1706 q(j)=f(j);
1707 double sum = 0;
1708 for (int i=j+1; i<=rows; i++)
1709 {
1710 sum += m(j,i)*q(i);
1711 }
1712 q(j) -= sum;
1713 }
1714 // if (rows>50) {lout << "Gauss-Pivot-Solve: ready\n" << flush;}
1715 TMStopTimer(11);
1716 return 1;
1717 }
1718
1719
SolveLapack(Vector & q)1720 int Matrix::SolveLapack(Vector& q)
1721 {
1722 mystatic Matrix m;
1723 m=*this;
1724 m.TpYs();
1725 int info = LapackSolve(rows, 1, &m(1,1), &q(1));
1726 if (!info) return 1;
1727 else return 0;
1728 }
1729
InvertLapack()1730 int Matrix::InvertLapack()
1731 {
1732 mystatic Matrix m;
1733 m=*this;
1734 int info = LapackInvert(rows, &m(1,1), mat);
1735 if (!info) return 1;
1736 else return 0;
1737 }
1738
1739 //$ PG 2013-10-30: function estimates reciprocal of the condition number via LAPACK, returns 0 on success!!!
EstimateReciprocalConditionNumber(double & rcond) const1740 int Matrix::EstimateReciprocalConditionNumber(double& rcond) const
1741 {
1742 #ifndef __QUICKMATH
1743 release_assert(cols==rows && cols*rows!=0); //only for sqare matrices
1744 #endif
1745
1746 int info = LapackEstimateReciprocalConditionNumber(rows, GetTp().GetMatPtr(), &rcond);
1747 return info;
1748 }
1749
1750 //$ PG 2013-10-30: used for solving an undetermined system (underdetermined: minimum norm solution |q|->min subject to Aq=f; overdetermined: least squares solution |Aq-f|^2->min)
1751 // f (input) right hand side
1752 // q (output) solution
1753 // rcond (input) reciprocal of estimated condition number of A (this matrix), used for recognizing linearly dependendt equations numerically
1754 // rank (output) computed rank of matrix A, depending on rcond.
UndeterminedSystemSolve(const Vector & f,Vector & q,double rcond,int & rank)1755 int Matrix::UndeterminedSystemSolve(const Vector& f, Vector& q, double rcond, int& rank)
1756 {
1757 #ifndef __QUICKMATH
1758 release_assert(f.GetLen()==rows && cols*rows!=0);
1759 #endif
1760
1761 double* m = new double[rows*cols];
1762 int offset = 0;
1763 for (int i=0; i<cols; i++)
1764 {
1765 for (int j=0; j<rows; j++)
1766 {
1767 m[offset + j] = this->mat[j*cols + i];
1768 }
1769 offset += rows;
1770 }
1771
1772 int N = max(rows,cols);
1773
1774 q.SetLen(N);
1775 q.Copy(f,1,1,rows);
1776 q.SetAll(0.,rows+1,N); // if rows == N, nothing is done here
1777
1778 int info = LapackUndeterminedSystemSolve(rows, cols, 1, m, &(q(1)), rcond, rank);
1779
1780 delete[] m;
1781
1782 if (!info) return 1;
1783 else return 0;
1784 }
1785
1786 //$ PG 2013-10-2: [ modified function Matrix::LU in order not to use source code, which is not compatible with the HOTINT licence.
1787 //$ PG 2013-10-2: browse the HOTINT CVS repository for older versions of linalg.cpp and linalg.h prior to 2013-10-2 in order to recover former functionality. ]
LU(IVector & indx)1788 int Matrix::LU(IVector& indx)
1789 {
1790 mystatic Matrix m;
1791 m=(*this).GetTp();
1792
1793 indx.SetLen(min(cols,rows));
1794 int info = LapackLU(rows, cols, &m(1,1), indx.GetDataPtr());
1795 *this=m.GetTp();
1796
1797 if (!info)
1798 return 1;
1799
1800 return 0;
1801 }
1802 //$ PG 2013-10-2: ]
1803
1804
LUBCKSUB(IVector & indx,Vector & b)1805 int Matrix::LUBCKSUB(IVector& indx, Vector& b)
1806 {
1807 //void lubksb(float **a, int n, int *indx, float b[])
1808 //Solves the set of n linear equations A�X = B. Here a[1..n][1..n] is input, not as the matrix
1809 //A but rather as its LU decomposition, determined by the routine ludcmp. indx[1..n] is input
1810 //as the permutation vector returned by ludcmp. b[1..n] is input as the right-hand side vector
1811 //B, and returns with the solution vector X. a, n, and indx are not modified by this routine
1812 //and can be left in place for successive calls with different right-hand sides b. This routine takes
1813 //into account the possibility that b will begin with many zero elements, so it is efficient for use
1814 //in matrix inversion.
1815
1816 //(*global_uo) << "indx=" << indx << "\n";
1817 //(*global_uo) << "b1=" << b << "\n";
1818
1819 int i,ii=0,ip,j;
1820 int n=rows;
1821 double sum;
1822 for (i=1;i<=n;i++)
1823 {
1824 /*
1825 if (i != indx(i))
1826 Swap(b(i),b(indx(i)));
1827
1828 if (ii)
1829 for (j=ii;j<=i-1;j++) b(i) -= Get(i,j)*b(j);
1830 else if (b(i)) ii=i; */
1831
1832 ip=indx(i);
1833 sum=b(ip);
1834 b(ip)=b(i);
1835 if (ii)
1836 for (j=ii;j<=i-1;j++) sum -= Get(i,j)*b(j);
1837 else if (sum) ii=i;
1838 b(i)=sum;
1839 }
1840 //(*global_uo) << "b2=" << b << "\n";
1841
1842 if (ii) //if whole vector b=0 --> solution: b=0 ...
1843 {
1844 for (i=n;i>=1;i--)
1845 {
1846 sum=b(i);
1847 for (j=i+1;j<=n;j++)
1848 sum -= Get(i,j)*b(j);
1849 if (Get(i,i)==0) return 0;
1850 b(i)=sum/Get(i,i);
1851 }
1852 }
1853 //(*global_uo) << "b3=" << b << "\n";
1854 return 1;
1855 }
1856
Bsolve(Vector f,Vector & q)1857 int Matrix::Bsolve(Vector f, Vector& q)
1858 {
1859 #ifndef __QUICKMATH
1860 release_assert(f.GetLen()==rows && cols==rows && cols*rows!=0 && mat!=NULL);
1861 #endif
1862 Matrix m=*this;
1863
1864 int bw = GetBandWidth();
1865
1866 int i,j,k;
1867 //Solve lower triangular matrix
1868 for (j=1; j<=cols; j++)
1869 {
1870 double pivot=fabs(m(j,j));
1871 int pivotpos=j;
1872 for (k=j+1; k<=rows; k++)
1873 {
1874 if (fabs(m(k,j)) > pivot) {pivotpos=k; pivot=fabs(m(k,j));}
1875 }
1876 if (pivot == 0)
1877 {
1878 (*global_uo) << "Invert: problems with column " << j << "\n"; return 0;
1879 }
1880
1881 m.SwapRows(pivotpos,j); Swap(f(pivotpos),f(j));
1882
1883 f(j)*=1/m(j,j); //diese Zeile mu� vor MulRow(j,1/m(j,j)) stehen!!!
1884 m.MulRow(j,1/m(j,j));
1885
1886 for (i=j+1; i<=rows; i++)
1887 {
1888 if (m(i,j)!=0)
1889 {
1890 f(i)+=-m(i,j)*f(j);
1891 m.AddRowVecB(j,i,-m(i,j),bw);
1892 }
1893 }
1894 // if (j>50 && j%50==1) {lout << "Gauss-Pivot-Solve: " << j << " Zeilen von " << rows << "\n" << flush;}
1895 }
1896
1897 q=Vector(rows); //initialized with zeros
1898 for (j=rows; j>=1; j--)
1899 {
1900 q(j)=f(j)-m.GetRowVec(j)*q;
1901 }
1902 // if (rows>50) {lout << "Gauss-Pivot-Solve: ready\n" << flush;}
1903 return 1;
1904 }
1905
CholeskyBsolve(Vector f,Vector & q)1906 int Matrix::CholeskyBsolve(Vector f, Vector& q)
1907 {
1908 //(*fout) << "size of B-Matrix = " << rows*cols*8 << " bytes \n";
1909
1910 #ifndef __QUICKMATH
1911 release_assert(f.GetLen()==rows && cols*rows!=0 && mat!=NULL);
1912 #endif
1913 Matrix m=*this;
1914
1915 int i,j,k;
1916
1917 // generate upper right triangulat matrix (LT)
1918 for (k=1; k<=rows; k++)
1919 {
1920 if (m(k,1)<=0) {(*global_uo) << "Invert: problems with row " << k << "\n"; return 0;}
1921 double lkkinv=1/sqrt(m(k,1));
1922 for (i=1; i<=cols; i++)
1923 {
1924 m(k,i)*=lkkinv;
1925 }
1926 for (i=1; (i<=cols && i+k<=rows); i++)
1927 {
1928 for (j=1; j<=cols-i; j++)
1929 {
1930 m(i+k,j)-=m(k,i+1)*m(k,j+i);
1931 }
1932 }
1933 // if (k>200 && k%200==1) {lout << "CholeskySolve: " << k << " Zeilen von " << rows << "\n" << flush;}
1934 }
1935
1936 Vector c(rows); //mot 0n init.
1937
1938 //L*c=f f�r c l�sen: (m=LT !!!)
1939 for (j=1; j<=rows; j++)
1940 {
1941 int stoff=0;
1942 if (j>cols) {stoff=j-cols;}
1943
1944 c(j)=f(j);
1945 for (i=1+stoff; i<j; i++)
1946 {
1947 c(j)-=c(i)*m(i,j-i+1);
1948 }
1949 c(j)*=1/m(j,1);
1950 }
1951
1952 q=Vector(rows); //with 0n init.
1953
1954 //LT*q=c f�r q l�sen:
1955 for (j=rows; j>=1; j--)
1956 {
1957 int stop=cols;
1958 if (rows-j+1<=cols) {stop=rows-j+1;}
1959
1960 q(j)=c(j);
1961 for (i=2; i<=stop; i++)
1962 {
1963 q(j)-=q(j+i-1)*m(j,i);
1964 }
1965 q(j)*=1/m(j,1);
1966 }
1967
1968 return 1;
1969 }
1970
1971
PrintToMatlab(ostream & os) const1972 void Matrix::PrintToMatlab(ostream& os) const
1973 {
1974 os << "[";
1975 for (int i=1; i<=Getrows(); i++)
1976 {
1977 for (int j=1; j<=Getcols(); j++)
1978 {
1979 char str[32];
1980 sprintf(str,"%1.24g",(*this)(i,j));
1981 os << str;
1982 if (j!=Getcols()) {os << " ";}
1983 }
1984 if (i!=Getrows()) {os << ";\n";}
1985 }
1986 os << "]";
1987 }
1988
1989
PrintToMatlabSparse(ostream & os,double tol)1990 void Matrix::PrintToMatlabSparse(ostream& os, double tol)
1991 {
1992 int mode = 2;
1993
1994 //count non-zero elements
1995 int n = 0;
1996 for (int i=1; i<=Getrows(); i++)
1997 {
1998 for (int j=1; j<=Getcols(); j++)
1999 {
2000 if (fabs((*this)(i,j)) > tol) n++;
2001 }
2002 }
2003
2004 IVector iv(n);
2005 IVector jv(n);
2006 Vector sv(n);
2007
2008 int cnt = 0;
2009 for (int i=1; i<=Getrows(); i++)
2010 {
2011 for (int j=1; j<=Getcols(); j++)
2012 {
2013 if (fabs((*this)(i,j)) > tol)
2014 {
2015 cnt++;
2016
2017 iv(cnt) = i;
2018 jv(cnt) = j;
2019 sv(cnt) = (*this)(i,j);
2020 }
2021 }
2022 }
2023
2024 if (mode == 1)
2025 {
2026
2027 os << "sparse([";
2028 for (int i=1; i<=n; i++)
2029 {
2030 char str[32];
2031 sprintf(str,"%d",iv(i));
2032 os << str;
2033 if (i!=n) {os << ";";}
2034 }
2035 os << "],[";
2036 for (int i=1; i<=n; i++)
2037 {
2038 char str[32];
2039 sprintf(str,"%d",jv(i));
2040 os << str;
2041 if (i!=n) {os << ";";}
2042 }
2043 os << "],[";
2044 for (int i=1; i<=n; i++)
2045 {
2046 char str[32];
2047 sprintf(str,"%1.16g",sv(i));
2048 os << str;
2049 if (i!=n) {os << ";";}
2050 }
2051 os << "]";
2052 os << "," << Getrows() << "," << Getcols();
2053 os << ")";
2054 }
2055 else
2056 {
2057 for (int i=1; i<=n; i++)
2058 {
2059 char str[32];
2060 sprintf(str,"%d ",iv(i));
2061 os << str;
2062 sprintf(str,"%d ",jv(i));
2063 os << str;
2064 sprintf(str,"%1.16g\n",sv(i));
2065 os << str;
2066 }
2067 os << Getrows() << " " << Getcols() << " 0\n"; //highest entry specifies size of matrix ... 0 is added to remaining entries
2068 }
2069
2070 }
2071
PrintToMaple(ostream & os)2072 void Matrix::PrintToMaple(ostream& os)
2073 {
2074 os << "matrix(" << Getrows()
2075 << ", " << Getcols() << ", [";
2076
2077 for (int i=1; i<=Getrows(); i++)
2078 {
2079 os << "[";
2080 for (int j=1; j<=Getcols(); j++)
2081 {
2082 char str[32];
2083 sprintf(str,"%1.16g",(*this)(i,j));
2084 os << str;
2085 if (j!=Getcols()) {os << ",";}
2086 }
2087 os << "]";
2088 if (i!=Getrows()) {os << ",\n";}
2089 }
2090 os << "]);\n" << endl;
2091 }
2092
PrintToMathematica()2093 mystr Matrix::PrintToMathematica()
2094 {
2095 mystr os;
2096
2097 int rows = Getrows();
2098 int cols = Getcols();
2099
2100 os += "{";
2101 for (int i=1; i<=rows; i++)
2102 {
2103 os += "{";
2104 for (int j=1; j<=cols; j++)
2105 {
2106 //char str[32];
2107 //sprintf(str,"%1.16g",(*this)(i,j));
2108 //os += str;
2109 os += (*this)(i,j);
2110 if (j < cols) os += ",";
2111 }
2112 os += mystr("}");
2113 if (i < rows) os += mystr(",");
2114 }
2115 os += mystr("}");
2116
2117 os.Replace("e","*^"); // for copy&pasting the string into mathematica
2118
2119 return os;
2120 }
2121
2122 // Write Matrix to string to export to C "double intro[rows][cols] = {.,.,.};"
MakeString(mystr intro)2123 mystr& Matrix::MakeString(mystr intro)
2124 {
2125 mystr* buffer = new mystr(intro);
2126 *buffer +=("["); *buffer += mystr(Getrows()*Getcols()); *buffer +=("]");
2127 *buffer +=(" = \n{ ");
2128 for (int i=0; i < this->Getrows(); i++)
2129 {
2130 for (int j=0; j < this->Getcols(); j++)
2131 {
2132 if( j ) *buffer += ", ";
2133 *buffer += mystr(Get0(i,j));
2134 }
2135 if (i<this->Getrows()-1) *buffer += ",\n";
2136 }
2137 *buffer += mystr(" };");
2138 return *buffer;
2139 }
2140
operator <<(ostream & os,const Matrix & m)2141 ostream& operator<<(ostream& os, const Matrix& m)
2142 {
2143 double max=0;
2144 int i;
2145 for (i=1; i<=m.Getrows(); i++)
2146 {
2147 for (int j=1; j<=m.Getcols(); j++)
2148 {
2149 if (fabs(m(i,j))>max) {max=fabs(m(i,j));}
2150 }
2151 }
2152 if (max==0) {max=1;}
2153 max=(int)log10(max);
2154 max=pow(10,max);
2155
2156 os << max << " *\n";
2157
2158 for (i=1; i<=m.Getrows(); i++)
2159 {
2160 os << "[";
2161 for (int j=1; j<=m.Getcols(); j++)
2162 {
2163 char str[32];
2164 sprintf(str,"% 1.9f",m(i,j)/max);
2165 os << str;
2166 if (j!=m.Getcols()) {os << ",";}
2167 }
2168 os << "]\n";
2169 }
2170
2171 return os;
2172 }
2173
operator +(const Matrix & m1,const Matrix & m2)2174 Matrix operator+ (const Matrix& m1, const Matrix& m2)
2175 {
2176 #ifndef __QUICKMATH
2177 release_assert(m1.cols==m2.cols && m1.rows==m2.rows);
2178 #endif
2179 Matrix result=m1;
2180 result+=m2;
2181 return result;
2182 }
2183
operator -(const Matrix & m1,const Matrix & m2)2184 Matrix operator- (const Matrix& m1, const Matrix& m2)
2185 {
2186 #ifndef __QUICKMATH
2187 release_assert(m1.cols==m2.cols && m1.rows==m2.rows);
2188 #endif
2189 Matrix result=m1;
2190 result-=m2;
2191 return result;
2192 }
2193
operator *(const Matrix & mat,const double & val)2194 Matrix operator* (const Matrix& mat, const double& val)
2195 {
2196 Matrix result=mat;
2197 result*=val;
2198 return result;
2199 }
2200
operator *(const double & val,const Matrix & mat)2201 Matrix operator* (const double& val, const Matrix& mat)
2202 {
2203 Matrix result=mat;
2204 result*=val;
2205 return result;
2206 }
2207
operator *(const Matrix & m1,const Matrix & m2)2208 Matrix operator* (const Matrix& m1, const Matrix& m2)
2209 {
2210 #ifndef __QUICKMATH
2211 release_assert(m1.cols==m2.rows);
2212 #endif
2213
2214 Matrix res(m1.rows,m2.cols);
2215
2216 double *rp=res.mat;
2217 double *mp1=m1.mat;
2218 double *mp2=m2.mat;
2219 int rows=res.rows;
2220 int cols=res.cols;
2221
2222 for (int i=0;i<rows;i++)
2223 {
2224 for (int j=0;j<cols;j++)
2225 {
2226 for (int k=0;k<m1.cols;k++)
2227 {
2228 rp[i*cols+j]+=mp1[i*m1.cols+k]*mp2[k*cols+j];
2229 }
2230 }
2231 }
2232 return res;
2233 }
2234
Mult(const Matrix & m1,const Matrix & m2,Matrix & res)2235 void Mult(const Matrix& m1, const Matrix& m2, Matrix& res)
2236 {
2237 #ifndef __QUICKMATH
2238 release_assert(m1.cols==m2.rows);
2239 #endif
2240
2241 res.SetSize(m1.rows,m2.cols);
2242
2243 double *rp=res.mat;
2244 double *mp1=m1.mat;
2245 double *mp2=m2.mat;
2246 int rows=res.rows;
2247 int cols=res.cols;
2248
2249 for (int i=0;i<rows;i++)
2250 {
2251 for (int j=0;j<cols;j++)
2252 {
2253 rp[i*cols+j] = 0;
2254 for (int k=0;k<m1.cols;k++)
2255 {
2256 rp[i*cols+j] += mp1[i*m1.cols+k] * mp2[k*cols+j];
2257 }
2258 }
2259 }
2260 }
2261
MultTp(const Matrix & m1,const Matrix & m2,Matrix & res)2262 void MultTp(const Matrix& m1, const Matrix& m2, Matrix& res)
2263 {
2264 #ifndef __QUICKMATH
2265 release_assert(m1.rows==m2.rows);
2266 #endif
2267
2268 res.SetSize(m1.cols,m2.cols);
2269
2270 double *rp=res.mat;
2271 double *mp1=m1.mat;
2272 double *mp2=m2.mat;
2273 int rows=res.rows;
2274 int cols=res.cols;
2275
2276 for (int i=0;i<rows;i++)
2277 {
2278 for (int j=0;j<cols;j++)
2279 {
2280 rp[i*cols+j] = 0;
2281 for (int k=0;k<m1.rows;k++)
2282 {
2283 rp[i*cols+j] += mp1[k*m1.cols+i] * mp2[k*cols+j];
2284 }
2285 }
2286 }
2287 }
2288
MultSym(const Matrix & m1,const Matrix & m2,Matrix & res)2289 void MultSym(const Matrix& m1, const Matrix& m2, Matrix& res) //computes res=m1*m2, result is assumed to be symmetric
2290 {
2291 #ifndef __QUICKMATH
2292 release_assert(m1.cols==m2.rows);
2293 #endif
2294
2295 res.SetSize(m1.rows,m2.cols);
2296
2297 double *rp=res.mat;
2298 double *mp1=m1.mat;
2299 double *mp2=m2.mat;
2300 int rows=res.rows;
2301 int cols=res.cols;
2302
2303 for (int i=0;i<rows;i++)
2304 {
2305 for (int j=0;j<=i;j++)
2306 {
2307 rp[i*cols+j] = 0;
2308 for (int k=0;k<m1.cols;k++)
2309 {
2310 rp[i*cols+j] += mp1[i*m1.cols+k] * mp2[k*cols+j];
2311 }
2312 }
2313 }
2314 for (int i=0;i<rows;i++)
2315 {
2316 for (int j=0;j<i;j++)
2317 {
2318 rp[j*cols+i] = rp[i*cols+j];
2319 }
2320 }
2321 }
2322
MultSymTp(const Matrix & m1,const Matrix & m2,Matrix & res)2323 void MultSymTp(const Matrix& m1, const Matrix& m2, Matrix& res) //computes res=m1*m2, result is assumed to be symmetric
2324 {
2325 #ifndef __QUICKMATH
2326 release_assert(m1.rows==m2.rows);
2327 #endif
2328
2329 res.SetSize(m1.cols,m2.cols);
2330
2331 double *rp=res.mat;
2332 double *mp1=m1.mat;
2333 double *mp2=m2.mat;
2334 int rows=res.rows;
2335 int cols=res.cols;
2336
2337 if (1)
2338 {
2339 //+++++++++++++++++++++++++++++++++++++++++
2340 //new, better caching:
2341 Matrix m1t = m1; m1t.TpYs();
2342 Matrix m2t = m2; m2t.TpYs();
2343 mp1=m1t.mat;
2344 mp2=m2t.mat;
2345
2346 int m1tcolsblocks = 8*((int)(m1t.cols/8));
2347
2348 for (int i=0;i<rows;i++)
2349 {
2350 for (int j=0;j<=i;j++)
2351 {
2352 rp[i*cols+j] = 0;
2353 for (int k=0;k<m1tcolsblocks;k+=8)
2354 {
2355 rp[i*cols+j] +=
2356 mp1[i*m1t.cols+k ] * mp2[j*m2t.cols+k ]+
2357 mp1[i*m1t.cols+k+1] * mp2[j*m2t.cols+k+1]+
2358 mp1[i*m1t.cols+k+2] * mp2[j*m2t.cols+k+2]+
2359 mp1[i*m1t.cols+k+3] * mp2[j*m2t.cols+k+3]+
2360 mp1[i*m1t.cols+k+4] * mp2[j*m2t.cols+k+4]+
2361 mp1[i*m1t.cols+k+5] * mp2[j*m2t.cols+k+5]+
2362 mp1[i*m1t.cols+k+6] * mp2[j*m2t.cols+k+6]+
2363 mp1[i*m1t.cols+k+7] * mp2[j*m2t.cols+k+7];
2364 }
2365 for (int k=m1tcolsblocks;k<m1t.cols;k++)
2366 {
2367 rp[i*cols+j] += mp1[i*m1t.cols+k] * mp2[j*m2t.cols+k];
2368 }
2369 }
2370 }
2371 //+++++++++++++++++++++++++++++++++++++++++
2372 }
2373 else
2374 {
2375 for (int i=0;i<rows;i++)
2376 {
2377 for (int j=0;j<=i;j++)
2378 {
2379 rp[i*cols+j] = 0;
2380 for (int k=0;k<m1.rows;k++)
2381 {
2382 rp[i*cols+j] += mp1[k*m1.cols+i] * mp2[k*cols+j];
2383 }
2384 }
2385 }
2386 }
2387 for (int i=0;i<rows;i++)
2388 {
2389 for (int j=0;j<i;j++)
2390 {
2391 rp[j*cols+i] = rp[i*cols+j];
2392 }
2393 }
2394 }
2395
Mult(const MatrixXD & m1,const Matrix & m2,Matrix & res)2396 void Mult(const MatrixXD& m1, const Matrix& m2, Matrix& res)
2397 {
2398 #ifndef __QUICKMATH
2399 release_assert(m1.cols==m2.rows);
2400 #endif
2401
2402 res.SetSize(m1.rows,m2.cols);
2403
2404 double *rp=res.mat;
2405 const double *mp1=m1.mat;
2406 double *mp2=m2.mat;
2407 int rows=res.rows;
2408 int cols=res.cols;
2409
2410 for (int i=0;i<rows;i++)
2411 {
2412 for (int j=0;j<cols;j++)
2413 {
2414 rp[i*cols+j] = 0;
2415 for (int k=0;k<m1.cols;k++)
2416 {
2417 rp[i*cols+j] += mp1[i*m1.cols+k] * mp2[k*cols+j];
2418 }
2419 }
2420 }
2421 }
2422
Mult(const Matrix & m1,const MatrixXD & m2,Matrix & res)2423 void Mult(const Matrix& m1, const MatrixXD& m2, Matrix& res)
2424 {
2425 #ifndef __QUICKMATH
2426 release_assert(m1.cols==m2.rows);
2427 #endif
2428
2429 res.SetSize(m1.rows,m2.cols);
2430
2431 double *rp=res.mat;
2432 double *mp1=m1.mat;
2433 const double *mp2=m2.mat;
2434 int rows=res.rows;
2435 int cols=res.cols;
2436
2437 for (int i=0;i<rows;i++)
2438 {
2439 for (int j=0;j<cols;j++)
2440 {
2441 rp[i*cols+j] = 0;
2442 for (int k=0;k<m1.cols;k++)
2443 {
2444 rp[i*cols+j] += mp1[i*m1.cols+k] * mp2[k*cols+j];
2445 }
2446 }
2447 }
2448 }
2449
operator *(const Vector & v,const Matrix & m)2450 Vector operator* (const Vector& v, const Matrix& m)
2451 {
2452 #ifndef __QUICKMATH
2453 release_assert(m.rows==v.l);
2454 #endif
2455 Vector res(m.cols,NOFILL);
2456
2457 for (int i=1;i<=res.l;i++)
2458 {
2459 res(i)=0;
2460 for (int j=1;j<=v.l;j++)
2461 {
2462 res(i)+=m(j,i)*v(j);
2463 }
2464 //res(i)=(m.GetColVec(i))*v;
2465 }
2466 return res;
2467 }
2468
operator *(const Matrix & m,const Vector & v)2469 Vector operator* (const Matrix& m, const Vector& v)
2470 {
2471 #ifndef __QUICKMATH
2472 release_assert(m.cols==v.l);
2473 #endif
2474
2475 Vector res(m.rows,NOFILL);
2476
2477 for (int i=1;i<=res.l;i++)
2478 {
2479 res(i)=0;
2480 for (int j=1;j<=v.l;j++)
2481 {
2482 res(i)+=m(i,j)*v(j);
2483 }
2484 }
2485 return res;
2486 }
2487
Mult(const Matrix & m,const Vector & v,Vector & res)2488 void Mult(const Matrix& m, const Vector& v, Vector& res) //computes res=m*v
2489 {
2490 #ifndef __QUICKMATH
2491 release_assert(m.Getcols()==v.GetLen());
2492 #endif
2493 res.SetLen(m.Getrows());
2494
2495 double* mm = m.mat;
2496 double* vv = v.vec;
2497 int rl=res.GetLen();
2498 int vl=v.GetLen();
2499 for (int i=0; i<rl; i++)
2500 {
2501 double val=0;
2502 double* mr = &mm[i*m.cols];
2503 for (int j=0; j<vl; j++)
2504 {
2505 val+=mr[j]*vv[j];
2506 //val+=m(i+1,j+1)*v(j+1);
2507 }
2508 res.vec[i] = val;
2509 }
2510 }
MultTp(const Matrix & m,const Vector & v,Vector & res)2511 void MultTp(const Matrix& m, const Vector& v, Vector& res) //computes res=m.GetTp()*v
2512 {
2513 #ifndef __QUICKMATH
2514 release_assert(m.Getrows()==v.GetLen());
2515 #endif
2516 res.SetLen(m.Getcols());
2517
2518 int rl=res.GetLen();
2519 int vl=v.GetLen();
2520 for (int i=0; i<rl; i++)
2521 {
2522 double val=0;
2523 for (int j=0; j<vl; j++)
2524 {
2525 val+=m(j+1,i+1)*v(j+1);
2526 }
2527 res.vec[i] = val;
2528 }
2529 }
2530
Mult(const Matrix & m,const Vector & v,Vector3D & res)2531 void Mult(const Matrix& m, const Vector& v, Vector3D& res)
2532 {
2533 int cols = m.Getcols();
2534 int rows = m.Getrows();
2535
2536 #ifndef __QUICKMATH
2537 release_assert(cols == 3 && rows == v.Length());
2538 #endif
2539
2540 for (int i=0; i < rows; i++)
2541 {
2542 res.vec[i] = 0;
2543 for (int j=0; j < cols; j++)
2544 {
2545 res.vec[i] += m.mat[i*cols+j]*v.vec[j];
2546 }
2547 }
2548 }
2549
MultTp(const Matrix & m,const Vector & v,Vector3D & res)2550 void MultTp(const Matrix& m, const Vector& v, Vector3D& res)
2551 {
2552 int cols = m.Getcols();
2553 int rows = m.Getrows();
2554
2555 #ifndef __QUICKMATH
2556 release_assert(cols == 3 && rows == v.Length());
2557 #endif
2558
2559 for (int i=0; i < cols; i++)
2560 {
2561 res.vec[i] = 0;
2562 for (int j=0; j < rows; j++)
2563 {
2564 res.vec[i] += m.mat[i+j*cols]*v.vec[j];
2565 }
2566 }
2567 }
2568
Mult(const Vector & v,const Matrix & m,Vector & res)2569 void Mult(const Vector& v, const Matrix& m, Vector& res) //computes res=v*m
2570 {
2571 #ifndef __QUICKMATH
2572 release_assert(m.Getrows()==v.GetLen());
2573 #endif
2574 res.SetLen(m.Getcols());
2575
2576 int rl=res.GetLen();
2577 int vl=v.GetLen();
2578 for (int i=0; i<rl; i++)
2579 {
2580 double val=0;
2581 for (int j=0; j<vl; j++)
2582 {
2583 val+=m(j+1,i+1)*v(j+1);
2584 }
2585 res.vec[i] = val;
2586 }
2587 }
2588
2589
MultBW(const Matrix & m,const Vector & v,Vector & res,int bw)2590 void MultBW(const Matrix& m, const Vector& v, Vector& res, int bw) //computes res=m*v
2591 {
2592 TMStartTimer(12);
2593 #ifndef __QUICKMATH
2594 release_assert(m.Getcols()==v.GetLen());
2595 #endif
2596 res.SetLen(m.Getrows());
2597
2598 if (bw==0) bw = m.cols+m.rows;
2599 double* mm = m.mat;
2600 double* vv = v.vec;
2601 int rl=res.GetLen();
2602 int vl=v.GetLen();
2603 for (int i=0; i<rl; i++)
2604 {
2605 double val=0;
2606 int bwoff = 0;
2607 int jend = vl;
2608 if (i > bw) bwoff = i-bw;
2609 if (i+bw < vl) jend = i+bw;
2610 double* mr = &mm[i*m.cols];
2611 for (int j=bwoff; j<jend; j++)
2612 {
2613 val+=mr[j]*vv[j];
2614 //val+=m(i+1,j+1)*v(j+1);
2615 }
2616 res.vec[i] = val;
2617 }
2618 TMStopTimer(12);
2619 }
2620
Mult(const Vector & v1,const Vector & v2,Matrix & res)2621 void Mult(const Vector& v1, const Vector& v2, Matrix& res) //computes res=v1*v2^T, where v1 and v2 are interpreted as a column vectors
2622 {
2623 double rows = v1.GetLen();
2624 double cols = v2.GetLen();
2625 res.SetSize(rows, cols);
2626
2627 double* mm = res.mat;
2628 for (int i=0; i<rows; i++)
2629 {
2630 double rowfactor = v1.vec[i];
2631 int off = i*cols;
2632 for (int j=0; j<cols; j++)
2633 {
2634 mm[off+j] = rowfactor*v2.vec[j];
2635 }
2636 }
2637 }
2638
Mult(const Matrix & m,const Vector3D & v,Vector & res)2639 void Mult(const Matrix& m, const Vector3D& v, Vector& res) //computes res=m*v
2640 {
2641 #ifndef __QUICKMATH
2642 release_assert(m.Getcols()==v.GetLen());
2643 #endif
2644 res.SetLen(m.Getrows());
2645
2646 double* mm = m.mat;
2647 for (int i=0; i<res.GetLen(); i++)
2648 {
2649 int off = i*m.cols;
2650 res.vec[i] = mm[off]*v.vec[0]+mm[off+1]*v.vec[1]+mm[off+2]*v.vec[2];
2651 }
2652 }
2653
Mult(const Matrix & m,const Vector2D & v,Vector & res)2654 void Mult(const Matrix& m, const Vector2D& v, Vector& res) //computes res=m*v
2655 {
2656 #ifndef __QUICKMATH
2657 release_assert(m.Getcols()==v.GetLen());
2658 #endif
2659 res.SetLen(m.Getrows());
2660
2661 double* mm = m.mat;
2662 for (int i=0; i<res.GetLen(); i++)
2663 {
2664 int off = i*m.cols;
2665 res.vec[i] = mm[off]*v.vec[0]+mm[off+1]*v.vec[1];
2666 }
2667 }
2668
Bmult(Vector & v) const2669 Vector Matrix::Bmult(Vector& v) const
2670 {
2671 #ifndef __QUICKMATH
2672 release_assert(v.GetLen()==rows);
2673 #endif
2674 Vector res(rows); //initialized with zeros
2675
2676 for (int i=1;i<=rows;i++)
2677 {
2678 int start=i-cols;
2679 int end=i+cols-1;
2680 if (start < 1) {start = 1;}
2681 if (end > rows) {end = rows;}
2682 for (int j=start;j<=end;j++)
2683 {
2684 res(i)+=v(j)*BGet(i,j);
2685 }
2686 }
2687 return res;
2688 }
2689
BCG(Vector f,Vector & q,const Vector & qs)2690 int Matrix::BCG(Vector f, Vector& q, const Vector& qs)
2691 {
2692 Vector x = qs;
2693 Vector r(f.GetLen());
2694 Vector rq(f.GetLen());
2695 Vector p(f.GetLen());
2696 Vector pq(f.GetLen());
2697 Vector Ap(f.GetLen());
2698 Vector App(f.GetLen());
2699
2700 double alpha, beta, rqr;
2701
2702 //initial conditions:
2703 r = f-(*this)*x;
2704 rq = r;
2705 p = r;
2706 pq = rq;
2707
2708 //error condition
2709 Vector err(f.GetLen());
2710 int end = 0;
2711 int it = 0;
2712
2713 int maxit = f.GetLen();
2714
2715 while(!end && it < maxit)
2716 {
2717 Ap = ((*this)*p);
2718
2719 alpha = (rq*r)/(pq*Ap);
2720 err = alpha * p;
2721 x = x + err;
2722
2723 rqr = rq*r;
2724 r = r - alpha * Ap;
2725 rq = rq - alpha * GetTp() * p;
2726
2727 beta = (rq*r)/rqr;
2728 p = r + beta * p;
2729 pq = rq + beta * pq;
2730
2731 it++;
2732 if (it % 100 == 0) {(*global_uo) << " CG:" << it << " its, err=" << fabs(err.MaxNorm()) << "\n";}
2733 if (fabs(err.MaxNorm()) < 1E-10) {end = 1;}
2734 }
2735 (*global_uo) << "CG: " << it << " its, err=" << fabs(err.MaxNorm()) << "\n";
2736
2737 q = x;
2738 if (it < maxit) {return 1;}
2739 else {return 0;}
2740 }
2741
2742
2743 double eigenvalue_err;
Eigenvalues(Vector & ev)2744 int Matrix::Eigenvalues(Vector& ev)
2745 {
2746 eigenvalue_err = 0;
2747 //evout << "Compute Eigenvalue:" << endl;
2748 int end = 0;
2749 double tol = 1e-5;
2750 const int maxit = 500;
2751 int i = 0;
2752
2753 Matrix M(*this);
2754 Matrix Q,R;
2755 Vector evstart = M.GetDiag();
2756 double err;
2757 double relval = evstart.GetNorm();
2758 if (relval == 0) {relval = 1;}
2759
2760 while(i < maxit && !end)
2761 {
2762 i++;
2763 M.QRDecomposition(Q,R);
2764 M = R*Q;
2765 ev = M.GetDiag();
2766 err = (ev-evstart).GetNorm()/relval;
2767 if (err < tol) {end = 1;}
2768 evstart = ev;
2769
2770 eigenvalue_err = err;
2771 //evout << "err=" << err << endl;
2772 }
2773
2774 //::cout << "EV-iterations=" << i << endl;
2775 int retv=1;
2776 if (i >= maxit)
2777 {
2778 retv=0;
2779 //evout << "ERROR: Eigenvalues not converged!!!" << endl;
2780 }
2781
2782
2783 return retv;
2784
2785 };
2786
EigenvaluesLapack(Vector & lami) const2787 int Matrix::EigenvaluesLapack(Vector& lami) const
2788 {
2789 mystatic Matrix m;
2790 m=*this;
2791 lami.SetLen(Getrows());
2792 mystatic Vector work;
2793 work.SetLen(4*Getrows());
2794 int info = LapackEVPSPD(Getrows(), &(m(1,1)), &(lami(1)), &(work(1)), work.Length());
2795 if (info) { (*global_uo) << "Matrix::EigenvaluesLapack: Eigenvalues could not be computed, Lapack error message " << info << "\n"; return 0;}
2796 else return 1;
2797 }
2798
QRDecomposition(Matrix & Q,Matrix & R)2799 int Matrix::QRDecomposition(Matrix& Q, Matrix& R)
2800 {
2801 #ifndef __QUICKMATH
2802 release_assert(IsSquare());
2803 #endif
2804 if (Q.Getcols()!=cols)
2805 {
2806 Q=Matrix(cols,cols);
2807 }
2808 else
2809 {
2810 Q.FillWithZeros();
2811 }
2812 if (R.Getcols()!=cols)
2813 {
2814 R = Matrix(cols,cols);
2815 }
2816 else
2817 {
2818 R.FillWithZeros();
2819 }
2820 mystatic Matrix Mu;
2821
2822 if (Mu.Getcols()!=cols)
2823 {
2824 Mu = Matrix(cols,cols);
2825 }
2826 else
2827 {
2828 Mu.FillWithZeros();
2829 }
2830
2831 //Vector v;
2832
2833 int i,j;
2834
2835 //Mu.SetColVec(this->GetColVec(1),1);
2836 Mu.SetColVec(*this,1,1);
2837
2838 //Q.SetColVec(Mu.GetColVec(1),1);
2839 Q.SetColVec(Mu,1,1);
2840
2841 for(i=1; i<= cols; i++)
2842 {
2843 R(i,i) = 1;
2844 }
2845
2846 for (j=2; j<=cols; j++)
2847 {
2848 //Vector sumv(cols);
2849
2850 //Mu.SetColVec(this->GetColVec(j),j);
2851 Mu.SetColVec(*this,j,j);
2852
2853 for (i=1; i<=j-1; i++)
2854 {
2855 //double u2=Mu.GetColVec(i)*Mu.GetColVec(i);
2856 double u2=Mu.MultCol(Mu,i,i);
2857
2858 if (u2 == 0)
2859 {
2860 //cout << "ERROR: QR-Decomposition, Matrix singular!!!" << endl;
2861 return 0;
2862 }
2863 R(j,i)=0;
2864 //R(i,j)=(1./u2)*Mu.GetColVec(i)*this->GetColVec(j);
2865 R(i,j)=(1./u2)*Mu.MultCol(*this,j,i);
2866
2867 //Mu.SetColVec(Mu.GetColVec(j)-R(i,j)*Mu.GetColVec(i),j);
2868 Mu.AddColVec(i,j,-R(i,j));
2869
2870 }
2871 for (i=1; i <= cols; i++)
2872 {
2873 Q(i,j) = Mu(i,j);
2874 }
2875 }
2876 Vector Dm(cols);
2877 for (i=1; i <= cols; i++)
2878 {
2879 //Dm(i,i) = sqrt(Mu.GetColVec(i)*Mu.GetColVec(i));
2880 Dm(i) = sqrt(Mu.MultCol(Mu,i,i));
2881 R.MulRow(i,Dm(i));
2882 Q.MulCol(i,1./Dm(i));
2883 }
2884 return 1;
2885
2886 }
2887
2888
2889 // Evaluate piecewise bilinear function on unit square [-1,1]^2 at point p,
2890 // Matrix entries specify function values
2891 // [ 1 2 3 ... width ] [ y=1 ]
2892 // [ w+1 w+2 w+3 ... 2w ] -----> [x=-1 x=1]
2893 //
2894 // [ (h-1)w+1 (h-1)w+1 (h-1)w+2 ... hw] [ y=-1 ]
Interpolate(const Vector2D & ploc) const2895 double Matrix::Interpolate(const Vector2D& ploc) const
2896 {
2897 int imin, jmin;
2898 double hy = 2./(Getrows()-1), hx = 2./(Getcols()-1.);
2899 // lower indices of cell containing ploc
2900 imin = int((1.-ploc.Y()) / hy ) +1; // int((ploc.Y()+1.) / hy ) +1;
2901 jmin = int((ploc.X()+1.) / hx ) +1;
2902 if (imin >= Getrows()) imin = Getrows()-1;
2903 if (jmin >= Getcols()) jmin = Getcols()-1;
2904 // "mesh sizes"
2905 double xmin = hx * (jmin-1)-1, ymin = 1-hy*(imin);
2906 double val = (*this)(imin, jmin)*(hx-ploc.X()+xmin)*(ploc.Y()-ymin)/(hx*hy) +
2907 (*this)(imin, jmin+1)*(ploc.X()-xmin)*(ploc.Y()-ymin)/(hx*hy) +
2908 (*this)(imin+1, jmin)*(hx-ploc.X()+xmin)*(hy-ploc.Y()+ymin)/(hx*hy) +
2909 (*this)(imin+1, jmin+1)*(ploc.X()-xmin)*(hy-ploc.Y()+ymin)/(hx*hy);
2910 return val;
2911 }
2912
2913 // Evaluate piecewise bilinear function on unit square [-1,1]^2 at x-coord xloc,
2914 // Vector entries(i) correspond to y-coordinates
2915 // Matrix entries specify function values
2916 // [ 1 2 3 ... width ] [ y=1 ]
2917 // [ w+1 w+2 w+3 ... 2w ] -----> [x=-1 x=1]
2918 //
2919 // [ (h-1)w+1 (h-1)w+1 (h-1)w+2 ... hw] [ y=-1 ]
InterpolateX(const double xloc,Vector & interpol_vector) const2920 void Matrix::InterpolateX(const double xloc, Vector& interpol_vector) const
2921 {
2922 interpol_vector.SetLen(Getrows());
2923 int jmin;
2924 double hy = 2./(Getrows()-1), hx = 2./(Getcols()-1.);
2925 // lower indices of cell containing ploc
2926 jmin = int((xloc+1.) / hx ) +1;
2927 if (jmin >= Getcols()) jmin = Getcols()-1;
2928 // "mesh sizes"
2929 double xmin = hx * (jmin-1)-1;
2930 for (int i=1; i<=Getrows(); i++)
2931 {
2932 interpol_vector(i) = (*this)(i, jmin)*(hx-xloc+xmin)/(hx) +
2933 (*this)(i, jmin+1)*(xloc-xmin)/(hx);
2934 }
2935 }
2936
PrintMatrix01(const Matrix & m)2937 void PrintMatrix01(const Matrix& m)
2938 {
2939 for (int i=1; i<=m.Getrows(); i++)
2940 {
2941 mystr str="";
2942 for (int j=1; j<=m.Getcols(); j++)
2943 {
2944 if (fabs(m.Get(i,j)) <= 1e-14) str+= "0";
2945 else str += "1";
2946 }
2947 str+="\n";
2948 (*global_uo) << str;
2949 }
2950 }
2951
2952
2953 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2954 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2955 //+++++++++++++++++++++ SPARSE MATRIX ++++++++++++++++++++++++++++++++++++++
2956 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2957 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2958
GenMat(int lalloc,int rowsi)2959 void SparseMatrix::GenMat(int lalloc, int rowsi)
2960 {
2961 #ifdef gen_sparsemat
2962 GenSparsemat();
2963 #endif
2964 dnull = 0;
2965 mat = new double[lalloc];
2966 colind = new int[lalloc];
2967 rowind = new int[rowsi+1]; //for convention with Harwell Boeing format
2968 rowlen = new int[rowsi];
2969
2970 //(*global_uo) << "genmat=" << lalloc*12./1.e6 << "MB\n";
2971 }
2972
2973 int reallocate_sparsematrix_cnt = 0;
ReAllocate()2974 void SparseMatrix::ReAllocate()
2975 {
2976 if (reallocate_sparsematrix_cnt%100 == 0) (*global_uo) << "Reallocate sparse matrix " << reallocate_sparsematrix_cnt++ << "times ==> switch to SolverOptions.Static.experimental_sparse_jacobian\n";
2977
2978 //calculate space needed
2979 int incelems = 0;
2980 const int incfact = 2;
2981 for (int row=0; row < rows; row++)
2982 {
2983 int rlmax = RowLenMax0(row);
2984 //(*global_uo) << rlmax << ",";
2985 if (rlmax/incfact < rowlen[row])
2986 {
2987 int addtorow = rowlen[row]*incfact-rlmax;
2988 incelems += addtorow;
2989 }
2990 }
2991 //(*global_uo) << "]\n";
2992
2993 int newsize = lalloc+incelems;
2994 //(*global_uo) << "Reallocate=" << newsize << "\n";
2995
2996 if (newsize==0) {return; }
2997
2998 double* nmat = new double[newsize];
2999 int* ncolind = new int[newsize];
3000 int* nrowind = new int[rows+1]; //for convention with Harwell Boeing format
3001 int* nrowlen = new int[rows];
3002 //(*global_uo) << "rnewmat=" << newsize*12./1.e6 << "MB\n";
3003
3004 if (mat!=NULL && lalloc!=0)
3005 {
3006 //generate new data:
3007 nrowind[0] = 0;
3008 for (int row=0; row < rows; row++)
3009 {
3010 int addtorow = 0;
3011 int rlmax = RowLenMax0(row); //accesses lalloc!!!
3012 if (rlmax/incfact < rowlen[row])
3013 {
3014 addtorow = rowlen[row]*incfact-rlmax;
3015 }
3016
3017 int newmaxlen = rlmax+addtorow;
3018 if (row < rows-1) nrowind[row+1] = nrowind[row]+newmaxlen;
3019 nrowlen[row] = rowlen[row];
3020
3021 for (int i = 0; i < rowlen[row]; i++)
3022 {
3023 nmat[nrowind[row]+i] = mat[rowind[row]+i];
3024 ncolind[nrowind[row]+i] = colind[rowind[row]+i];
3025 }
3026
3027 int nrlmax; if (row < rows-1) nrlmax = nrowind[row+1]-nrowind[row]; else nrlmax = lalloc-nrowind[row];
3028 for (int i = rowlen[row]; i < nrlmax; i++)
3029 {
3030 nmat[nrowind[row]+i] = 0; //not necessary
3031 ncolind[nrowind[row]+i] = sparseMatrixVoid; //not necessary
3032 }
3033 }
3034 }
3035
3036 Destroy();
3037
3038 lalloc = newsize;
3039 mat = nmat;
3040 colind = ncolind;
3041 rowind = nrowind;
3042 rowlen = nrowlen;
3043 }
3044
GetMatrix(Matrix & m) const3045 void SparseMatrix::GetMatrix(Matrix& m) const
3046 {
3047 m.SetSize(rows,cols);
3048 m.FillWithZeros();
3049 for (int i = 0; i < rows; i++)
3050 {
3051 //(*global_uo) << "row=" << i << ": ";
3052 for (int j = 0; j < rowlen[i]; j++)
3053 {
3054 //(*global_uo) << "(" << colind[rowind[i]+j] << "," << mat[rowind[i]+j] << "),";
3055
3056 m.Elem0(i,colind[rowind[i]+j]) = mat[rowind[i]+j];
3057 }
3058 //(*global_uo) << "\n";
3059 }
3060 }
3061
3062
CopyFrom(const Matrix & mati)3063 void SparseMatrix::CopyFrom(const Matrix& mati)
3064 {
3065 int nrows = mati.Getrows();
3066 int ncols = mati.Getcols();
3067
3068 int neededmem = 0;
3069 for (int i = 0; i < nrows*ncols; i++)
3070 {
3071 if (fabs(mati.GetMatPtr()[i]) > sparsetolzero)
3072 {
3073 neededmem++;
3074 }
3075 }
3076
3077 if (neededmem > lalloc || nrows > rows)
3078 {
3079 Destroy();
3080
3081 lalloc = neededmem;
3082 GenMat(lalloc, nrows);
3083 }
3084 rows = mati.Getrows();
3085 cols = mati.Getcols();
3086
3087 int cnt = 0;
3088 for (int i = 0; i < rows; i++)
3089 {
3090 rowind[i] = cnt;
3091 int colcnt = 0;
3092 for (int j = 0; j < cols; j++)
3093 {
3094 if (fabs(mati.GetMatPtr()[i*cols+j]) > sparsetolzero)
3095 {
3096 colind[cnt] = j;
3097 colcnt++;
3098 mat[cnt++] = mati.GetMatPtr()[i*cols+j];
3099 }
3100 }
3101 rowlen[i] = colcnt;
3102 }
3103 }
3104
CopyFrom(const Matrix & mati,int row1,int col1,int row2,int col2)3105 void SparseMatrix::CopyFrom(const Matrix& mati, int row1, int col1, int row2, int col2)
3106 {
3107 int nrows = row2-row1+1;
3108 int ncols = col2-col1+1;
3109
3110 int neededmem = 0;
3111 for (int i = row1; i <= row2; i++)
3112 {
3113 for (int j = col1; j <= col2; j++)
3114 {
3115 if (fabs(mati(i,j)) > sparsetolzero)
3116 {
3117 neededmem++;
3118 }
3119 }
3120 }
3121
3122 if (neededmem > lalloc || nrows > rows)
3123 {
3124 Destroy();
3125
3126 lalloc = neededmem;
3127 GenMat(lalloc, nrows);
3128 }
3129 rows = nrows;
3130 cols = ncols;
3131
3132 int cnt = 0;
3133 for (int i = 0; i < rows; i++)
3134 {
3135 rowind[i] = cnt;
3136 int colcnt = 0;
3137 for (int j = 0; j < cols; j++)
3138 {
3139 if (fabs(mati(i+row1,j+col1)) > sparsetolzero)
3140 {
3141 colind[cnt] = j;
3142 colcnt++;
3143 mat[cnt++] = mati(i+row1,j+col1);
3144 }
3145 }
3146 rowlen[i] = colcnt;
3147 }
3148 }
3149
3150 //row1, col1, row2, col2 are 1-based!!!
CopyFrom(const SparseMatrix & mati,int row1,int col1,int row2,int col2)3151 void SparseMatrix::CopyFrom(const SparseMatrix& mati, int row1, int col1, int row2, int col2)
3152 {
3153
3154 //(*global_uo) << "sparsematrix::copyfrom(sparsematrix) does not work yet!\n";
3155
3156 int nrows = row2-row1+1;
3157 int ncols = col2-col1+1;
3158
3159 int neededmem = 0;
3160 for (int i = row1-1; i <= row2-1; i++)
3161 {
3162 for (int j = 0; j < mati.rowlen[i]; j++)
3163 {
3164 int c = mati.colind[mati.rowind[i]+j];
3165 if (c >= col1-1 && c <= col2-1) neededmem++;
3166 }
3167 }
3168
3169 if (neededmem > lalloc || nrows > rows)
3170 {
3171 Destroy();
3172
3173 lalloc = neededmem;
3174 GenMat(lalloc, nrows);
3175 }
3176
3177 rows = nrows;
3178 cols = ncols;
3179
3180 int cnt = 0;
3181 for (int i = 0; i < rows; i++)
3182 {
3183 rowind[i] = cnt;
3184 int matirow = i+row1-1;
3185 int rind = mati.rowind[matirow];
3186 int k = 0;
3187 while (k < mati.rowlen[matirow] && mati.colind[k+rind] < col1-1) k++;
3188
3189 int j = 0;
3190 while (mati.colind[k+j+rind] <= col2-1 && (k+j) < mati.rowlen[matirow])
3191 {
3192 colind[j+rowind[i]] = mati.colind[rind+j+k]-col1+1;
3193 mat[j+rowind[i]] = mati.mat[rind+j+k];
3194 cnt++; j++;
3195 }
3196 rowlen[i] = j;
3197 }
3198 }
3199
3200 //generate new matrix from sparse matrix, only from row1 to row2, col1 to col2 (incl.), resort with vector r
CopyFrom(const SparseMatrix & mati,int row1,int col1,int row2,int col2,const IVector & r)3201 void SparseMatrix::CopyFrom(const SparseMatrix& mati, int row1, int col1, int row2, int col2, const IVector& r)
3202 {
3203 int nrows = row2-row1+1;
3204 int ncols = col2-col1+1;
3205
3206 mystatic IVector invr;
3207 invr.SetLen(r.Length());
3208 invr.SetAll(-1);
3209 for (int i=1; i <= r.Length(); i++) invr(r(i)) = i;
3210
3211 int neededmem = 0;
3212
3213
3214 for (int i = 0; i < nrows; i++)
3215 {
3216 int matirow = r(i+row1)-1;
3217 int rind = mati.rowind[matirow];
3218
3219 //read out necessary entries and transform by r:
3220 for (int j = 0; j < mati.rowlen[matirow]; j++)
3221 {
3222 int c = mati.colind[rind+j]; //0-based
3223 int cr = invr(c+1); //1-based
3224 if (cr >= col1 && cr <= col2) neededmem++;
3225 }
3226 }
3227
3228
3229 if (neededmem > lalloc || nrows > rows)
3230 {
3231 Destroy();
3232
3233 lalloc = neededmem;
3234 GenMat(lalloc, nrows);
3235 }
3236
3237 rows = nrows;
3238 cols = ncols;
3239
3240
3241
3242 mystatic TArray<double> rsort; //resorted values
3243 mystatic TArray<int> rsortind; //resorted column indices 0-based for destination
3244
3245
3246 //sm 1 2 3 4 5 6 7 8 9
3247 //rind 1 3 6 8
3248 //mat 11 12 13 14
3249 //r 1 2 8 3 4 5 9 6 7
3250 //invr 1 2 4 5 6 8 9 3 7
3251 //m 11 14 12 13
3252 //
3253
3254 int c, cr;
3255 int cnt = 0;
3256
3257 //(*global_uo) << "lalloc=" << lalloc << ", nrows=" << nrows << "\n";
3258
3259 //mat[i++] = m(r(row),r(col));
3260
3261
3262 //TMStartTimer(24);
3263 for (int i = 0; i < rows; i++)
3264 {
3265 rsortind.SetLen(0);
3266 rsort.SetLen(0);
3267
3268 int matirow = r(i+row1)-1;
3269 int rind = mati.rowind[matirow];
3270
3271 //read out necessary entries and transform by r:
3272 for (int j = 0; j < mati.rowlen[matirow]; j++)
3273 {
3274 c = mati.colind[rind+j]; //0-based
3275 cr = invr(c+1); //1-based
3276 if (cr >= col1 && cr <= col2)
3277 {
3278 rsortind.Add(invr(c+1)-col1); //0-based index for destination
3279 //runsortind.Add(c-col1+1); //0-based index for destination
3280 rsort.Add(mati.mat[rind+j]); //entry
3281 }
3282 }
3283 //sort entries
3284 Quicksort(rsortind, rsort);
3285
3286 //fill in entries in i-th row
3287 rowind[i] = cnt;
3288 int cnt2 = 0;
3289
3290 /*
3291 (*global_uo) << "row=" << i << ", rowind=" << rowind[i] << "\n";
3292 //(*global_uo) << "rsortind_len=(" << rsortind.Length() << "=" << rsort.Length() << ")\n";
3293 (*global_uo) << "rsortind=" << rsortind << "\n";
3294 (*global_uo) << "rsort =" << rsort << "\n";
3295 */
3296
3297
3298 for (int j=0; j < rsortind.Length(); j++)
3299 {
3300 if (rsort.Get0(j) != 0)
3301 {
3302 colind[cnt2+rowind[i]] = rsortind.Get0(j);
3303 //(*global_uo) << "(" << cnt2+rowind[i] << "," << colind[cnt2+rowind[i]] << ") ";
3304
3305 mat[cnt2+rowind[i]] = rsort.Get0(j);
3306 cnt2++;
3307 }
3308 }
3309 //(*global_uo) << "\n";
3310
3311 rowlen[i] = cnt2;
3312 cnt+=cnt2;
3313 }
3314 //TMStopTimer(24);
3315 }
3316
GetBandWidths(int & lb,int & ub) const3317 void SparseMatrix::GetBandWidths(int& lb, int& ub) const
3318 {
3319 lb = 0;
3320 ub = 0;
3321 for (int i = 0; i < rows; i++)
3322 {
3323 //(*global_uo) << "\nrow " << i << ": ";
3324 //(*global_uo) << "(len=" << rowlen[i] << ") ";
3325
3326 for (int j = 0; j < rowlen[i]; j++)
3327 {
3328
3329 //(*global_uo) << colind[rowind[i]+j] << ", ";
3330 if (mat[rowind[i]+j] != 0)
3331 {
3332 if (colind[rowind[i]+j]>i) {ub = Maximum(ub,colind[rowind[i]+j]-i);}
3333 if (colind[rowind[i]+j]<i) {lb = Maximum(lb,i-colind[rowind[i]+j]);}
3334 }
3335 }
3336
3337 //(*global_uo) << "\n";
3338 }
3339 }
3340
ComputeTranspose(SparseMatrix & res) const3341 void SparseMatrix::ComputeTranspose(SparseMatrix& res) const //computes res=m1^T
3342 {
3343 int n = CountEntries();
3344 int rows = Getrows();
3345 int cols = Getcols();
3346
3347 int rowlen = n/cols + 1;
3348 res.SetSize(cols, rows, rowlen);
3349 if (rowlen*cols < n) (*global_uo) << "Warning: SparseMatrix::ComputeTranspose allocated insufficient memory!\n";
3350
3351 //IVector ncolind(cols);
3352 TArray<int> ncolind;
3353 ncolind.SetLen(cols);
3354 int i;
3355
3356 for (i=1; i<=cols; i++)
3357 {
3358 ncolind(i) = 0;
3359 }
3360
3361 for (i=1; i<=rows; i++)
3362 {
3363 for (int j=1; j <= RowLen(i); j++)
3364 {
3365 int ci = GetRowColind(i,j);
3366 ncolind(ci+1)++;
3367 }
3368 }
3369
3370
3371 int actrowind = 0;
3372 for (i=1; i<=cols; i++)
3373 {
3374 res.rowlen[i-1] = 0;
3375 res.rowind[i-1] = actrowind;
3376 actrowind += ncolind(i);
3377 }
3378
3379 for (i=1; i<=rows; i++) //run over rows of m1 ==> columns of new matrix
3380 {
3381 for (int j=1; j <= RowLen(i); j++)
3382 {
3383 int resrow = GetRowColind(i,j);
3384 res.colind[res.rowind[resrow] + res.rowlen[resrow]] = i-1;
3385 res.mat[res.rowind[resrow] + res.rowlen[resrow]] = GetRowEntry(i,j);
3386 res.rowlen[resrow]++;
3387 }
3388 }
3389
3390
3391 }
3392
MultRow(const SparseMatrix & m1,const SparseMatrix & m2,int row1,int row2)3393 double MultRow(const SparseMatrix& m1, const SparseMatrix& m2, int row1, int row2) //mult row1 of m1 times row2 of m2
3394 {
3395 double val = 0;
3396 int m2j = 1;
3397 int m1rowlen = m1.RowLen(row1);
3398 int m2rowlen = m2.RowLen(row2);
3399
3400 for (int m1j=1; m1j <= m1rowlen; m1j++)
3401 {
3402 int m1ci = m1.GetRowColind(row1, m1j);
3403 int m2ci = m2.GetRowColind(row2, m2j);
3404 int end = 0;
3405 while (m2j < m2rowlen && m2ci < m1ci && !end)
3406 {
3407 m2j++;
3408 m2ci = m2.GetRowColind(row2, m2j);
3409 if (m2ci > m1ci)
3410 {
3411 m2j--;
3412 m2ci = m2.GetRowColind(row2, m2j);
3413 end = 1;
3414 }
3415 }
3416 if (m1ci == m2ci) val += m1.GetRowEntry(row1,m1j) * m2.GetRowEntry(row2, m2j);
3417 }
3418 return val;
3419 }
3420
Mult(const SparseMatrix & m1,const SparseMatrix & m2,SparseMatrix & res)3421 void Mult(const SparseMatrix& m1, const SparseMatrix& m2, SparseMatrix& res) //computes res=m1*m2
3422 {
3423 release_assert(m2.Getrows() == m1.Getcols());
3424
3425 SparseMatrix m2T;
3426 m2.ComputeTranspose(m2T);
3427
3428 int minrowsize = m1.GetLAlloc()/m1.Getrows() + m2T.GetLAlloc()/m2.Getrows();
3429 res.SetSize(m1.Getrows(), m2T.Getcols(), minrowsize);
3430
3431 int n = 0;
3432 int i, j;
3433 for (i=1; i<=m1.Getrows(); i++)
3434 {
3435 for (j=1; j<=m2T.Getrows(); j++)
3436 {
3437 double val = 0;
3438
3439 val = MultRow(m1, m2T, i, j);
3440
3441 /*
3442 int m2j = 1;
3443 int m2rowlen = m2T.RowLen(i);
3444 for (int m1j=1; m1j <= m1.RowLen(i); m1j++)
3445 {
3446 int m1ci = m1.GetRowColind(i, m1j);
3447 int m2ci = m2T.GetRowColind(j, m2j);
3448 int end = 0;
3449 while (m2j < m2rowlen && m2ci < m1ci && !end)
3450 {
3451 m2j++;
3452 m2ci = m2T.GetRowColind(j, m2j);
3453 if (m2ci > m1ci)
3454 {
3455 m2j--;
3456 m2ci = m2T.GetRowColind(j, m2j);
3457 end = 1;
3458 }
3459 }
3460 if (m1ci == m2ci) val += m1.GetRowEntry(i,m1j) * m2T.GetRowEntry(j, m2j);
3461 }
3462 */
3463 if (val != 0)
3464 {
3465 res(i,j) = val;
3466 }
3467 }
3468 }
3469
3470 }
3471
Mult(const SparseMatrix & m,const Vector & v,Vector & res)3472 void Mult(const SparseMatrix& m, const Vector& v, Vector& res)
3473 {
3474 #ifndef __QUICKMATH
3475 release_assert(m.Getcols()==v.GetLen());
3476 #endif
3477 TMStartTimer(12);
3478
3479 //if (res.GetLen() != m.rows)
3480 res.SetLen(m.rows);
3481
3482 for(int row = 0; row < m.rows; row++)
3483 {
3484 res.vec[row] = 0;
3485 for(int i = 0; i < m.rowlen[row]; i++)
3486 {
3487 res.vec[row] += m.mat[m.rowind[row]+i]*v.vec[m.colind[m.rowind[row]+i]];
3488 }
3489 }
3490 TMStopTimer(12);
3491 }
3492
operator *(const SparseMatrix & m1,const Matrix & m2)3493 Matrix operator* (const SparseMatrix& m1, const Matrix& m2)
3494 {
3495 #ifndef __QUICKMATH
3496 release_assert(m1.cols==m2.rows);
3497 #endif
3498
3499 Matrix res(m1.rows,m2.cols);
3500
3501
3502 for(int row = 0; row < res.rows; row++)
3503 {
3504 for(int col = 0; col < res.cols; col++)
3505 {
3506 //res.mat[row*res.cols+col] = 0; //matrix is already filled with zeros
3507 for(int i = 0; i < m1.rowlen[row]; i++)
3508 {
3509 res.mat[row*res.cols+col] += m1.mat[m1.rowind[row]+i]*m2.mat[m1.colind[m1.rowind[row]+i]*m2.cols+col];
3510 }
3511 }
3512 }
3513 return res;
3514 }
3515
Mult(const SparseMatrix & m1,const Matrix & m2,Matrix & res)3516 void Mult(const SparseMatrix& m1, const Matrix& m2, Matrix& res) //computes res=m1*m2
3517 {
3518 #ifndef __QUICKMATH
3519 release_assert(m1.cols==m2.rows);
3520 #endif
3521
3522 res.SetSize(m1.rows,m2.cols);
3523
3524 for(int row = 0; row < res.rows; row++)
3525 {
3526 for(int col = 0; col < res.cols; col++)
3527 {
3528 res.mat[row*res.cols+col] = 0;
3529 for(int i = 0; i < m1.rowlen[row]; i++)
3530 {
3531 res.mat[row*res.cols+col] += m1.mat[m1.rowind[row]+i]*m2.mat[m1.colind[m1.rowind[row]+i]*m2.cols+col];
3532 }
3533 }
3534 }
3535 }
3536
3537 //alex for double indices/not sorted indices rowref/colref
AddMatrix(const TArray<int> & rowref,const TArray<int> & colref,int lrow,int lcol,const Matrix & m)3538 void SparseMatrix::AddMatrix(const TArray<int>& rowref, const TArray<int>& colref, int lrow, int lcol, const Matrix& m)
3539 {
3540 int k;
3541 if (colref.Length() == 0) return;
3542 int rlmax, rind;
3543 //(*global_uo) << "colref=" << colref << "\n";
3544 for (int i=1; i <= lrow; i++)
3545 {
3546 int row = rowref(i)-1;
3547 k = 0; //temporary variable for searching index of actual row
3548 int j = 1;
3549 #if _DEBUG
3550 int j_old = j-1;
3551 #endif
3552 while(j <= lcol)
3553 {
3554 #if _DEBUG
3555 assert (j > j_old || k == 0);
3556 j_old = j;
3557 #endif
3558 if (m(i,j) != 0)
3559 {
3560 rlmax = RowLenMax0(row);
3561 rind = rowind[row];
3562
3563 if (k!=0 && k < rowlen[row] && rowlen[row] != 0 && colind[k+rind] > colref(j)-1) k = 0;
3564
3565 while (k < rowlen[row] && colind[k+rind] < colref(j)-1) k++;
3566
3567 if ((k < rowlen[row]) && (colref(j)-1 == colind[k+rind]))
3568 {
3569 mat[k+rind] += m(i,j);
3570 j++;
3571 k++; //$ AD: NOTE: this line may cause infinite loops when the DOF of a new entry occurs multiple times in the matrix m resp. colref array ("double indices")
3572 }
3573 else
3574 {
3575 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3576 if (k == rowlen[row] && k < rlmax)
3577 {
3578 //add new entry, because index not found
3579
3580 //int sk = 0;
3581 int lastcol = -1;
3582 if (k > 0) lastcol = colind[k-1+rind];
3583
3584 while(j <= lcol && k < rlmax && lastcol < (colref(j)-1))
3585 //$ AD: NOTE: further entries are likely to be missing as well
3586 // bailout 1: k < rlmax --> array full
3587 // bailout 2: lastcol < (colref(j)-1) --> in case colref is not ascending order
3588 {
3589 if (m(i,j) != 0)
3590 {
3591 //sk = 1;
3592 rowlen[row]++;
3593 mat[k+rind] = m(i,j);
3594 colind[k+rind] = colref(j)-1;
3595 lastcol = colref(j)-1;
3596 k++;
3597 }
3598 j++;
3599 }
3600 //if (sk) k--;
3601 //if (j <= lcol && lastcol > colref(j)-1) k = 0; //old
3602 if (j <= lcol && lastcol >= colref(j)-1) k = 0; //new proposal JG 4.4.2013
3603 }
3604 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3605 else //other case, e.g. no space for entry
3606 {
3607 (*this)(row+1,colref(j)) += m(i,j); // += also possible ...
3608 j++;
3609 }
3610 }
3611 }
3612 else
3613 {
3614 j++;
3615 }
3616 }
3617 }
3618 }
3619
3620 //Take Submatrix sm at (sr,sc), multiply with x and add it to local matrix at (r,c), size: (nr x nc)
AddSubmatrix(const Matrix & sm,int sr,int sc,int r,int c,int nr,int nc,double x)3621 void SparseMatrix::AddSubmatrix(const Matrix& sm, int sr, int sc, int r, int c, int nr, int nc, double x)
3622 {
3623 if (nr==0 || nc ==0) {return;}
3624 int k;
3625 int rlmax, rind;
3626 int offr = r-sr;
3627 int offc = c-sc;
3628 for (int i=sr; i < sr+nr; i++)
3629 {
3630 int row = i-1+offr;
3631 k = 0;
3632 int j = sc;
3633
3634 while (j < sc+nc)
3635 {
3636 if (sm(i,j) != 0)
3637 {
3638 //(*this)(row+1,j+offc) += x*sm(i,j);
3639 //j++;
3640 rlmax = RowLenMax0(row);
3641 rind = rowind[row];
3642
3643 while (k < rowlen[row] && colind[k+rind] < j-1+offc) k++;
3644
3645 if ((k < rowlen[row]) && (j-1+offc == colind[k+rind]))
3646 {
3647 mat[k+rind] += x*sm(i,j);
3648 j++; k++;
3649 }
3650 else
3651 {
3652 if (k == rowlen[row] && k < rlmax)
3653 {
3654 while(j < sc+nc && k < rlmax)
3655 {
3656 rowlen[row]++;
3657 mat[k+rind] = x*sm(i,j);
3658 colind[k+rind] = j-1+offc;
3659 j++; k++;
3660 }
3661 }
3662 else
3663 {
3664 (*this)(row+1,j+offc) += x*sm(i,j);
3665 j++;
3666 }
3667 }
3668
3669 }
3670 else j++;
3671 }
3672 }
3673 }
3674
3675 //Take Submatrix sm at (sr,sc), multiply with x and add it to local matrix at (r,c), size: (nr x nc)
AddSubmatrix(const SparseMatrix & sm,int sr,int sc,int r,int c,int nr,int nc,double x)3676 void SparseMatrix::AddSubmatrix(const SparseMatrix& sm, int sr, int sc, int r, int c, int nr, int nc, double x)
3677 {
3678 if (nr==0 || nc ==0) {return;}
3679 int k;
3680 int rlmax, rind;
3681
3682 int offr = r-sr;
3683 int offc = c-sc;
3684
3685 //int failed = 0;
3686
3687 for (int i=sr; i < sr+nr; i++)
3688 {
3689 int row = i-1+offr;
3690 k = 0;
3691 int scol = 0; //sparse column
3692 int ksm = 0;
3693 int rindsm = sm.rowind[i-1];
3694 int rowlensm = sm.rowlen[i-1];
3695
3696 //find starting index in sm-matrix
3697 while(sm.colind[ksm+rindsm] < sc-1 && ksm < rowlensm)
3698 {
3699 ksm++;
3700 }
3701 //do until end index sc+nc reached
3702 while(sm.colind[ksm+rindsm] < sc+nc-1 && ksm < rowlensm)
3703 {
3704 //(*this)(row+1,sm.colind[ksm+rindsm]+1+offc) += x*sm.mat[ksm+rindsm];
3705 //ksm++;
3706
3707 rlmax = RowLenMax0(row);
3708 rind = rowind[row];
3709
3710 int actcol = sm.colind[ksm+rindsm]+offc;
3711 //find starting index in this matrix
3712 while (k < rowlen[row] && colind[k+rind] < actcol) k++;
3713
3714 if ((k < rowlen[row]) && (colind[k+rind] == actcol))
3715 {
3716 mat[k+rind] += x*sm.mat[ksm+rindsm];
3717 k++; ksm++;
3718 }
3719 else
3720 {
3721 if (k == rowlen[row] && k < rlmax && sm.colind[ksm+rindsm] < sc+nc-1 && ksm < rowlensm)
3722 {
3723 while(sm.colind[ksm+rindsm] < sc+nc-1 && k < rlmax && ksm < rowlensm)
3724 {
3725 rowlen[row]++;
3726 mat[k+rind] = x*sm.mat[ksm+rindsm];
3727 colind[k+rind] = sm.colind[ksm+rindsm]+offc;
3728 k++; ksm++;
3729 }
3730 }
3731 else
3732 {
3733 //failed++;
3734 //rescue strategy, should not happen; is very inefficient!
3735 (*this)(row+1,sm.colind[ksm+rindsm]+1+offc) += x*sm.mat[ksm+rindsm];
3736 k++; ksm++;
3737 }
3738 }
3739
3740 }
3741 }
3742 //(*global_uo) << "AddSubmatrix rescue count=" << failed << "\n";
3743 }
3744
3745
3746 //set column vector, only entries of v from vr1 to vr2, insert at column col
SetColVector(const Vector & v,int vr1,int vr2,int col)3747 void SparseMatrix::SetColVector(const Vector& v, int vr1, int vr2, int col)
3748 {
3749 int row;
3750 for (int i = vr1; i <= vr2; i++)
3751 {
3752 if (fabs(v(i)) > sparsetolzero)
3753 {
3754 row = i-1;
3755 if ((rowlen[row] == 0 || col-1 > colind[rowind[row]+rowlen[row]-1]) && rowlen[row] < RowLenMax0(row))
3756 {
3757 mat[rowind[row]+rowlen[row]] = v(i);
3758 colind[rowind[row]+rowlen[row]] = col-1;
3759 rowlen[row]++;
3760 }
3761 else
3762 {
3763 (*this)(i,col) = v(i);
3764 }
3765 }
3766 }
3767 }
3768
SetColVector(const SparseVector & v,int col)3769 void SparseMatrix::SetColVector(const SparseVector& v, int col)
3770 {
3771 int row;
3772 double val;
3773 for (int i = 1; i <= v.NEntries(); i++)
3774 {
3775 v.GetEntry(i, row, val);
3776 if (fabs(val) > sparsetolzero)
3777 {
3778 row--;
3779 if ((rowlen[row] == 0 || col-1 > colind[rowind[row]+rowlen[row]-1]) && rowlen[row] < RowLenMax0(row))
3780 {
3781 mat[rowind[row]+rowlen[row]] = val;
3782 colind[rowind[row]+rowlen[row]] = col-1;
3783 rowlen[row]++;
3784 }
3785 else
3786 {
3787 (*this)(row+1,col) = val;
3788 }
3789 }
3790 }
3791 }
3792
3793 //eliminate zero entries in sparse matrix (caused by zero entries in stiffness matrices or by adding of pieces of matrices!)
3794 //otherwise, SuperLU does not work!!!
EliminateZeroEntries(double mindouble)3795 int SparseMatrix::EliminateZeroEntries(double mindouble)
3796 {
3797 int n = 0; //global count
3798
3799 if (rows != 0) release_assert(rowind[0] == 0);
3800
3801 for (int i = 0; i < rows; i++)
3802 {
3803 int rz = 0; //zeros in row
3804 int oldrowind = rowind[i];
3805 if (i > 0) rowind[i] = rowind[i-1]+rowlen[i-1]; //there might be emtpy entries in the SparseMatrix structure!
3806
3807 for (int j = 0; j < rowlen[i]; j++)
3808 {
3809 mat[rowind[i]+j-rz] = mat[oldrowind+j];
3810 colind[rowind[i]+j-rz] = colind[oldrowind+j];
3811
3812 //if (fabs(mat[rowind[i]+j]) <= mindouble)
3813 if (mat[oldrowind+j] == 0.)
3814 {
3815 rz++;
3816 }
3817 }
3818 rowlen[i] -= rz; //reduce actual length
3819 n += rz;
3820 }
3821 return n;
3822 }
3823
PrintData() const3824 void SparseMatrix::PrintData() const
3825 {
3826 for (int i=0; i < rows; i++)
3827 {
3828 (*global_uo) << "row " << i << ", rowptr=" << rowind[i] << ", rowlen=" << rowlen[i] << ": ";
3829 for (int j = 0; j < rowlen[i]; j++)
3830 {
3831 (*global_uo) << "(" << colind[rowind[i]+j] << "," << mat[rowind[i]+j] << "), ";
3832 }
3833 (*global_uo) << "\n";
3834 }
3835 }
3836
3837
operator <<(ostream & os,const SparseMatrix & m)3838 ostream& operator<<(ostream& os, const SparseMatrix& m)
3839 {
3840 double max=0;
3841 int i;
3842 max = m.MaxNorm();
3843 if (max==0) {max=1;}
3844 max=(int)log10(max);
3845 max=pow(10,max);
3846
3847 os << max << " *\n";
3848
3849 for (i=1; i<=m.Getrows(); i++)
3850 {
3851 os << "[";
3852 for (int j=1; j<=m.Getcols(); j++)
3853 {
3854 char str[32];
3855 sprintf(str,"% 1.4f",m(i,j)/max);
3856 os << str;
3857 if (j!=m.Getcols()) {os << ",";}
3858 }
3859 os << "]";
3860 os << "max=" << m.RowLenMax0(i) << "\n";
3861 }
3862
3863 return os;
3864 }
3865
3866
PrintToMatlabSparse(ostream & os)3867 void SparseMatrix::PrintToMatlabSparse(ostream& os)
3868 {
3869 int mode = 2;
3870
3871
3872 if (mode == 1)
3873 {
3874 os << "sparse([";
3875 //write rowindex in matrix (deliminated by space and ";")
3876 for (int i = 0; i < rows; i++)
3877 {
3878 for (int j = 0; j < rowlen[i]; j++)
3879 {
3880 char str[32];
3881 sprintf(str,"%d",i+1);
3882 os << str;
3883 if (!(i == rows-1 && j == rowlen[i]-1)) os << ";";
3884 }
3885 }
3886 os << "],[";
3887 //write colindex in matrix (deliminated by space and ";")
3888 for (int i = 0; i < rows; i++)
3889 {
3890 for (int j = 0; j < rowlen[i]; j++)
3891 {
3892 char str[32];
3893 sprintf(str,"%d",colind[rowind[i]+j]+1);
3894 os << str;
3895 if (!(i == rows-1 && j == rowlen[i]-1)) os << ";";
3896 }
3897 }
3898 os << "],[";
3899 //write matrix-entry in matrix (deliminated by space and ";")
3900 for (int i = 0; i < rows; i++)
3901 {
3902 for (int j = 0; j < rowlen[i]; j++)
3903 {
3904 char str[32];
3905 sprintf(str,"%1.16g",mat[rowind[i]+j]);
3906 os << str;
3907 if (!(i == rows-1 && j == rowlen[i]-1)) os << ";";
3908 }
3909 }
3910 os << "]";
3911 os << "," << Getrows() << "," << Getcols();
3912 os << ")";
3913 }
3914 else
3915 {
3916 //write rowindex, colindex, mat (deliminated by space)
3917 for (int i = 0; i < rows; i++)
3918 {
3919 for (int j = 0; j < rowlen[i]; j++)
3920 {
3921 char str[32];
3922 sprintf(str,"%d ",i+1);
3923 os << str;
3924 sprintf(str,"%d ",colind[rowind[i]+j]+1);
3925 os << str;
3926 sprintf(str,"%1.16g\n",mat[rowind[i]+j]);
3927 os << str;
3928 }
3929 }
3930 os << Getrows() << " " << Getcols() << " 0\n"; //highest entry specifies size of matrix ... 0 is added to remaining entries
3931 }
3932 }
3933
3934
3935
3936
3937
3938 // AP -- SuperLU
3939
~SuperLUInverse()3940 SuperLUInverse:: ~SuperLUInverse()
3941 {
3942 DestroySuperLUFactorization(SuperL, SuperU);
3943 delete[] perm_r;
3944 delete[] perm_c;
3945 }
3946
Solve(Vector & q)3947 int SuperLUInverse:: Solve(Vector& q)
3948 {
3949 rows = sparseA->Getrows();
3950 cols = sparseA->Getcols();
3951
3952 int* colind = NULL;
3953 double* mat = NULL;
3954 int* rowptr = NULL;
3955
3956 const_cast<SparseMatrix*>(sparseA)->EliminateZeroEntries();
3957 sparseA->GetMatPtrs(colind, rowptr, mat);
3958
3959 int nnonzero = sparseA->CountEntries();
3960 rowptr[rows] = nnonzero;
3961
3962 int output = 0;
3963 if (rows > 50000) output = 1;
3964 if (output) (*global_uo) << "SuperLU Solve ...";
3965 CallSuperLUSparseRow(rows, cols, nnonzero, rowptr, colind, mat, q.GetVecPtr());
3966 if (output) (*global_uo) << "done!\n";
3967
3968 return 1;
3969 }
3970
Factorize()3971 int SuperLUInverse:: Factorize()
3972 {
3973
3974 int* colind = NULL;
3975 double* mat = NULL;
3976 int* rowptr = NULL;
3977
3978 const_cast<SparseMatrix*>(sparseA)->EliminateZeroEntries();
3979 const_cast<SparseMatrix*>(sparseA)->EliminateZeroEntries();
3980 sparseA->GetMatPtrs(colind, rowptr, mat);
3981 int nnonzero = sparseA->CountEntries();
3982 rowptr[rows] = nnonzero;
3983 rows = sparseA->Getrows();
3984 cols = sparseA->Getcols();
3985
3986 delete[] perm_r;
3987 delete[] perm_c;
3988 perm_r = new int(rows);
3989 perm_c = new int(cols);
3990
3991 //int* perm_r_ptr = perm_r.GetDataPtr();
3992 //int* perm_c_ptr = perm_c.GetDataPtr();
3993 int rv = CallSuperLUFactorize(rows, cols, nnonzero, rowptr, colind, mat,
3994 SuperA, SuperL, SuperU, perm_r, perm_c, isFirstStep);
3995
3996 isFactorized = 1;
3997 if (rv == 0) return 1;
3998 else return 0;
3999 }
4000
SuperLUApply(Vector & q)4001 int SuperLUInverse:: SuperLUApply(Vector& q)
4002 {
4003 if (!isFactorized) Factorize();
4004
4005 rows = sparseA->Getrows();
4006 cols = sparseA->Getcols();
4007
4008 //int* perm_r_ptr = perm_r.GetDataPtr();
4009 //int* perm_c_ptr = perm_c.GetDataPtr();
4010 int rv = CallSuperLUApply(rows, cols, q.GetVecPtr(1), SuperA, SuperL, SuperU, perm_r, perm_c);
4011
4012 //(*global_uo) << "SuperLUApply returns " << rv << "\n";
4013
4014 if (rv == 0) return 1;
4015 else return 0;
4016 }
4017
4018
4019
4020 #ifdef USE_MKL
4021 #include <mkl_pardiso.h>
4022
~PardisoInverse()4023 PardisoInverse::~PardisoInverse()
4024 {
4025 //int maxfct=1, mnum=1;
4026 //int phase=-1;
4027 //int* ja=0, *ia=0;
4028 double* a=0;
4029 //int nrhs=1;
4030 //int msglevel=1;
4031 double*x=0;
4032 double*b=0;
4033 _INTEGER_t maxfct=1, mnum=1, phase=-1;
4034 _INTEGER_t* ja=0, *ia=0;
4035 _INTEGER_t nrhs=1, msglevel=1;
4036
4037
4038 pardiso (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia, ja, perm, &nrhs, iparm, &msglevel, b, x, &error);
4039
4040 delete[] perm;
4041 }
4042
SetDefaultIParm()4043 void PardisoInverse::SetDefaultIParm()
4044 {
4045 iparm[0] = 0; // use default values
4046 iparm[1] = 0; // 0..minimum degree ordering
4047 iparm[2] = 1; // number of processors used
4048 iparm[3] = 0; // direct..0, or PCG..1/2
4049 iparm[4] = 0; // 0..dont use user-permutation, 1..user-defined perm, 2..perm returned
4050 iparm[5] = 0; // 1 .. store solution in b, 0 .. store solution in x
4051 iparm[7] = 1; // iterative refinement steps
4052 iparm[8] = 0; // not used, has to be zero
4053 iparm[9] = 8; // accuracy
4054 if (mtype == 11 || mtype == 13) // unsymmetric matrices
4055 {
4056 iparm[10] = 1; // scaling
4057 iparm[12] = 1; // improved accuracy
4058 }
4059 else // mtype = -2, -4, 6, symmetric matrices
4060 {
4061 iparm[10] = 0; // no scaling
4062 iparm[12] = 0; // no improved accuracy
4063 }
4064 iparm[11] = 0; // not used, has to be zero
4065 iparm[17] = -1; // report number of non-zero entries if < 0
4066 iparm[18] = -1; // report number of MFlops
4067 iparm[20] = 0; // 1x1 pivoting
4068 iparm[26] = 0; // check arrays ia, ja
4069 iparm[27] = 0; // 0..double precision, 1..single
4070 iparm[59] = 0; // in-core..0, out of core=1
4071 }
4072
4073
Solve(Vector & q)4074 int PardisoInverse::Solve(Vector& q)
4075 {
4076 // initialize internal data pointer (necessary for MKL-pardiso)
4077 for (int i=0; i<64; i++)
4078 pt[i] = 0;
4079
4080 //int maxfct=1, mnum=1;
4081 //int phase=13;
4082 //int* ja, *ia;
4083 double* a;
4084 //int nrhs=1;
4085 //int msglevel=1;
4086 _INTEGER_t maxfct=1, mnum=1, phase=13;
4087 int* ja,* ia;
4088 _INTEGER_t nrhs=1, msglevel=1;
4089
4090
4091 Vector sol(q.Length());
4092
4093 iparm[5] = 1; // replace right hand side by solution
4094 double*b=q.GetVecPtr();
4095 double*x=sol.GetVecPtr();
4096
4097 const_cast<SparseMatrix*>(sparseA)->EliminateZeroEntries();
4098 sparseA->GetMatPtrs(ja, ia, a);
4099 int nnz = sparseA->CountEntries();
4100 _INTEGER_t* ia2 = new _INTEGER_t[size+1];
4101 for (int i=0; i<size; i++)
4102 ia2[i] = ia[i]+1;
4103 ia2[size] = nnz+1;
4104 _INTEGER_t* ja2 = new _INTEGER_t[nnz];
4105 for (int i=0; i<nnz; i++)
4106 ja2[i] = ja[i]+1;
4107
4108
4109 PARDISO (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia2, ja2, perm, &nrhs, iparm, &msglevel, b, x, &error);
4110
4111 isFactorized = 1;
4112 isFirstStep = 0;
4113 //q = sol;
4114
4115 delete [] ia2;
4116 delete [] ja2;
4117 if (!error) return 1;
4118 else return 0;
4119 }
4120
Factorize()4121 int PardisoInverse::Factorize()
4122 {
4123 // initialize internal data pointer (necessary for MKL-pardiso)
4124 for (int i=0; i<64; i++)
4125 pt[i] = 0;
4126
4127 //int maxfct=1, mnum=1;
4128 //int phase=13;
4129 //int* ja, *ia;
4130 double* a;
4131 //int nrhs=1;
4132 //int msglevel=1;
4133 _INTEGER_t maxfct=1, mnum=1, phase=12;
4134 int* ja,* ia;
4135 _INTEGER_t nrhs=1, msglevel=1;
4136
4137 // initialize internal data pointer (necessary for MKL-pardiso)
4138 for (int i=0; i<64; i++)
4139 pt[i] = 0;
4140
4141 double*b=0;
4142 double*x=0;
4143
4144 const_cast<SparseMatrix*>(sparseA)->EliminateZeroEntries();
4145 sparseA->GetMatPtrs(ja, ia, a);
4146 int nnz = sparseA->CountEntries();
4147 _INTEGER_t* ia2 = new _INTEGER_t[size+1];
4148 for (int i=0; i<size; i++)
4149 ia2[i] = ia[i]+1;
4150 ia2[size] = nnz+1;
4151 _INTEGER_t* ja2 = new _INTEGER_t[nnz];
4152 for (int i=0; i<nnz; i++)
4153 ja2[i] = ja[i]+1;
4154
4155 PARDISO (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia2, ja2, perm, &nrhs, iparm, &msglevel, b, x, &error);
4156 //PARDISO (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia2, ja2, perm, &nrhs, iparm, &msglevel, b, x, &error);
4157
4158 isFactorized = 1;
4159 isFirstStep = 0;
4160 delete [] ia2;
4161 delete [] ja2;
4162 if (!error) return 1;
4163 else return 0;
4164 }
Apply(Vector & q)4165 int PardisoInverse::Apply(Vector& q)
4166 {
4167 //int maxfct=1, mnum=1;
4168 //int phase=33;
4169 //int *ja, *ia;
4170 double* a;
4171 //int nrhs=1;
4172 //int msglevel=1;
4173 _INTEGER_t maxfct=1, mnum=1, phase=33;
4174 int* ja,* ia;
4175 _INTEGER_t nrhs=1, msglevel=1;
4176
4177 iparm[5] = 1; // replace right hand side by solution
4178
4179 double*b = q.GetVecPtr();
4180 double*x = new double[q.Length()];
4181 sparseA->GetMatPtrs(ja, ia, a);
4182 int nnz = sparseA->CountEntries();
4183 _INTEGER_t* ia2 = new _INTEGER_t[size+1];
4184 for (int i=0; i<size; i++)
4185 ia2[i] = ia[i]+1;
4186 ia2[size] = nnz+1;
4187 _INTEGER_t* ja2 = new _INTEGER_t[nnz];
4188 for (int i=0; i<nnz; i++)
4189 ja2[i] = ja[i]+1;
4190
4191 PARDISO (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia2, ja2, perm, &nrhs, iparm, &msglevel, b, x, &error);
4192 //PARDISO (pt, &maxfct, &mnum, &mtype, &phase, &size, a, ia2, ja2, perm, &nrhs, iparm, &msglevel, b, x, &error);
4193
4194 delete [] ia2;
4195 delete [] ja2;
4196 delete [] x;
4197
4198 if (!error) return 1;
4199 else return 0;
4200 }
4201
4202 #else
4203
SetDefaultIParm()4204 void PardisoInverse::SetDefaultIParm()
4205 {
4206 }
4207
~PardisoInverse()4208 PardisoInverse::~PardisoInverse()
4209 {
4210 delete[] perm;
4211 }
4212
Solve(Vector & q)4213 int PardisoInverse::Solve(Vector& q)
4214 {
4215 (*global_uo) << "Error in PardisoInverse::Solve: Pardiso Solver not available!\n";
4216 return 0;
4217 }
4218
Factorize()4219 int PardisoInverse::Factorize()
4220 {
4221 (*global_uo) << "Error in PardisoInverse::Factorize: Pardiso Solver not available!\n";
4222 return 0;
4223 }
Apply(Vector & q)4224 int PardisoInverse::Apply(Vector& q)
4225 {
4226 (*global_uo) << "Error in PardisoInverse::Apply: Pardiso Solver not available!\n";
4227 return 0;
4228 }
4229
4230 #endif //USE_MKL for Pardiso
4231
4232
SparseInverse(const SparseMatrix & matA)4233 SparseInverse::SparseInverse(const SparseMatrix & matA)
4234 {
4235 #ifdef USE_MKL
4236 pardisoinv = new PardisoInverse(matA);
4237 superluinv = NULL;
4238 #else
4239 superluinv = new SuperLUInverse(matA);
4240 pardisoinv = NULL;
4241 #endif
4242 }
4243
4244
4245 ///--------------------
4246 /// SparseInverse wrapper - choose pardiso or superlu
~SparseInverse()4247 SparseInverse::~SparseInverse()
4248 {
4249 delete pardisoinv;
4250 delete superluinv;
4251 }
4252
SetSparseMatrix(const SparseMatrix & matA)4253 void SparseInverse::SetSparseMatrix(const SparseMatrix& matA)
4254 {
4255 #ifdef USE_MKL
4256 pardisoinv->SetSparseMatrix(matA);
4257 #else
4258 superluinv->SetSparseMatrix(matA);
4259 #endif
4260 }
4261
Solve(Vector & q)4262 int SparseInverse::Solve(Vector& q)
4263 {
4264 #ifdef USE_MKL
4265 return pardisoinv->Solve(q);
4266 #else
4267 return superluinv->Solve(q);
4268 #endif
4269
4270 }
Factorize()4271 int SparseInverse::Factorize()
4272 {
4273 #ifdef USE_MKL
4274 return pardisoinv->Factorize();
4275 #else
4276 return superluinv->Factorize();
4277 #endif
4278 }
Apply(Vector & q)4279 int SparseInverse::Apply(Vector& q)
4280 {
4281 #ifdef USE_MKL
4282 return pardisoinv->Apply(q);
4283 #else
4284 return superluinv->Apply(q);
4285 #endif
4286
4287 }
4288
IsFirstStep()4289 int& SparseInverse::IsFirstStep()
4290 {
4291 #ifdef USE_MKL
4292 return pardisoinv->IsFirstStep();
4293 #else
4294 return superluinv->IsFirstStep();
4295 #endif
4296 }
IsFirstStep() const4297 const int& SparseInverse::IsFirstStep() const
4298 {
4299 #ifdef USE_MKL
4300 return pardisoinv->IsFirstStep();
4301 #else
4302 return superluinv->IsFirstStep();
4303 #endif
4304 }
4305
4306
SuperLUSolve(SparseMatrix & A,Vector & q)4307 int SuperLUSolve(SparseMatrix& A, Vector& q)
4308 {
4309 int* colind = NULL;
4310 double* mat = NULL;
4311 int* rowptr = NULL;
4312
4313
4314 /*
4315 Matrix AF = A.GetMatrix();
4316 (*global_uo) << "AF=" << AF << "\n";
4317 A.PrintData();
4318 */
4319
4320 A.EliminateZeroEntries();
4321 A.GetMatPtrs(colind, rowptr, mat);
4322
4323 //A.CopyFrom(AF);
4324 //A.PrintData();
4325 //A.PrintToMatlabSparse(fout);
4326
4327 int nnonzero = A.CountEntries();
4328 int rows = A.Getrows();
4329 int cols = A.Getcols();
4330 rowptr[rows] = nnonzero;
4331
4332 int output = 0;
4333 if (rows > 50000) output = 1;
4334 if (output) (*global_uo) << "SuperLU Solve ...";
4335 CallSuperLUSparseRow(rows, cols, nnonzero, rowptr, colind, mat, q.GetVecPtr());
4336 if (output) (*global_uo) << "done!\n";
4337
4338 return 1;
4339 }
4340
SuperLUFactorize(SparseMatrix & A,SuperMatrix * & SuperA,SuperMatrix * & L,SuperMatrix * & U,int * & perm_r,int * & perm_c,int isFirstStep)4341 int SuperLUFactorize(SparseMatrix& A, SuperMatrix*& SuperA, SuperMatrix*& L, SuperMatrix*& U, int*& perm_r, int*& perm_c, int isFirstStep)
4342 {
4343 int* colind = NULL;
4344 double* mat = NULL;
4345 int* rowptr = NULL;
4346
4347 A.EliminateZeroEntries();
4348 A.GetMatPtrs(colind, rowptr, mat);
4349 int nnonzero = A.CountEntries();
4350 int rows = A.Getrows();
4351 int cols = A.Getcols();
4352 rowptr[rows] = nnonzero;
4353
4354 int rv = CallSuperLUFactorize(rows, cols, nnonzero, rowptr, colind, mat,
4355 SuperA, L, U, perm_r, perm_c, isFirstStep);
4356
4357 // (*global_uo) << "SuperLUFactorize returns " << rv << "\n";
4358
4359 if (rv == 0) return 1;
4360 else return 0;
4361 }
4362
SuperLUApply(SparseMatrix & A,SuperMatrix * & SuperA,SuperMatrix * & L,SuperMatrix * & U,int * & perm_r,int * & perm_c,Vector & q)4363 int SuperLUApply(SparseMatrix& A, SuperMatrix*& SuperA, SuperMatrix*& L, SuperMatrix*& U, int*& perm_r, int*& perm_c, Vector& q)
4364 {
4365 int rows = A.Getrows();
4366 int cols = A.Getcols();
4367
4368 int rv = CallSuperLUApply(rows, cols, q.GetVecPtr(), SuperA, L, U, perm_r, perm_c);
4369
4370 // (*global_uo) << "SuperLUApply returns " << rv << "\n";
4371
4372 if (rv == 0) return 1;
4373 else return 0;
4374 //return 1;
4375 }
4376
4377
4378
GetExtremeValues(const Vector & data,TArray<int> & max_indices,TArray<double> & max_values,TArray<int> & min_indices,TArray<double> & min_values,int compute_minima)4379 void GetExtremeValues(const Vector& data, TArray<int>& max_indices,TArray<double>& max_values, TArray<int>& min_indices,TArray<double>& min_values, int compute_minima)
4380 {
4381 max_indices.SetLen(0);
4382 max_values.SetLen(0);
4383
4384 if(compute_minima)
4385 {
4386 min_indices.SetLen(0);
4387 min_values.SetLen(0);
4388 }
4389
4390 if(data.GetLen()==1)
4391 {
4392 return;
4393 }
4394 int sign = 1; //search maxima
4395 double dataoldold = data.Get(1)*sign;
4396 double dataold = data.Get(2)*sign;
4397
4398 // check if first value is maxima or minima
4399 if(dataoldold > dataold)
4400 {
4401 max_indices.Add(1);
4402 max_values.Add(dataoldold);
4403 }
4404 if(compute_minima && dataoldold < dataold)
4405 {
4406 min_indices.Add(1);
4407 min_values.Add(dataoldold);
4408 }
4409
4410 // check local minima inside
4411 for(int j=1;j<=2;j++)
4412 {
4413 if(j==2)sign=-1; //search minima
4414
4415 for(int i=3;i<=data.Length();i++)
4416 {
4417 if(dataoldold < dataold && dataold > data.Get(i)*sign)
4418 {
4419 // data(i-1)*factor is already a maximum
4420 if(sign>0)
4421 {
4422 max_indices.Add(i-1);
4423 max_values.Add(dataold);
4424 }
4425 else if(compute_minima)
4426 {
4427 min_indices.Add(i-1);
4428 min_values.Add(dataold);
4429 }
4430 }
4431 dataoldold = dataold;
4432 dataold = data.Get(i)*sign;
4433 }
4434 }
4435
4436 // check if first value is maxima or minima
4437 if(data.Get(data.Length()) > data.Get(data.Length()-1))
4438 {
4439 max_indices.Add(data.Length());
4440 max_values.Add(data.Get(data.Length()));
4441 }
4442 if(compute_minima && data.Get(data.Length()) < data.Get(data.Length()-1))
4443 {
4444 min_indices.Add(data.Length());
4445 min_values.Add(data.Get(data.Length()));
4446 }
4447 }
4448
4449 //symmetrical smoothing
SmoothenDoubleArray(Vector & values,Vector & result,int smoothing_length)4450 void SmoothenDoubleArray(Vector& values, Vector& result, int smoothing_length)
4451 {
4452 if (smoothing_length == 0)
4453 {
4454 result = values;
4455 return;
4456 }
4457 for(int i=1; i<=values.Length(); i++)
4458 {
4459 double avg = 0;
4460 int cnt = 0;
4461 for (int j = -smoothing_length; j<=smoothing_length; j++)
4462 {
4463 if (i+j >= 1 && i+j<=values.Length())
4464 {
4465 avg += values(i+j);
4466 cnt++;
4467 }
4468 }
4469 if (cnt)
4470 result(i) = avg/(double)cnt;
4471 else
4472 result(i) = 0;
4473 }
4474 }
4475
4476 /*
4477 //does not work: old version:
4478
4479 //generate new matrix from sparse matrix, only from row1 to row2, col1 to col2 (incl.), resort with vector r
4480 void SparseMatrix::CopyFrom(const SparseMatrix& mati, int row1, int col1, int row2, int col2, const IVector& r)
4481 {
4482 int nrows = row2-row1+1;
4483 int ncols = col2-col1+1;
4484
4485 mystatic IVector invr;
4486 invr.SetLen(r.Length());
4487 for (int i=1; i <= r.Length(); i++) invr(r(i)) = i;
4488
4489 int neededmem = 0;
4490
4491
4492 for (int i = 0; i < nrows; i++)
4493 {
4494 int matirow = r(i+row1)-1;
4495 int rind = mati.rowind[matirow];
4496
4497 //read out necessary entries and transform by r:
4498 for (int j = 0; j < mati.rowlen[matirow]; j++)
4499 {
4500 int c = mati.colind[rind+j]; //0-based
4501 int cr = r(c+1); //1-based
4502 if (cr >= col1 && cr <= col2) neededmem++;
4503 }
4504 }
4505
4506
4507 if (neededmem > lalloc || nrows > rows)
4508 {
4509 Destroy();
4510
4511 lalloc = neededmem;
4512 GenMat(lalloc, nrows);
4513 }
4514
4515 rows = nrows;
4516 cols = ncols;
4517
4518
4519
4520 mystatic TArray<double> rsort; //resorted values
4521 mystatic TArray<int> rsortind; //resorted column indices 0-based for destination
4522
4523
4524 //sm 1 2 3 4 5 6 7 8 9
4525 //rind 1 3 6 8
4526 //mat 11 12 13 14
4527 //r 1 2 8 3 4 5 9 6 7
4528 //invr 1 2 4 5 6 8 9 3 7
4529 //m 11 14 12 13
4530 //
4531
4532 int c, cr;
4533 int cnt = 0;
4534
4535 //(*global_uo) << "lalloc=" << lalloc << ", nrows=" << nrows << "\n";
4536
4537 //TMStartTimer(24);
4538 for (int i = 0; i < rows; i++)
4539 {
4540 rsortind.SetLen(0);
4541 rsort.SetLen(0);
4542
4543 int matirow = r(i+row1)-1;
4544 int rind = mati.rowind[matirow];
4545
4546 //read out necessary entries and transform by r:
4547 for (int j = 0; j < mati.rowlen[matirow]; j++)
4548 {
4549 c = mati.colind[rind+j]; //0-based
4550 cr = r(c+1); //1-based
4551 if (cr >= col1 && cr <= col2)
4552 {
4553 rsortind.Add(invr(c+1)-col1); //0-based index for destination
4554 //runsortind.Add(c-col1+1); //0-based index for destination
4555 rsort.Add(mati.mat[rind+j]); //entry
4556 }
4557 }
4558 //sort entries
4559 //TMStartTimer(25);
4560 Quicksort(rsortind, rsort);
4561 //TMStopTimer(25);
4562
4563 //fill in entries in i-th row
4564 rowind[i] = cnt;
4565 int cnt2 = 0;
4566
4567
4568
4569 for (j=0; j < rsortind.Length(); j++)
4570 {
4571 if (rsort.Get0(j) != 0)
4572 {
4573 colind[cnt2+rowind[i]] = rsortind.Get0(j);
4574 //(*global_uo) << "(" << cnt2+rowind[i] << "," << colind[cnt2+rowind[i]] << ") ";
4575
4576 mat[cnt2+rowind[i]] = rsort.Get0(j);
4577 cnt2++;
4578 }
4579 }
4580 //(*global_uo) << "\n";
4581 */