47 namespace TRT_DetElementsRoadUtils_xk {
58 double Ax[3] = {T(0,0),T(1,0),T(2,0)};
59 double Ay[3] = {T(0,1),T(1,1),T(2,1)};
60 double x[4],
y[4],
z[4];
64 P[ 0] = sqrt(x0*x0+y0*y0);
65 P[ 1] =
z0;
if(fabs(P[1])<.00004) P[1]=0.;
67 P[ 3] = A[0]*x0+A[1]*y0+A[2]*
z0;
68 if(P[3]<0.) {P[3]*=-1.; A[0]*=-1.; A[1]*=-1.; A[2]*=-1.;}
69 double B = A[0]*A[0]+A[1]*A[1];
70 if(B>.01) P[4]=atan2(A[1],A[0]);
else P[4]=P[2];
72 sincos(P[4],&P[5],&P[6]);
74 if(fabs(P[5])<.00004) P[5]=0.;
75 if(fabs(P[6])<.00004) P[6]=0.;
76 P[ 7] = sqrt(A[0]*A[0]+A[1]*A[1]);
if(fabs(P[7])<.00004) P[7]=0.;
77 P[ 8] = A[2];
if(fabs(P[8])<.00004) P[8]=0.;
79 double dx =
rb->halflengthX();
80 double dy =
rb->halflengthY();
82 x[ 0] = x0+
dx*Ax[0]+
dy*Ay[0];
83 y[ 0] = y0+
dx*Ax[1]+
dy*Ay[1];
86 x[ 1] = x0+
dx*Ax[0]-
dy*Ay[0];
87 y[ 1] = y0+
dx*Ax[1]-
dy*Ay[1];
90 x[ 2] = x0-
dx*Ax[0]-
dy*Ay[0];
91 y[ 2] = y0-
dx*Ax[1]-
dy*Ay[1];
94 x[ 3] = x0-
dx*Ax[0]+
dy*Ay[0];
95 y[ 3] = y0-
dx*Ax[1]+
dy*Ay[1];
100 Ax[0]=T(0,1); Ax[1]=T(1,1); Ax[2]=T(2,1);
101 Ay[0]=T(0,0); Ay[1]=T(1,0); Ay[2]=T(2,0);
103 P[ 0] = .5*(
db->rMin()+
db->rMax());
105 P[ 2] = atan2(Ay[1],Ay[0]);
106 P[ 3] = A[0]*x0+A[1]*y0+A[2]*
z0;
107 if(P[3]<0.) {P[3]*=-1.; A[0]*=-1.; A[1]*=-1.; A[2]*=-1.;}
110 sincos(P[4],&P[5],&P[6]);
111 P[ 7] = sqrt(A[0]*A[0]+A[1]*A[1]);
if(fabs(P[7])<.00004) P[7]=0.;
112 P[ 8] = A[2];
if(fabs(P[8])<.00004) P[8]=0.;
115 double df =
db->halfPhiSector();
116 double sn1,sn2,cs1,cs2;
118 sincos(P[2]+
df,&sn1,&cs1);
119 sincos(P[2]-
df,&sn2,&cs2);
121 x[ 0] =
db->rMax()*cs1;
122 y[ 0] =
db->rMax()*sn1;
125 x[ 1] =
db->rMin()*cs1;
126 y[ 1] =
db->rMin()*sn1;
129 x[ 2] =
db->rMin()*cs2;
130 y[ 2] =
db->rMin()*sn2;
133 x[ 3] =
db->rMax()*cs2;
134 y[ 3] =
db->rMax()*sn2;
139 double rmin= 10000., rmax=-10000.;
141 double fmin= 10000., fmax=-10000.;
142 for(
int i=0;
i!=4; ++
i) {
159 P[15] = P[4]-P[2];
if(P[15]<-
pi) P[15]+=
pi2;
else if(P[15]>
pi) P[15]-=
pi2;
160 P[16] = A[0]*x0+A[1]*y0;
161 P[17] = atan2(Ay[1],Ay[0]);
162 P[17] = P[17]-P[2];
if(P[17]<-
pi) P[17]+=
pi2;
else if(P[17]>
pi) P[17]-=
pi2;
166 P[18] = y0*P[6]-x0*P[5];
167 P[19] = P[8]*(x0*P[6]+y0*P[5])-P[7]*
z0;
177 for(
int i=0;
i!=4; ++
i) {
180 int k2 =
i+1;
if(k2>3) k2=0;
181 double x1 =
y[k1]*P[6]-
x[k1]*P[5] -P[18];
182 double y1 = P[8]*(
x[k1]*P[6]+
y[k1]*P[5])-P[7]*
z[k1]-P[19];
183 double x2 =
y[k2]*P[6]-
x[k2]*P[5] -P[18];
184 double y2 = P[8]*(
x[k2]*P[6]+
y[k2]*P[5])-P[7]*
z[k2]-P[19];
188 if(
ax<P[20]) {P[20]=
ax; P[21]=
ay;}
189 if(
ax>P[24]) {P[24]=
ax; P[25]=
ay;}
190 if(
ay<P[27]) {P[26]=
ax; P[27]=
ay;}
191 if(
ay>P[23]) {P[22]=
ax; P[23]=
ay;}
193 for(
int i=15;
i!=28; ++
i) {
if(fabs(P[
i])<.0004 ) P[
i]=0.;}
197 P[29] = Ax[1]*P[6]-Ax[0]*P[5];
198 P[30] = P[8]*(Ax[0]*P[6]+Ax[1]*P[5])-P[7]*Ax[2];
199 P[31] = Ay[1]*P[6]-Ay[0]*P[5];
200 P[32] = P[8]*(Ay[0]*P[6]+Ay[1]*P[5])-P[7]*Ay[2];