
// Matrix.cpp: implementation of the CMatrix class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"

#include "Matrix.h"
#include "math.h"


#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CMatrix::CMatrix()
{

}

CMatrix::~CMatrix()
{

}



void CMatrix::ludcmp(double **a, int n, int *indx, double *d)
{

	int i,imax,j,k;
	double big,dum,sum,temp;
	double *vv;

	vv = (double *) malloc(n*sizeof(double));
	*d = 1.0;
	for(i=1;i<=n;i++){
		big = 0.0;
		for(j=1;j<=n;j++)
			if((temp= fabs (a[i-1][j-1]))>big) big =temp;
			if(big ==0.0) ::AfxMessageBox("Error");
			vv[i-1]=1.0/big;
	}
	for (j=1;j<=n;j++){
		for(i=1;i<j;i++){
			sum = a[i-1][j-1];
			for(k=1;k<i;k++) sum-=a[i-1][k-1]*a[k-1][j-1];
			a[i-1][j-1] = sum;
		}
		big = 0.0;
		for(i=j;i<=n;i++){
			sum=a[i-1][j-1];
			for(k=1;k<j;k++)
				sum -=a[i-1][k-1]*a[k-1][j-1];
			a[i-1][j-1]= sum;
			if((dum=vv[i-1]*fabs(sum))>=big){
				big = dum;
				imax = i;
			}
		}
		if(j!=imax){
			for(k=1;k<=n;k++){
				dum=a[imax-1][k-1];
				a[imax-1][k-1] =a[j-1][k-1];
				a[j-1][k-1] = dum;
			}
			*d =-(*d);
            vv[imax-1]=vv[j-1];

		}
		indx[j-1] = imax;
		if(a[j-1][j-1]==0.0) a[j-1][j-1] = TINY;
		if(j!=n){
			dum = 1.0/(a[j-1][j-1]);
			for(i=j+1;i<=n;i++)
				a[i-1][j-1] *=dum;
		}
	}
	delete vv;
}

//DEL void CMatrix::lubksb(a, n, indx, b)
//DEL {
//DEL 
//DEL }

void CMatrix::lubksb(double **a, int n, int *indx, double *b)
{
	int i,ii=0,ip,j;
	double sum;

	for(i=1;i<=n;i++){
		ip=indx[i-1];
		sum=b[ip-1];
		b[ip-1]=b[i-1];
		if(ii)
			for(j=ii;j<=i-1;j++) sum -=a[i-1][j-1]*b[j-1];

		else if(sum) ii=i;
		b[i-1]= sum;
	}
	for (i=n;i>=1;i--){
		sum = b[i-1];
		for(j=i+1;j<=n;j++) sum -=a[i-1][j-1]*b[j-1];
		b[i-1]= sum/a[i-1][i-1];
	}

}

void CMatrix::getequationanswer(double **a, double *c, int n)
{
	int *indx ;
	indx = (int *) malloc(n*sizeof(int));
	double d;
	ludcmp(a,n,indx,&d);
	lubksb(a,n,indx,c);


/*	int i;
	int j;
	for(j=0;j<n-1;j++){
		for(i=n-1;i>j;i--){
			if(a[j][j]
			double subtract = a[i][j]/a[j][j];
			for(int k=0;k<n;k++){
				a[i][k] = a[i][k]- subtract*a[j][k];
			
			};
			b[i]=b[i]-subtract*b[j];
		 
		}
		
	
	};


    for(i=n-1;i>=0;i--){
		double sum = 0.0;
		for(j=n-1;j>i;j--){
			sum = sum +a[i][j]*c[j]/a[i][i];
		};
		c[i]=b[i]/a[i][i]-sum;
	};
	*/
}


void CMatrix::matrixmultiply(double **a, double **b, double **c, int m, int n, int k)
{

	for(int i=0;i<m;i++){
		for(int j=0;j<k;j++){
			double temp=0;
			for(int w=0;w<n;w++){
				temp +=a[i][w]*b[w][j];
			}
			c[i][j]=temp;
		}
	}
}

