sanweixiangsibianhuan.txt

坐标三位相似变换 坐标三位相似变换 坐标三位相似变换 坐标三位相似变换 坐标三位相似变换

typedef struct  
{
	double dx,dy,dz;
	double fai,omiga,kapa;
	double lanmda;
}SIMILARITYTRANSFORMATION;

typedef struct 
{	
	double x,y,z;
}POINT3D;


POINT3D CLinkStrip::SimilarTrans(SIMILARITYTRANSFORMATION similarcoef,POINT3D XYZ)
{
	double R[9];
	rotation(similarcoef.fai,similarcoef.omiga,similarcoef.kapa,R);
    double temp1[3],temp2[3];
	temp1[0]=XYZ.x;temp1[1]=XYZ.y;temp1[2]=XYZ.z;
	mult(R,temp1,temp2,3,3,1);

	POINT3D returnp;
	returnp.x=similarcoef.dx+temp2[0];
	returnp.y=similarcoef.dy+temp2[1];
	returnp.z=similarcoef.dz+temp2[2];

	return returnp;
}
void CLinkStrip::FindSimilarCoef(POINT3D *cur,POINT3D *before,int num)
{
	//from cur to before
	//////////////////////////////////////////////////////////////////////////
	//////////////////
	//Xp  dx    X
	//Yp= dy +R Y
	//Zp  dz    Z
	/////////////////
	
	//void SimilarityTransformation3D(SIMILARITYTRANSFORMATION simitrans,double X,double Y,double Z,
	//	double Xp,double Yp,double Zp,
	//	double *Fx,double *Fy,double *Fz,double &lx,double &ly,double &lz)
	//////////////////////////////////////////////////////////////////////////

	double Fx[6],Fy[6],Fz[6],lx,ly,lz;
	double aa[36],al[6];
	int i;

	double sigemapre,sigemanow;
	POINT3D XYZP;

	sigemapre=sigemanow=1e10;
	int itm=0;
	do 
	{
		sigemapre=sigemanow;
		memset(aa,0,sizeof(double)*36);
		memset(al,0,sizeof(double)*6);		
		for(i=0;i<num;i++)
		{
			SimilarityTransformation3D(similarcoef,cur[i].x,cur[i].y,cur[i].z,
							before[i].x,before[i].y,before[i].z,
							Fx,Fy,Fz,lx,ly,lz);
			pNormal(Fx,6,lx,aa,al,1.0);
			pNormal(Fy,6,ly,aa,al,1.0);
			pNormal(Fz,6,lz,aa,al,1.0);			
		}
		Gauss(aa,al,6);
		similarcoef.dx+=al[0];similarcoef.dy+=al[1];similarcoef.dz+=al[2];
		similarcoef.fai+=al[3];similarcoef.omiga+=al[4];similarcoef.kapa+=al[5];
		sigemanow=0;
		for(i=0;i<num;i++)
		{
			XYZP=SimilarTrans(similarcoef,cur[i]);
			sigemanow+=(before[i].x-XYZP.x)*(before[i].x-XYZP.x)+
				(before[i].y-XYZP.y)*(before[i].y-XYZP.y)+
				(before[i].z-XYZP.z)*(before[i].z-XYZP.z);

		}
		itm++;
		if(itm>10) break;
		
	}while(sigemanow<sigemapre);
	
}


//////////////////
//Xp  dx           X
//Yp= dy +lamda×R Y
//Zp  dz           Z
/////////////////

