matrix3d.cpp

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

// Matrix3D.cpp: implementation of the RxMatrix3D class.
//
//////////////////////////////////////////////////////////////////////
//	
//	Title: Matrix Calculation
//
//////////////////////////////////////////////////////////////////////
//
//	Author: Helen Hong, 3DMed co. LTD
//	138-dong 417-ho Seoul National Univ.
//	San 56-1 Shinlim-dong Kwanak-gu, Seoul, Korea
//	Email. [e-mail]
//
//	Date	: 2002. 9. 6.
//	Update	: 2002. 9. 6.
//
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
//	include
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Matrix3D.h"

#include "Transform3DInfo.h"

#include <cmath>

//////////////////////////////////////////////////////////////////////
//	define
//////////////////////////////////////////////////////////////////////

#define	PI			3.1415926535897931160
#define	PI_2		6.2831853071795862320
#define	pi_180		PI / 180.0
#define	pi_360		PI / 360.0

//////////////////////////////////////////////////////////////////////
//	declaration
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

RxMatrix3D::RxMatrix3D()
{

}

RxMatrix3D::~RxMatrix3D()
{

}

//////////////////////////////////////////////////////////////////////

//********************************************************************
//	make_transmat3D(): make translation matrix in 3D
//********************************************************************
BOOL RxMatrix3D::make_transmat3D(double transmat[4][4], double xtrans, double ytrans, double ztrans)
{
	double	matrix[4][4];

	clear_matrix3D(matrix);
	matrix[0][3] = xtrans;
	matrix[1][3] = ytrans;
	matrix[2][3] = ztrans;
	mult_matrix3D(matrix, transmat, transmat);

	return TRUE;
}

//********************************************************************
//	make_rotmat3D(): make rotation matrix in 3D
//********************************************************************
BOOL RxMatrix3D::make_rotmat3D(double rotmat[4][4], double xrot, double yrot, double zrot)
{
	double	matrix[4][4];

	// generate x rotation matrix
	clear_matrix3D(matrix);
	matrix[1][1] = cos(xrot * pi_180);
	matrix[1][2] = -sin(xrot * pi_180);
	matrix[2][1] = sin(xrot * pi_180);
	matrix[2][2] = cos(xrot * pi_180);
	mult_matrix3D(matrix, rotmat, rotmat);

	// generate y rotation matrix
	clear_matrix3D(matrix);
	matrix[0][0] = cos(yrot * pi_180);
	matrix[0][2] = sin(yrot * pi_180);
	matrix[2][0] = -sin(yrot * pi_180);
	matrix[2][2] = cos(yrot * pi_180);
	mult_matrix3D(matrix, rotmat, rotmat);

	// generate z rotation matrix
	clear_matrix3D(matrix);
	matrix[0][0] = cos(zrot * pi_180);
	matrix[0][1] = -sin(zrot * pi_180);
	matrix[1][0] = sin(zrot * pi_180);
	matrix[1][1] = cos(zrot * pi_180);
	mult_matrix3D(matrix, rotmat, rotmat);

	return TRUE;
}

//********************************************************************
//	make_scalemat3D(): make scalig matrix in 3D
//********************************************************************
BOOL RxMatrix3D::make_scalemat3D(double scalemat[4][4], double xscale, double yscale, double zscale)
{
	double	matrix[4][4];

	clear_matrix3D(matrix);
	matrix[0][0] = xscale;
	matrix[1][1] = yscale;
	matrix[2][2] = zscale;
	mult_matrix3D(matrix, scalemat, scalemat);

	return TRUE;
}

//********************************************************************
//	make_scalemat3D(): make scalig matrix in 3D
//********************************************************************
BOOL RxMatrix3D::make_viewmat3D(double viewmat[4][4], RxTransform3DInfo *Trans)
{
	clear_matrix3D(viewmat);
	make_scalemat3D(viewmat,Trans->xscale,Trans->yscale,Trans->zscale);
	make_rotmat3D(viewmat,Trans->xrot,Trans->yrot,Trans->zrot);
	make_transmat3D(viewmat,Trans->xmove,Trans->ymove,Trans->zmove);

	return TRUE;
}

