matrix.cpp

《陈必红算法》一书的附带的源代码

}

int matrix::ispositive()		// 判定矩阵是否对称非负定阵，如是返回1，否则返回0
{
	if(!issym) return 0;
	matrix ev;
	eigen(ev);
	for(size_t i=0; i<rownum; i++)
		if(ev(i)<0) return 0;
	return 1;
}

matrix matrix::cholesky(matrix& dd)	// 用乔里斯基分解法求对称正定阵的线性
		// 方程组ax=d返回方程组的解，本身为a，改变为分解阵u,d不变
{
	if(!issym) throw TMESSAGE("not symetic!");
	if(dd.rownum != colnum) throw TMESSAGE("dd's rownum not right!");
	matrix md(dd);
	size_t i,j,k,u,v;
	if(value(0,0)<=0.0) throw TMESSAGE("not positive");
	set(0,0,sqrt(value(0,0))); //	 a[0]=sqrt(a[0]);
	buffer& a = (*buf);
	buffer& d = (*(md.buf));
	size_t n = rownum;
	size_t m = dd.colnum;
	for (j=1; j<n; j++) a[j]=a[j]/a[0];
	for (i=1; i<n; i++)
		{ u=i*n+i;
		  for (j=1; j<=i; j++)
			 { v=(j-1)*n+i;
				a[u]=a[u]-a[v]*a[v];
			 }
		  if (a[u]<=0.0) throw TMESSAGE("not positive");
		  a[u]=sqrt(a[u]);
		  if (i!=(n-1))
			 { for (j=i+1; j<n; j++)
				  { v=i*n+j;
					 for (k=1; k<=i; k++)
						a[v]=a[v]-a[(k-1)*n+i]*a[(k-1)*n+j];
					 a[v]=a[v]/a[u];
				  }
			 }
		}
	for (j=0; j<m; j++)
		{ d[j]=d[j]/a[0];
		  for (i=1; i<=n-1; i++)
			 { u=i*n+i; v=i*m+j;
				for (k=1; k<=i; k++)
				  d[v]=d[v]-a[(k-1)*n+i]*d[(k-1)*m+j];
				d[v]=d[v]/a[u];
			 }
		}
	for (j=0; j<=m-1; j++)
		{ u=(n-1)*m+j;
		  d[u]=d[u]/a[n*n-1];
		  for (k=n-1; k>=1; k--)
			 { u=(k-1)*m+j;
				for (i=k; i<=n-1; i++)
				  { v=(k-1)*n+i;
					 d[u]=d[u]-a[v]*d[i*m+j];
				  }
				v=(k-1)*n+k-1;
				d[u]=d[u]/a[v];
			 }
		}
	if(n>1)
	for(j=1; j<n; j++)
	for(i=0; i<j; i++)
		a[i+j*n]=0.0;
	return md;
}

