localization.cpp

使用大恒采集卡的图像显示

}

WORLDPOINT CLocalization::TwoPoints(float w, int h, float s)
{
	WORLDPOINT P;

	float a;
	a=(float)(h/cos(w));

	P.x=(int)(a*sin(w)*cos(s));
	P.y=(int)(a*sin(w)*sin(s));
	P.z=h;

	return P;
}

POSITIONDIRECT CLocalization::GetPosition(WORLDPOINT A, WORLDPOINT B, WORLDPOINT C, 
										  WORLDPOINT pa, WORLDPOINT pb, WORLDPOINT pc)
{
	int MABX,MABY,MABZ,MACX,MACY,MACZ,MBCX,MBCY,MBCZ;
	int mabx,maby,mabz,macx,macy,macz,mbcx,mbcy,mbcz;

	double cos1,cos2,cos3;
	double sin1,sin2,sin3;
	int ss1,ss2,ss3;
	int xo1,yo1,xo2,yo2,xo3,yo3;
	int xo,yo,zo,so;

	MABX=A.x-B.x;
	MABY=A.y-B.y;
	MABZ=A.z-B.z;

	MACX=A.x-C.x;
	MACY=A.y-C.y;
	MACZ=A.z-C.z;

	MBCX=B.x-C.x;
	MBCY=B.y-C.y;
	MBCZ=B.z-C.z;

	mabx=pa.x-pb.x;
	maby=pa.y-pb.y;
	mabz=pa.z-pb.z;

	macx=pa.x-pc.x;
	macy=pa.y-pc.y;
	macz=pa.z-pc.z;

	mbcx=pb.x-pc.x;
	mbcy=pb.y-pc.y;
	mbcz=pb.z-pc.z;

	cos1=(double)(mabx*MABX+maby*MABY)/(double)(mabx*mabx+maby*maby);
	sin1=(double)(mabx*MABY-maby*MABX)/(double)(mabx*mabx+maby*maby);
	xo1=(int)(A.x-pa.x*cos1+pa.y*sin1);
	yo1=(int)(A.y-pa.x*sin1-pa.y*cos1);
	ss1=(int)(atan2(sin1,cos1)*180.0/PIE);
	if(ss1<0) ss1+=360;

	cos2=(double)(macx*MACX+macy*MACY)/(double)(macx*macx+macy*macy);
	sin2=(double)(macx*MACY-macy*MACX)/(double)(macx*macx+macy*macy);
	xo2=(int)(A.x-pa.x*cos2+pa.y*sin2);
	yo2=(int)(A.y-pa.x*sin2-pa.y*cos2);
	ss2=(int)(atan2(sin2,cos2)*180.0/PIE);
	if(ss2<0) ss2+=360;

	cos3=(double)(mbcx*MBCX+mbcy*MBCY)/(double)(mbcx*mbcx+mbcy*mbcy);
	sin3=(double)(mbcx*MBCY-mbcy*MBCX)/(double)(mbcx*mbcx+mbcy*mbcy);	
	xo3=(int)(B.x-pb.x*cos3+pb.y*sin3);
	yo3=(int)(B.y-pb.x*sin3-pb.y*cos3);
	ss3=(int)(atan2(sin3,cos3)*180.0/PIE);
	if(ss3<0) ss3+=360;


	xo=(xo1+xo2+xo3)/3;
	yo=(yo1+yo2+yo3)/3;
	zo=14;
	so=(ss1+ss2+ss3)/3;
	if(so>180) so-=360;
	
	POSITIONDIRECT P;
	P.iz=P.iy=0;
	P.ix=so;
	P.o.x=xo;
	P.o.y=yo;
	P.o.z=zo;

	return P;


}

POSITIONDIRECT CLocalization::GetPosition2(WORLDPOINT A, WORLDPOINT B, WORLDPOINT C, WORLDPOINT pa, WORLDPOINT pb, WORLDPOINT pc)
{
	int MABX,MABY,MABZ,MACX,MACY,MACZ,MBCX,MBCY,MBCZ;
	int mabx,maby,mabz,macx,macy,macz,mbcx,mbcy,mbcz;
	double a[9],b[9], c[9];
/*	double cos1,cos2,cos3;
	double sin1,sin2,sin3;
	int ss1,ss2,ss3;
	int xo1,yo1,xo2,yo2,xo3,yo3;*/
	int xo,yo,zo,so;

	MABX=A.x-B.x;
	MABY=A.y-B.y;
	MABZ=A.z-B.z;
	a[0]=MABX;a[3]=MABY;a[6]=MABZ;
	MACX=A.x-C.x;
	MACY=A.y-C.y;
	MACZ=A.z-C.z;
	a[1]=MACX;a[4]=MACY;a[7]=MACZ;
	MBCX=B.x-C.x;
	MBCY=B.y-C.y;
	MBCZ=B.z-C.z;
	a[2]=MBCX;a[5]=MBCY;a[8]=MBCZ;
	mabx=pa.x-pb.x;
	maby=pa.y-pb.y;
	mabz=pa.z-pb.z;
	b[0]=mabx;b[3]=maby;b[6]=mabz;
	macx=pa.x-pc.x;
	macy=pa.y-pc.y;
	macz=pa.z-pc.z;
	b[1]=macx;b[4]=macy;b[7]=macz;
	mbcx=pb.x-pc.x;
	mbcy=pb.y-pc.y;
	mbcz=pb.z-pc.z;
	b[2]=macx;b[5]=macy;b[8]=macz;

	rinv(b,3);
	trmul(a,b,c,3,3,3);
	xo=(int)(A.x-(c[0]*pa.x+c[1]*pa.y));
	yo=(int)(A.y-(c[3]*pa.x+c[4]*pa.y));
	zo=(int)(A.z-pa.z);

	so=0;
	POSITIONDIRECT P;
	P.ix=P.iy=0;
	P.iz=so;
	P.o.x=xo;
	P.o.y=yo;
	P.o.z=zo;

	return P;
}