//********************************************************************
//	clear_matrix3D(): Clear matrix in 3D
//********************************************************************
BOOL RxMatrix3D::clear_matrix3D(double matrix[4][4])
{
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			matrix[i][j] = 0.0;
		}
		matrix[i][i] = 1.0;
	}

	return TRUE;
}

//********************************************************************
//	mult_matrix3D(): Multiply matrix for translation, rotation, scaling
//********************************************************************
BOOL RxMatrix3D::mult_matrix3D(double m1[4][4], double m2[4][4], double mout[4][4])
{
	int		i, j;
	double	result[4][4];

	for (i = 0; i < 4; i++) 
		for (j = 0; j < 4; j++)
			result[i][j] = m1[0][j]*m2[i][0] + m1[1][j]*m2[i][1] +
			               m1[2][j]*m2[i][2] + m1[3][j]*m2[i][3];

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++) 
			mout[i][j] = result[i][j];

	return TRUE;
}

//********************************************************************
//	copy_matrix3D(): Copy m1 matrix to m2 matrix in 3D
//********************************************************************
BOOL RxMatrix3D::copy_matrix3D(double m1[4][4], double m2[4][4])
{
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++) 
			m2[i][j] = m1[i][j];

	return TRUE;
}

//********************************************************************
//	matrix2D_vector3(): Multiply 3x3 matrix and vector 3
//********************************************************************
BOOL RxMatrix3D::matrix2D_vector3(double m[3][3], double in[3], double out[3])
{
	out[0] = m[0][0]*in[0] + m[0][1]*in[1] + m[0][2]*in[2];
	out[1] = m[1][0]*in[0] + m[1][1]*in[1] + m[1][2]*in[2];
	out[2] = m[2][0]*in[0] + m[2][1]*in[1] + m[2][2]*in[2];
	
	return TRUE;
}

//********************************************************************
//	matrix22_vector21(): Multiply 2x2 matrix and 2x1 vector
//********************************************************************
BOOL RxMatrix3D::matrix22_vector21(double m[2][2], double v[2], double out[2])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1];

	return TRUE;
}

//********************************************************************
//	matrix33_vector31(): Multiply 3x3 matrix and 3x1 vector
//********************************************************************
BOOL RxMatrix3D::matrix33_vector31(double m[3][3], double v[3], double out[3])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
	out[2]	= m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];

	return TRUE;
}

//********************************************************************
//	matrix44_vector41(): Multiply 4x4 matrix and 4x1 vector
//********************************************************************
BOOL RxMatrix3D::matrix44_vector41(double m[4][4], double v[4], double out[4])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];
	out[2]	= m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];
	out[3]	= m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];

	return TRUE;
}

//********************************************************************
//	matrix3D_vector3(): Multiply 4x4 matrix and vector 3
//********************************************************************
BOOL RxMatrix3D::matrix3D_vector3(double m[4][4], double in[3], double out[3])
{
	out[0] = m[0][0]*in[0] + m[0][1]*in[1] + m[0][2]*in[2];
	out[1] = m[1][0]*in[0] + m[1][1]*in[1] + m[1][2]*in[2];
	out[2] = m[2][0]*in[0] + m[2][1]*in[1] + m[2][2]*in[2];

	return TRUE;
}

//********************************************************************
//	matrix3D_vector4(): Multiply matrix and vector 4
//********************************************************************
BOOL RxMatrix3D::matrix3D_vector4(double m[4][4], double in[4], double out[4])
{
	out[0] = m[0][0]*in[0] + m[0][1]*in[1] + m[0][2]*in[2] + m[0][3]*in[3];
	out[1] = m[1][0]*in[0] + m[1][1]*in[1] + m[1][2]*in[2] + m[1][3]*in[3];
	out[2] = m[2][0]*in[0] + m[2][1]*in[1] + m[2][2]*in[2] + m[2][3]*in[3];
	out[3] = m[3][0]*in[0] + m[3][1]*in[1] + m[3][2]*in[2] + m[3][3]*in[3];

	return TRUE;
}

