//***************************************************************************
#include <iostream.h>
#include <fstream.h>
#include <stdlib.h>             /* ANSI C standard library routines */
#include <string.h>             /* ANSI standard string routines */
#include <limits.h>
#include <math.h>
#include <stdio.h>
#include "matrix.h"
//***************************************************************************
Matrix::Matrix ( )	// Constructor
{
allocateFlag = 0;
}
//***************************************************************************
Matrix::Matrix (int m, int n)	// Constructor
{
allocateFlag = 0;
nx = m;
ny = n;
allocateMem (m, n);
}
//***************************************************************************
Matrix::~Matrix ( )	// Destructor
{
deAllocateMem( );
}
//***************************************************************************
void Matrix::allocateMem (int m, int n)
{
if (allocateFlag)
        {
         cerr << "Matrix::allocateMem: Memory already allocated!!" << endl;
         return;
        }
allocateFlag = 1;

p = new double *[m];
for (int i = 0; i < m; i++)
	{
	 p[i] = new double [n];
	 if (!p[i])
		{
		 cerr << "Matrix: Not enough memory for allocateMem!!"
			<< endl;
		 exit(-1);
		}
	 for (int j = 0; j < n; j++)
		p[i][j] = 0.0;
	}
}
//***************************************************************************
void Matrix::deAllocateMem ( )
{
if (allocateFlag)
	{
	 for (int i = 0; i < nx; i++)
		delete p[i];
	 delete p;
	 allocateFlag = 0;
	}
}
//***************************************************************************
void Matrix::Identity ()
{
if (nx != ny)
	{
	 cerr << "Identity(): Nx not equal to Ny !!" << endl;
	 exit(-1);
	}
for (int i = 0; i < nx; i++)
	{
	 for (int j = 0; j < ny; j++)
		p[i][j] = 0.0;
	 p[i][i] = 1.0;
	}
}
//***************************************************************************
Matrix &Matrix::Transpose ()
{
Matrix *M = new Matrix;
Matrix temp(ny,nx);

for (int i = 0; i < ny; i++)
	for (int j = 0; j < nx; j++)
		temp.p[i][j] = p[j][i];
*M = temp;
return *M;
}
//***************************************************************************
Matrix &Matrix::operator= (const Matrix &M)
{
if (this == &M)
	return *this;

nx = M.nx;
ny = M.ny;

if (!allocateFlag)
	allocateMem (nx, ny);

for (int i = 0; i < nx; i++)
	for (int j = 0; j < ny; j++)
		p[i][j] = M.p[i][j];

return *this;
}
//***************************************************************************
Matrix &Matrix::operator+ (Matrix &M1)
{
Matrix *M = new Matrix;

if (M1.nx != nx || M1.ny != ny)
	{
	 cerr << "operator +: Both matrices are not of the same size!!" << endl;
	 exit(-1);
	}
*M = M1;
for (int i = 0; i < nx; i++)
	for (int j = 0; j < ny; j++)
		M->p[i][j] += p[i][j];
return *M;
}
//***************************************************************************
Matrix &Matrix::operator- (Matrix &M1)
{
Matrix *M = new Matrix;

if (M1.nx != nx || M1.ny != ny)
	{
	 cerr << "operator -: Both matrices are not of the same size!!" << endl;
	 exit(-1);
	}
*M = M1;
for (int i = 0; i < nx; i++)
	for (int j = 0; j < ny; j++)
		M->p[i][j] = p[i][j] - M->p[i][j];
return *M;
}
//***************************************************************************
Matrix &Matrix::operator* (double k)
{
Matrix *M = new Matrix;
Matrix temp(nx,ny);

temp = *this;

for (int i = 0; i < nx; i++)
	for (int j = 0; j < ny; j++)
		temp.p[i][j] *= k;
*M = temp;
return *M;
}
//***************************************************************************
Matrix &Matrix::operator/ (double k)
{
Matrix *M = new Matrix;
Matrix temp(nx,ny);

temp = *this;

for (int i = 0; i < nx; i++)
	for (int j = 0; j < ny; j++)
		temp.p[i][j] /= k;
*M = temp;
return *M;
}
//***************************************************************************
Matrix &Matrix::operator* (Matrix &M1)
{
Matrix *M = new Matrix;

if (ny != M1.nx)
	{
	 cerr << "operator *: Cannot multiply " << nx << "x" << ny
		<< " matrix by " << M1.nx << "x" << M1.ny << " matrix!!"
		<< endl;
	 exit(-1);
	}
*M = M1;
Matrix temp(nx,M1.ny);
for (int i = 0; i < temp.nx; i++)
	for (int j = 0; j < temp.ny; j++)
		{
		 temp.p[i][j] = 0.0;
		 for (int k = 0; k < M1.nx; k++)
			temp.p[i][j] += p[i][k]*M1.p[k][j];
		}
*M = temp;
return *M;
}
//***************************************************************************
ostream &operator<< (ostream &Out, Matrix &M)
{
for (int i = 0; i < M.nx; i++)
        {
        for (int j = 0; j < M.ny; j++)
                {
		 Out.setf(ios::fixed,ios::floatfield);
		 Out.precision(4);
		 if (M.p[i][j] >= 0.0)
		 	Out << " ";
		 Out << M.p[i][j] << " ";
                }
         Out << endl;
        }
Out << endl;
Out.precision(6);
Out.setf(0,ios::floatfield);

return Out;
}
//***************************************************************************
double Matrix::operator() (int i, int j)
{
return p[i][j];
}
//***************************************************************************
Matrix &Transpose (Matrix &M)
{
return M.Transpose();
}
//***************************************************************************
Matrix &Identity (int n)
{
Matrix *M = new Matrix;
Matrix temp(n,n);

temp.Identity();

*M = temp;
return *M;
}
//***************************************************************************
Matrix &xRotate (double angle)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[1][1] = temp.p[2][2] = cos(angle);
temp.p[1][2] = sin(angle);
temp.p[2][1] = -sin(angle);
temp.p[0][0] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &yRotate (double angle)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[0][0] = temp.p[2][2] = cos(angle);
temp.p[0][2] = sin(angle);
temp.p[2][0] = -sin(angle);
temp.p[1][1] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &zRotate (double angle)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[0][0] = temp.p[1][1] = cos(angle);
temp.p[0][1] = sin(angle);
temp.p[1][0] = -sin(angle);
temp.p[2][2] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &Translate (double x, double y, double z)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[0][3] = x;
temp.p[1][3] = y;
temp.p[2][3] = z;
temp.p[0][0] = temp.p[1][1] = temp.p[2][2] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &Scale (double x, double y, double z)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[0][0] = x;
temp.p[1][1] = y;
temp.p[2][2] = z;
temp.p[0][0] = temp.p[1][1] = temp.p[2][2] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &Perspective (double lambda)
{
Matrix *M = new Matrix;
Matrix temp(4,4);

temp.p[3][2] = -1.0/lambda;
temp.p[0][0] = temp.p[1][1] = temp.p[2][2] = temp.p[3][3] = 1.0;

*M = temp;
return *M;
}
//***************************************************************************
Matrix &TransformToImagePlane (double x0, double y0, double z0, double xC,
	double yC, double zC, double r1, double r2, double r3, double lambda)
{
Matrix *M = new Matrix;

double x = xC - x0;
double y = yC - y0;
double z = zC - z0;
double r = sqrt(x*x+y*y+z*z);
double theta = atan2(x,-y);
double alpha = acos(z/r);

if (x == 0.0 && y == 0.0)	// bugfix for Linux egcs-c++ atan2(,) function
	theta = 0.0;

Matrix P = Perspective(lambda);
Matrix Tw0 = Translate(-x0,-y0,-z0);
Matrix R_theta = zRotate(theta);
Matrix R_alpha = xRotate(alpha);
Matrix Tr = Translate(-r1,-r2,-r3);

Matrix T = P * Tr * R_alpha * R_theta * Tw0;

//cout << "alpha = " << alpha/DEG2RAD << " deg" << endl;
//cout << "theta = " << theta/DEG2RAD << " deg" << endl;
 
*M = T;
return *M;
}
//***************************************************************************
void PerspectiveTransform (Matrix T, double x, double y, double z, 
	double *xp, double *yp)
{
Matrix w(4,1), c_h(4,1);

w.p[0][0] = x;
w.p[1][0] = y;
w.p[2][0] = z;
w.p[3][0] = 1.0;

c_h = T * w;

if (fabs(c_h(3,0)) > 1.0e-4)
	{
	 *xp = c_h(0,0)/c_h(3,0); 
	 *yp = c_h(1,0)/c_h(3,0); 
	}
else
	{
	 *xp = 1.0e+10;
	 *yp = 1.0e+10;
	}
}
//***************************************************************************