void SimilarityTransformation3DEx(SIMILARITYTRANSFORMATION simitrans,double X,double Y,double Z,double Xp,double Yp,double Zp,double *Fx,double *Fy,double *Fz,double &lx,double &ly,double &lz)
{
	double R[9],R2fai[9],R2omiga[9],R2kapa[9],XYZ[3],XYZF[3];
	int i;
	
	rotation(simitrans.fai,simitrans.omiga,simitrans.kapa,R);
	PartialDerivativeAUXrtofai(simitrans.fai,simitrans.omiga,simitrans.kapa,R2fai);
	PartialDerivativeAUXrtoomiga(simitrans.fai,simitrans.omiga,simitrans.kapa,R2omiga);
	PartialDerivativeAUXrtokapa(simitrans.fai,simitrans.omiga,simitrans.kapa,R2kapa);
	
	XYZ[0]=X,XYZ[1]=Y,XYZ[2]=Z;
    //dx
	Fx[0]=1;Fy[0]=0;Fz[0]=0;
    //dy
	Fx[1]=0;Fy[1]=1;Fz[1]=0;
    //dz
	Fx[2]=0;Fy[2]=0;Fz[2]=1;
    //fai
	mult(R2fai,XYZ,XYZF,3,3,1);
	for(i=0;i<3;i++)
		XYZF[i]*=simitrans.lanmda;	
	Fx[3]=XYZF[0];Fy[3]=XYZF[1];Fz[3]=XYZF[2];
	
	//omiga
	mult(R2omiga,XYZ,XYZF,3,3,1);
	for(i=0;i<3;i++)
		XYZF[i]*=simitrans.lanmda;	
	Fx[4]=XYZF[0];Fy[4]=XYZF[1];Fz[4]=XYZF[2];
	//kapa
	mult(R2kapa,XYZ,XYZF,3,3,1);
	for(i=0;i<3;i++)
		XYZF[i]*=simitrans.lanmda;
	Fx[5]=XYZF[0];Fy[5]=XYZF[1];Fz[5]=XYZF[2];
	//lambda
	mult(R,XYZ,XYZF,3,3,1);
	Fx[6]=XYZF[0];Fy[6]=XYZF[1];Fz[6]=XYZF[2];
	//l
    lx=simitrans.dx-Xp;ly=simitrans.dy-Yp;lz=simitrans.dz-Zp;
	mult(R,XYZ,XYZF,3,3,1);
	for(i=0;i<3;i++)
		XYZF[i]*=simitrans.lanmda;
	lx+=XYZF[0];ly+=XYZF[1];lz+=XYZF[2];	
	lx=-lx;ly=-ly;lz=-lz;

}


void rotation(double phi,double omiga,double kappa,double *R)
{
	R[0]=  cos(phi)*cos(kappa)-sin(phi)*sin(omiga)*sin(kappa);
	R[1]= -cos(phi)*sin(kappa)-sin(phi)*sin(omiga)*cos(kappa);
	R[2]= -sin(phi)*cos(omiga);
	R[3]=  cos(omiga)*sin(kappa);
	R[4]=  cos(omiga)*cos(kappa);
	R[5]= -sin(omiga);
	R[6]=  sin(phi)*cos(kappa)+cos(phi)*sin(omiga)*sin(kappa);
	R[7]= -sin(phi)*sin(kappa)+cos(phi)*sin(omiga)*cos(kappa);
	R[8]=  cos(phi)*cos(omiga);
}