void CMatrix::inversematrix(double **a, double **y, int n)
{


	//getequationanswer(double **a, double *b, double d, int n, int *indx)
    int i,j;
	int * indx;
	indx = (int *)malloc(n*sizeof(int));
	double *b;
	b = (double *)malloc(n*sizeof(double));
	double *d;
	d = (double *)malloc(n*sizeof(double));

	double **aa;
	aa =(double **)malloc(n*sizeof(double*));
	for( i=0;i<n;i++){
		aa[i] = (double *) malloc(n*sizeof(double));
	};

	for(j=0;j<n;j++){
	    for(int m=0;m<n;m++)
			for(int q=0;q<n;q++)
			{
				aa[m][q] = a[m][q];
			};

		for(m=0;m<n;m++){
			    b[m] = 0;
		};
		
		b[j] = 1;
        getequationanswer(aa,b,n);
		for(i=0;i<n;i++){
		
			y[i][j] = b[i];
		};
	
	};

	for(i=0;i<n;i++)
		delete aa[i];
	delete aa;
	delete indx;
	delete b;
	delete d;
	/*int i,j;
	double d;
    
	int * indx;
    double *col;
	
	indx = (int *)malloc(n*sizeof(int));
	for(i=0;i<n;i++)
	{
		indx[i]=0;
	};
    col =(double *)malloc(n*sizeof(double));
	ludcmp(a,n,indx,&d);
	for(j=0;j<n;j++){
		for(i=0;i<n;i++) col[i]=0.0;
		col[j]=1.0;
		lubksb(a,n,indx,col);
		for(i=0;i<n;i++) y[i][j] = col[i];
	}
	delete indx;
	delete col;
*/
}

void CMatrix::matrixvectormultiply(double **a, double *b,double *c, int m, int n)
{

	for( int j=0;j<m;j++){
		c[j] = 0.0;
	};
	
	for (int i=0;i<m;i++){
		for(int j=0;j<n;j++){
			c[i] = c[i] +a[i][j]*b[j];
		};
	

	};

}

double CMatrix::bilinearinterpolation(double x, double y, double p00, double p10, double p01, double p11)
{

	double px0,px1,pxy;

	px0 = p00 +x*(p10-p00);
	px1 = p01+x*(p11-p01);
	pxy = px0+y*(px1-px0);
	return pxy;
}

void CMatrix::matrixreverse(double **a, double **b, int m, int n)
{

	for(int i=0;i<m;i++)
		for(int j=0;j<n;j++){
            b[j][i] = a[i][j];			
		}
}

BOOL CMatrix::IsPointInTriangle(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3)
{

	double xp, yp;

	xp = (x1>x2)?x1:x2;
	xp = (xp>x3)?xp:x3;

	xp = 2*xp;

	yp = (y1>y2)?y1:y2;
	yp = (yp>y3)?yp:y3;

	yp = 3*yp;

    double t1;
	double t2;
	double t3;
    
    if((x0>x1)&&(x0>x2)&&(x0>x3)) return FALSE;
	if((x0<x1)&&(x0<x2)&&(x0<x3)) return FALSE;
	if((y0>y1)&&(y0>y2)&&(y0>y3)) return FALSE;
	if((y0<y1)&&(y0<y2)&&(y0<y3)) return FALSE;



	t1 = findintersection(x0,y0,xp,yp,x1,y1,x2,y2);
	t2 = findintersection(x0,y0,xp,yp,x2,y2,x3,y3);
	t3 = findintersection(x0,y0,xp,yp,x3,y3,x1,y1);

	int count = 0;
	if(t1>=0){
		count++;
		if(t2>=0){
			if(fabs(t1-t2)>TINYT){
			  count++;
			  if(t3>=0){
				  if((fabs(t1-t3)>TINYT)&&(fabs(t2-t3)>TINYT)){
					count++;
				  }else{
				  }

			  }else{
			  }
			}else{
				if(t3>=0){
					if(fabs(t3-t1)>TINY){
						count++;
					}else{
					};
				}else{
				};
			}
		}else{
			
			 if(t3>=0){
				  if(fabs(t1-t3)>TINYT){
					count++;
				  }else{
				  }

			  }else{
			  }


		}
	}else{
		if(t2>=0){
			count++;
			if(t3>=0){
				if(fabs(t2-t3)>TINYT){
					count++;
				}else{
				}
			}else{
			};
		}else{
			if(t3>=0){
				count++;
			}else{
			};
		};


	};

	if(count==1) return TRUE;
	else return FALSE;
	


}

