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) {
160 P[16] =
A[0]*x0+
A[1]*y0;
161 P[17] = atan2(Ay[1],Ay[0]);
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];
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];