POSITIONDIRECT CLocalization::GetPoint(WORLDPOINT A, WORLDPOINT B, WORLDPOINT C, float wa, float wb, float wc, float sa, float sb, float sc,WORLDPOINT* P1,WORLDPOINT* P2,WORLDPOINT* P3)
{
	int xo,yo,zo;
	double kab1,kab2,kab3,kac1,kac2,kac3,kbc1,kbc2,kbc3;
	double cos1,cos2,cos3;
	double cos2wa,cos2wb,cos2wc,coswab,coswac,coswbc;
	double d1,d2,d3;
	int za,zb,zc;
	double z1,z2,z3,z4,z5,z6,z;
//	double a1,a2,b1,b2,c1,c2;
	double a,b,c;
	double k1,k2,k3,k4,k5,k6;
	double c1,c2,c3;
	WORLDPOINT SS;
	d1=sqrt((A.x-B.x)*(A.x-B.x)+(A.y-B.y)*(A.y-B.y)+(A.z-B.z)*(A.z-B.z));
	d2=sqrt((A.x-C.x)*(A.x-C.x)+(A.y-C.y)*(A.y-C.y)+(A.z-C.z)*(A.z-C.z));
	d3=sqrt((C.x-B.x)*(C.x-B.x)+(C.y-B.y)*(C.y-B.y)+(C.z-B.z)*(C.z-B.z));

	cos1=cos(wa)*cos(wb)+sin(wa)*sin(wb)*cos(sa-sb);
	cos2=cos(wa)*cos(wc)+sin(wa)*sin(wc)*cos(sa-sc);
	cos3=cos(wb)*cos(wc)+sin(wb)*sin(wc)*cos(sb-sc);

	cos2wa=cos(wa)*cos(wa);
	cos2wb=cos(wb)*cos(wb);
	cos2wc=cos(wc)*cos(wc);

	coswab=cos(wa)*cos(wb);
	coswac=cos(wa)*cos(wc);
	coswbc=cos(wb)*cos(wc);
	
	za=A.z;
	zb=B.z;
	zc=C.z;

	kab1=1/cos2wa+1/cos2wb-2*cos1/coswab;
	kab2=za/cos2wa+zb/cos2wb-(za+zb)*cos1/coswab;
	kab3=za*za/cos2wa+zb*zb/cos2wb-2*za*zb*cos1/coswab-d1*d1;
//	z1=(kab2+sqrt((kab2*kab2-kab1*kab3)))/kab1;
	z2=(kab2-sqrt((kab2*kab2-kab1*kab3)))/kab1;
	z1=d1*sqrt((1/kab1));
//	a1=(za-z2)/cos(wa);
//	b1=(zb-z2)/cos(wb);

	kac1=1/cos2wa+1/cos2wc-2*cos2/coswac;
	kac2=za/cos2wa+zc/cos2wc-(za+zc)*cos2/coswac;
	kac3=za*za/cos2wa+zc*zc/cos2wc-2*za*zc*cos2/coswac-d2*d2;
//	z3=(kac2+sqrt((kac2*kac2-kac1*kac3)))/kac1;
	z4=(kac2-sqrt((kac2*kac2-kac1*kac3)))/kac1;
	z3=d2*sqrt((1/kac1));
//	a2=(za-z4)/cos(wa);
//	c1=(zc-z4)/cos(wc);

	kbc1=1/cos2wb+1/cos2wc-2*cos3/coswbc;
	kbc2=zb/cos2wb+zc/cos2wc-(zb+zc)*cos3/coswbc;
	kbc3=zb*zb/cos2wb+zc*zc/cos2wc-2*zb*zc*cos3/coswbc-d3*d3;
//	z5=(kbc2+sqrt((kbc2*kbc2-kbc1*kbc3)))/kbc1;
	z6=(kbc2-sqrt((kbc2*kbc2-kbc1*kbc3)))/kbc1;
	z5=d3*sqrt((1/kbc1));
//	b2=(zb-z6)/cos(wb);
//	c2=(zc-z6)/cos(wc);

	z=(z2+z4+z6)/3;
	a=(za-z)/cos(wa);
	b=(zb-z)/cos(wb);
	c=(zc-z)/cos(wc);


	k1=2*(B.x-A.x);
	k2=2*(B.y-A.y);
	k3=a*a-b*b-(za-z)*(za-z)+(zb-z)*(zb-z)-A.x*A.x-A.y*A.y+B.x*B.x+B.y*B.y;

	k4=2*(C.x-A.x);
	k5=2*(C.y-A.y);
	k6=a*a-c*c-(za-z)*(za-z)+(zc-z)*(zc-z)-A.x*A.x-A.y*A.y+C.x*C.x+C.y*C.y;
	
	c1=k3*k5-k2*k6;
	c2=k1*k5-k2*k4;
	c3=k1*k6-k3*k4;
	xo=(int)(c1/c2);
	yo=(int)(c3/c2);	
	zo=(int)z;
		
	P1->x=(int)(a*sin(wa)*cos(sa));
	P1->y=(int)(a*sin(wa)*sin(sa));
	P1->z=(int)(a*cos(wa));

	P2->x=(int)(b*sin(wb)*cos(sb));
	P2->y=(int)(b*sin(wb)*sin(sb));
	P2->z=(int)(b*cos(wb));

	P3->x=(int)(c*sin(wc)*cos(sc));
	P3->y=(int)(c*sin(wc)*sin(sc));
	P3->z=(int)(c*cos(wc));


	SS=GetDirect(A,B,C,*P1,*P2,*P3,xo,yo,zo);
	POSITIONDIRECT P;
	P.ix=SS.x;
	P.iy=SS.y;
	P.iz=SS.z;
	P.o.x=xo;
	P.o.y=yo;
	P.o.z=zo;

	return P;


}

