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.;
97 double zmin= 10000., zmax=-10000.;
98 double fmin= 10000., fmax=-10000.;
99 for(
int i=0; i!=4; ++i) {
100 double r = sqrt(
x[i]*
x[i]+
y[i]*
y[i]);
101 double f = atan2(
y[i],
x[i])-
P[2];
if(f<-
pi) f+=pi2;
else if(f>
pi) f-=pi2;
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];
142 double d = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
143 double ax =-(y2-y1)*(y1*x2-x1*y2)/d;
144 double ay = (x2-x1)*(y1*x2-x1*y2)/d;
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();