double CMatrix::findintersection(double x0, double y0, double xp, double yp, double x1, double y1, double x2, double y2)
{

	double m;
	double n;
    double t1;
	double t2;
	m = (yp-y0)*(x1-x0)-(y1-y0)*(xp-x0);
	n = (y2-y1)*(xp-x0)-(yp-y0)*(x2-x1);

	if(fabs(n)<TINY) return -1;
	else{
		t2 = m/n; 
		if (t2<0) return -1;
		if (t2>1) return -1;
		if(fabs(xp-x0)>TINY){
			t1 = (x1-x0+(x2-x1)*t2)/(xp-x0);
			
		}else{
			if(fabs(yp-y0)>TINY){
				t1 = (y1-y0+(y2-y1)*t2)/(yp-y0);
			}else{
				t1 = (-1);
			};
		}
		
	}
    if(t1<0) return -1;
	if(t1>1) return -1;
	return t1;
}

double CMatrix::getaveragevalue(double p1, double p2, double p3, double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3)
{

/*	double m,n;

	double pm,pn;
	double t1,t2;
	double xm,ym;
	m = (y3-y2)*(x1-x0)-(y1-y0)*(x3-x2);
    n = (y1-y0)*(x2-x0)-(y2-y0)*(x1-x0);
    
	if(fabs(m)<TINY) {
		::AfxMessageBox("Too TINY");
		return -p1;
	
	}
	else{
		t2 = n/m;
		
	};
	if((t2<0)||(t2>1)) {


		return -p1;
	
	};
	pm = p2*(1-t2)+p3*t2;
	
	xm = x2+(x3-x2)*t2;
    ym = y2+(y3-y2)*t2;

    double d01,d0m;
    
	d01 = sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));

	d0m = sqrt((x0-xm)*(x0-xm)+(y0-ym)*(y0-ym));

	t1 = d0m/(d0m+d01);

	pn = pm*(1-t1)+t1*p1;
	return pn;

*/
	double r01;
	double r02;
	double r03;

	r01 = sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0));
	if (r01<TINY) r01 = 1000000;
	else r01 = 1/r01;

	r02 = sqrt((x2-x0)*(x2-x0)+(y2-y0)*(y2-y0));
	if (r02<TINY) r02 = 1000000;
	else r02 = 1/r02;

	r03 = sqrt((x3-x0)*(x3-x0)+(y3-y0)*(y3-y0));
	if (r03<TINY) r03 = 1000000;
	else r03 = 1/r03;

	double pvalue;
	pvalue = (r01*p1+r02*p2+r03*p3)/(r01+r02+r03);
	return pvalue;

	

}

BOOL CMatrix::isvalidtriangle(double x1, double y1, double x2, double y2, double x3, double y3)
{

	BOOL istriangle;
	istriangle = TRUE;

	double d12,d13,d23;
	
	d12 = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
	d13 = sqrt((x1-x3)*(x1-x3)+(y1-y3)*(y1-y3));
	d23 = sqrt((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2));
	
	if(d12<TINYS) istriangle = FALSE;
	if(d13<TINYS) istriangle = FALSE;
	if(d23<TINYS) istriangle = FALSE;

	return istriangle;

}


void CMatrix::getvectorcrossproduct(double a[3],double b[3],double c[3])
{
	c[0] = a[1]*b[2]-a[2]*b[1];
	c[1] = a[2]*b[0]-a[0]*b[2];
	c[2] = a[0]*b[1]-a[1]*b[0];
}

