
// 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)) {

	//	::AfxMessageBox("Invalid t2");
		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;




	

}

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;

}
