14 #ifndef __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__ 15 #define __INCLUDED_SCOPIRA_BASEKIT_MATRIXMATH_H__ 17 #include <scopira/tool/output.h> 18 #include <scopira/basekit/math.h> 19 #include <scopira/basekit/narray.h> 38 template <
class T,
class SCA>
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);
72 template <
class D,
class S>
81 template <
class SRC,
class DEST>
90 template <
class SRC,
class DEST>
103 bool lu_decomposition(narray<T,2>& dest,
const narray<T,2>& src,narray<int>& idx,
bool& odd);
116 void lu_backsubstitution(narray<T>& dest,
const narray<T,2>& inv_a,
const narray<int>& index);
128 bool svd(narray<T,2>& Mu,narray<T>& Vw,narray<T,2>& Mv);
142 void svd_backsubstitution(narray<T,2> &Mu, narray<T> &Vw, narray<T,2> &Mv, narray<T> &b, narray<T> &x);
171 void eigen_hessenberg(narray<T,2> &matrix, narray<T> &eigen_real, narray<T> &eigen_imaginary);
184 void gauss_jordan(narray<T,2> &matrix, narray<T,2> &rhs);
195 void sort_covariance(narray<T,2> &matrix, narray<int> &fitted,
int mfit);
199 template <
class T,
class SCA>
202 typename T::iterator tar, endtar;
205 for (tar = dest.begin(); tar != endtar; ++tar)
209 template <
class T,
class T2,
class T3>
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;
240 assert( (t1 ? m1.height() : m1.width()) == (t2 ? m2.width() : m2.height()) );
247 for (x=0; x<w; x++) {
252 for (i=0; i<len; i++) {
255 sum += m1(rx1, ry1) * m2(rx2, ry2);
264 typedef typename T::data_type data_type;
273 m_orig.
resize(n, src.height());
274 dest.resize(n, src.height());
281 if (!scopira::basekit::lu_decomposition<data_type>(m_orig, src, v_idx, odd))
285 for (j=0; j<n; j++) {
288 scopira::basekit::lu_backsubstitution<data_type>(v_col, m_orig, v_idx);
290 dest.set(j, i, v_col.get(i));
296 template <
class D,
class S>
308 dest(y,x) = src(x,y);
311 template <
class SRC,
class DEST>
314 typedef typename DEST::data_type data_type;
316 if (dest.empty() || src.empty())
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());
324 template <
class SRC,
class DEST>
327 typedef typename DEST::data_type data_type;
328 double xsrc_rate, xleft, xremainer, xfer;
330 double ysrc_rate, yleft, yremainer, yfer;
333 if (dest.empty() || src.empty())
338 xsrc_rate =
static_cast<double>(src.width()) / dest.width();
339 ysrc_rate =
static_cast<double>(src.height()) / dest.height();
341 xdest_rate =
static_cast<double>(dest.width()) / src.width();
342 ydest_rate =
static_cast<double>(dest.height()) / src.height();
346 for (
size_t ydest=0; ydest<dest.height(); ++ydest) {
351 if (yfer + yremainer > 1)
352 yfer = 1 - yremainer;
356 for (
size_t xdest=0; xdest<dest.width(); ++xdest) {
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);
367 if (xremainer >= 1) {
377 if (yremainer >= 1) {
391 T largest, dum,
sum, tmp;
405 for (i=0; i<n; i++) {
407 for (j=0; j<n; j++) {
408 tmp = ::fabs(dest.
get(j,i));
416 scaling.
set(i, 1.0/largest);
420 for (j=0; j<n; j++) {
421 for (i=0; i<j; i++) {
424 sum -= dest.
get(k, i) * dest.
get(j, k);
430 for (i=j; i<n; i++) {
431 sum = dest.
get(j, i);
433 sum -= dest.
get(k, i) * dest.
get(j, k);
436 dum=scaling.
get(i) * ::fabs(sum);
437 if (dum >= largest) {
445 for (k=0; k<n; k++) {
446 dum = dest.
get(k, max);
447 dest.
set(k, max, dest.
get(k, j));
453 scaling.
set(max, scaling.
get(j));
456 if (dest.
get(j, j) == 0)
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);
483 for (i=0; i<n; i++) {
488 for (j=m; j<= i-1; j++)
489 sum -= inv_a.
get(j, i) * dest.
get(j);
498 for (i=n-1; i>=0; i--) {
500 for (j=i+1; j<n; j++)
501 sum-=inv_a.
get(j,i) * dest.
get(j);
503 dest.
set(i, sum/inv_a.
get(i,i));
514 inline T
svd_sign(T v1, T v2) {
if (v2>=0.0)
return fabs(v1);
else return -fabs(v1); }
523 int i,j,k,left,height,width,nm,q,p;
524 T a,b,c,num_r,h,s,scale,x,y,z;
542 for(i=0;i<width;i++){
549 for(k=i;k<height;k++)
550 scale += fabs(u(i,k));
552 for(k=i;k<height;k++) {
554 s += u(i,k) * u(i,k);
558 b = -svd_sign<T>(sqrt(s),a);
562 for(j=left;j<width;j++) {
564 for(k=i;k<height;k++)
565 s += u(i,k) * u(j,k);
567 for(k=i;k<height;k++)
568 u(j,k)=u(j,k)+ a * u(i,k);
570 for(k=i;k<height;k++)
579 if(i<height && i+1 != width) {
581 for (k=left;k<width;k++)
582 scale += fabs(u(k,i));
585 for (k=left;k<width;k++){
586 u(k,i) = u(k,i)/ scale;
587 s += u(k,i) * u(k,i);
591 b = -svd_sign<T>(sqrt(s),a);
594 for (k=left;k<width;k++)
596 for (j=left;j<height;j++){
598 for (k=left;k<width;k++)
600 for (k=left;k<width;k++)
601 u(k,j) = u(k,j) + s * vec_t[k];
603 for (k=left;k<width;k++)
604 u(k,i) = u(k,i) * scale;
607 num_r=std::max<T>(num_r,(fabs(w[i])+fabs(vec_t[i])));
611 for (i=width-1;i>=0;i--){
615 for (j=left;j<width;j++)
616 v(i,j)=(u(j,i) /u(left,i))/b;
618 for (j=left;j<width;j++){
620 for (k=left;k<width;k++)
622 for (k=left;k<width;k++)
623 v(j,k) = v(j,k)+ s * v(i,k);
626 for (j=left;j<width;j++){
637 for (i=std::min(height,width)-1;i>=0;i--)
641 for (j=left;j<width;j++)
645 for (j=left;j<width;j++){
647 for (k=left;k<height;k++)
650 for (k=i;k<height;k++)
651 u(j,k) = u(j,k) + a * u(i,k);
653 for (j=i;j<height;j++)
657 for (j=i;j<height;j++)
665 for (k=width-1;k>=0;k--){
668 for (left=k;left>=0;left--){
671 if (static_cast<T>(fabs(vec_t[left])+num_r) == static_cast<T>(num_r)){
675 if (static_cast<T>(fabs(w[nm])+num_r) == static_cast<T>(num_r))
681 for (i=left;i<=k;i++) {
684 vec_t[i] = c * vec_t[i];
686 if (static_cast<T>(fabs(a)+num_r) == static_cast<T>(num_r))
690 h= pythagorean<T>(a,b);
696 for (j=0;j<height;j++){
712 for (j=0;j<width;j++)
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;
731 for (j=left;j<=nm;j++){
737 z= pythagorean<T>(a,h);
745 for (q=0;q<width;q++){
751 z=pythagorean<T>(a,h);
760 for (q=0;q<height;q++){
788 for (i=0; i<w; i++) {
791 for (j=0; j<h; j++) {
799 for (i=0; i<w; i++) {
801 for (j=0; j<w; j++) {
802 s+= Mv(j,i) * temp[j];
818 return(abs_b == 0.0 ? 0.0 : abs_b * sqrt(pow((abs_a / abs_b),2) +1.0));
820 return(abs_a * sqrt(pow((abs_b/abs_a),2) +1.0));
831 for (m=1; m<n-1; m++) {
834 for (j=m; j<n; j++) {
835 if (fabs(matrix(m-1,j)) > fabs(x)) {
841 for (j = m-1; j<n; j++) {
843 matrix(j,i) = matrix(j,m);
846 for (j=0; j<n; j++) {
848 matrix(i,j) = matrix(m,j);
853 for (i=m+1; i<n; i++) {
859 matrix(j,i) -= y*matrix(j,m);
861 matrix(m,j) += y*matrix(i,j);
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;;
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));
887 for (l=nn;l>=1; l--) {
888 s = fabs(matrix(l-1,l-1)) + fabs(matrix(l,l));
891 if ((fabs(matrix(l-1,l)) + s) == s)
896 eigen_real[nn] = x + t;
897 eigen_imaginary[nn--] = 0.0;
899 y = matrix(nn-1,nn-1);
900 w = matrix(nn-1,nn)*matrix(nn,nn-1);
908 eigen_real[nn-1] = eigen_real[nn] = x + z;
910 eigen_real[nn] = x - w/z;
911 eigen_imaginary[nn-1] = eigen_imaginary[nn] = 0.0;
913 eigen_real[nn-1] = eigen_real[nn] = x + p;
914 eigen_imaginary[nn-1] = -(eigen_imaginary[nn] = z);
918 if (iterations == 30) {
919 OUTPUT <<
"Too many iterations while computing Hessenberg eigenvalues\n";
922 if (iterations == 10 || iterations == 20) {
924 for (i=0; i <= nn; i++)
926 s = fabs(matrix(nn-1,nn)) + fabs(matrix(nn-2,nn-1));
931 for (m=nn-2; m>=l; m--) {
935 p = (r*s-w)/matrix(m,m+1) + matrix(m+1,m);
936 q = matrix(m+1,m+1) - z - r - s;
938 s = fabs(p) + fabs(q) + fabs(r);
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)));
949 for (i=m+2; i<=nn; i++) {
954 for (k=m; k<=nn-1; k++) {
961 if ((x = fabs(p) + fabs(q) + fabs(r)) != 0.0) {
967 if ((s = SIGN(sqrt(p*p + q*q + r*r),p)) != 0.0) {
970 matrix(k-1,k) = -matrix(k-1,k);
972 matrix(k-1,k) = -s*x;
980 for (j=k; j<=nn; j++) {
981 p = matrix(j,k) + q*matrix(j,k+1);
983 p += r*matrix(j,k+2);
984 matrix(j,k+2) -= p*z;
986 matrix(j,k+1) -= p*y;
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);
993 p += z*matrix(k+2,i);
994 matrix(k+2,i) -= p*r;
996 matrix(k+1,i) -= p*q;
1012 int i, x, y, j, k, l, ll, n, m;
1013 T
max, pivot_inv, temp;
1022 for (i=0; i<n; i++) {
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));
1033 }
else if (index_pivot[k] > 1) {
1034 OUTPUT <<
"Singular matrix-1 in Gauss-Jordan elimination\n";
1042 for (l=0; l<n; l++) {
1044 matrix(l,y) = matrix(l,x);
1047 for (l=0; l<m; l++) {
1049 rhs(l,y) = rhs(l,x);
1056 if (matrix(x,x) == 0.0) {
1057 OUTPUT <<
"Singular matrix-1 in Gauss-Jordan elimination\n";
1060 pivot_inv = 1.0/matrix(x,x);
1063 matrix(l,x) *= pivot_inv;
1065 rhs(l,x) *= pivot_inv;
1066 for (ll=0; ll<n; ll++) {
1068 temp = matrix(x,ll);
1071 matrix(l,ll) -= matrix(l,x)*temp;
1073 rhs(l,ll) -= rhs(l,x)*temp;
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;
1096 for (i=mfit; i<n; i++) {
1097 for (j=0; j<=i; j++) {
1098 matrix(j,i) = matrix(i,j) = 0.0;
1102 for (j=n-1; j>=0; j--) {
1104 for (i=0; i<n; i++) {
1106 matrix(k,i) = matrix(j,i);
1109 for (i=0; i<n; i++) {
1111 matrix(i,k) = matrix(i,j);
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
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