localization.cpp

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

// Localization.cpp: implementation of the CLocalization class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Localization.h"
#include <math.h>
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_DYNAMIC(CLocalization,CImagePointProcess)
CLocalization::CLocalization()
{

}

CLocalization::~CLocalization()
{

}
#ifdef _DEBUG
void CLocalization::Dump(CDumpContext& dc)const
{
	CImagePointProcess::Dump(dc);
}
void CLocalization::AssertValid()const
{
	CImagePointProcess::AssertValid();
}
#endif

POSITIONDIRECT CLocalization::PositionPrecess(WORLDPOINT A, WORLDPOINT B, WORLDPOINT C,
								CPoint A1, CPoint B1, CPoint C1, CPoint center,int f)
{
	float wa,wb,wc;
	float sa,sb,sc;
	double a1o1,a1o,b1o1,b1o,c1o1,c1o;
	float d1,d2,d3;

	int ya1,xa1,yb1,xb1,yc1,xc1;
	int la,lb,lc;

	d1=(float)(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=(float)(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=(float)(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)));

	ya1=A1.y-center.y;
	xa1=A1.x-center.x;
	yb1=B1.y-center.y;
	xb1=B1.x-center.x;
	yc1=C1.y-center.y;
	xc1=C1.x-center.x;

	la=ya1*ya1+xa1*xa1;
	lb=yb1*yb1+xb1*xb1;
	lc=yc1*yc1+xc1*xc1;


	a1o1=sqrt((double)la);
	b1o1=sqrt((double)lb);
	c1o1=sqrt((double)lc);

	a1o=sqrt((f*f+la));
	b1o=sqrt((f*f+lb));
	c1o=sqrt((f*f+lc));

	if(la>0)
		sa=(float)atan2((double)ya1,(double)xa1);
	else sa=0;
	if(lb>0)
		sb=(float)atan2((double)yb1,(double)xb1);
	else sa=0;
	if(lc>0)
		sc=(float)atan2((double)yc1,(double)xc1);
	else sc=0;

//	wa1=(float)(atan2(a1o1,(double)f)*180.0/PIE);
//	wb1=(float)(atan2(b1o1,(double)f)*180.0/PIE);
//	wc1=(float)(atan2(c1o1,(double)f)*180.0/PIE);
/*	float ra1,rb1,rc1;
	double c1,c2;
	ra1=a1o1/(float)f;
	rb1=b1o1/(float)f;
	rc1=c1o1/(float)f;

	c1=f*sin(70*PIE/180);
	c2=2*f*sin(75*PIE/360);

	if(a1o1<c1)
		wa=asin(ra1)*180.0/PIE;
	else if(a1o1<c2)
		wa=2*asin((ra1/2.0))*180.0/PIE;
	else 
		wa=ra1*180.0/PIE;

	if(b1o1<c1)
		wb=asin(rb1)*180.0/PIE;
	else if(b1o1<c2)
		wb=2*asin((rb1/2.0))*180.0/PIE;
	else 
		wb=rb1*180.0/PIE;

	if(c1o1<c1)
		wc=asin(rc1)*180.0/PIE;
	else if(c1o1<c2)
		wc=2*asin((rc1/2.0))*180.0/PIE;
	else 
		wc=rc1*180.0/PIE;*/

	wa=GetRay(a1o1,(double)f,60,65);
	wb=GetRay(b1o1,(double)f,60,65);
	wc=GetRay(c1o1,(double)f,60,65);

/*	double cc1,cc2,cc3;
	double aa,bb,cc;
	double ssab,ssac,ssbc;
	aa=116/cos(wa);
	bb=116/cos(wb);
	cc=116/cos(wc);

	cc1=(aa*aa+bb*bb-d1*d1)/(2*aa*bb);
	cc2=(aa*aa+cc*cc-d2*d2)/(2*aa*cc);
	cc3=(bb*bb+cc*cc-d3*d3)/(2*bb*cc);
	ssab=acos(cc1)*180.0/PIE;
	ssac=acos(cc2)*180.0/PIE;
	ssbc=acos(cc3)*180.0/PIE;*/

	WORLDPOINT pa,pb,pc;
	POSITIONDIRECT P;

//	ThreePoints(d1,d2,d3,wa,wb,wc,sa,sb,sc,&pa,&pb,&pc);

	P=GetPoint(A,B,C,wa,wb,wc,sa,sb,sc,&pa,&pb,&pc);

/*	pa=TwoPoints(wa,116,sa);
	pb=TwoPoints(wb,116,sb);
	pc=TwoPoints(wc,116,sc);		
	P=GetPosition(A,B,C,pa,pb,pc);*/
	return P;




}