double CMatrix::getdotproduct(double a[], double b[])
{
 
	double sum;
	sum =  a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
    return sum;

}
/************************************************************************/
/*	        singular value decomposition 				*/
/*	a ----- input data matrix ;while output a[i][i] is singular	*/
/*		value 				m*n			*/
/*	u ----- left eigenvector matrix		m*m			*/
/*	v ----- right eigenvector matrix	n*n			*/
/*	m,n----	order of matrix						*/
/************************************************************************/


int CMatrix::SVD(double *a, int m, int n, double *u, double *v, double eps)
{

  int              i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks,ka;
  double       	   d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];
  double       	   *s,*e,*w;
  
  ka=m>n?m:n;
  ka++;
  s=(double *)malloc(ka*sizeof(double));
  e=(double *)malloc(ka*sizeof(double));
  w=(double *)malloc(ka*sizeof(double));
  it=60; k=n;
  if (m-1<n) k=m-1;
  l=m;
  if (n-2<m) l=n-2;
  if (l<0) l=0;
  ll=k;
  if (l>k) ll=l;
  if (ll>=1)
    { for (kk=1; kk<=ll; kk++)
      { if (kk<=k)
	{ d=0.0;
	for (i=kk; i<=m; i++)
	  { ix=(i-1)*n+kk-1; d=d+a[ix]*a[ix];}
	s[kk-1]=sqrt(d);
	if (s[kk-1]!=0.0)
	  { ix=(kk-1)*n+kk-1;
	  if (a[ix]!=0.0)
	    { s[kk-1]=fabs(s[kk-1]);
	    if (a[ix]<0.0) s[kk-1]=-s[kk-1];
	    }
	  for (i=kk; i<=m; i++)
	    { iy=(i-1)*n+kk-1;
	    a[iy]=a[iy]/s[kk-1];
	    }
	  a[ix]=1.0+a[ix];
	  }
	s[kk-1]=-s[kk-1];
	}
      if (n>=kk+1)
	{ for (j=kk+1; j<=n; j++)
	  { if ((kk<=k)&&(s[kk-1]!=0.0))
	    { d=0.0;
	    for (i=kk; i<=m; i++)
	      { ix=(i-1)*n+kk-1;
	      iy=(i-1)*n+j-1;
	      d=d+a[ix]*a[iy];
	      }
	    d=-d/a[(kk-1)*n+kk-1];
	    for (i=kk; i<=m; i++)
	      { ix=(i-1)*n+j-1;
	      iy=(i-1)*n+kk-1;
	      a[ix]=a[ix]+d*a[iy];
	      }
	    }
	  e[j-1]=a[(kk-1)*n+j-1];
	  }
	}
      if (kk<=k)
	{ for (i=kk; i<=m; i++)
	  { ix=(i-1)*m+kk-1; iy=(i-1)*n+kk-1;
	  u[ix]=a[iy];
	  }
	}
      if (kk<=l)
	{ d=0.0;
	for (i=kk+1; i<=n; i++)
	  d=d+e[i-1]*e[i-1];
	e[kk-1]=sqrt(d);
	if (e[kk-1]!=0.0)
	  { if (e[kk]!=0.0)
	    { e[kk-1]=fabs(e[kk-1]);
	    if (e[kk]<0.0) e[kk-1]=-e[kk-1];
	    }
	  for (i=kk+1; i<=n; i++)
	    e[i-1]=e[i-1]/e[kk-1];
	  e[kk]=1.0+e[kk];
	  }
	e[kk-1]=-e[kk-1];
	if ((kk+1<=m)&&(e[kk-1]!=0.0))
	  { for (i=kk+1; i<=m; i++) w[i-1]=0.0;
	  for (j=kk+1; j<=n; j++)
	    for (i=kk+1; i<=m; i++)
	      w[i-1]=w[i-1]+e[j-1]*a[(i-1)*n+j-1];
	  for (j=kk+1; j<=n; j++)
	    for (i=kk+1; i<=m; i++)
	      { ix=(i-1)*n+j-1;
	      a[ix]=a[ix]-w[i-1]*e[j-1]/e[kk];
	      }
	  }
	for (i=kk+1; i<=n; i++)
	  v[(i-1)*n+kk-1]=e[i-1];
	}
      }
    }
  mm=n;
  if (m+1<n) mm=m+1;
  if (k<n) s[k]=a[k*n+k];
  if (m<mm) s[mm-1]=0.0;
  if (l+1<mm) e[l]=a[l*n+mm-1];
  e[mm-1]=0.0;
  nn=m;
  if (m>n) nn=n;
  if (nn>=k+1)
    { for (j=k+1; j<=nn; j++)
      { for (i=1; i<=m; i++)
	u[(i-1)*m+j-1]=0.0;
      u[(j-1)*m+j-1]=1.0;
      }
    }
  if (k>=1)
    { for (ll=1; ll<=k; ll++)
      { kk=k-ll+1; iz=(kk-1)*m+kk-1;
      if (s[kk-1]!=0.0)
	{ if (nn>=kk+1)
	  for (j=kk+1; j<=nn; j++)
	    { d=0.0;
	    for (i=kk; i<=m; i++)
	      { ix=(i-1)*m+kk-1;
	      iy=(i-1)*m+j-1;
	      d=d+u[ix]*u[iy]/u[iz];
	      }
	    d=-d;
	    for (i=kk; i<=m; i++)
	      { ix=(i-1)*m+j-1;
	      iy=(i-1)*m+kk-1;
	      u[ix]=u[ix]+d*u[iy];
	      }
	    }
	for (i=kk; i<=m; i++)
	  { ix=(i-1)*m+kk-1; u[ix]=-u[ix];}
	u[iz]=1.0+u[iz];
	if (kk-1>=1)
	  for (i=1; i<=kk-1; i++)
	    u[(i-1)*m+kk-1]=0.0;
	}
      else
	{ for (i=1; i<=m; i++)
	  u[(i-1)*m+kk-1]=0.0;
	u[(kk-1)*m+kk-1]=1.0;
	}
      }
    }
  for (ll=1; ll<=n; ll++)
    { kk=n-ll+1; iz=kk*n+kk-1;
    if ((kk<=l)&&(e[kk-1]!=0.0))
      { for (j=kk+1; j<=n; j++)
	{ d=0.0;
	for (i=kk+1; i<=n; i++)
	  { ix=(i-1)*n+kk-1; iy=(i-1)*n+j-1;
                    d=d+v[ix]*v[iy]/v[iz];
	  }
	d=-d;
	for (i=kk+1; i<=n; i++)
	  { ix=(i-1)*n+j-1; iy=(i-1)*n+kk-1;
	  v[ix]=v[ix]+d*v[iy];
	  }
	}
      }
    for (i=1; i<=n; i++)
      v[(i-1)*n+kk-1]=0.0;
    v[iz-n]=1.0;
    }
    for (i=1; i<=m; i++)
      for (j=1; j<=n; j++)
	a[(i-1)*n+j-1]=0.0;
    m1=mm; it=60;
    while (1==1)
      { if (mm==0)
	{ ppp(a,e,s,v,m,n);
	free(s); free(e); free(w); return(1);
	}
      if (it==0)
	{ ppp(a,e,s,v,m,n);
	free(s); free(e); free(w); return(-1);
	}
      kk=mm-1;
      while ((kk!=0)&&(fabs(e[kk-1])!=0.0))
	{ d=fabs(s[kk-1])+fabs(s[kk]);
	dd=fabs(e[kk-1]);
	if (dd>eps*d) kk=kk-1;
	else e[kk-1]=0.0;
	}
      if (kk==mm-1)
	{ kk=kk+1;
	if (s[kk-1]<0.0)
	  { s[kk-1]=-s[kk-1];
	  for (i=1; i<=n; i++)
	    { ix=(i-1)*n+kk-1; v[ix]=-v[ix];}
	  }
	while ((kk!=m1)&&(s[kk-1]<s[kk]))
	  { d=s[kk-1]; s[kk-1]=s[kk]; s[kk]=d;
	  if (kk<n)
	    for (i=1; i<=n; i++)
	      { ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;
	      d=v[ix]; v[ix]=v[iy]; v[iy]=d;
	      }
	  if (kk<m)
	    for (i=1; i<=m; i++)
	      { ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;
	      d=u[ix]; u[ix]=u[iy]; u[iy]=d;
	      }
	  kk=kk+1;
	  }
	it=60;
	mm=mm-1;
	}
      else
	{ ks=mm;
	while ((ks>kk)&&(fabs(s[ks-1])!=0.0))
	  { d=0.0;
	  if (ks!=mm) d=d+fabs(e[ks-1]);
	  if (ks!=kk+1) d=d+fabs(e[ks-2]);
	  dd=fabs(s[ks-1]);
	  if (dd>eps*d) ks=ks-1;
	  else s[ks-1]=0.0;
	  }
	if (ks==kk)
	  { kk=kk+1;
	  d=fabs(s[mm-1]);
	  t=fabs(s[mm-2]);
	  if (t>d) d=t;
	  t=fabs(e[mm-2]);
	  if (t>d) d=t;
	  t=fabs(s[kk-1]);
	  if (t>d) d=t;
	  t=fabs(e[kk-1]);
	  if (t>d) d=t;
	  sm=s[mm-1]/d; sm1=s[mm-2]/d;
	  em1=e[mm-2]/d;
	  sk=s[kk-1]/d; ek=e[kk-1]/d;
	  b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0;
	  c=sm*em1; c=c*c; shh=0.0;
	  if ((b!=0.0)||(c!=0.0))
	    { shh=sqrt(b*b+c);
	    if (b<0.0) shh=-shh;
	    shh=c/(b+shh);
	    }
	  fg[0]=(sk+sm)*(sk-sm)-shh;
	  fg[1]=sk*ek;
	  for (i=kk; i<=mm-1; i++)
	    { sss(fg,cs);
	    if (i!=kk) e[i-2]=fg[0];
	    fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
	    e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
	    fg[1]=cs[1]*s[i];
	    s[i]=cs[0]*s[i];
                    if ((cs[0]!=1.0)||(cs[1]!=0.0))
                      for (j=1; j<=n; j++)
                        {
	          ix=(j-1)*n+i-1;
	          iy=(j-1)*n+i;
	          d=cs[0]*v[ix]+cs[1]*v[iy];
	          v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
	          v[ix]=d;
                        }
                    sss(fg,cs);
                    s[i-1]=fg[0];
                    fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
                    s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
                    fg[1]=cs[1]*e[i];
                    e[i]=cs[0]*e[i];
                    if (i<m)
                      if ((cs[0]!=1.0)||(cs[1]!=0.0))
                        for (j=1; j<=m; j++)
                          { ix=(j-1)*m+i-1;
			  iy=(j-1)*m+i;
			  d=cs[0]*u[ix]+cs[1]*u[iy];
			  u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
			  u[ix]=d;
                          }
	    }
	  e[mm-2]=fg[0];
	  it=it-1;
	  }
	else
	  { if (ks==mm)
	    { kk=kk+1;
	    fg[1]=e[mm-2]; e[mm-2]=0.0;
	    for (ll=kk; ll<=mm-1; ll++)
	      { i=mm+kk-ll-1;
	      fg[0]=s[i-1];
	      sss(fg,cs);
                        s[i-1]=fg[0];
                        if (i!=kk)
                          { fg[1]=-cs[1]*e[i-2];
			  e[i-2]=cs[0]*e[i-2];
                          }
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=n; j++)
                            { ix=(j-1)*n+i-1;
			    iy=(j-1)*n+mm-1;
                              d=cs[0]*v[ix]+cs[1]*v[iy];
                              v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
                              v[ix]=d;
                            }
	      }
	    }
	  else
                  { kk=ks+1;
		  fg[1]=e[kk-2];
		  e[kk-2]=0.0;
		  for (i=kk; i<=mm; i++)
                      { fg[0]=s[i-1];
		      sss(fg,cs);
		      s[i-1]=fg[0];
		      fg[1]=-cs[1]*e[i-1];
		      e[i-1]=cs[0]*e[i-1];
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=m; j++)
                            { ix=(j-1)*m+i-1;
			    iy=(j-1)*m+kk-2;
                              d=cs[0]*u[ix]+cs[1]*u[iy];
                              u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
                              u[ix]=d;
                            }
                      }
                  }
	  }
	}
      }
    return(1);

}

