function G(p,P)
global A l;
p1=[36589.41 ;25273.32 ; 2195.17];p2=[37631.08; 31324.51 ;728.69];
p3=[39100.97; 24934.98;2386.50];p4=[40426.54 ; 30319.81 ; 757.31];
arg1=0;arg2=0;arg3=0;Px=((p1+p2+p3+p4)/4);X=Px(1 ,1);Y=Px(1 ,1);f=153.24*0.001;m=50000;h=m*f;Z=h;
a1=cos(arg1)*cos(arg3)-sin(arg2)*sin(arg2)*sin(arg3);
a2=-cos(arg1)*sin(arg3)-sin(arg1)*sin(arg2)*cos(arg3);
a3=-sin(arg1)*cos(arg2);
b1=cos(arg2)*sin(arg3);
b2=cos(arg2)*cos(arg3);
b3=-sin(arg2);
c1=sin(arg1)*cos(arg3)+cos(arg1)*sin(arg2)*sin(arg3);
c2=-sin(arg1)*sin(arg3)+cos(arg1)*sin(arg2)*cos(arg3);
c3=cos(arg1)*cos(arg2);
x=P(1,1);y=P(2,1);
a11=-f/h;a12=0;a13=-x/h;a14=-f*(1+(x^2)/(f^2));a15=-x*y/f;a16=y;
a21=0;a22=-f/h;a23=-y/h;a24=-x*y/f;a25=-f*(1+(y.^2)/(f.^2));a26=-x;
A=[a11 a12 a13 a14 a15 a16 ;a21 a22 a23 a24 a25 a26];
x3=-f*(([a1 b1 c1]*(p -[X ;Y; Z]))/([a3 b3 c3]*(p-[X ;Y; Z])));
y3=-f*(([a2 b2 c2]*(p -[X ;Y; Z]))/([a3 b3 c3]*(p-[X ;Y; Z])));
l=[P(1,1)-x3,P(2,1)-y3];
clear a1 a2 a3 b1 b2 b3 c1 c2 c3;
clear a13 a14 a15 a16 a23 a24 a25 a26;
评论0