matrix.cpp

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

{
	if(!issym) throw TMESSAGE("not symatry");
	size_t i,j,k;
	DOUBLE h,f,g,h2,a;
	 for (i=rownum-1; i>0; i--)
		{ h=0.0;
		  if (i>1)
			 for (k=0; k<i; k++)
				{ a = value(i,k); h += a*a;}
		  if (h == 0.0)
			 { c[i] = 0.0;
				if (i==1) c[i] = value(i,i-1);
				b[i] = 0.0;
			 }
		  else
			 { c[i] = sqrt(h);
				a = value(i,i-1);
				if (a > 0.0) c[i] = -c[i];
				h -= a*c[i];
				set(i,i-1,a-c[i]);
				f=0.0;
				for (j=0; j<i; j++)
				  { set(j,i,value(i,j)/h);
					 g=0.0;
					 for (k=0; k<=j; k++)
						g += value(j,k)*value(i,k);
					 if(j<=i-2)
						for (k=j+1; k<i; k++)
							g += value(k,j)*value(i,k);
					 c[j] = g/h;
					 f += g*value(j,i);
				  }
				h2=f/(2*h);
				for (j=0; j<i; j++)
				  { f=value(i,j);
					 g=c[j] - h2*f;
					 c[j] = g;
					 for (k=0; k<=j; k++)
						set(j,k, value(j,k)-f*c[k]-g*value(i,k) );
				  }
				b[i] = h;
			 }
		}
	 for (i=0; i<=rownum-2; i++) c[i] = c[i+1];
	 c[rownum-1] = 0.0;
	 b[0] = 0.0;
	 for (i=0; i<rownum; i++)
		{ if ((b[i]!=0.0)&&(i>=1))
			 for (j=0; j<i; j++)
				{ g=0.0;
				  for (k=0; k<i; k++)
					 g=g+value(i,k)*value(k,j);
				  for (k=0; k<i; k++)
					 set(k,j,value(k,j)-g*value(k,i));
				}
		  b[i] = value(i,i);
		  set(i,i,1.0);
		  if (i>=1)
			 for (j=0; j<=i-1; j++)
				{ set(i,j,0.0);
				  set(j,i,0.0); }
		}
	 return;
}

void matrix::trieigen(buffer& b, buffer& c, size_t l, DOUBLE eps)
						// 计算三对角阵的全部特征值与特征向量
{	size_t i,j,k,m,it;
	double d,f,h,g,p,r,e,s;
	c[rownum-1]=0.0; d=0.0; f=0.0;
	for (j=0; j<rownum; j++)
		{ it=0;
		  h=eps*(fabs(b[j])+fabs(c[j]));
		  if (h>d) d=h;
		  m=j;
		  while ((m<rownum)&&(fabs(c[m])>d)) m+=1;
		  if (m!=j)
			 { do
				  { if (it==l) throw TMESSAGE("fial to calculate eigen value");
					 it += 1;
					 g=b[j];
					 p=(b[j+1]-g)/(2.0*c[j]);
					 r=sqrt(p*p+1.0);
					 if (p>=0.0) b[j]=c[j]/(p+r);
					 else b[j]=c[j]/(p-r);
					 h=g-b[j];
					 for (i=j+1; i<rownum; i++)
						b[i]-=h;
					 f=f+h; p=b[m]; e=1.0; s=0.0;
					 for (i=m-1; i>=j; i--)
						{ g=e*c[i]; h=e*p;
						  if (fabs(p)>=fabs(c[i]))
							 { e=c[i]/p; r=sqrt(e*e+1.0);
								c[i+1]=s*p*r; s=e/r; e=1.0/r;
							 }
						  else
				{ e=p/c[i]; r=sqrt(e*e+1.0);
								c[i+1]=s*c[i]*r;
								s=1.0/r; e=e/r;
							 }
						  p=e*b[i]-s*g;
						  b[i+1]=h+s*(e*g+s*b[i]);
						  for (k=0; k<rownum; k++)
							 {
								h=value(k,i+1);
								set(k,i+1, s*value(k,i)+e*h);;
								set(k,i,e*value(k,i)-s*h);
							 }
						  if(i==0) break;
						}
					 c[j]=s*p; b[j]=e*p;
				  }
				while (fabs(c[j])>d);
			 }
		  b[j]+=f;
		}
	 for (i=0; i<=rownum; i++)
		{ k=i; p=b[i];
		  if (i+1<rownum)
			 { j=i+1;
				while ((j<rownum)&&(b[j]<=p))
				  { k=j; p=b[j]; j++;}
			 }
		  if (k!=i)
			 { b[k]=b[i]; b[i]=p;
				for (j=0; j<rownum; j++)
				  { p=value(j,i);
					 set(j,i,value(j,k));
					 set(j,k,p);
				  }
			 }
		}
}

