matrix4d.cpp

3D reconstruction, medical image processing from colons, using intel image processing for based clas

// Matrix4D.cpp : implementation of the CSegRender class
//
#include "stdafx.h"
#include "Matrix4D.h"
#include <math.h>

static const int X_AXIS = 0;
static const int Y_AXIS = 1;
static const int Z_AXIS = 2;
static const double M_PI=3.141592653589793238462643383279502884197169399375105820975;

#define M2DET(x00, x01, x10, x11) \
  ((x00)*(x11) - (x01)*(x10))


#define M3DET(x00, x01, x02, x10, x11, x12, x20, x21, x22) \
  (  (x00) * M2DET((x11), (x12), (x21), (x22)) \
   - (x01) * M2DET((x10), (x12), (x20), (x22)) \
   + (x02) * M2DET((x10), (x11), (x20), (x21))) 


/*#define m4cof(m, r, c, r0, r1, r2, c0, c1, c2) \
  (((((r) + (c)) % 2)? -1.0 : 1.0) \
   * M3DET(m[r0][c0], m[r0][c1], m[r0][c2], \
	   m[r1][c0], m[r1][c1], m[r1][c2], \
	   m[r2][c0], m[r2][c1], m[r2][c2]))*/

RxMatrix4D::RxMatrix4D()
{
	for(int i=0; i<4 ; i++)
		for(int j=0; j<4 ; j++)
			m[i][j] = 0;

	for(i=0; i<4 ; i++)
		m[i][i] = 1;

}

RxMatrix4D::RxMatrix4D(const RxMatrix4D& mat)
{
	for(int i=0; i<4 ; i++)
		for(int j=0; j<4 ; j++)
			m[i][j] = mat.m[i][j];
}

RxMatrix4D::RxMatrix4D(double m00,double m01,double m02,double m03,
					 double m10,double m11,double m12,double m13,
					 double m20,double m21,double m22,double m23, 
					 double m30,double m31,double m32,double m33)
{
	m[0][0] = m00; m[0][1]=m01; m[0][2]=m02;  m[0][3]=m03;
	m[1][0] = m10; m[1][1]=m11; m[1][2]=m12; m[1][3]=m13;
	m[2][0] = m20; m[2][1]=m21; m[2][2]=m22; m[2][3]=m23;
	m[3][0] = m30; m[3][1]=m31; m[3][2]=m32; m[3][3]=m33;
}

RxMatrix4D::RxMatrix4D(const double mat[4][4])
{
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			m[i][j] = mat[i][j];
}

RxMatrix4D::RxMatrix4D(const double mat[16])
{
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			m[i][j] = mat[i+4*j];
}

RxMatrix4D::~RxMatrix4D(void)
{
}

double RxMatrix4D::m4cof(int r, int c, int r0, int r1, int r2, int c0, int c1, int c2)
{
	return (((r + c) % 2)? -1.0 : 1.0) * M3DET(m[r0][c0], m[r0][c1], m[r0][c2], m[r1][c0], m[r1][c1], m[r1][c2], m[r2][c0], m[r2][c1], m[r2][c2]);
	//return (((r + c) % 2)? -1.0 : 1.0) * M3DET(m[r0][c0], m[r1][c0], m[r2][c0], m[r0][c1], m[r1][c1], m[r2][c1], m[r0][c2], m[r1][c2], m[r2][c2]);
}

double RxMatrix4D::m4det( )
{
	double d;

	d =	(m[0][0] * m4cof(0,0, 1,2,3, 1,2,3) +
		 m[0][1] * m4cof(0,1, 1,2,3, 0,2,3) +
		 m[0][2] * m4cof(0,2, 1,2,3, 0,1,3) +
         m[0][3] * m4cof(0,3, 1,2,3, 0,1,2));

	return d;
}


