project_matrix.c

基于UG的自动装配技术

#include <math.h> 
//矩阵复制
void Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4])
{
	int i,j;

	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			to_mtx[i][j]=from_mtx[i][j];
}
//矩阵初始化
void Project_Matrix_Identity(double mtx[4][4])
{
	int i,j;
	
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			if(i==j)
				mtx[i][j]=1.0;
			else
				mtx[i][j]=0.0;
}
//矩阵相乘
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])
{

	int i,j,k;
	double left[4][4],right[4][4];

	Project_Matrix_Copy(left_mtx,left);
	Project_Matrix_Copy(right_mtx,right);
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
		{
			get_mtx[i][j]=0.0;
			for(k=0;k<4;k++)
				get_mtx[i][j]+=left[i][k]*right[k][j];
		}
}
//转置矩阵
void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])
{
	int i,j;
	for(i=0;i<4;i++)
	{
		for(j=0;j<4;j++)
		{
			transpose_mtx[i][j]=mtx[j][i];
		}
	}
}
//从11元组获取变换矩阵
void Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])
{

	//Project_Matrix_Get_Rotation(rotation,point,angle,pos);
	int i,j;
	for(i=0;i<4;i++)
	for(j=0;j<4;j++)
		if(i==j)
			pos[i][j]=1.0;
		else
			pos[i][j]=0.0;
	pos[3][0]+=translation[0];
	pos[3][1]+=translation[1];
	pos[3][2]+=translation[2];
}
//向量缩放
void Project_Vector_Scale(double a[3],double scale,double b[3])
{
	b[0]=a[0]*scale;
	b[1]=a[1]*scale;
	b[2]=a[2]*scale;
}