void PartialDerivativeAUXrtofai(double fai,double omiga,double kapa,double *R)
{
	R[0]=  -sin(fai)*cos(kapa)-cos(fai)*sin(omiga)*sin(kapa);
	R[1]=  sin(fai)*sin(kapa)-cos(fai)*sin(omiga)*cos(kapa);
	R[2]=  -cos(fai)*cos(omiga);
	R[3]=  0;
	R[4]=  0;
	R[5]=  0;
	R[6]=  cos(fai)*cos(kapa)-sin(fai)*sin(omiga)*sin(kapa);
	R[7]=  -cos(fai)*sin(kapa)-sin(fai)*sin(omiga)*cos(kapa);
	R[8]=  -sin(fai)*cos(omiga);
}
void PartialDerivativeAUXrtoomiga(double fai,double omiga,double kapa,double *R)
{
	R[0]=  -sin(fai)*cos(omiga)*sin(kapa);
	R[1]=  -sin(fai)*cos(omiga)*cos(kapa);
	R[2]=  sin(fai)*sin(omiga);
	R[3]=  -sin(omiga)*sin(kapa);
	R[4]=  -sin(omiga)*cos(kapa);
	R[5]=  -cos(omiga);
	R[6]=  cos(fai)*cos(kapa)*sin(kapa);
	R[7]=  cos(fai)*cos(kapa)*cos(kapa);
	R[8]=  -cos(fai)*sin(omiga);
}
void PartialDerivativeAUXrtokapa(double fai,double omiga,double kapa,double *R)
{
	R[0]=  -cos(fai)*sin(kapa)-sin(fai)*sin(omiga)*cos(kapa);
	R[1]=  -cos(fai)*cos(kapa)+sin(fai)*sin(omiga)*sin(kapa);
	R[2]=  0;
	R[3]=  cos(omiga)*cos(kapa);
	R[4]=  -cos(omiga)*sin(kapa);
	R[5]=  0;
	R[6]=  -sin(fai)*sin(kapa)+cos(fai)*sin(omiga)*cos(kapa);
	R[7]=  -sin(fai)*cos(kapa)-cos(fai)*sin(omiga)*sin(kapa);
	R[8]=  0;
}
void mult(double *m1,double *m2,double *result,int i_1,int j_12,int j_2)
{ 
	
	int i,j,k;
	for(i=0;i<i_1;i++)
		for(j=0;j<j_2;j++){
			result[i*j_2+j]=0.0;
			for(k=0;k<j_12;k++)
				result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];
		}
		return;
}


void pNormal(double *a,int n,double b,double *aa, double *ab,double p)
{
	int  i,j;
	
	for (i=0; i<n; i++) 
	{
		for (j=0; j<n; j++) 
			*aa++ += p* a[i] * a[j];
		
		*ab++ += p* a[i] * b;
	}
}
int Gauss(double *A,double *b,int n)
{
	invers_matrix(A,n);
	double *bb=new double[n];
	mult(A,b,bb,n,n,1);
	memcpy(b,bb,sizeof(double)*n);
	delete []bb,bb=NULL;
	return 1;
}

int invers_matrix(double *m1,int n)
{ 
	int *is,*js;

	int i,j,k,l,u,v;

	double temp,max_v;

	is=(int *)malloc(n*sizeof(int));

	js=(int *)malloc(n*sizeof(int));

	if(is==NULL||js==NULL)
	{
    
		printf("out of memory!\n");
    
		return(0);

	}

for(k=0;k<n;k++){
	max_v=0.0;
	for(i=k;i<n;i++)
		for(j=k;j<n;j++){
			temp=fabs(m1[i*n+j]);
			if(temp>max_v){
				max_v=temp; is[k]=i; js[k]=j;
			}
		}
		if(max_v==0.0){
			free(is); free(js);
			printf("invers is not availble!\n");
			return(0);
		}
		if(is[k]!=k)
			for(j=0;j<n;j++){
				u=k*n+j; v=is[k]*n+j;
				temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
			}
			if(js[k]!=k)
				for(i=0;i<n;i++){
					u=i*n+k; v=i*n+js[k];
					temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
				}
				l=k*n+k;
				m1[l]=1.0/m1[l];
				for(j=0;j<n;j++)
					if(j!=k){
						u=k*n+j;
						m1[u]*=m1[l];
					}
					for(i=0;i<n;i++)
						if(i!=k)
							for(j=0;j<n;j++)
								if(j!=k){
									u=i*n+j;
									m1[u]-=m1[i*n+k]*m1[k*n+j];
								}
								for(i=0;i<n;i++)
									if(i!=k){
										u=i*n+k;
										m1[u]*=-m1[l];
									}
}
for(k=n-1;k>=0;k--){
	if(js[k]!=k)
		for(j=0;j<n;j++){
			u=k*n+j; v=js[k]*n+j;
			temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
		}
		if(is[k]!=k)
			for(i=0;i<n;i++){
				u=i*n+k; v=i*n+is[k];
				temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
			}
}
free(is); free(js);
return(1);
}