RxMatrix4D RxMatrix4D::Inverse( )
{
	RxMatrix4D resM;

	double d = m4det( );
	
	resM.m[0][0] = m4cof(0,0, 1,2,3, 1,2,3) / d;
	resM.m[1][0] = m4cof(0,1, 1,2,3, 0,2,3) / d;
	resM.m[2][0] = m4cof(0,2, 1,2,3, 0,1,3) / d;
	resM.m[3][0] = m4cof(0,3, 1,2,3, 0,1,2) / d;
	
	resM.m[0][1] = m4cof(1,0, 0,2,3, 1,2,3) / d;
	resM.m[1][1] = m4cof(1,1, 0,2,3, 0,2,3) / d;
	resM.m[2][1] = m4cof(1,2, 0,2,3, 0,1,3) / d;
	resM.m[3][1] = m4cof(1,3, 0,2,3, 0,1,2) / d;
	
	resM.m[0][2] = m4cof(2,0, 0,1,3, 1,2,3) / d;
	resM.m[1][2] = m4cof(2,1, 0,1,3, 0,2,3) / d;
	resM.m[2][2] = m4cof(2,2, 0,1,3, 0,1,3) / d;
	resM.m[3][2] = m4cof(2,3, 0,1,3, 0,1,2) / d;

	resM.m[0][3] = m4cof(3,0, 0,1,2, 1,2,3) / d;
	resM.m[1][3] = m4cof(3,1, 0,1,2, 0,2,3) / d;
	resM.m[2][3] = m4cof(3,2, 0,1,2, 0,1,3) / d;
	resM.m[3][3] = m4cof(3,3, 0,1,2, 0,1,2) / d;

	//resM.m[0][3] = 0;
	//resM.m[1][3] = 0;
	//resM.m[2][3] = 0;
	//resM.m[3][3] = 1;
	

	/*resM.m[0][0] = m4cof(0,0, 1,2,3, 1,2,3) / d;
	resM.m[1][0] = m4cof(0,1, 1,2,3, 0,2,3) / d;
	resM.m[2][0] = m4cof(0,2, 1,2,3, 0,1,3) / d;
	
	resM.m[0][3] = m4cof(0,3, 1,2,3, 0,1,2) / d;
	
	resM.m[0][1] = m4cof(1,0, 0,2,3, 1,2,3) / d;
	resM.m[1][1] = m4cof(1,1, 0,2,3, 0,2,3) / d;
	resM.m[2][1] = m4cof(1,2, 0,2,3, 0,1,3) / d;
	
	resM.m[1][3] = m4cof(1,3, 0,2,3, 0,1,2) / d;
	
	resM.m[0][2] = m4cof(2,0, 0,1,3, 1,2,3) / d;
	resM.m[1][2] = m4cof(2,1, 0,1,3, 0,2,3) / d;
	resM.m[2][2] = m4cof(2,2, 0,1,3, 0,1,3) / d;
	
	resM.m[2][3] = m4cof(2,3, 0,1,3, 0,1,2) / d;

	resM.m[3][0] = 0;
	resM.m[3][1] = 0;
	resM.m[3][2] = 0;
	
	resM.m[3][3] = 1;*/
	

	

	return resM;
}

double RxMatrix4D::m3cof(int r, int c, int r0, int r1, int c0, int c1) 
{
  return ( ((r + c) % 2)? -1.0 : 1.0) * M2DET(m[r0][c0], m[r0][c1], m[r1][c0], m[r1][c1]);
}

double RxMatrix4D::m3det( )
{
	return (M3DET(m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1], m[2][2]));
}

RxMatrix4D RxMatrix4D::Inverse3D()
{
	RxMatrix4D resM;
	
	double d = m3det( );

	resM.m[0][0] = m3cof(0,0, 1,2, 1,2) / d;
	resM.m[1][0] = m3cof(0,1, 1,2, 0,2) / d;
	resM.m[2][0] = m3cof(0,2, 1,2, 0,1) / d;
	
	resM.m[0][1] = m3cof(1,0, 0,2, 1,2) / d;
	resM.m[1][1] = m3cof(1,1, 0,2, 0,2) / d;
	resM.m[2][1] = m3cof(1,2, 0,2, 0,1) / d;
	
	resM.m[0][2] = m3cof(2,0, 0,1, 1,2) / d;
	resM.m[1][2] = m3cof(2,1, 0,1, 0,2) / d;
	resM.m[2][2] = m3cof(2,2, 0,1, 0,1) / d;

	return resM;
}

RxMatrix4D RxMatrix4D::operator=(const RxMatrix4D& mat)
{
	for(int i=0; i<4 ; i++)
		for(int j=0; j<4 ; j++)
			m[i][j] = mat.m[i][j];

	return (*this);
}

RxMatrix4D RxMatrix4D::operator*(const RxMatrix4D mat)
{
	RxMatrix4D resM(0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0);

	for(int i=0; i< 4; i++)
		for(int j=0; j< 4; j++) 
			for(int k=0; k< 4;k++) 
				resM.m[i][j] += m[i][k]*mat.m[k][j];
	
	return resM;
}

BOOL RxMatrix4D::operator==(const RxMatrix4D& mat)
{
	int iRetval = memcmp(m, mat.m, sizeof(m));
	return (iRetval == 0);
}

void RxMatrix4D::SetMatrix4D(double m00,double m01,double m02,double m03,
							double m10,double m11,double m12,double m13,
							double m20,double m21,double m22,double m23, 
							double m30,double m31,double m32,double m33)
{
	m[0][0] = m00; m[0][1]=m01; m[0][2]=m02; m[0][3]=m03;
	m[1][0] = m10; m[1][1]=m11; m[1][2]=m12; m[1][3]=m13;
	m[2][0] = m20; m[2][1]=m21; m[2][2]=m22; m[2][3]=m23;
	m[3][0] = m30; m[3][1]=m31; m[3][2]=m32; m[3][3]=m33;
}

void RxMatrix4D::LoadIdentity(void)
{
	for(int i=0; i<4 ; i++)
		for(int j=0; j<4 ; j++)
			m[i][j] = 0;

	for(i=0; i<4 ; i++)
		m[i][i] = 1;
}