DOUBLE lineopt(matrix& aa,matrix& bb, matrix& cc, matrix & xx)
 // 线性规划最优点寻找程序，aa为mXn不等式约束条件左端系数矩阵，bb为不等式约束
 // 条件的右端项，为m维向量，cc为目标函数系数，n维向量，xx返回极小点，为n维向量
{
	if(aa.rownum != bb.rownum || aa.colnum != cc.rownum ||
		aa.colnum != xx.rownum) throw TMESSAGE("dimenstion not right!");
	size_t n=aa.colnum, m=aa.rownum;
	size_t i,mn,k,j;
	matrix a(m,n+m);
	for(i=0;i<m;i++) {
		for(j=0;j<n;j++)
			a.set(i,j,aa(i,j));
		for(j=n;j<n+m;j++)
			if(j-n == i) a.set(i,j,1.0);
			else a.set(i,j,0.0);
	}
	matrix c(m+n);
	c = 0.0;
	for(i=0;i<m;i++)
		c.set(i,cc(i));
	lbuffer* jjs = getnewlbuffer(m);
	lbuffer& js = (*jjs);
	DOUBLE s,z,dd,y; //,*p,*d;

	for (i=0; i<m; i++) js[i]=n+i;
	matrix p(m,m);
	matrix d;
	mn=m+n; s=0.0;
	matrix x(mn);
	while (1)
		{ for (i=0; i<m; i++)
			 for (j=0; j<m; j++)
				p.set(i,j,a(i,(size_t)js[j]));
		  p.inv();
			d = p*a;
			x = 0.0;
		  for (i=0; i<m; i++)
			 { s=0.0;
				for (j=0; j<=m-1; j++)
					s+=p(i,j)*bb(j);
				x.set((size_t)js[i],s);
			 }
		  k=mn; dd=1.0e-35;
		  for (j=0; j<mn; j++)
			 { z=0.0;
				for (i=0; i<m; i++)
					z+=c((size_t)js[i])*d(i,j);
				z-=c(j);
				if (z>dd) { dd=z; k=j;}
			 }
		  if (k==mn)
			 { s=0.0;
				for (j=0; j<n; j++) {
					xx.set(j,x(j));
					s+=c(j)*x(j);
				}
				delete jjs;
				return s;
			 }
		  j=m;
		  dd=1.0e+20;
		  for (i=0; i<=m-1; i++)
			 if (d(i,k)>=1.0e-20)   // [i*mn+k]>=1.0e-20)
				{ y=x(size_t(js[i]))/d(i,k);
				  if (y<dd) { dd=y; j=i;}
				}
		  if (j==m) { delete jjs;
							throw TMESSAGE("lineopt failed!");
						}
		  js[j]=k;
		}
}
int matrix::doolicp(matrix &L,matrix &U,matrix &ik)//进行Doolitter分解程序（矩阵的三角分解）
{
int k,i,j,i0;
double pelement;
int DIM=rownum;
if(rownum!= colnum) return 0;
matrix B(*this);
ik=matrix(DIM,1);
L=matrix(DIM,DIM);
U=matrix(DIM,DIM);
L=0;
U=0;
for(k=0;k<DIM;k++)
  {
  /* for principal element*/
  pelement=fabs( B(k,k));

  ik.set(k,0,(double)k);  //A[k][k] );       ik[k]=k;
  for(i=k;i<DIM;i++)
    if( fabs(B(i,k))> pelement ) { pelement=fabs(B(i,k)); ik.set(k,0,(double)i); }//A[i][k]) > pelement ) { pelement=fabs(A[i][k]); ik[k]=i; }
  i0=ik(k,0);//ik[k];
  if( i0 != k )
    for(j=0;j<DIM;j++)
      { pelement=B(k,j); B.set(k,j,B(i0,j));B.set(i0,j,pelement);}//A[k][j]; A[k][j]=A[i0][j]; A[i0][j]=pelement; }
  if( fabs(B(k,k))<1e-10) goto aa;//A[k][k]) < 1e-10) return(1);
  for(i=k+1;i<DIM;i++)
    {    
	  B.set(i,k,B(i,k)/B(k,k));
    for(j=k+1;j<DIM;j++) 
		B.set(i,j,B(i,j)-B(i,k)*B(k,j));	
    }
  }
aa:
  for(i=0;i<DIM;i++)
{
  for(j=0;j<DIM;j++)
  {
	  if(i>j) {L.set(i,j,B(i,j));U.set(i,j,0.0);}//L[i][j]=A[i][j];U[i][j]=0;}
      if(i==j){L.set(i,j,1.0);U.set(i,j,B(i,j));}//L[i][j]=1;U[i][j]=A[i][j];}
	  if (i<j){L.set(i,j,0.0);U.set(i,j,B(i,j));}//L[i][j]=0;U[i][j]=A[i][j];}
  }
}
return(1);
}
DOUBLE sum(matrix &a)//求矩阵的和
{
  DOUBLE result=0;
  for(int i=0;i<a.rownum;i++)
	  for(int j=0;j<a.colnum;j++)
	     result+=a(i,j);
 return result;
}
matrix diag(matrix &a)//求矩阵的对角元,返回n行一列
{
  matrix result(a.rownum ,1);
  for(int i=0;i<a.rownum;i++)
	  for(int j=0;j<a.colnum;j++)
		  if(i==j){result.set(i,0,a(i,j)); break;}
  return result;
}
matrix fabs(matrix &a)//求矩阵的绝对值,返回每个元素的绝对值组成的
{
 matrix result(a.rownum,a.colnum);
 for(int i=0;i<a.rownum;i++)
	 for(int j=0;j<a.colnum;j++)
		 result.set(i,j,fabs(a(i,j)));
	 return result;
}
matrix log(matrix &a)
{
 matrix result(a.rownum,a.colnum);
 for(int i=0;i<a.rownum;i++)
	 for(int j=0;j<a.colnum;j++)
	 {
		 if(a(i,j)<0) 
			 throw TMESSAGE("log minius");
	     else
		result.set(i,j,log(a(i,j)));
	 }
	 return result;
}
int matrix::hessenbH(matrix &Qv )//用householder 方法
{
const int N=rownum;
if(rownum!=colnum) return 0;
matrix &a=(*this);
double *u=new double[N];
double alpha;
 double *p=new double[N];
 double *q=new double[N];
 double ki;
 double EPSILON=1e-12;
Qv=matrix(rownum,colnum);
int i,j,k;
for(k=0;k<N-2;k++)
  {
  ki=0.0;
  for(i=k+1;i<N;i++) ki=ki+a(i,k)*a(i,k);//a[i][k]*a[i][k];
  ki=sqrt(ki);
  if( ki < EPSILON*EPSILON )  goto conti;
  if( a(k+1,k)>0.0 ) ki=-ki;
  u[k+1]=a(k+1,k)-ki;//a[k+1][k]-ki;
  for(i=k+2;i<N;i++) { u[i]=a(i,k); a.set(i,k,0.0); }
  alpha=1.0/(ki*(ki-a(k+1,k)));
 // a[k+1][k]=ki;
  a.set(k+1,k,ki);
  for(i=0;i<N;i++)  { p[i]=0.0; q[i]=0.0; }
  for(j=k+1;j<N;j++)             /* for computing HA */
    {
    for(i=k+1;i<N;i++) q[j]=q[j]+a(i,j)*u[i];
    q[j]=alpha*q[j];
    }
  for(j=k+1;j<N;j++) for(i=k+1;i<N;i++) a.set(i,j,a(i,j)-q[j]*u[i]);//a[i][j]=a[i][j]-q[j]*u[i];
  for(i=0;i<N;i++) q[i]=0.0;     /* for computing AH, QvH */
  for(i=0;i<N;i++)
    {
    for(j=k+1;j<N;j++)
      {
      p[i]=p[i]+a(i,j)*u[j];
      q[i]=q[i]+Qv(i,j)*u[j];
      }
    p[i]=alpha*p[i];
    q[i]=alpha*q[i];
    }
  for(i=0;i<N;i++)
    for(j=k+1;j<N;j++)
      {
      a.set(i,j,a(i,j)-p[i]*u[j]);
      Qv.set(i,j,Qv(i,j)-q[i]*u[j]);//=Qv[i][j]-q[i]*u[j];
      }
  conti:;
  }                 /* end for(k=0;.....*/
delete []u;
delete []p;
delete []q;
return(1);
}
double sign(double a)
{
 if( a>0) return 1;
 if(a<0) return -1;
 return 0;
}
double fmax(double a,double b)
{
 if(a>=b) return a;
 else     return b;
}
int diff (matrix & a,matrix &out)
{
 int row=a.rownum;
 int col=a.colnum;
 int i=0;
 if(row==1 && col==1){return 0;}
 if(row==1)
 {
   out=matrix(1,col-1);
   for(i=0;i<col-1;i++)  out.set(0,i,a(0,i+1)-a(0,i));
   return 1;
 }
 if(col==1)
 {
	  out=matrix(row-1,1);
	  for(i=0;i<row-1;i++)out.set(i,0,a(i+1,0)-a(i,0));	 
	  return 1;
 }
 out=matrix(row-1,col);
 int j=0;
 for(j=0;j<col;j++)
 {
   for(i=0;i<row-1;i++)
   {
   out.set(i,j,a(i+1,j)-a(i,j)); 
   }
 }
 return 1;
}
matrix 	matrix::pMul(matrix& m)
{
  matrix result(rownum,colnum);
  if(rownum!=m.rownum||colnum!=m.colnum){return result;}
   for(int i=0;i<rownum;i++)
	 for(int j=0;j<colnum;j++)
		 result.set(i,j,value(i,j)*m(i,j));
  return result;
}
matrix matrix::pDiv(matrix &m)
{
 matrix result(rownum,colnum);
  if(rownum!=m.rownum||colnum!=m.colnum){return result;}
   for(int i=0;i<rownum;i++)
	 for(int j=0;j<colnum;j++)
		 result.set(i,j,value(i,j)/m(i,j));
  return result;

}
int spline(matrix &x,matrix &y,matrix & XI,matrix &outY)//进行样条插值，要求x,y必须从小到大排序,不能有重复点,x,都是列向量，并且行数相同
{                                                       //4个点以上,XI是排好序的
  
  matrix dx,dy;
  int n=x.rownum;
  diff(x,dx);
  diff(y,dy);
  matrix divdif(dy.pDiv(dx));
  matrix b(n,1);
  b=0;
  int i,j;
  double value=0;
  for(i=1;i<n-1;i++)
  {
    value=3*(dx(i,0)*divdif(i-1,0)+dx(i-1,0)*divdif(i,0));
	b.set(i,0,value);  
  }
  double x31=x(2,0)-x(0,0);
  double xn=x(n-1,0)-x(n-3,0);
  value=((dx(0,0)+2*x31)*dx(1,0)*divdif(0,0)+dx(0,0)*dx(0,0)*divdif(1,0))/x31;
  b.set(0,0,value);
  value=(dx(n-2,0)*dx(n-2,0)*divdif(n-3,0)+(2*xn+dx(n-2,0))*dx(n-3,0)*divdif(n-2,0))/xn;
  b.set(n-1,0,value);
  matrix  center(n,1);
  center.set(0,0,dx(1,0));
  center.set(n-1,0,dx(n-3,0));
  for(i=1;i<n-1;i++)
  {
   center.set(i,0,2*(dx(i,0)+dx(i-1,0)));
  }
  matrix c(n,n);
  c=0;
  matrix up(n,1);
  matrix down(n,1);
  up.set(0,0,0);
  up.set(1,0,x31);
  for(i=2;i<n;i++) up.set(i,0,dx(i-2,0));
  down.set(n-1,0,0);
  down.set(n-2,0,xn);
  for(i=0;i<n-2;i++) down.set(i,0,dx(i+1,0));
   for(i=0;i<n;i++)
  {
    c.set(i,i,center(i,0));
    if((i-1)>-1) 
		c.set(i-1,i,up(i,0));
	if((i+1)<n)  
		c.set(i+1,i,down(i,0));	 
  }
   matrix s((~c)*b);
   matrix c4(n-1,1);
   for(i=0;i<n-1;i++)
   {
   value=(s(i,0)+s(i+1,0)-2*divdif(i,0))/dx(i,0);
   c4.set(i,0,value);
   }
   matrix c3(n-1,1);
   for(i=0;i<n-1;i++)
   {
   value=(divdif(i,0)-s(i,0))/dx(i,0)-c4(i,0);
   c3.set(i,0,value);
   }
   matrix coefs(n-1,4);
   for(i=0;i<n-1;i++)
   {
    coefs.set(i,0,c4(i,0)/dx(i,0));
	coefs.set(i,1,c3(i,0));
	coefs.set(i,2,s(i,0));
	coefs.set(i,3,y(i,0));
   }
   int l= n-1;   
   int d=1;
   int lx=XI.rownum*XI.colnum;
   int k=4;
   matrix xs(XI.t());
   matrix breaks(x.t());
   matrix xt(1,breaks.colnum-1);
   for(i=0;i<xt.colnum;i++) xt.set(0,i,breaks(0,i));
   matrix index;
   sortForSpline(xt,xs,index);
   for(i=1;i<=lx;i++)
   {
    index.set(0,i-1,index(0,i-1)-i);
	if(index(0,i-1)<1) index.set(0,i-1,1);   
   }
   for(i=0;i<xs.colnum;i++)
	   xs.set(0,i,xs(0,i)-breaks(0,index(0,i)-1));
    matrix v(index.colnum,1);
   for(i=0;i<v.rownum;i++)
	   v.set(i,0,coefs(index(0,i)-1,0));
   v.trans();
   matrix ct(1,v.colnum);
  for(i=1;i<k;i++) 
  {   
     for(j=0;j<index.colnum;j++)
		 ct.set(0,j,coefs(index(0,j)-1,i));
	  v=xs.pMul(v)+ct;
  }
  v.trans();
  outY=matrix(v);
  return 1;
}

int length(matrix &a)
{
  int result=a.rownum;
  if(result<a.colnum) result=a.colnum;
  return result;
}
void  sortForSpline(matrix &x,matrix &xs,matrix &index)//要求X,xs,均为排好序的行向量
{
  int i,j;
  index=matrix(1,xs.colnum);
  index.set(0,0,0);
  int tem=0;
  for(i=0;i<xs.colnum;i++)
  {
    for(j=tem;j<x.colnum;j++)
	{
	  if(x(0,j)>xs(0,i))//找到第一个比xs(0，i)大的数
	  {
		index.set(0,i,j+i+1);
		tem=j;
	    break;  
	  }	 
	}
   if(j==x.colnum) index.set(0,i,x.colnum+i+1);
  }
 


}