matrix3.cpp

#include <iostream>
#include <iomanip>
#include "matrix3.h"
#include <float.h>
#include <cmath>

Matrix3::Matrix3(double m11, double m12, double m13, double m21,
	double m22, double m23, double m31, double m32, double m33)
{
	m[0][0]=m11; m[0][1]=m12; m[0][2]=m13;
	m[1][0]=m21; m[1][1]=m22; m[1][2]=m23;
	m[2][0]=m31; m[2][1]=m32; m[2][2]=m33;
}

Matrix3::Matrix3(double md)
{
	m[0][0]=m[1][1]=m[2][2]=md;
	m[0][1]=m[0][2]=m[1][0]=m[1][2]=m[2][0]=m[2][1]=0.0;
}

Matrix3::Matrix3(double m1, double m2, double m3)
{
	m[0][0]=m1;
	m[1][1]=m2;
	m[2][2]=m3;
	m[0][1]=m[0][2]=m[1][0]=m[1][2]=m[2][0]=m[2][1]=0.0;
}

Matrix3::Matrix3()
{
	m[0][0]=m[1][1]=m[2][2]=0.0;
	m[0][1]=m[0][2]=m[1][0]=m[1][2]=m[2][0]=m[2][1]=0.0;
}

Matrix3::~Matrix3()
{
}

std::ostream& operator << (std::ostream& s, Matrix3  matrix)
{
	s << "#MATRIX:\n";
	s << std::setw(8) << std::setprecision(7) << matrix.m[0][0] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[0][1] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[0][2] << std::endl;
	s << std::setw(8) << std::setprecision(7) << matrix.m[1][0] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[1][1] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[1][2] << std::endl;
	s << std::setw(8) << std::setprecision(7) << matrix.m[2][0] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[2][1] << " ";
	s << std::setw(8) << std::setprecision(7) << matrix.m[2][2] << std::endl;
	return s;
}

Matrix3 operator + (const Matrix3& m1, const Matrix3& m2)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=m1.m[i][j]+m2.m[i][j];
	return m0;
}

Matrix3 operator - (const Matrix3& m1, const Matrix3& m2)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=m1.m[i][j]-m2.m[i][j];
	return m0;
}

Matrix3 operator * (const Matrix3& m1, const Matrix3& m2)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=m1.m[i][0]*m2.m[0][j]+m1.m[i][1]*m2.m[1][j]+m1.m[i][2]*m2.m[2][j];
	return m0;
}

Matrix3 operator * (const Matrix3& m1, const double e)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=e*m1.m[i][j];
	return m0;
}

Matrix3 operator * (const double e, const Matrix3& m1)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=e*m1.m[i][j];
	return m0;
}

Matrix3 operator / (const Matrix3& m1, const double e)
{
	Matrix3 m0(0.0);
	int i,j;

	for (i=0; i<3; i++) for (j=0; j<3; j++)
		m0.m[i][j]=m1.m[i][j]/e;
	return m0;
}

double& Matrix3::operator () (int row, int col)
{
	return m[row-1][col-1];
}

double det(const Matrix3& mat)
{
	double d;
	d=mat.m[0][0]*(mat.m[1][1]*mat.m[2][2]-mat.m[2][1]*mat.m[1][2]);
	d-=mat.m[0][1]*(mat.m[1][0]*mat.m[2][2]-mat.m[2][0]*mat.m[1][2]);
	d+=mat.m[0][2]*(mat.m[1][0]*mat.m[2][1]-mat.m[2][0]*mat.m[1][1]);
	return d;
}

Matrix3 inverse(const Matrix3& mat)
{
	double d;
	Matrix3 inv;

	d=det(mat);
	inv.m[0][0]=(mat.m[1][1]*mat.m[2][2]-mat.m[2][1]*mat.m[1][2])/d;
	inv.m[0][1]=-(mat.m[0][1]*mat.m[2][2]-mat.m[2][1]*mat.m[0][2])/d;
	inv.m[0][2]=(mat.m[0][1]*mat.m[1][2]-mat.m[1][1]*mat.m[0][2])/d;
	inv.m[1][0]=-(mat.m[1][0]*mat.m[2][2]-mat.m[2][0]*mat.m[1][2])/d;
	inv.m[1][1]=(mat.m[0][0]*mat.m[2][2]-mat.m[2][0]*mat.m[0][2])/d;
	inv.m[1][2]=-(mat.m[0][0]*mat.m[1][2]-mat.m[1][0]*mat.m[0][2])/d;
	inv.m[2][0]=(mat.m[1][0]*mat.m[2][1]-mat.m[2][0]*mat.m[1][1])/d;
	inv.m[2][1]=-(mat.m[0][0]*mat.m[2][1]-mat.m[2][0]*mat.m[0][1])/d;
	inv.m[2][2]=(mat.m[0][0]*mat.m[1][1]-mat.m[1][0]*mat.m[0][1])/d;
	return inv;
}

Matrix3 transpose(const Matrix3& mat)
{
	Matrix3 tr(0.0);
	tr.m[0][0]=mat.m[0][0]; tr.m[1][1]=mat.m[1][1]; tr.m[2][2]=mat.m[2][2];
	tr.m[0][1]=mat.m[1][0]; tr.m[0][2]=mat.m[2][0];
	tr.m[1][0]=mat.m[0][1]; tr.m[1][2]=mat.m[2][1];
	tr.m[2][0]=mat.m[0][2]; tr.m[2][1]=mat.m[1][2];
	return tr;
}