void RxMatrix4D::Scale(double sx, double sy, double sz)
{
	RxMatrix4D tempM(sx,0,0,0, 0,sy,0,0, 0,0,sz,0, 0,0,0,1);
	*this = tempM*(*this);
}

void RxMatrix4D::Translate(double tx, double ty, double tz)
{
	RxMatrix4D tempM(1,0,0,tx, 0,1,0,ty, 0,0,1,tz, 0,0,0,1);
	*this = tempM*(*this);
}

// angle is degree
void RxMatrix4D::Rotate(int axis, double degree)
{
	RxMatrix4D tempM;
	double angle = (degree/180.) * M_PI;

	switch(axis){
	case X_AXIS:
		tempM.m[1][1] = cos(angle);  tempM.m[2][1] = sin(angle);
		tempM.m[1][2] = -sin(angle); tempM.m[2][2] = cos(angle);
		break;

	case Y_AXIS:
		tempM.m[0][0] = cos(angle); tempM.m[2][0] = -sin(angle);
		tempM.m[0][2] = sin(angle); tempM.m[2][2] = cos(angle);
		break;

	case Z_AXIS:
		tempM.m[0][0] = cos(angle); tempM.m[1][0] = sin(angle);
		tempM.m[0][1] = -sin(angle);tempM.m[1][1] = cos(angle);
		break;
	}
	
	*this = tempM*(*this);
}

double RxMatrix4D::CalcuSx()
{
	return (m[1][1]*m[0][2] - m[0][1]*m[1][2])/(m[0][0]*m[1][1]-m[0][1]*m[1][0]);
}

double RxMatrix4D::CalcuSy()
{
	return (m[0][0]*m[1][2] - m[1][0]*m[0][2])/(m[0][0]*m[1][1] - m[0][1]*m[1][0]);
}

RxMatrix4D RxMatrix4D::Warp2D(double ti, double tj)
{
	RxMatrix4D resM;

	resM.SetMatrix4D(m[0][0], m[0][1], m[0][3]-ti*m[0][0]-tj*m[0][1], 0,
					 m[1][0], m[1][1], m[1][3]-ti*m[1][0]-tj*m[1][1], 0,
					  0,0,1,0,
					 0,0,0,1);

	return resM;
}

double RxMatrix4D::ViewVectX()
{
	return m[0][1] * m[1][2] - m[1][1] * m[0][2];
}

double RxMatrix4D::ViewVectY()
{
	return m[1][0] * m[0][2] - m[0][0] * m[1][2];
}

double RxMatrix4D::ViewVectZ()
{
	return m[0][0] * m[1][1] - m[1][0] * m[0][1];
}

void RxMatrix4D::TransformVect3D(double x, double y,double z, double *px, double *py, double *pz)
{
	*px = x * m[0][0] + y * m[0][1] + z * m[0][2];
	*py = x * m[1][0] + y * m[1][1] + z * m[1][2];
	*pz = x * m[2][0] + y * m[2][1] + z * m[2][2];
}

RxVect4D RxMatrix4D::operator*(const RxVect4D vect)
{
	RxVect4D resV(0,0,0,0);

	for(int i=0; i< 4; i++)
		for(int j=0; j< 4; j++) 
			resV.m[i] += m[i][j]*vect[j];
	
	return resV;
}

int RxMatrix4D::GetPrincipalAxis(double fRatio)
{
	double v_x, v_y, v_z;

	v_x = m[0][1] * m[1][2] - m[1][1] * m[0][2];
	v_y = m[1][0] * m[0][2] - m[0][0] * m[1][2];
	v_z = m[0][0] * m[1][1] - m[1][0] * m[0][1];
	
	v_x = fabs(v_x);
	v_y = fabs(v_y);
	v_z = fabs(v_z)*fRatio;
	
	if ( v_z >= v_y ) {
		if ( v_z >= v_x ) return Z_AXIS;
		else return X_AXIS;
	} else {
		if ( v_y >= v_x ) return Y_AXIS;
		else return X_AXIS;
	}
}

RxMatrix4D RxMatrix4D::Transpose()
{
	RxMatrix4D resM;
	for(int i=0; i<4; i++) {
		resM.m[0][i] = m[i][0];
		resM.m[1][i] = m[i][1];
		resM.m[2][i] = m[i][2];
		resM.m[3][i] = m[i][3];
	}
	return resM;
}

RxMatrix4D RxMatrix4D::GetRotateSubMatrix()
{
	RxMatrix4D resM;

	for(int i=0; i<3; i++) {
		resM.m[0][i] = m[0][i];
		resM.m[1][i] = m[1][i];
		resM.m[2][i] = m[2][i];
	}

	return resM;
}

void RxMatrix4D::SetTranslateSubMatrix(RxMatrix4D mxTranslateSub)
{
	RxMatrix4D resM;

	for(int i=0; i<3; i++) 
		m[3][i] = mxTranslateSub.m[3][i];
}