matrix matrix::eigen(matrix & eigenvalue, DOUBLE eps, size_t l)
	 // 计算对称阵的全部特征向量和特征值
		// 返回按列排放的特征向量，而eigenvalue将返回一维矩阵，为所有特征值
		// 组成的列向量
{
	if(!issym) throw TMESSAGE("it is not symetic matrix");
	eigenvalue = matrix(rownum); // 产生n行1列向量准备放置特征值
	matrix m(*this); // 复制自己产生一新矩阵
	if(rownum == 1) {	// 如果只有1X1矩阵，则特征向量为1，特征值为value(0,0)
		m.set(0,0,1.0);
		eigenvalue.set(0,value(0,0));
		return m;
	}
	buffer * b, *c;
	b = getnewbuffer(rownum);
	c = getnewbuffer(rownum);
	m.house(*b,*c);	// 转换成三对角阵
	m.trieigen(*b,*c,l,eps); // 算出特征向量和特征值
	for(size_t i=0; i<rownum; i++) // 复制b的内容到eigenvalue中
		eigenvalue.set(i,(*b)[i]);
	return m;
}

void matrix::hessenberg()	// 将一般实矩阵约化为赫申伯格矩阵
{
	 size_t i,j,k;
	 double d,t;
	 for (k=1; k<rownum-1; k++)
		{ d=0.0;
		  for (j=k; j<rownum; j++)
			 { t=value(j,k-1);
				if (fabs(t)>fabs(d))
				  { d=t; i=j;}
			 }
		  if (fabs(d)!=0.0)
			 { if (i!=k)
				  { for (j=k-1; j<rownum; j++)
						{
						  t = value(i,j);
						  set(i,j,value(k,j));
						  set(k,j,t);
						}
					 for (j=0; j<rownum; j++)
						{
						  t = value(j,i);
						  set(j,i,value(j,k));
						  set(j,k,t);
						}
				  }
				for (i=k+1; i<rownum; i++)
				  {
					 t = value(i,k-1)/d;
					 set(i,k-1,0.0);
					 for (j=k; j<rownum; j++)
						  set(i,j,value(i,j)-t*value(k,j));
					 for (j=0; j<rownum; j++)
						  set(j,k,value(j,k)+t*value(j,i));
				  }
			 }
		}
}

void matrix::qreigen(matrix & u1, matrix & v1, size_t jt, DOUBLE eps)
 // 求一般实矩阵的所有特征根