void CLocalization::trmul(double a[], double b[], double c[], int m, int n, int k)
{
	int i,j,l,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { u=i*k+j; c[u]=0.0;
        for (l=0; l<=n-1; l++)
          c[u]=c[u]+a[i*n+l]*b[l*k+j];
      }
    return;
}

int CLocalization::rinv(double a[], int n)
{
	int *is,*js,i,j,k,l,u,v;
    double d,p;
    is=(int*)malloc(n*sizeof(int));
    js=(int*)malloc(n*sizeof(int));
    for (k=0; k<=n-1; k++)
      { d=0.0;
        for (i=k; i<=n-1; i++)
        for (j=k; j<=n-1; j++)
          { l=i*n+j; p=fabs(a[l]);
            if (p>d) { d=p; is[k]=i; js[k]=j;}
          }
        if (d+1.0==1.0)
          { free(is); free(js); printf("err**not inv\n");
            return(0);
          }
        if (is[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=is[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (js[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+js[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        l=k*n+k;
        a[l]=1.0/a[l];
        for (j=0; j<=n-1; j++)
          if (j!=k)
            { u=k*n+j; a[u]=a[u]*a[l];}
        for (i=0; i<=n-1; i++)
          if (i!=k)
            for (j=0; j<=n-1; j++)
              if (j!=k)
                { u=i*n+j;
                  a[u]=a[u]-a[i*n+k]*a[k*n+j];
                }
        for (i=0; i<=n-1; i++)
          if (i!=k)
            { u=i*n+k; a[u]=-a[u]*a[l];}
      }
    for (k=n-1; k>=0; k--)
      { if (js[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=js[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (is[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+is[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
      }
    free(is); free(js);
    return(1);
}

WORLDPOINT CLocalization::GetDirect(WORLDPOINT A, WORLDPOINT B, WORLDPOINT C, WORLDPOINT pa, WORLDPOINT pb, WORLDPOINT pc, int xo, int yo, int zo)
{
	double a[9],b[9],c[9];
	WORLDPOINT S;
	a[0]=A.x-xo;a[3]=A.y-yo;a[6]=A.z-zo;
	a[1]=B.x-xo;a[4]=B.y-yo;a[7]=B.z-zo;
	a[2]=C.x-xo;a[5]=C.y-yo;a[8]=C.z-zo;

	b[0]=pa.x;b[3]=pa.y;b[6]=pa.z;
	b[1]=pb.x;b[4]=pb.y;b[7]=pb.z;
	b[2]=pc.x;b[5]=pc.y;b[8]=pc.z;

	rinv(b,3);
	trmul(a,b,c,3,3,3);

	Change(c);
	
	S.x=(int)(atan2(c[3],c[0])*180.0/PIE);
//	if(S.x<0) S.x+=360;
	S.y=(int)((PIE/2.0-acos(c[7]))*180.0/PIE);
	S.z=(int)((PIE/2.0-acos(c[6]))*180.0/PIE);
//	S.y=(int)(acos(c[7])*180.0/PIE);
//	S.z=(int)(acos(c[6])*180.0/PIE);
	return S;
}

void CLocalization::Change(double a[9])
{
	double e[3];
	e[0]=sqrt(a[0]*a[0]+a[3]*a[3]+a[6]*a[6]);
	e[1]=sqrt(a[1]*a[1]+a[4]*a[4]+a[7]*a[7]);
	e[2]=sqrt(a[2]*a[2]+a[5]*a[5]+a[8]*a[8]);

	for(int i=0;i<3;i++)
		for(int j=0;j<3;j++)
			a[i+3*j]/=e[i];	
}