//********************************************************************
//	vector3_matrix33(): Multiply 3-vector and 3x3 matrix
//********************************************************************
BOOL RxMatrix3D::vector3_matrix33(double in[3], double m[3][3], double out[3])
{
	out[0] = in[0]*m[0][0]+in[1]*m[1][0]+in[2]*m[2][0];
	out[1] = in[0]*m[0][1]+in[1]*m[1][1]+in[2]*m[2][1];
	out[2] = in[0]*m[0][2]+in[1]*m[1][2]+in[2]*m[2][2];
	
	return TRUE;
}

//********************************************************************
//	vector3_matrix35(): Multiply 3-vector and 3x5 matrix
//********************************************************************
BOOL RxMatrix3D::vector3_matrix35(double in[3], double m[3][5], double out[5])
{
	out[0] = in[0]*m[0][0]+in[1]*m[1][0]+in[2]*m[2][0];
	out[1] = in[0]*m[0][1]+in[1]*m[1][1]+in[2]*m[2][1];
	out[2] = in[0]*m[0][2]+in[1]*m[1][2]+in[2]*m[2][2];
	out[3] = in[0]*m[0][3]+in[1]*m[1][3]+in[2]*m[2][3];
	out[4] = in[0]*m[0][4]+in[1]*m[1][4]+in[2]*m[2][4];

	return TRUE;
}

//********************************************************************
//	matrix33_matrix33(): Multiply 3x3 matrix and 3x3 matrix
//********************************************************************
BOOL RxMatrix3D::matrix33_matrix33(double m1[3][3], double m2[3][3], double out[3][3])
{
	int		i, j;
	double	result[3][3];

	for (i = 0; i < 3; i++) 
		for (j = 0; j < 3; j++) 
			result[i][j] = m1[i][j] - m2[i][j];

	for (i = 0; i < 3; i++) 
		for (j = 0; j < 3; j++) 
			out[i][j] = result[i][j];

	return TRUE;
}

//********************************************************************
//	matrix22_vector21(): Multiply 2x2 matrix and 2x1 vector
//********************************************************************
/*
BOOL RxMatrix3D::matrix22_vector21(double m[2][2], double v[2], double out[2])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1];

	return TRUE;
}

//********************************************************************
//	matrix33_vector31(): Multiply 3x3 matrix and 3x1 vector
//********************************************************************
BOOL RxMatrix3D::matrix33_vector31(double m[3][3], double v[3], double out[3])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
	out[2]	= m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];

	return TRUE;
}

//********************************************************************
//	matrix44_vector41(): Multiply 4x4 matrix and 4x1 vector
//********************************************************************
BOOL RxMatrix3D::matrix44_vector41(double m[4][4], double v[4], double out[4])
{
	out[0]	= m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];
	out[1]	= m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];
	out[2]	= m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];
	out[3]	= m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];

	return TRUE;
}
		
*/

//********************************************************************
//	minus_vector(): vector minus vector
//********************************************************************
BOOL RxMatrix3D::minus_vector(double v1[3], double v2[3], double (*out)[3])
{
	(*out)[0] = v1[0] - v2[0];
	(*out)[1] = v1[1] - v2[1];
	(*out)[2] = v1[2] - v2[2];

	return TRUE;
}

//********************************************************************
//	minus_vector(): vector minus vector
//********************************************************************
BOOL RxMatrix3D::minus_vector(double v1[4], double v2[4], double (*out)[4])
{
	(*out)[0] = v1[0] - v2[0];
	(*out)[1] = v1[1] - v2[1];
	(*out)[2] = v1[2] - v2[2];
	(*out)[3] = v1[3] - v2[3];

	return TRUE;
}

//********************************************************************
//	vector13_matrix33(): Multiply 1x3 vector and 3x3 matrix
//********************************************************************
BOOL RxMatrix3D::vector13_matrix33(double v[3], double m[3][3], double out[3])
{
	out[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];
	out[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];
	out[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];

	return TRUE;
}

//********************************************************************
//	vector12_vector21(): Multiply 1x2 vector and 2x1 vector
//********************************************************************
BOOL RxMatrix3D::vector12_vector21(double v1[2], double v2[2], double *out)