// a和b均返回rownum行一列的列向量矩阵，返回所有特征根的实部和虚部
{
	matrix a(*this);
	a.hessenberg();	// 先算出赫申伯格矩阵
	u1 = matrix(rownum);
	v1 = matrix(rownum);
	buffer * uu = getnewbuffer(rownum);
	buffer * vv = getnewbuffer(rownum);
	buffer &u = *uu;
	buffer &v = *vv;
	 size_t m,it,i,j,k,l;
	 size_t iir,iic,jjr,jjc,kkr,kkc,llr,llc;
	 DOUBLE b,c,w,g,xy,p,q,r,x,s,e,f,z,y;
	 it=0; m=rownum;
	 while (m!=0)
		{ l=m-1;
		  while ( l>0 && (fabs(a.value(l,l-1))>eps*
			(fabs(a.value(l-1,l-1))+fabs(a.value(l,l))))) l--;
		  iir = m-1; iic = m-1;
		  jjr = m-1; jjc = m-2;
		  kkr = m-2; kkc = m-1;
		  llr = m-2; llc = m-2;
		  if (l==m-1)
			 { u[m-1]=a.value(m-1,m-1); v[m-1]=0.0;
				m--; it=0;
			 }
		  else if (l==m-2)
			 { b=-(a.value(iir,iic)+a.value(llr,llc));
				c=a.value(iir,iic)*a.value(llr,llc)-
					a.value(jjr,jjc)*a.value(kkr,kkc);
				w=b*b-4.0*c;
				y=sqrt(fabs(w));
				if (w>0.0)
				  { xy=1.0;
					 if (b<0.0) xy=-1.0;
					 u[m-1]=(-b-xy*y)/2.0;
					 u[m-2]=c/u[m-1];
					 v[m-1]=0.0; v[m-2]=0.0;
				  }
				else
				  { u[m-1]=-b/2.0; u[m-2]=u[m-1];
					 v[m-1]=y/2.0; v[m-2]=-v[m-1];
				  }
				m=m-2; it=0;
			 }
		  else
			 {
			 if (it>=jt) {
				delete uu;
				delete vv;
				throw TMESSAGE("fail to calculate eigenvalue");
			 }
				it++;
				for (j=l+2; j<m; j++)
					a.set(j,j-2,0.0);
				for (j=l+3; j<m; j++)
					a.set(j,j-3,0.0);
				for (k=l; k+1<m; k++)
				  { if (k!=l)
						{ p=a.value(k,k-1); q=a.value(k+1,k-1);
						  r=0.0;
						  if (k!=m-2) r=a.value(k+2,k-1);
						}
					 else
						{
						  x=a.value(iir,iic)+a.value(llr,llc);
						  y=a.value(llr,llc)*a.value(iir,iic)-
								a.value(kkr,kkc)*a.value(jjr,jjc);
						  iir = l; iic = l;
						  jjr = l; jjc = l+1;
						  kkr = l+1; kkc = l;
						  llr = l+1; llc = l+1;
						  p=a.value(iir,iic)*(a.value(iir,iic)-x)
								+a.value(jjr,jjc)*a.value(kkr,kkc)+y;
						  q=a.value(kkr,kkc)*(a.value(iir,iic)+a.value(llr,llc)-x);
						  r=a.value(kkr,kkc)*a.value(l+2,l+1);
						}
					 if ((fabs(p)+fabs(q)+fabs(r))!=0.0)
						{ xy=1.0;
						  if (p<0.0) xy=-1.0;
						  s=xy*sqrt(p*p+q*q+r*r);
						  if (k!=l) a.set(k,k-1,-s);
						  e=-q/s; f=-r/s; x=-p/s;
						  y=-x-f*r/(p+s);
						  g=e*r/(p+s);
						  z=-x-e*q/(p+s);
						  for (j=k; j<m; j++)
							 {
								iir = k; iic = j;
								jjr = k+1; jjc = j;
								p=x*a.value(iir,iic)+e*a.value(jjr,jjc);
								q=e*a.value(iir,iic)+y*a.value(jjr,jjc);
								r=f*a.value(iir,iic)+g*a.value(jjr,jjc);
								if (k!=m-2)
								  { kkr = k+2; kkc = j;
									 p=p+f*a.value(kkr,kkc);
									 q=q+g*a.value(kkr,kkc);
									 r=r+z*a.value(kkr,kkc);
									 a.set(kkr,kkc,r);
								  }
								a.set(jjr,jjc,q);
								a.set(iir,iic,p);
							 }
						  j=k+3;
						  if (j>=m-1) j=m-1;
						  for (i=l; i<=j; i++)
							 { iir = i; iic = k;
								jjr = i; jjc = k+1;
								p=x*a.value(iir,iic)+e*a.value(jjr,jjc);
								q=e*a.value(iir,iic)+y*a.value(jjr,jjc);
								r=f*a.value(iir,iic)+g*a.value(jjr,jjc);
								if (k!=m-2)
								  { kkr = i; kkc = k+2;
									 p=p+f*a.value(kkr,kkc);
									 q=q+g*a.value(kkr,kkc);
									 r=r+z*a.value(kkr,kkc);
									 a.set(kkr,kkc,r);
								  }
								a.set(jjr,jjc,q);
								a.set(iir,iic,p);
							 }
						}
				  }
			 }
		}
	for(i=0;i<rownum;i++) {
		u1.set(i,u[i]);
		v1.set(i,v[i]);
	}
	delete uu;
	delete vv;
}

DOUBLE gassrand(int rr)	// 返回一零均值单位方差的正态分布随机数
{
	static DOUBLE r=3.0;	// 种子
	if(rr) r = rr;
	int i,m;
	DOUBLE s,w,v,t;
	s=65536.0; w=2053.0; v=13849.0;
	t=0.0;
	for (i=1; i<=12; i++)
		{ r=r*w+v; m=(int)(r/s);
		  r-=m*s; t+=r/s;
		}
	t-=6.0;
	return(t);
}

gassvector::gassvector(matrix & r):	//r必须是正定对称阵，为正态随机向量的协方差
	a(r)
{
	if(r.rownum != r.colnum) throw TMESSAGE("must be a sqare matrix");
	if(!r.issym) throw TMESSAGE("must be a symetic matrix");
	matrix evalue;
	a = a.eigen(evalue);
	for(size_t i=0; i<a.colnum; i++) {
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}
}

matrix gassvector::operator()(matrix & r) // 返回给定协方差矩阵的正态随机向量
{
	a = r;
	matrix evalue;
	a = a.eigen(evalue);
	size_t i;
	for(i=0; i<a.colnum; i++) {
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}
	matrix rr(a.rownum);	// 产生列向量
	for(i=0; i<a.rownum; i++)
		rr.set(i,gassrand());
	return a*rr;
}

matrix gassvector::operator()()	// 返回已设定的协方差矩阵的正态随机向量
{
	matrix rr(a.rownum);
	for(size_t i=0; i<a.rownum; i++)
		rr.set(i,gassrand());
	return a*rr;
}

void gassvector::setdata(matrix & r) // 根据协方差矩阵设置增益矩阵
{
	if(!r.issym) throw TMESSAGE("r must be symetic!");
	a = r;
	matrix evalue;
	a = a.eigen(evalue);
	for(size_t i=0; i<a.colnum; i++) {
   	if(evalue(i)<0.0) throw TMESSAGE("var matrix not positive!");
		DOUBLE e = sqrt(evalue(i));
		for(size_t j=0; j<a.rownum; j++)
			a.set(j,i,a.value(j,i)*e);
	}
}

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;
		}
}