58 double A[3]={AT.x(),AT.y(),AT.z()};
60 P[ 0] = sqrt(x0*x0+y0*y0);
63 P[ 3] = A[0]*x0+A[1]*y0+A[2]*
z0;
64 if(P[3]<0.) {P[3]*=-1.; A[0]*=-1.; A[1]*=-1.; A[2]*=-1.;}
65 P[ 7] = sqrt(A[0]*A[0]+A[1]*A[1]);
67 if(P[7] >.000001) P[4]=atan2(A[1],A[0]);
else P[4]=P[2];
68 sincos(P[4],&P[5],&P[6]);
71 if( fabs(P[7]) <= .000001) {
74 if(P[8] > 0.) P[8] = 1.;
else P[8] = -1.;
78 double x[4],
y[4],
z[4];
80 x[0] = x0+
AF.x()*Swmax+
AE.x()*Sl;
81 y[0] = y0+
AF.y()*Swmax+
AE.y()*Sl;
82 z[0] =
z0+
AF.z()*Swmax+
AE.z()*Sl;
84 x[1] = x0+
AF.x()*Swmin-
AE.x()*Sl;
85 y[1] = y0+
AF.y()*Swmin-
AE.y()*Sl;
86 z[1] =
z0+
AF.z()*Swmin-
AE.z()*Sl;
88 x[2] = x0-
AF.x()*Swmin-
AE.x()*Sl;
89 y[2] = y0-
AF.y()*Swmin-
AE.y()*Sl;
90 z[2] =
z0-
AF.z()*Swmin-
AE.z()*Sl;
92 x[3] = x0-
AF.x()*Swmax+
AE.x()*Sl;
93 y[3] = y0-
AF.y()*Swmax+
AE.y()*Sl;
94 z[3] =
z0-
AF.z()*Swmax+
AE.z()*Sl;
96 double rmin= 10000., rmax=-10000.;
98 double fmin= 10000., fmax=-10000.;
99 for(
int i=0;
i!=4; ++
i) {
116 P[15] = P[4]-P[2];
if(P[15]<-
pi) P[15]+=
pi2;
else if(P[15]>
pi) P[15]-=
pi2;
117 P[16] = A[0]*x0+A[1]*y0;
118 P[17] = atan2(
AE.y(),
AE.x());
119 P[17] = P[17]-P[2];
if(P[17]<-
pi) P[17]+=
pi2;
else if(P[17]>
pi) P[17]-=
pi2;
123 P[18] = y0*P[6]-x0*P[5];
124 P[19] = P[8]*(x0*P[6]+y0*P[5])-P[7]*
z0;
134 for(
int i=0;
i!=4; ++
i) {
137 int k2 =
i+1;
if(k2>3) k2=0;
138 double x1 =
y[k1]*P[6]-
x[k1]*P[5] -P[18];
139 double y1 = P[8]*(
x[k1]*P[6]+
y[k1]*P[5])-P[7]*
z[k1]-P[19];
140 double x2 =
y[k2]*P[6]-
x[k2]*P[5] -P[18];
141 double y2 = P[8]*(
x[k2]*P[6]+
y[k2]*P[5])-P[7]*
z[k2]-P[19];
145 if(
ax<P[20]) {P[20]=
ax; P[21]=
ay;}
146 if(
ax>P[24]) {P[24]=
ax; P[25]=
ay;}
147 if(
ay<P[27]) {P[26]=
ax; P[27]=
ay;}
148 if(
ay>P[23]) {P[22]=
ax; P[23]=
ay;}
153 P[29] =
AF.y()*P[6]-
AF.x()*P[5];
154 P[30] = P[8]*(
AF.x()*P[6]+
AF.y()*P[5])-P[7]*
AF.z();
155 P[31] =
AE.y()*P[6]-
AE.x()*P[5];
156 P[32] = P[8]*(
AE.x()*P[6]+
AE.y()*P[5])-P[7]*
AE.z();