Scopira  20080306
matrixmath.h
1 
2 /*
3  * Copyright (c) 2002-2003 National Research Council
4  *
5  * All rights reserved.
6  *
7  * This material is confidential and proprietary information of
8  * National Research Council Canada ("Confidential Information").
9  * This Confidential Information may only be used and reproduced
10  * in accordance with the terms of the license agreement.
11  *
12  */
13 
14 #ifndef __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__
15 #define __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__
16 
17 #include <scopira/tool/output.h>
18 #include <scopira/basekit/math.h>
19 #include <scopira/basekit/narray.h>
20 
21 // THIS FILE HAS BEEN FULLY DOCUMENTED
22 
23 namespace scopira
24 {
25  namespace basekit
26  {
27  /*************************
28  new data model stuff
29  *************************/
30 
38  template <class T, class SCA>
39  void mul_matrix(T &dest, SCA d);
40 
51  template <class T, class T2, class T3>
52  void mul_matrix(T &dest, const T2 &m1, const T3 &m2, bool t1 = false, bool t2 = false);
53 
62  template <class T>
63  bool invert_matrix(T &dest, const T& src);
64 
72  template <class D, class S>
73  void transpose_matrix(D &dest, const S& src);
74 
81  template <class SRC, class DEST>
82  void resample_nn(DEST &dest, const SRC &src);
83 
90  template <class SRC, class DEST>
91  void resample_linear(DEST &dest, const SRC &src);
92 
102  template <class T>
103  bool lu_decomposition(narray<T,2>& dest, const narray<T,2>& src,narray<int>& idx, bool& odd);
104 
115  template <class T>
116  void lu_backsubstitution(narray<T>& dest, const narray<T,2>& inv_a, const narray<int>& index);
117 
127  template <class T>
128  bool svd(narray<T,2>& Mu,narray<T>& Vw,narray<T,2>& Mv);
129 
141  template <class T>
142  void svd_backsubstitution(narray<T,2> &Mu, narray<T> &Vw, narray<T,2> &Mv, narray<T> &b, narray<T> &x);
143 
148  template <class DT>
149  DT pythagorean(DT a, DT b);
150 
158  template <class T>
159  void elim_hessian(narray<T,2> &matrix);
160 
170  template <class T>
171  void eigen_hessenberg(narray<T,2> &matrix, narray<T> &eigen_real, narray<T> &eigen_imaginary);
172 
183  template <class T>
184  void gauss_jordan(narray<T,2> &matrix, narray<T,2> &rhs);
185 
194  template <class T>
195  void sort_covariance(narray<T,2> &matrix, narray<int> &fitted, int mfit);
196  }
197 }
198 
199 template <class T, class SCA>
200  void scopira::basekit::mul_matrix(T &dest, SCA d)
201 {
202  typename T::iterator tar, endtar;
203 
204  endtar = dest.end();
205  for (tar = dest.begin(); tar != endtar; ++tar)
206  *tar *= d;
207 }
208 
209 template <class T, class T2, class T3>
210  void scopira::basekit::mul_matrix(T &dest, const T2 &m1, const T3 &m2, bool t1, bool t2)
211 {
212  typedef typename T::data_type data_type;
213  size_t w, h, x, y, i, len;
214  size_t rx1, ry1, *x1, *y1;
215  size_t rx2, ry2, *x2, *y2;
216  data_type sum;
217 
218  // calc my dimensions and settings by the transpose flags
219  if (t1) {
220  h = m1.width();
221  len = m1.height();
222  x1 = &ry1;
223  y1 = &rx1;
224  } else {
225  h = m1.height();
226  len = m1.width();
227  x1 = &rx1;
228  y1 = &ry1;
229  }
230  if (t2) {
231  w = m2.height();
232  x2 = &ry2;
233  y2 = &rx2;
234  } else {
235  w = m2.width();
236  x2 = &rx2;
237  y2 = &ry2;
238  }
239 
240  assert( (t1 ? m1.height() : m1.width()) == (t2 ? m2.width() : m2.height()) );
241 
242  // resize myself
243  dest.resize(w, h);
244 
245  // do the multiply
246  for (y=0; y<h; y++)
247  for (x=0; x<w; x++) {
248  // start the accumulator
249  sum = 0;
250  *y1 = y;
251  *x2 = x;
252  for (i=0; i<len; i++) {
253  *x1 = i;
254  *y2 = i;
255  sum += m1(rx1, ry1) * m2(rx2, ry2);
256  }
257  dest(x,y) = sum;
258  }
259 }
260 
261 template <class T>
262  bool scopira::basekit::invert_matrix(T &dest, const T& src)
263 {
264  typedef typename T::data_type data_type;
265  size_t i, j, n;
266  bool odd;
267  narray<data_type,2> m_orig;
268  narray<int> v_idx;
269  narray<data_type> v_col;
270 
271  n = src.width();
272 
273  m_orig.resize(n, src.height());
274  dest.resize(n, src.height());
275  v_idx.resize(n);
276  v_idx.clear();
277  v_col.resize(n);
278  v_col.clear();
279 
280  //decompose the matrix just once
281  if (!scopira::basekit::lu_decomposition<data_type>(m_orig, src, v_idx, odd))
282  return false; // a singularity in the time continuum has been detected captain, abort!
283 
284  //find inverse by columns
285  for (j=0; j<n; j++) {
286  v_col.clear();
287  v_col.set(j, 1.0);
288  scopira::basekit::lu_backsubstitution<data_type>(v_col, m_orig, v_idx);
289  for (i=0; i<n; i++)
290  dest.set(j, i, v_col.get(i));
291  }
292 
293  return true;
294 }
295 
296 template <class D, class S>
297  void scopira::basekit::transpose_matrix(D &dest, const S& src)
298 {
299  size_t x, y, w, h;
300 
301  w = src.width();
302  h = src.height();
303 
304  dest.resize(h, w);
305 
306  for (y=0; y<h; ++y)
307  for (x=0; x<w; ++x)
308  dest(y,x) = src(x,y);
309 }
310 
311 template <class SRC, class DEST>
312  void scopira::basekit::resample_nn(DEST &dest, const SRC &src)
313 {
314  typedef typename DEST::data_type data_type;
315 
316  if (dest.empty() || src.empty())
317  return;
318 
319  for (size_t y=0; y<dest.height(); ++y)
320  for (size_t x=0; x<dest.width(); ++x)
321  dest(x,y) = src(src.width() * x / dest.width(), src.height() * y / dest.height());
322 }
323 
324 template <class SRC, class DEST>
325  void scopira::basekit::resample_linear(DEST &dest, const SRC &src)
326 {
327  typedef typename DEST::data_type data_type;
328  double xsrc_rate, xleft, xremainer, xfer; // all in "src" units
329  double xdest_rate;
330  double ysrc_rate, yleft, yremainer, yfer; // all in "src" units
331  double ydest_rate;
332 
333  if (dest.empty() || src.empty())
334  return;
335 
336  dest.set_all(0);
337 
338  xsrc_rate = static_cast<double>(src.width()) / dest.width();
339  ysrc_rate = static_cast<double>(src.height()) / dest.height();
340  // used a mutliplier to convert src-units to dest
341  xdest_rate = static_cast<double>(dest.width()) / src.width();
342  ydest_rate = static_cast<double>(dest.height()) / src.height();
343 
344  yremainer = 0;
345  size_t ysrc = 0;
346  for (size_t ydest=0; ydest<dest.height(); ++ydest) {
347  yleft = ysrc_rate;
348 
349  while (!scopira::basekit::is_zero(yleft)) {
350  yfer = yleft;
351  if (yfer + yremainer > 1)
352  yfer = 1 - yremainer;
353 
354  xremainer = 0;
355  size_t xsrc = 0;
356  for (size_t xdest=0; xdest<dest.width(); ++xdest) {
357  xleft = xsrc_rate;
358 
359  // while we still have work to do, iterate over the src blocks
360  while (!scopira::basekit::is_zero(xleft)) {
361  xfer = xleft;
362  if (xfer + xremainer > 1)
363  xfer = 1 - xremainer;
364  dest(xdest,ydest) += static_cast<data_type>(src(xsrc,ysrc) * xfer * xdest_rate * yfer * ydest_rate);
365  xleft -= xfer;
366  xremainer += xfer;
367  if (xremainer >= 1) {
368  assert(scopira::basekit::is_equal(xremainer,1.0)); // >1 shouldnt happen
369  xremainer = 0;
370  ++xsrc;
371  }
372  }//while xleft
373  }//for xdest
374 
375  yleft -= yfer;
376  yremainer += yfer;
377  if (yremainer >= 1) {
378  assert(scopira::basekit::is_equal(yremainer,1.0));
379  yremainer = 0;
380  ++ysrc;
381  }
382  }//while yleft
383  }//for ydest
384 }
385 
386 // Calculates LU decomposition of a rowise permutation.
387 template <class T>
388  bool scopira::basekit::lu_decomposition(narray<T,2>& dest, const narray<T,2>& src, narray<int>& v_idx, bool& odd)
389 {
390  int i, max, j, k, n;
391  T largest, dum, sum, tmp;
392  //stores the implicit scaling of each row
393  narray<T> scaling;
394 
395  n = src.width();
396  dest.copy(src);
397  scaling.resize(n);
398  scaling.clear();
399  //no rows interchanged yet
400  odd = false;
401  // to avoid compile warning
402  tmp = 0;
403 
404  //loop over the rows to get the implicit scaling information
405  for (i=0; i<n; i++) {
406  largest = 0;
407  for (j=0; j<n; j++) {
408  tmp = ::fabs(dest.get(j,i));
409  if (tmp > largest)
410  largest = tmp;
411  }
412  // no non-zero largest element
413  if (largest == 0)
414  return false;
415  //save the scaling
416  scaling.set(i, 1.0/largest);
417  }
418 
419  //loop over columns
420  for (j=0; j<n; j++) {
421  for (i=0; i<j; i++) {
422  sum = dest.get(j,i);
423  for (k=0; k<i; k++)
424  sum -= dest.get(k, i) * dest.get(j, k);
425  dest.set(j, i, sum);
426  }
427  //init the search for the largest pivot element
428  largest = 0;
429  max = 0;
430  for (i=j; i<n; i++) {
431  sum = dest.get(j, i);
432  for (k=0; k<j; k++)
433  sum -= dest.get(k, i) * dest.get(j, k);
434  dest.set(j, i, sum);
435  //is the figure of merit for the pivot better than the best so far
436  dum=scaling.get(i) * ::fabs(sum);
437  if (dum >= largest) {
438  largest = dum;
439  max = i;
440  }
441  }
442  //do we need to interchange rows
443  if (j != max) {
444  //yes, do so...
445  for (k=0; k<n; k++) {
446  dum = dest.get(k, max);
447  dest.set(k, max, dest.get(k, j));
448  dest.set(k, j, dum);
449  }
450  //...and change the parity of odd
451  odd = !odd;
452  //also interchange the scale factor
453  scaling.set(max, scaling.get(j));
454  }
455  v_idx.set(j, max);
456  if (dest.get(j, j) == 0)
457  dest.set(j, j, tinynum_c);
458  //if the pivot element is zero, the matrix is singular (at least to the precision of
459  //the algorithm. For some applications on singular matrices, it is desirable to
460  //substitue tinynum_c for zero
461  if (j != (n-1)) {
462  //now, finally, divide by the pivot element
463  dum = 1.0 / dest.get(j, j);
464  for (i=j+1; i<n; i++)
465  dest.set(j, i, dest.get(j,i)*dum);
466  }
467  } //go back for the next column in the reduction
468  return true;
469 }
470 
471 
472 // Solves the set of n linear equations a�x = b.
473 template <class T>
475 {
476  int i, j,k, m, n;
477  T sum;
478 
479  n = inv_a.width();
480  m = -1;
481 
482  //forward substitution,
483  for (i=0; i<n; i++) {
484  k = index.get(i);
485  sum = dest.get(k);
486  dest.set(k, dest.get(i));
487  if (m != -1)
488  for (j=m; j<= i-1; j++)
489  sum -= inv_a.get(j, i) * dest.get(j);
490  else
491  if (sum != 0.0)
492  m=i;
493  dest.set(i, sum);
494  }
495 
496  dest.copy(dest);
497  //backsubstitution,
498  for (i=n-1; i>=0; i--) {
499  sum = dest.get(i);
500  for (j=i+1; j<n; j++)
501  sum-=inv_a.get(j,i) * dest.get(j);
502  //store a component of the solution vector dest.
503  dest.set(i, sum/inv_a.get(i,i));
504  }
505 }
506 
507 
508 namespace scopira
509 {
510  namespace basekit
511  {
513  template <class T>
514  inline T svd_sign(T v1, T v2) { if (v2>=0.0) return fabs(v1); else return -fabs(v1); }
515  }
516 }
517 
518 //construct the singular value decomposition of the matrix u
519 template <class T>
521 {
522 
523  int i,j,k,left,height,width,nm,q,p;
524  T a,b,c,num_r,h,s,scale,x,y,z;
525  bool flag;
526  narray<T> vec_t;
527 
528  width = u.width();
529  height=u.height();
530 
531  v.resize(width,width);
532  v.clear();
533  w.resize(width);
534  w.clear();
535 
536  b=scale=num_r=0.0;
537  left= nm = 0;
538 
539  vec_t.resize(width);
540  vec_t.clear();
541  //householder reduction to bidiagonal form
542  for(i=0;i<width;i++){
543  //left-hand reduction
544  left = i+1;
545  vec_t[i]=(scale*b);
546  b = s = scale = 0.0;
547 
548  if(i < height) {
549  for(k=i;k<height;k++)
550  scale += fabs(u(i,k));
551  if(scale != 0.0) {
552  for(k=i;k<height;k++) {
553  u(i,k)=u(i,k)/scale;
554  s += u(i,k) * u(i,k);
555  }
556 
557  a = u(i,i);
558  b = -svd_sign<T>(sqrt(s),a);
559  h = a * b - s;
560  u(i,i)=a - b;
561 
562  for(j=left;j<width;j++) {
563  s=0.0;
564  for(k=i;k<height;k++)
565  s += u(i,k) * u(j,k);
566  a = s / h;
567  for(k=i;k<height;k++)
568  u(j,k)=u(j,k)+ a * u(i,k);
569  }
570  for(k=i;k<height;k++)
571  u(i,k)=u(i,k)*scale;
572  }
573  }
574 
575  w[i]=scale *b;
576  b=s=scale=0.0;
577 
578  //right-hand reduction
579  if(i<height && i+1 != width) {
580 
581  for (k=left;k<width;k++)
582  scale += fabs(u(k,i));
583 
584  if (scale != 0.0){
585  for (k=left;k<width;k++){
586  u(k,i) = u(k,i)/ scale;
587  s += u(k,i) * u(k,i);
588  }
589 
590  a=u(left,i);
591  b = -svd_sign<T>(sqrt(s),a);
592  h=a * b - s;
593  u(left,i)= a-b;
594  for (k=left;k<width;k++)
595  vec_t[k] = u(k,i)/h;
596  for (j=left;j<height;j++){
597  s=0.0;
598  for (k=left;k<width;k++)
599  s += u(k,j)* u(k,i);
600  for (k=left;k<width;k++)
601  u(k,j) = u(k,j) + s * vec_t[k];
602  }
603  for (k=left;k<width;k++)
604  u(k,i) = u(k,i) * scale;
605  }
606  }
607  num_r=std::max<T>(num_r,(fabs(w[i])+fabs(vec_t[i])));
608  }
609 
610  //accumulation of right-hand transformations.
611  for (i=width-1;i>=0;i--){
612  if(i < width-1){
613  if (b !=0.0){
614  //double division to avoid possible underflow.
615  for (j=left;j<width;j++)
616  v(i,j)=(u(j,i) /u(left,i))/b;
617 
618  for (j=left;j<width;j++){
619  s=0.0;
620  for (k=left;k<width;k++)
621  s+=u(k,i)* v(j,k);
622  for (k=left;k<width;k++)
623  v(j,k) = v(j,k)+ s * v(i,k);
624  }
625  }
626  for (j=left;j<width;j++){
627  v(j,i) = 0.0;
628  v(i,j) = 0.0;
629  }
630  }
631  v(i,i) = 1.0;
632  b=vec_t[i];
633  left=i;
634  }
635 
636  //accumulation of left-hand transformations.
637  for (i=std::min(height,width)-1;i>=0;i--)
638  {
639  left=i+1;
640  b=w[i];
641  for (j=left;j<width;j++)
642  u(j,i) = 0.0;
643  if (b !=0.0){
644  b = 1.0 / b;
645  for (j=left;j<width;j++){
646  s=0.0;
647  for (k=left;k<height;k++)
648  s+= u(i,k)* u(j,k);
649  a=(s/u(i,i) )*b;
650  for (k=i;k<height;k++)
651  u(j,k) = u(j,k) + a * u(i,k);
652  }
653  for (j=i;j<height;j++)
654  u(i,j) = u(i,j) * b;
655  }
656  else
657  for (j=i;j<height;j++)
658  u(i,j) = 0.0;
659 
660  u(i,i) = u(i,i) + 1;
661  }
662 
663  //diagonalization of the bidiagonal form: Loop over
664  //singular values, and over allowed iterations.
665  for (k=width-1;k>=0;k--){
666  for (p=0;p<30;p++){
667  flag=true;
668  for (left=k;left>=0;left--){
669  //test for splitting.
670  nm = left-1; //note that vec_t[1] is always zero.
671  if (static_cast<T>(fabs(vec_t[left])+num_r) == static_cast<T>(num_r)){
672  flag=false;
673  break;
674  }
675  if (static_cast<T>(fabs(w[nm])+num_r) == static_cast<T>(num_r))
676  break;
677  }
678  if (flag){
679  c=0.0; //cancellation of vec_t[l], ifl>1.
680  s=1.0;
681  for (i=left;i<=k;i++) {
682 
683  a=s * vec_t[i];
684  vec_t[i] = c * vec_t[i];
685 
686  if (static_cast<T>(fabs(a)+num_r) == static_cast<T>(num_r))
687  break;
688 
689  b=w[i];
690  h= pythagorean<T>(a,b);
691  w[i] = h;
692  h=1.0/h;
693  c=b*h;
694  s = -a*h;
695 
696  for (j=0;j<height;j++){
697  y=u(nm,j);
698  z=u(i,j);
699  u(nm,j) = y*c+z*s;
700  u(i,j) = z*c-y*s;
701  }
702  }
703  }
704 
705  z=w[k];
706 
707  if (left == k){
708  //convergence.
709  if (z < 0.0){
710  //singular value is made nonnegative.
711  w[k] = -z;
712  for (j=0;j<width;j++)
713  u(k,j) =-(u(k,j));
714  }
715  break;
716  }
717 
718  if (p == 29)
719  return false;
720 
721  x=w[left]; //shift from bottom 2-by-2minor.
722  nm=k-1;
723  y=w[nm];
724  b=vec_t[nm];
725  h=vec_t[k];
726  a=((y-z)*(y+z)+(b-h)*(b+h))/(2.0*h*y);
727  b= pythagorean<T>(a,1.0);
728  a = ((x - z) * (x + z) + h * ((y / (a + svd_sign(b,a))) - h)) / x;
729  c=s=1.0;
730  //next QR transformation:
731  for (j=left;j<=nm;j++){
732  i=j+1;
733  b=vec_t[i];
734  y=w[i];
735  h=s*b;
736  b=c*b;
737  z= pythagorean<T>(a,h);
738  vec_t[j] = z;
739  c=a/z;
740  s=h/z;
741  a=x*c+b*s;
742  b = b*c-x*s;
743  h=y*s;
744  y *= c;
745  for (q=0;q<width;q++){
746  x=v(j,q);
747  z=v(i,q);
748  v(j,q) = x*c+z*s;
749  v(i,q) = z*c-x*s;
750  }
751  z=pythagorean<T>(a,h);
752  w[j] = z; //rotation can be arbitrary if z = 0.
753  if (z != 0.0){
754  z=1.0/z;
755  c=a*z;
756  s=h*z;
757  }
758  a=c*b+s*y;
759  x=c*y-s*b;
760  for (q=0;q<height;q++){
761  y=u(j,q);
762  z=u(i,q);
763  u(j,q) = y*c+z*s;
764  u(i,q) = z*c-y*s;
765  }
766  }
767 
768  vec_t[left] = 0.0;
769  vec_t[k] = a;
770  w[k] = x;
771  }
772  }
773  return true;
774 }
775 
776 // solves Dx = b for x using backsubstitution and the results of SVD
777 template <class T>
779 {
780  narray<T> temp;
781  T s;
782  int w, h, i, j;
783 
784  w = Mu.width();
785  h = Mu.height();
786  temp.resize(w);
787 
788  for (i=0; i<w; i++) {
789  s = 0;
790  if (Vw[i] != 0) {
791  for (j=0; j<h; j++) {
792  s += Mu(i,j) * b[j];
793  }
794  s = s/Vw[i];
795  }
796  temp[i] = s;
797  }
798 
799  for (i=0; i<w; i++) {
800  s = 0;
801  for (j=0; j<w; j++) {
802  s+= Mv(j,i) * temp[j];
803  }
804  x[i] = s;
805  }
806 }
807 
808 //Computes Pythagorean Theorem: (a^2 + b^2)^0.5 without destructive underflow or overflow
809 template <class DT>
811 {
812  DT abs_a,abs_b;
813 
814  abs_a = fabs(a);
815  abs_b = fabs(b);
816 
817  if(abs_a < abs_b)
818  return(abs_b == 0.0 ? 0.0 : abs_b * sqrt(pow((abs_a / abs_b),2) +1.0));
819  else
820  return(abs_a * sqrt(pow((abs_b/abs_a),2) +1.0));
821 }
822 
823 // reduces a general matrix to hessenberg form
824 template <class T>
826 {
827  int i, j, m, n;
828  T x, y, temp;
829 
830  n = matrix.height();
831  for (m=1; m<n-1; m++) {
832  x = 0.0;
833  i = m;
834  for (j=m; j<n; j++) {
835  if (fabs(matrix(m-1,j)) > fabs(x)) {
836  x = matrix(m-1,j);
837  i = j;
838  }
839  }
840  if (i != m) {
841  for (j = m-1; j<n; j++) {
842  temp = matrix(j,i);
843  matrix(j,i) = matrix(j,m);
844  matrix(j,m) = temp;
845  }
846  for (j=0; j<n; j++) {
847  temp = matrix(i,j);
848  matrix(i,j) = matrix(m,j);
849  matrix(m,j) = temp;
850  }
851  }
852  if (x != 0.0) {
853  for (i=m+1; i<n; i++) {
854  y = matrix(m-1,i);
855  if (y != 0.0) {
856  y /= x;
857  matrix(m-1,i) = y;
858  for (j=m; j<n; j++)
859  matrix(j,i) -= y*matrix(j,m);
860  for (j=0; j<n; j++)
861  matrix(m,j) += y*matrix(i,j);
862  }
863  }
864  }
865  }
866 }
867 
868 // calculates the eigenvalues of a Hessenberg matrix
869 template <class T>
870  void scopira::basekit::eigen_hessenberg(narray<T,2> &matrix, narray<T> &eigen_real, narray<T> &eigen_imaginary)
871 {
872  int n,nn,m,l,k,mmin,i, j, iterations;
873  T matrix_norm, z,y,x,w,v,u,t,s,r,q,p;;
874 
875  n = matrix.height();
876  matrix_norm = fabs(matrix(0,0));
877  for (i=1; i<n; i++) {
878  for (j=i-1; j<n; j++) {
879  matrix_norm += fabs(matrix(j,i));
880  }
881  }
882  nn = n-1;
883  t = 0.0;
884  while (nn >= 0) {
885  iterations = 0;
886  do {
887  for (l=nn;l>=1; l--) {
888  s = fabs(matrix(l-1,l-1)) + fabs(matrix(l,l));
889  if (s == 0.0)
890  s = matrix_norm;
891  if ((fabs(matrix(l-1,l)) + s) == s)
892  break;
893  }
894  x = matrix(nn,nn);
895  if (l == nn) {
896  eigen_real[nn] = x + t;
897  eigen_imaginary[nn--] = 0.0;
898  } else {
899  y = matrix(nn-1,nn-1);
900  w = matrix(nn-1,nn)*matrix(nn,nn-1);
901  if (l == (nn-1)) {
902  p = 0.5*(y-x);
903  q = p*p + w;
904  z = sqrt(fabs(q));
905  x += t;
906  if (q >= 0.0) {
907  z = p + SIGN(z,p);
908  eigen_real[nn-1] = eigen_real[nn] = x + z;
909  if (z != 0.0)
910  eigen_real[nn] = x - w/z;
911  eigen_imaginary[nn-1] = eigen_imaginary[nn] = 0.0;
912  } else {
913  eigen_real[nn-1] = eigen_real[nn] = x + p;
914  eigen_imaginary[nn-1] = -(eigen_imaginary[nn] = z);
915  }
916  nn -= 2;
917  } else {
918  if (iterations == 30) {
919  OUTPUT << "Too many iterations while computing Hessenberg eigenvalues\n";
920  return;
921  }
922  if (iterations == 10 || iterations == 20) {
923  t += x;
924  for (i=0; i <= nn; i++)
925  matrix(i,i) -= x;
926  s = fabs(matrix(nn-1,nn)) + fabs(matrix(nn-2,nn-1));
927  y = x = 0.75*s;
928  w = -0.4375*s*s;
929  }
930  ++iterations;
931  for (m=nn-2; m>=l; m--) {
932  z = matrix(m,m);
933  r = x - z;
934  s = y - z;
935  p = (r*s-w)/matrix(m,m+1) + matrix(m+1,m);
936  q = matrix(m+1,m+1) - z - r - s;
937  r = matrix(m+1,m+2);
938  s = fabs(p) + fabs(q) + fabs(r);
939  p /= s;
940  q /= s;
941  r /= s;
942  if (m == l)
943  break;
944  u = fabs(matrix(m-1,m)) * (fabs(q) + fabs(r));
945  v = fabs(p) * (fabs(matrix(m-1,m-1)) + fabs(z) + fabs(matrix(m+1,m+1)));
946  if ((u + v) == v)
947  break;
948  }
949  for (i=m+2; i<=nn; i++) {
950  matrix(i-2,i) = 0.0;
951  if (i != (m+2))
952  matrix(i-3,i) = 0.0;
953  }
954  for (k=m; k<=nn-1; k++) {
955  if (k != m) {
956  p = matrix(k-1,k);
957  q = matrix(k-1,k+1);
958  r = 0.0;
959  if (k != nn-1)
960  r = matrix(k-1,k+2);
961  if ((x = fabs(p) + fabs(q) + fabs(r)) != 0.0) {
962  p /= x;
963  q /=x;
964  r /= x;
965  }
966  }
967  if ((s = SIGN(sqrt(p*p + q*q + r*r),p)) != 0.0) {
968  if (k == m) {
969  if (l != m)
970  matrix(k-1,k) = -matrix(k-1,k);
971  } else {
972  matrix(k-1,k) = -s*x;
973  }
974  p += s;
975  x = p/s;
976  y = q/s;
977  z = r/s;
978  q /= p;
979  r /= p;
980  for (j=k; j<=nn; j++) {
981  p = matrix(j,k) + q*matrix(j,k+1);
982  if (k != nn-1) {
983  p += r*matrix(j,k+2);
984  matrix(j,k+2) -= p*z;
985  }
986  matrix(j,k+1) -= p*y;
987  matrix(j,k) -= p*x;
988  }
989  mmin = nn < k+3 ? nn : k+3;
990  for (i=l; i<=mmin; i++) {
991  p = x*matrix(k,i) + y*matrix(k+1,i);
992  if (k != nn-1) {
993  p += z*matrix(k+2,i);
994  matrix(k+2,i) -= p*r;
995  }
996  matrix(k+1,i) -= p*q;
997  matrix(k,i) -= p;
998  }
999  }
1000  }
1001  }
1002  }
1003  } while (l < nn-1);
1004  }
1005 }
1006 
1007 // Gauss-Jordan matrix inversion and linear equation solution
1008 template <class T>
1010 {
1011  narray<int> index_x, index_y, index_pivot;
1012  int i, x, y, j, k, l, ll, n, m;
1013  T max, pivot_inv, temp;
1014 
1015  n = matrix.height();
1016  m = rhs.width();
1017  index_x.resize(n);
1018  index_y.resize(n);
1019  index_pivot.resize(n);
1020  for (j=0; j<n; j++)
1021  index_pivot[j] = 0;
1022  for (i=0; i<n; i++) {
1023  max = 0.0;
1024  for (j=0; j<n; j++) {
1025  if (index_pivot[j] != 1) {
1026  for (k=0; k<n; k++) {
1027  if (index_pivot[k] == 0) {
1028  if (fabs(matrix(k,j)) >= max) {
1029  max = fabs(matrix(k,j));
1030  x = k;
1031  y = j;
1032  }
1033  } else if (index_pivot[k] > 1) {
1034  OUTPUT << "Singular matrix-1 in Gauss-Jordan elimination\n";
1035  return;
1036  }
1037  }
1038  }
1039  }
1040  index_pivot[x]++;
1041  if (x != y) {
1042  for (l=0; l<n; l++) {
1043  temp = matrix(l,y);
1044  matrix(l,y) = matrix(l,x);
1045  matrix(l,x) = temp;
1046  }
1047  for (l=0; l<m; l++) {
1048  temp = rhs(l,y);
1049  rhs(l,y) = rhs(l,x);
1050  rhs(l,x) = temp;
1051  }
1052  }
1053 
1054  index_y[i] = y;
1055  index_x[i] = x;
1056  if (matrix(x,x) == 0.0) {
1057  OUTPUT << "Singular matrix-1 in Gauss-Jordan elimination\n";
1058  return;
1059  }
1060  pivot_inv = 1.0/matrix(x,x);
1061  matrix(x,x) = 1.0;
1062  for (l=0; l<n; l++)
1063  matrix(l,x) *= pivot_inv;
1064  for (l=0; l<m; l++)
1065  rhs(l,x) *= pivot_inv;
1066  for (ll=0; ll<n; ll++) {
1067  if (ll != x) {
1068  temp = matrix(x,ll);
1069  matrix(x,ll) = 0.0;
1070  for (l=0; l<n; l++)
1071  matrix(l,ll) -= matrix(l,x)*temp;
1072  for (l=0; l<m; l++)
1073  rhs(l,ll) -= rhs(l,x)*temp;
1074  }
1075  }
1076  }
1077  for (l=n-1; l>=0; l--) {
1078  if (index_y[l] != index_x[l]) {
1079  for (k=0; k<n; k++) {
1080  temp = matrix(index_y[l],k);
1081  matrix(index_y[l],k) = matrix(index_x[l],k);
1082  matrix(index_x[l],k) = temp;
1083  }
1084  }
1085  }
1086 }
1087 
1088 // rearranges covariance matrix
1089 template <class T>
1091 {
1092  int i,j,k,n;
1093  T temp;
1094 
1095  n = matrix.height();
1096  for (i=mfit; i<n; i++) {
1097  for (j=0; j<=i; j++) {
1098  matrix(j,i) = matrix(i,j) = 0.0;
1099  }
1100  }
1101  k = mfit-1;
1102  for (j=n-1; j>=0; j--) {
1103  if (fitted[j]) {
1104  for (i=0; i<n; i++) {
1105  temp = matrix(k,i);
1106  matrix(k,i) = matrix(j,i);
1107  matrix(j,i) = temp;
1108  }
1109  for (i=0; i<n; i++) {
1110  temp = matrix(i,k);
1111  matrix(i,k) = matrix(i,j);
1112  matrix(i,j) = temp;
1113  }
1114  k--;
1115  }
1116  }
1117 }
1118 
1119 #endif
bool svd(narray< T, 2 > &Mu, narray< T > &Vw, narray< T, 2 > &Mv)
Definition: matrixmath.h:520
void transpose_matrix(D &dest, const S &src)
Definition: matrixmath.h:297
const double tinynum_c
Definition: math.h:105
Definition: archiveflow.h:20
bool lu_decomposition(narray< T, 2 > &dest, const narray< T, 2 > &src, narray< int > &idx, bool &odd)
Definition: matrixmath.h:388
DT pythagorean(DT a, DT b)
Definition: matrixmath.h:810
size_t height(void) const
height
Definition: narray.h:863
void svd_backsubstitution(narray< T, 2 > &Mu, narray< T > &Vw, narray< T, 2 > &Mv, narray< T > &b, narray< T > &x)
Definition: matrixmath.h:778
void lu_backsubstitution(narray< T > &dest, const narray< T, 2 > &inv_a, const narray< int > &index)
Definition: matrixmath.h:474
Definition: narray.h:96
void set(index_type c, T v)
setter
Definition: narray.h:1090
void eigen_hessenberg(narray< T, 2 > &matrix, narray< T > &eigen_real, narray< T > &eigen_imaginary)
Definition: matrixmath.h:870
void mul_matrix(T &dest, SCA d)
Definition: matrixmath.h:200
void sum(const V &v, T &out)
Definition: vectormath.h:319
bool invert_matrix(T &dest, const T &src)
Definition: matrixmath.h:262
void resample_linear(DEST &dest, const SRC &src)
Definition: matrixmath.h:325
T svd_sign(T v1, T v2)
internal function for svd
Definition: matrixmath.h:514
size_t width(void) const
width
Definition: narray.h:861
void copy(const this_type &at)
deep copy
Definition: narray.h:1511
void clear(void)
set all to 0
Definition: narray.h:1065
bool is_equal(T v1, T v2)
Definition: math.h:125
size_t max(const V &v, T &out)
Definition: vectormath.h:297
void resample_nn(DEST &dest, const SRC &src)
Definition: matrixmath.h:312
T get(index_type c) const
getter
Definition: narray.h:1095
void sort_covariance(narray< T, 2 > &matrix, narray< int > &fitted, int mfit)
Definition: matrixmath.h:1090
bool is_zero(T v)
Definition: math.h:132
void elim_hessian(narray< T, 2 > &matrix)
Definition: matrixmath.h:825
void resize(size_t len)
Definition: narray.h:877
void gauss_jordan(narray< T, 2 > &matrix, narray< T, 2 > &rhs)
Definition: matrixmath.h:1009