bool Jacobi(const Matrix3& mat, Matrix3& d, Matrix3& v, int nstep)
{
	int i,p,q;  // indices
	int iter=0;
	double offsum;  // sum of off-diagonals
	double b[3],z[3]; // used for the summation of t*a[p][q]
	double tresh;	// absolute threshold for the off-diagonals == 0
	double rtrs;	// threshold relative to the diagonals
	double t,th,c,s,tau; // t, theta, cos, sin, tau of the Jacobi method
	double x,y; // working variables

	Matrix3 a(mat); // working matrix
	v=Matrix3(1.0); // eigenvectors
	d=Matrix3(0.0); // eigenvalues
	b[0]=d.m[0][0]=a.m[0][0];
	b[1]=d.m[1][1]=a.m[1][1];
	b[2]=d.m[2][2]=a.m[2][2];

	z[0]=z[1]=z[2]=0.0;

	while (iter < nstep)
	{
	offsum=fabs(a.m[0][1])+fabs(a.m[0][2])+fabs(a.m[1][2]); // symmetric!

	if (offsum < DBL_EPSILON) return true;
	
	if (iter < 4) tresh=offsum/45.0; else tresh=0.0;

	for (p=0; p<2; p++)
	{
		for (q=p+1; q<3; q++)
		{
			rtrs=100.0*fabs(a.m[p][q]);		
			if ((iter > 4) 
				&& (rtrs < DBL_EPSILON*fabs(d.m[p][p])) 
				&& (rtrs < DBL_EPSILON*fabs(d.m[q][q])))
				a.m[p][q]=0.0;
			else if (fabs(a.m[p][q]) > tresh)
			{
				x=d.m[q][q]-d.m[p][p];
				if (rtrs < DBL_EPSILON*fabs(x))
					t=a.m[p][q]/x;
				else
				{
					th=0.5*x/(a.m[p][q]);
					t=1.0/(fabs(th)+sqrt(1.0+th*th));
					if (th < 0.0) t=-t;
				}
				c=1.0/sqrt(1.0+t*t);
				s=t*c;
				tau=s/(1.0+c);
				x=t*a.m[p][q];
				z[p]-=x; z[q]+=x;
				d.m[p][p]-=x; d.m[q][q]+=x;
				a.m[p][q]=0.0;
				for (i=0; i<3; i++)
				{
					if (i<p)
					{
					x=a.m[i][p]; y=a.m[i][q];
					a.m[i][p]=x-s*(y+tau*x);
					a.m[i][q]=y+s*(x-y*tau);
					} else if ((i>p) && (i<q))
					{
					x=a.m[p][i]; y=a.m[i][q];
					a.m[p][i]=x-s*(y+tau*x);
					a.m[i][q]=y+s*(x-y*tau);
					} else if (i>q)
					{
					x=a.m[p][i]; y=a.m[q][i];
					a.m[p][i]=x-s*(y+tau*x);
					a.m[q][i]=y+s*(x-y*tau);
					}
				}
				for (i=0; i<3; i++)
				{
					x=v.m[i][p]; y=v.m[i][q];
					v.m[i][p]=x-s*(y+tau*x);
					v.m[i][q]=y+s*(x-y*tau);
				}
			}
		}	
	}
	for (i=0; i<3; i++)
	{
		b[i]+=z[i]; d.m[i][i]=b[i]; z[i]=0.0;
	}
	iter++;
	}
	return false;
}

Vector3::Vector3(double v1, double v2, double v3)
{
	v[0]=v1; v[1]=v2; v[2]=v3;
}

Vector3::Vector3(double v1)
{
	v[0]=v1; v[1]=v1; v[2]=v1;
}

Vector3::Vector3()
{
	v[0]=0.0; v[1]=0.0; v[2]=0.0;
}

Vector3::~Vector3()
{

}

std::ostream& operator << (std::ostream& s, Vector3 vector)
{
	s << "#VECTOR: ";
	s << std::setw(8) << std::setprecision(7) << vector.v[0] << " ";
	s << std::setw(8) << std::setprecision(7) << vector.v[1] << " ";
	s << std::setw(8) << std::setprecision(7) << vector.v[2] << std::endl;
	return s;
}

Vector3 operator + (const Vector3& v1, const Vector3& v2)
{
	return Vector3(v1.v[0]+v2.v[0],v1.v[1]+v2.v[1],v1.v[2]+v2.v[2]);
}

Vector3 operator - (const Vector3& v1, const Vector3& v2)
{
	return Vector3(v1.v[0]-v2.v[0],v1.v[1]-v2.v[1],v1.v[2]-v2.v[2]);
}

Vector3 operator * (const Vector3& v1, const double e)
{
	return(Vector3(v1.v[0]*e,v1.v[1]*e,v1.v[2]*e));
}

Vector3 operator * (const double e, const Vector3& v1)
{
	return(Vector3(v1.v[0]*e,v1.v[1]*e,v1.v[2]*e));
}

Vector3 operator / (const Vector3& v1, const double e)
{
	return(Vector3(v1.v[0]/e,v1.v[1]/e,v1.v[2]/e));
}

Vector3 operator * (Matrix3 m1, const Vector3& v1)
{
	Vector3 v0(0.0);
	double x,y,z;
	x=v1.v[0]; y=v1.v[1]; z=v1.v[2];

	v0.v[0]=m1(1,1)*x+m1(1,2)*y+m1(1,3)*z;
	v0.v[1]=m1(2,1)*x+m1(2,2)*y+m1(2,3)*z;
	v0.v[2]=m1(3,1)*x+m1(3,2)*y+m1(3,3)*z;

	return v0;
}

double& Vector3::operator () (int i)
{
	return v[i-1];
}