matrix3d.cpp

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

{
	*out = v1[0]*v2[0] + v1[1]*v2[1];

	return TRUE;
}

//********************************************************************
//	vector13_vector31(): Multiply 1x3 vector and 3x1 vector
//********************************************************************
BOOL RxMatrix3D::vector13_vector31(double v1[3], double v2[3], double *result)
{
	*result = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];

	return TRUE;
}

//********************************************************************
//	vector14_vector41(): Multiply 1x4 vector and 4x1 vector
//********************************************************************
BOOL RxMatrix3D::vector14_vector41(double v1[4], double v2[4], double *result)
{
	*result = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] + v1[3]*v2[3];

	return TRUE;
}

//********************************************************************
//	vector3_vector3(): Multiply 3x1 vector and 1x3 vector
//********************************************************************
BOOL RxMatrix3D::vector3_vector3(double v1[3], double v2[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] = v1[i]*v2[j];
	for (i = 0; i < 3; i++) 
		for (j = 0; j < 3; j++)
			out[i][j] = result[i][j];

	return TRUE;
}

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

	return TRUE;
}

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

	return TRUE;
}

//********************************************************************
//	vnorm3(): Normalize vector
//********************************************************************
BOOL RxMatrix3D::vnorm3(double vec[3])
{
	double vabs = sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);
	double z_inv = 1.0 / vabs;

	vec[0]	*= z_inv;
	vec[1]	*= z_inv;
	vec[2]	*= z_inv;

	return TRUE;
}

//********************************************************************
//	invert_mat(): Make inverse 2x2 matrix
//********************************************************************
BOOL RxMatrix3D::invert_mat(double matin[2][2], double matout[2][2])
{
	double determinant=(matin[0][0]*matin[1][1])-(matin[0][1]*matin[1][0]);
	
	if (determinant != 0) {
		matout[0][0] = matin[1][1] * (1.0/determinant);
		matout[0][1] = matin[0][1] * (-1.0/determinant);
		matout[1][0] = matin[1][0] * (-1.0/determinant);
		matout[1][1] = matin[0][0] * (1.0/determinant);
	}
	
	return TRUE;
}

//********************************************************************
//	invert_mat3D(): Make inverse matrix in 3D
//********************************************************************
BOOL RxMatrix3D::invert_mat3D(double matin[4][4], double matout[4][4])
{
	double		d, determinant, mdet[3][3];
	int			i, j, a, b, row, col;

	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			// Initialize variable
			row = 0;
			col = 0;
			for (a = 0; a < i; a++) {
				for (b = 0; b < j; b++) {
					mdet[row][col] = matin[a][b];
					col++;
				}
				col = 0;
				row++;
			} 

			// Initialize variable
			row = 0;
			col = j;
			for (a = 0; a  < i; a++) {
				for (b = j+1; b < 4; b++) {
					mdet[row][col] = matin[a][b];
					col++;
				} 
				col = j;
				row++;
			} 

			// Initialize variable
			row = i;
			col = 0;
			for (a = i+1; a < 4; a++) {
				for (b = 0; b < j; b++) {
					mdet[row][col] = matin[a][b];
					col++;
				} 
				col = 0;
				row++;
			} 

			// Initialize variable
			row = i;
			col = j;
			for (a = i+1; a < 4; a++) {
				for (b = j+1; b < 4; b++) {
					mdet[row][col] = matin[a][b];
					col++;
				} 
				col = j;
				row++;
			} 

			d = determinate3(mdet);
			if ((i+j) % 2 == 1)
				d = -d;
			matout[i][j] = d;
		} 
	} 

	// Transpose matrix 
	make_transpomat3D(matout, matout);
	determinant = determinate4(matin);

	if(determinant != 0.0)
		make_fscalemat(1.0/determinant, matout, matout);

	/*
	if (determinant == 0.0)
		TRACE("\n * ERROR: Cannot invert this matrix\n");
	else
		make_fscalemat(1.0/determinant, matout, matout);
		*/

	return TRUE;
}

//********************************************************************
//	determinate2(): Calculate the determinate of a 2x2 matrix
//********************************************************************
double RxMatrix3D::determinate2(double m[2][2])
{
	double	result;
	
	result = (m[0][0]*m[1][1])-(m[0][1]*m[1][0]);
	
	return result;
}

//********************************************************************
//	determinate3(): Calculate the determinate of a 3x3 matrix
//********************************************************************
double RxMatrix3D::determinate3(double m[3][3])
{
	double result =	m[0][0] * m[1][1] * m[2][2] +
					m[0][1] * m[1][2] * m[2][0] +
					m[0][2] * m[1][0] * m[2][1] - 
					m[0][2] * m[1][1] * m[2][0] -
					m[0][0] * m[1][2] * m[2][1] - 
					m[0][1] * m[1][0] * m[2][2];

	return result;
}