void CMatrix::ppp(double *a, double *e, double *s, double *v, int m, int n)
{

	int i,j,p,q;
    double d;
    if (m>=n) i=n;
    else i=m;
    for (j=1; j<=i-1; j++)
		{ a[(j-1)*n+j-1]=s[j-1];
			a[(j-1)*n+j]=e[j-1];
	}
    a[(i-1)*n+i-1]=s[i-1];
    if (m<n) a[(i-1)*n+i]=e[i-1];
    for (i=1; i<=n-1; i++)
      for (j=i+1; j<=n; j++)
	{ p=(i-1)*n+j-1; q=(j-1)*n+i-1;
        d=v[p]; v[p]=v[q]; v[q]=d;
	}
    
}

void CMatrix::sss(double *fg, double *cs)
{

	double r,d;
if ((fabs(fg[0])+fabs(fg[1]))==0.0)
      { cs[0]=1.0; cs[1]=0.0; d=0.0;}
else 
      { d=sqrt(fg[0]*fg[0]+fg[1]*fg[1]);
      if (fabs(fg[0])>fabs(fg[1]))
	{ d=fabs(d);
	if (fg[0]<0.0) d=-d;
	}
        if (fabs(fg[1])>=fabs(fg[0]))
          { d=fabs(d);
	  if (fg[1]<0.0) d=-d;
          }
        cs[0]=fg[0]/d; cs[1]=fg[1]/d;
      }
    r=1.0;
    if (fabs(fg[0])>fabs(fg[1])) r=cs[1];
    else
      if (cs[0]!=0.0) r=1.0/cs[0];
    fg[0]=d; fg[1]=r;
    
}

