00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "G4ToroidalSurface.hh"
00037 #include "G4PhysicalConstants.hh"
00038
00039 G4ToroidalSurface::G4ToroidalSurface()
00040 : MinRadius(0.), MaxRadius(0.), TransMatrix(0), EQN_EPS(1e-9)
00041 {
00042 }
00043
00044 G4ToroidalSurface::G4ToroidalSurface(const G4Vector3D& Location,
00045 const G4Vector3D& Ax,
00046 const G4Vector3D& Dir,
00047 G4double MinRad,
00048 G4double MaxRad)
00049 : EQN_EPS(1e-9)
00050 {
00051 Placement.Init(Dir, Ax, Location);
00052
00053 MinRadius = MinRad;
00054 MaxRadius = MaxRad;
00055 TransMatrix= new G4PointMatrix(4,4);
00056 }
00057
00058
00059 G4ToroidalSurface::~G4ToroidalSurface()
00060 {
00061 delete TransMatrix;
00062 }
00063
00064
00065 void G4ToroidalSurface::CalcBBox()
00066 {
00067
00068
00069 G4Point3D Origin = Placement.GetLocation();
00070
00071 G4Point3D Min(Origin.x()-MaxRadius,
00072 Origin.y()-MaxRadius,
00073 Origin.z()-MaxRadius);
00074 G4Point3D Max(Origin.x()+MaxRadius,
00075 Origin.y()+MaxRadius,
00076 Origin.z()+MaxRadius);
00077
00078 bbox = new G4BoundingBox3D(Min,Max);
00079 }
00080
00081
00082 G4Vector3D G4ToroidalSurface::SurfaceNormal(const G4Point3D&) const
00083 {
00084 return G4Vector3D(0,0,0);
00085 }
00086
00087
00088 G4double G4ToroidalSurface::ClosestDistanceToPoint(const G4Point3D &Pt)
00089 {
00090
00091
00092 G4Point3D Origin = Placement.GetLocation();
00093
00094 G4double Dist = Pt.distance(Origin);
00095
00096 return ((Dist - MaxRadius)*(Dist - MaxRadius));
00097 }
00098
00099
00100 G4int G4ToroidalSurface::Intersect(const G4Ray& Ray)
00101 {
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 G4Point3D Base = Ray.GetStart();
00132 G4Vector3D DCos = Ray.GetDir();
00133 G4int nhits=0;
00134 G4double rhits[4];
00135 G4double hits[4] = {0.,0.,0.,0.};
00136 G4double rho, a0, b0;
00137 G4double f, l, t, g1, q, m1, u;
00138 G4double C[5];
00139
00140
00141
00142
00143
00144
00145
00146
00147 G4double rnorm = MaxRadius - MinRadius;
00148 rho = MinRadius*MinRadius / (rnorm*rnorm);
00149 a0 = 4. * MaxRadius*MaxRadius;
00150 b0 = MaxRadius*MaxRadius - MinRadius*MinRadius;
00151
00152
00153 f = 1. - DCos.y()*DCos.y();
00154 l = 2. * (Base.x()*DCos.x() + Base.z()*DCos.z());
00155 t = Base.x()*Base.x() + Base.z()*Base.z();
00156 g1 = f + rho * DCos.y()*DCos.y();
00157 q = a0 / (g1*g1);
00158 m1 = (l + 2.*rho*DCos.y()*Base.y()) / g1;
00159 u = (t + rho*Base.y()*Base.y() + b0) / g1;
00160
00161
00162
00163 C[4] = 1.0;
00164 C[3] = 2. * m1;
00165 C[2] = m1*m1 + 2.*u - q*f;
00166 C[1] = 2.*m1*u - q*l;
00167 C[0] = u*u - q*t;
00168
00169
00170 nhits = SolveQuartic (C,rhits);
00171
00172
00173 m1 = rhits[0]; u = rhits[1]; rhits[0] = u; rhits[1] = m1;
00174 m1 = rhits[2]; u = rhits[3]; rhits[2] = u; rhits[3] = m1;
00175
00176
00177
00178 if(nhits != 0)
00179 {
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 G4Point3D BoxMin = bbox->GetBoxMin();
00197 G4Point3D BoxMax = bbox->GetBoxMax();
00198 G4Point3D Hit;
00199 G4int c1 = 0;
00200 G4int c2;
00201 G4double tempVec[4];
00202
00203 for(G4int a=0;a<nhits;a++)
00204 {
00205 while ( (c1 < 4) && (hits[c1] <= rhits[a]) )
00206 {
00207 tempVec[c1]=hits[c1];
00208 c1++;
00209 }
00210
00211 for(c2=c1+1;c2<4;c2++)
00212 tempVec[c2]=hits[c2-1];
00213
00214 if(c1<4)
00215 {
00216 tempVec[c1]=rhits[a];
00217
00218 for(c2=0;c2<4;c2++)
00219 hits[c2]=tempVec[c2];
00220 }
00221 }
00222
00223 for(G4int b=0;b<nhits;b++)
00224 {
00225
00226 if(hits[b] >=kCarTolerance*0.5)
00227 {
00228 Hit = Base + (hits[b]*DCos);
00229
00230 if( (Hit.x() > BoxMin.x()) &&
00231 (Hit.x() < BoxMax.x()) &&
00232 (Hit.y() > BoxMin.y()) &&
00233 (Hit.y() < BoxMax.y()) &&
00234 (Hit.z() > BoxMin.z()) &&
00235 (Hit.z() < BoxMax.z()) )
00236 {
00237 closest_hit = Hit;
00238 distance = hits[b]*hits[b];
00239 return 1;
00240 }
00241
00242
00243 }
00244 }
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260 return 1;
00261 }
00262 return 0;
00263 }
00264
00265
00266 G4int G4ToroidalSurface::SolveQuartic(G4double cc[], G4double ss[] )
00267 {
00268
00269
00270 G4double coeffs[ 4 ];
00271 G4double z, u, v, sub;
00272 G4double A, B, C, D;
00273 G4double sq_A, p, q, r;
00274 G4int i, num;
00275
00276
00277
00278 A = cc[ 3 ] / cc[ 4 ];
00279 B = cc[ 2 ] / cc[ 4 ];
00280 C = cc[ 1 ] / cc[ 4 ];
00281 D = cc[ 0 ] / cc[ 4 ];
00282
00283
00284
00285
00286 sq_A = A * A;
00287 p = - 3.0/8 * sq_A + B;
00288 q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
00289 r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
00290
00291 if (IsZero(r))
00292 {
00293
00294
00295 coeffs[ 0 ] = q;
00296 coeffs[ 1 ] = p;
00297 coeffs[ 2 ] = 0;
00298 coeffs[ 3 ] = 1;
00299
00300 num = SolveCubic(coeffs, ss);
00301
00302 ss[ num++ ] = 0;
00303 }
00304 else
00305 {
00306
00307 coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
00308 coeffs[ 1 ] = - r;
00309 coeffs[ 2 ] = - 1.0/2 * p;
00310 coeffs[ 3 ] = 1;
00311
00312 (void) SolveCubic(coeffs, ss);
00313
00314
00315 z = ss[ 0 ];
00316
00317
00318 u = z * z - r;
00319 v = 2 * z - p;
00320
00321 if (IsZero(u))
00322 u = 0;
00323 else if (u > 0)
00324 u = std::sqrt(u);
00325 else
00326 return 0;
00327
00328 if (IsZero(v))
00329 v = 0;
00330 else if (v > 0)
00331 v = std::sqrt(v);
00332 else
00333 return 0;
00334
00335 coeffs[ 0 ] = z - u;
00336 coeffs[ 1 ] = q < 0 ? -v : v;
00337 coeffs[ 2 ] = 1;
00338
00339 num = SolveQuadric(coeffs, ss);
00340
00341 coeffs[ 0 ]= z + u;
00342 coeffs[ 1 ] = q < 0 ? v : -v;
00343 coeffs[ 2 ] = 1;
00344
00345 num += SolveQuadric(coeffs, ss + num);
00346 }
00347
00348
00349
00350 sub = 1.0/4 * A;
00351
00352 for (i = 0; i < num; ++i)
00353 ss[ i ] -= sub;
00354
00355 return num;
00356 }
00357
00358
00359 G4int G4ToroidalSurface::SolveCubic(G4double cc[], G4double ss[] )
00360 {
00361
00362 G4int i, num;
00363 G4double sub;
00364 G4double A, B, C;
00365 G4double sq_A, p, q;
00366 G4double cb_p, D;
00367
00368
00369 A = cc[ 2 ] / cc[ 3 ];
00370 B = cc[ 1 ] / cc[ 3 ];
00371 C = cc[ 0 ] / cc[ 3 ];
00372
00373
00374
00375 sq_A = A * A;
00376 p = 1.0/3 * (- 1.0/3 * sq_A + B);
00377 q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
00378
00379
00380 cb_p = p * p * p;
00381 D = q * q + cb_p;
00382
00383 if (IsZero(D))
00384 {
00385 if (IsZero(q))
00386 {
00387 ss[ 0 ] = 0;
00388 num = 1;
00389 }
00390 else
00391 {
00392 G4double u = std::pow(-q,1./3.);
00393 ss[ 0 ] = 2 * u;
00394 ss[ 1 ] = - u;
00395 num = 2;
00396 }
00397 }
00398 else if (D < 0)
00399 {
00400 G4double phi = 1.0/3 * std::acos(-q / std::sqrt(-cb_p));
00401 G4double t = 2 * std::sqrt(-p);
00402
00403 ss[ 0 ] = t * std::cos(phi);
00404 ss[ 1 ] = - t * std::cos(phi + pi / 3);
00405 ss[ 2 ] = - t * std::cos(phi - pi / 3);
00406 num = 3;
00407 }
00408 else
00409 {
00410 G4double sqrt_D = std::sqrt(D);
00411 G4double u = std::pow(sqrt_D - q,1./3.);
00412 G4double v = - std::pow(sqrt_D + q,1./3.);
00413
00414 ss[ 0 ] = u + v;
00415 num = 1;
00416 }
00417
00418
00419 sub = 1.0/3 * A;
00420
00421 for (i = 0; i < num; ++i)
00422 ss[ i ] -= sub;
00423
00424 return num;
00425 }
00426
00427
00428 G4int G4ToroidalSurface::SolveQuadric(G4double cc[], G4double ss[] )
00429 {
00430
00431 G4double p, q, D;
00432
00433
00434 p = cc[ 1 ] / (2 * cc[ 2 ]);
00435 q = cc[ 0 ] / cc[ 2 ];
00436
00437 D = p * p - q;
00438
00439 if (IsZero(D))
00440 {
00441 ss[ 0 ] = - p;
00442 return 1;
00443 }
00444 else if (D < 0)
00445 {
00446 return 0;
00447 }
00448 else if (D > 0)
00449 {
00450 G4double sqrt_D = std::sqrt(D);
00451
00452 ss[ 0 ] = sqrt_D - p;
00453 ss[ 1 ] = - sqrt_D - p;
00454 return 2;
00455 }
00456
00457 return 0;
00458 }