//********************************************************************
//	determinate4(): Calculate the determinate of a 4x4 matrix
//********************************************************************
double RxMatrix3D::determinate4(double matin[4][4])
{
	double	d, determinant, mdet[3][3];
	int		i, a, b, n;

	// Initialize variable
	determinant = 0;

	for (i = 0; i < 4; i++) {
		n = 0;
		for (a = 0; a < i; a++) {
			for (b = 1; b < 4; b++) {
				mdet[n][b-1] = matin[a][b];
			} 
			n++;
		} 
		
		for (a = i+1; a < 4; a++) {
			for (b = 1; b < 4; b++) {
				mdet[n][b-1] = matin[a][b];
			} 
			n++;
		} 

		d = determinate3(mdet);

		if (i % 2 == 0)
			determinant += matin[i][0] * d;
		else
			determinant -= matin[i][0] * d;
	} 

	return determinant;
}

//********************************************************************
//	make_fscalemat(): Make scale matrix with real number 
//********************************************************************
BOOL RxMatrix3D::make_fscalemat(double t, double matin[4][4], double matout[4][4])
{
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			matout[i][j] = t * matin[i][j];
		}
	}

	return TRUE;
}

//********************************************************************
//	make_transpomat3D(): Make transpose matrix
//********************************************************************
BOOL RxMatrix3D::make_transpomat3D(double matin[4][4], double matout[4][4])
{
	double	matrix[4][4];
	int		i, j;

	// transpose
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			matrix[i][j] = matin[j][i];
		} 
	} 

	// copy into the output
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			matout[i][j] = matrix[i][j];
		} 
	} 

	return TRUE;
}

//********************************************************************
//	normal_vector3(): Normalize vector 3
//********************************************************************
BOOL RxMatrix3D::normal_vector3(double vec[3])
{
	double	squvec,				// square value of vector
			absvec;				// absolute value of vector

	squvec = (vec[0]*vec[0])+(vec[1]*vec[1])+(vec[2]*vec[2]);
	absvec = sqrt(squvec);

	if (absvec != 0.0) {
		vec[0] /= absvec;
		vec[1] /= absvec;
		vec[2] /= absvec;
	}

	return TRUE;
}

//********************************************************************
//	textureNormals(): Calculate texture normals
//********************************************************************
BOOL RxMatrix3D::make_textureNormals(double xrot, double yrot, double zrot, double (*normals)[3][3])
{
	double	matrix[4][4];
	double	n0[3], n1[3], n2[3];

	// Create rotation matrix
	clear_matrix3D(matrix);
	make_rotmat3D(matrix, xrot, yrot, zrot);

	// Calculate normals in x-axis
	n0[0] = 1.0;
	n0[1] = 0.0;
	n0[2] = 0.0;
	matrix3D_vector3(matrix, n0, (*normals)[0]);

	// Calculate normals in y-axis
	n1[0] = 0.0;
	n1[1] = 1.0;
	n1[2] = 0.0;
	matrix3D_vector3(matrix, n1, (*normals)[1]);

	// Calculate normals in z-axis
	n2[0] = 0.0;
	n2[1] = 0.0;
	n2[2] = 1.0;
	matrix3D_vector3(matrix, n2, (*normals)[2]);

	return TRUE;
}

//********************************************************************
//	planeNormals(): Calculate plane normals
//********************************************************************
BOOL RxMatrix3D::make_planeNormals(double xrot, double yrot, double zrot, int plane, double (*normals)[3])
{
	double	matrix[4][4];
	double	in[3];

	// Create rotation matrix
	clear_matrix3D(matrix);
	make_rotmat3D(matrix, xrot, yrot, zrot);

	// Calculate normals
	switch (plane) {
	case 0:							// axial plane normals
		in[0] = 0.0;
		in[1] = 0.0;
		in[2] = 1.0;
		matrix3D_vector3(matrix, in, *normals);
		break;
	case 1:							// coronal plane normals
		in[0] = 0.0;
		in[1] = 1.0;
		in[2] = 0.0;
		matrix3D_vector3(matrix, in, *normals);
		break;
	case 2:							// sagittal plane normals
		in[0] = 1.0;
		in[1] = 0.0;
		in[2] = 0.0;
		matrix3D_vector3(matrix, in, *normals);
		break;
	}

	return TRUE;
}