void CMatrix::vectormultiplyvectorprim(double *a, double *aprim, double ** matrix,int n)
{


	for(int i=0;i<n;i++)
		for(int j=0;j<n;j++){
			matrix[i][j] = a[i]*aprim[j];
		
		};


}

double CMatrix::getmol(double *vector,int n)
{

	double s=0;
	for(int i=0;i<n;i++){
		s = s+vector[i]*vector[i];

	};
	double realmol;
	realmol = sqrt(s);
	return realmol;
}



void CMatrix::getstandardvector(double *vector,int n)
{
	double mol;
	mol = getmol(vector,n);
	for(int i=0;i<n;i++){
		vector[i] = vector[i]/mol;
	};

}

double CMatrix::ibilinearinterpolation(double x, double y, unsigned char p00, unsigned char p10, unsigned char p01, unsigned char p11)
{

	double px0,px1,pxy;

	px0 = p00 +x*(p10-p00);
	px1 = p01+x*(p11-p01);
	pxy = px0+y*(px1-px0);
	return pxy;
}

double CMatrix::getmaxoftwodouble(double x1, double x2)
{

	if(x1>x2) return x1; 
	else return x2;
}

double CMatrix::getminoftwodouble(double x1, double x2)
{

	if(x1<x2) return x1; 
	else return x2;
}

double CMatrix::getmaxoffourdouble(double x1,double x2,double x3,double x4)
{

	double tmpx1,tmpx2;
	tmpx1 = getmaxoftwodouble(x1,x2);
	tmpx2 = getmaxoftwodouble(x3,x4);
	return getmaxoftwodouble(tmpx1,tmpx2);
}
double CMatrix::getminoffourdouble(double x1,double x2,double x3,double x4)
{

	double tmpx1,tmpx2;
	tmpx1 = getminoftwodouble(x1,x2);
	tmpx2 = getminoftwodouble(x3,x4);
	return getminoftwodouble(tmpx1,tmpx2);
}