float CLocalization::GetRay(double r, double f, int w1, int w2)
{
	double r1,c1,c2;
	float ww;
	r1=r/f;
	c1=f*sin(w1*PIE/180.0);
	c2=2*f*sin(w2*PIE/360.0);

	if(r<c1)
		ww=(float)asin(r1);
	else if(r<c2)
		ww=(float)(2*asin((r1/2.0)));
	else 
		ww=(float)r1;
/*	double R=f+30;
	c1=9*R/sqrt(4*R*R-3*r*r);
	c2=asin(r/f/c1);
	ww=(float)(c1*c2);*/
	return ww;
}

WORLDPOINT CLocalization::ThreePoints(float d1,float d2,float d3, float w1, float w2,float w3, float s1, float s2,float s3,WORLDPOINT* P1,WORLDPOINT* P2,WORLDPOINT* P3)
{
	WORLDPOINT P;
	double c1,c2,c3,c4,c5,c6,c7,c8;
	double cos1,cos2,cos3;
	double sin1,sin2,sin3;
	double sab,sac,sbc;
	double a,b,c,b1,b2;
	double ss1,ss2,ss3,ss4,ss5;
	double ff1,ff2,ff3,ff4,ff,FF;
	double ffd[3],ssb,ssc;

	int ss=1;

	c1=cos(w1);
	c2=sin(w1);
	c3=cos(w2);
	c4=sin(w2);
	c5=cos(s1);
	c6=sin(s1);
	c7=cos(s2);
	c8=sin(s2);

	cos1=cos(w1)*cos(w2)+sin(w1)*sin(w2)*cos(s1-s2);
	cos2=cos(w1)*cos(w3)+sin(w1)*sin(w3)*cos(s1-s3);
	cos3=cos(w2)*cos(w3)+sin(w2)*sin(w3)*cos(s2-s3);

	sin1=sqrt((1-cos1*cos1));
	sin2=sqrt((1-cos2*cos2));
	sin3=sqrt((1-cos3*cos3));
	
	sab=acos(cos1);
	sac=acos(cos2)*180.0/PIE;
	sbc=acos(cos3);


	for(ss=0;ss<2;ss++)
	{
		a=d2/sin2*sin((ss*PIE/180.0));
		ss1=180-sac-ss;
		c=d2/sin2*sin((ss1*PIE/180.0));
		ss2=asin((a/d1*sin1));

		ss3=asin((c/d3*sin3));

		ff1=d1*sin((ss2+sab))/sin1;
		ff2=d3*sin((ss3+sbc))/sin3;
		ff=ABS(ff1-ff2);
		ss4=PIE-ss2-sab;
		ss5=PIE-ss3-sbc;
		if(ss2>sab) 
		{
			ff3=d1*sin((ss2-sab))/sin1;
			FF=ABS(ff3-ff2);
			if(FF<ff) { ff=FF; ss4=ss2-sab;}
			if(ss3>sbc) 
			{
				ff4=d3*sin((ss3-sbc))/sin3;
				FF=ABS(ff3-ff4);
				if(FF<ff) { ff=FF; ss4=ss2-sab;ss5=ss3-sbc;}
			}
		}
		
		if(ss3>sbc) 
		{
			ff4=d3*sin((ss3-sbc))/sin3;
			FF=ABS(ff1-ff4);
			if(FF<ff) {ff=FF;ss4=PIE-ss2-sab;ss5=ss3-sbc;}
		}

		ffd[ss]=ff;
		ssb=ss4;
		ssc=ss5;
	}
	double ee1;
	for(ss=2;ss<180-sac;ss++)
	{
		a=d2/sin2*sin((ss*PIE/180.0));
		ss1=180-sac-ss;
		c=d2/sin2*sin((ss1*PIE/180.0));
		ee1=a/d1*sin1;
		ss2=asin(ee1);

		ss3=asin((c/d3*sin3));

		ff1=d1*sin((ss2+sab))/sin1;
		ff2=d3*sin((ss3+sbc))/sin3;
		ff=ABS(ff1-ff2);
		ss4=PIE-ss2-sab;
		ss5=PIE-ss3-sbc;
		if(ss2>sab) 
		{
			ff3=d1*sin((ss2-sab))/sin1;
			FF=ABS(ff3-ff2);
			if(FF<ff) { ff=FF; ss4=ss2-sab;}
			if(ss3>sbc) 
			{
				ff4=d3*sin((ss3-sbc))/sin3;
				FF=ABS(ff3-ff4);
				if(FF<ff) { ff=FF; ss4=ss2-sab;ss5=ss3-sbc;}
			}
		}
		if(ss3>sbc) 
		{
			ff4=d3*sin((ss3-sbc))/sin3;
			FF=ABS(ff1-ff4);
			if(FF<ff) {ff=FF;ss4=PIE-ss2-sab;ss5=ss3-sbc;}
		}

		ffd[2]=ff;
		if(ffd[1]<ffd[0]&&ffd[1]<ffd[2])
			break;
		ffd[0]=ffd[1];
		ffd[1]=ffd[2];
		ssb=ss4;
		ssc=ss5;
	}

	ss=ss-1;
	a=d2/sin2*sin((ss*PIE/180.0));
	ss1=180-sac-ss;
	c=d2/sin2*sin((ss1*PIE/180.0));

	b1=d1*sin(ssb)/sin1;
	b2=d3*sin(ssc)/sin3;
	b=(b1+b2)/2.0;

	double e1,e2,e3;

	e1=a*a+b*b-2*a*b*cos1-d1*d1;
	e2=a*a+c*c-2*a*c*cos2-d2*d2;
	e3=b*b+c*c-2*b*c*cos3-d3*d3;
/*	fa[0]=ABS(d1*d1+d2*d2-2*d1*d2*cos3-d3*d3);
	b=sqrt((d1*d1-1+cos1*cos1))+cos1;
	c=sqrt((d2*d2-1+cos2*cos2))+cos2;
	fa[1]=ABS(b*b+c*c-2*b*c*cos3-d3*d3);
	AA=d1/sqrt((1-cos1*cos1));
	BB=d2/sqrt((1-cos2*cos2));
	if(AA>BB) AA=BB;
	for(a=2;a<AA;a++)
	{
		b=sqrt((d1*d1-a*a*(1-cos1*cos1)))+a*cos1;
		c=sqrt((d2*d2-a*a*(1-cos2*cos2)))+a*cos2;

		fa[2]=ABS(b*b+c*c-2*b*c*cos3-d3*d3);		
		if(fa[1]<fa[0] && fa[1]<fa[2])
			break;
		fa[0]=fa[1];
		fa[1]=fa[2];
	}
	a=a-1;
	b=sqrt((d1*d1-a*a*(1-cos1*cos1)))+a*cos1;
	c=sqrt((d2*d2-a*a*(1-cos2*cos2)))+a*cos2;

/*	e1=float(c1*tan(w2));
	e2=(float)(2*c2*e1*cos(s1+s2));
	h=(float)sqrt((e1*e1+c2*c2-e2));

	e3=(float)(c1/c3);
	m=(1+e3*e3-h*h)/(2*e3);
	n=(e3*e3+h*h-1)/(2*e3*h);

	g=m+n*h-e3;
	s=1-m*m;
	t=h*h*(1-n*n);

	e4=g*g*d*d;
	e5=(s-t)*(s-t)+(s+t)*g*g;
	a=(float)sqrt((e4/e5));
	
	e6=d*d-a*a*(1-m*m);
	b=(float)(a*m+sqrt(e6));*/

	P.x=(int)(a*sin(w1)*cos(s1));
	P.y=(int)(a*sin(w1)*sin(s1));
	P.z=(int)(a*cos(w1));
	
	P1->x=(int)(a*sin(w1)*cos(s1));
	P1->y=(int)(a*sin(w1)*sin(s1));
	P1->z=(int)(a*cos(w1));

	P2->x=(int)(b*sin(w2)*cos(s2));
	P2->y=(int)(b*sin(w2)*sin(s2));
	P2->z=(int)(b*cos(w2));

	P3->x=(int)(c*sin(w3)*cos(s3));
	P3->y=(int)(c*sin(w3)*sin(s3));
	P3->z=(int)(c*cos(w3));


//	*P2=P1;
	return P;