source: trunk/source/geometry/solids/test/X11RayTracer/torus.c

Last change on this file was 1350, checked in by garnier, 14 years ago

update to last version 4.9.4
File size: 30.4 KB
Line 
1//
2// E.Medernach 2000
3//
4
5#define EPSILON 1e-12
6#define INFINITY 1e+12
7#define TORUSPRECISION 0.001 //1.0  // or whatever you want for precision (it is TorusEquation related)
8
9#define NBPOINT 6
10#define ITERATION 8 //20 But 8 is really enough for Newton with a good guess
11#define NOINTERSECTION -1//kInfinity
12
13#define DEBUGTORUS 0
14
15/*
16  Torus implementation with Newton Method and Bounding volume
17 */
18
19
20#define G4double double
21
22
23#include <stdio.h>
24#include <math.h>
25#include "torus.h"
26
27double cos(double x);
28double sin(double x);
29
30
31double sqrt(double x);
32double fabs(double x);
33
34
35inline int CheckAngle (double x,double y,double phi,double deltaphi)
36{
37  /** Note: this is possble to avoid atan by projecing -PI;PI to -inf;inf **/
38 
39                double theta ;
40 
41                theta = atan(x/y);
42                if (y < 0.0) theta += M_PI;
43                if (theta < 0.0) theta += 2*M_PI;
44
45                if ((theta >= phi) && (theta <= (phi + deltaphi))) {
46                                return 1;
47                } else {
48                                return 0;
49                }
50}
51
52inline double IntersectPlanarSection (double x,double y,double dx,double dy,double phi,double deltaphi)
53{
54  /*** Intersect a ray with plan (phi) and (phi + deltaphi) ***/
55  /*** the point is outside phi..phi+deltaphi ***/
56  double Lambda1,Lambda2 ;
57  Lambda1 = -(y - x*tan(phi))/(dy - dx*tan(phi));
58  Lambda2 = -(y - x*tan(phi + deltaphi))/(dy - dx*tan(phi + deltaphi));
59  if (Lambda1 < Lambda2) {
60        return Lambda1;
61  } else {
62        return Lambda2;
63  }
64}
65
66inline  double TorusEquation (x, y, z, R0, R1)
67double x;
68double y;
69double z;
70double R0;
71double R1;
72{
73                /*
74                  An interesting property is that the sign
75                  tell if the point is inside or outside
76                  or if > EPSILON on the surface
77                */
78                double temp;
79
80                temp = ((x*x + y*y + z*z) + R0*R0 - R1*R1) ;
81                temp = temp*temp ;
82                temp = temp - 4*R0*R0*(x*x + y*y) ;
83
84                /*
85                  > 0 Outside
86                  < 0 Inside
87                */
88                return temp ;
89}
90
91
92inline double TorusDerivativeX (x, y, z, R0, R1)
93double x;
94double y;
95double z;
96double R0;
97double R1;
98{
99                return 4*x*(x*x + y*y + z*z +  R0*R0 - R1*R1) - 8*R0*R0*x ;
100}
101
102inline double TorusDerivativeY (x, y, z, R0, R1)
103double x;
104double y;
105double z;
106double R0;
107double R1;
108{
109                return 4*y*(x*x + y*y + z*z +  R0*R0 - R1*R1) - 8*R0*R0*y ;
110}
111
112
113inline double TorusDerivativeZ (x, y, z, R0, R1)
114double x;
115double y;
116double z;
117double R0;
118double R1;
119{
120                return 4*z*(x*x + y*y + z*z +  R0*R0 - R1*R1) ;
121}
122
123
124inline  double ParaboloidEquation (x, y, z, H, L)
125     double x;
126     double y;
127     double z;
128     double H;
129     double L;
130     
131{
132  return z - H*(x*x + y*y)/(L*L) ;
133}
134
135inline  double ParaboloidDerX (x, y, z, H, L)
136     double x;
137     double y;
138     double z;
139     double H;
140     double L;
141     
142{
143  return - 2*H*x/(L*L) ;
144}
145
146inline  double ParaboloidDerY (x, y, z, H, L)
147     double x;
148     double y;
149     double z;
150     double H;
151     double L;
152     
153{
154  return - 2*H*y/(L*L) ;
155}
156
157inline  double ParaboloidDerZ (x, y, z, H, L)
158     double x;
159     double y;
160     double z;
161     double H;
162     double L;
163     
164{
165  return 1 ;
166}
167
168
169inline  double HyperboloidEquation (x, y, z, H, L)
170     double x;
171     double y;
172     double z;
173     double H;
174     double L;
175     
176{
177  return (x*x + y*y) - z*z + H*H - L*L ;
178}
179
180inline  double HyperboloidDerX (x, y, z, H, L)
181     double x;
182     double y;
183     double z;
184     double H;
185     double L;
186     
187{
188  return 2*x ;
189}
190
191inline  double HyperboloidDerY (x, y, z, H, L)
192     double x;
193     double y;
194     double z;
195     double H;
196     double L;
197     
198{
199  return 2*y ;
200}
201
202inline  double HyperboloidDerZ (x, y, z, H, L)
203     double x;
204     double y;
205     double z;
206     double H;
207     double L;
208     
209{
210  return -2*z ;
211}
212
213
214void BVMParaboloidIntersection (G4double x,G4double y,G4double z,
215                                G4double dx,G4double dy,G4double dz,
216                                G4double H, G4double L,
217                                G4double *NewL,int *valid)
218{
219  /* We use the box [-L L]x[-L L]x[0 H] */
220  /* there is only one interval at maximum */
221
222  /* NewL and valid are array of 6 elements */
223
224  if (dz != 0) {
225    /* z = 0 */
226    NewL[0] = -z/dz ;
227    if ((fabs(x + NewL[0]*dx) < L) && (fabs(y + NewL[0]*dy) < L)) {
228      valid[0] = 1;
229    } else {
230      valid[0] = 0;
231    }
232   
233    /* z = H */
234    NewL[1] = -(z-H)/dz ;
235    if ((fabs(x + NewL[1]*dx) < L) && (fabs(y + NewL[1]*dy) < L)) {
236      valid[1] = 1;
237    } else {
238      valid[1] = 0;
239    }
240   
241  } else {
242    NewL[0] = -1.0 ;
243    NewL[1] = -1.0 ;
244    valid[0] = 0;
245    valid[1] = 0;
246  }
247
248  if (dx != 0) {
249    /* x = -L */
250    NewL[2] = -(x+L)/dx ;
251    if ((fabs(z - H/2 +NewL[2]*dz) < H/2) && (fabs(y + NewL[2]*dy) < L)) {
252      valid[2] = 1;
253    } else {
254      valid[2] = 0;
255    }
256   
257    /* z = H */
258    NewL[3] = -(x-L)/dx ;
259    if ((fabs(z - H/2 + NewL[3]*dz) < H/2) && (fabs(y + NewL[3]*dy) < L)) {
260      valid[3] = 1;
261    } else {
262      valid[3] = 0;
263    }
264   
265  } else {
266    NewL[2] = -1.0 ;
267    NewL[3] = -1.0 ;
268    valid[2] = 0;
269    valid[3] = 0;
270  }
271
272  if (dy != 0) {
273    /* y = -L */
274    NewL[4] = -(y+L)/dy ;
275    if ((fabs(z - H/2 +NewL[4]*dz) < H) && (fabs(y + NewL[4]*dy) < L)) {
276      valid[4] = 1;
277    } else {
278      valid[4] = 0;
279    }
280   
281    /* z = H */
282    NewL[5] = -(y-L)/dy ;
283    if ((fabs(z - H/2 + NewL[5]*dz) < H) && (fabs(y + NewL[5]*dy) < L)) {
284      valid[5] = 1;
285    } else {
286      valid[5] = 0;
287    }
288   
289  } else {
290    NewL[4] = -1.0 ;
291    NewL[5] = -1.0 ;
292    valid[4] = 0;
293    valid[5] = 0;
294  }
295
296}
297
298void BVMHyperboloidIntersection (G4double x,G4double y,G4double z,
299                                G4double dx,G4double dy,G4double dz,
300                                G4double H, G4double L,
301                                G4double *NewL,int *valid)
302{
303  /* We use the box [-L L]x[-L L]x[-H H] */
304  /* there is only one interval at maximum */
305
306  /* NewL and valid are array of 6 elements */
307
308  if (dz != 0) {
309    /* z = -H */
310    NewL[0] = -(z+H)/dz ;
311    if ((fabs(x + NewL[0]*dx) < L) && (fabs(y + NewL[0]*dy) < L)) {
312      valid[0] = 1;
313    } else {
314      valid[0] = 0;
315    }
316   
317    /* z = H */
318    NewL[1] = -(z-H)/dz ;
319    if ((fabs(x + NewL[1]*dx) < L) && (fabs(y + NewL[1]*dy) < L)) {
320      valid[1] = 1;
321    } else {
322      valid[1] = 0;
323    }
324   
325  } else {
326    NewL[0] = -1.0 ;
327    NewL[1] = -1.0 ;
328    valid[0] = 0;
329    valid[1] = 0;
330  }
331
332  if (dx != 0) {
333    /* x = -L */
334    NewL[2] = -(x+L)/dx ;
335    if ((fabs(z +NewL[2]*dz) < H) && (fabs(y + NewL[2]*dy) < L)) {
336      valid[2] = 1;
337    } else {
338      valid[2] = 0;
339    }
340   
341    /* z = H */
342    NewL[3] = -(x-L)/dx ;
343    if ((fabs(z + NewL[3]*dz) < H) && (fabs(y + NewL[3]*dy) < L)) {
344      valid[3] = 1;
345    } else {
346      valid[3] = 0;
347    }
348   
349  } else {
350    NewL[2] = -1.0 ;
351    NewL[3] = -1.0 ;
352    valid[2] = 0;
353    valid[3] = 0;
354  }
355
356  if (dy != 0) {
357    /* y = -L */
358    NewL[4] = -(y+L)/dy ;
359    if ((fabs(z +NewL[4]*dz) < H) && (fabs(y + NewL[4]*dy) < L)) {
360      valid[4] = 1;
361    } else {
362      valid[4] = 0;
363    }
364   
365    /* z = H */
366    NewL[5] = -(y-L)/dy ;
367    if ((fabs(z + NewL[5]*dz) < H) && (fabs(y + NewL[5]*dy) < L)) {
368      valid[5] = 1;
369    } else {
370      valid[5] = 0;
371    }
372   
373  } else {
374    NewL[4] = -1.0 ;
375    NewL[5] = -1.0 ;
376    valid[4] = 0;
377    valid[5] = 0;
378  }
379
380}
381
382void BVMIntersection(G4double x,G4double y,G4double z,
383                              G4double dx,G4double dy,G4double dz,
384                              G4double Rmax, G4double Rmin,
385                              G4double *NewL,int *valid)
386{
387
388  if (dz != 0) {
389    G4double DistToZ ;
390    /* z = + Rmin */
391    NewL[0] = (Rmin - z)/dz ;
392    /* z = - Rmin */
393    NewL[1] = (-Rmin - z)/dz ;
394    /* Test validity here (*** To be optimized ***) */
395    if (NewL[0] < 0.0) valid[0] = 0;
396    if (NewL[1] < 0.0) valid[1] = 0;
397    DistToZ = (x+NewL[0]*dx)*(x+NewL[0]*dx) + (y+NewL[0]*dy)*(y+NewL[0]*dy);
398    if (DistToZ - (Rmax + Rmin)*(Rmax + Rmin) > 0)
399      valid[0] = 0;
400    if (DistToZ - (Rmax - Rmin)*(Rmax - Rmin) < 0)
401      valid[0] = 0;
402    DistToZ = (x+NewL[1]*dx)*(x+NewL[1]*dx) + (y+NewL[1]*dy)*(y+NewL[1]*dy);
403    if (DistToZ - (Rmax + Rmin)*(Rmax + Rmin) > 0)
404      valid[1] = 0;
405    if (DistToZ - (Rmax - Rmin)*(Rmax - Rmin) < 0)
406      valid[1] = 0;
407  } else {
408    /* if dz == 0 we could know the exact solution */
409    /* Well, this is true but we have not expected precision issue from sqrt .. */
410    NewL[0] = -1.0;
411    NewL[1] = -1.0;
412    valid[0] = 0;
413    valid[1] = 0;
414  }
415
416  /* x² + y² = (Rmax + Rmin)² */
417  if ((dx != 0) || (dy != 0)) {
418    G4double a,b,c,d;
419   
420    a = dx*dx + dy*dy ;
421    b = 2*(x*dx + y*dy) ;
422    c = x*x + y*y - (Rmax + Rmin)*(Rmax + Rmin) ;
423    d = b*b - 4*a*c ;
424   
425    if (d < 0) {
426      valid[2] = 0;
427      valid[3] = 0;
428      NewL[2] = -1.0;
429      NewL[3] = -1.0;
430    } else {
431      d = sqrt(d) ;
432      NewL[2] = (d - b)/(2*a);
433      NewL[3] = (-d - b)/(2*a);
434      if (NewL[2] < 0.0) valid[2] = 0;
435      if (fabs(z + NewL[2]*dz) - Rmin > EPSILON) valid[2] = 0;
436      if (NewL[3] < 0.0) valid[3] = 0;
437      if (fabs(z + NewL[3]*dz) - Rmin > EPSILON) valid[3] = 0;
438    }
439  } else {
440    /* only dz != 0 so we could know the exact solution */
441    /* this depends only for the distance to Z axis */
442    /* BUT big precision problem near the border.. */
443    /* I like so much Newton to increase precision you know.. => */
444
445    NewL[2] = -1.0;
446    NewL[3] = -1.0;
447    valid[2] = 0;
448    valid[3] = 0;
449       
450    /*** Try This to see precision issue with sqrt(~ 0)
451         G4double DistToZ ;
452         G4double result;
453         G4double guess;
454       
455         DistToZ = sqrt(x*x + y*y) ;
456       
457         if ((DistToZ < (Rmax - Rmin)) || (DistToZ > (Rmax + Rmin))) {
458         return -1.0 ;
459         }
460       
461         result = sqrt((Rmin + Rmax - DistToZ)*(Rmin - Rmax + DistToZ));
462
463         if (dz < 0) {
464         if (z > result) {
465         return (result - z)/dz;
466         } else {
467         if (z > -result) {
468         return (-result - z)/dz;
469         } else
470         return -1.0;
471         }
472         } else {
473         if (z < -result) {
474         return (z + result)/dz;
475         } else {
476         if (z < result) {
477         return (z - result)/dz;
478         } else
479         return -1.0;
480         }
481         }
482    */
483  }
484 
485
486  /* x² + y² = (Rmax - Rmin)² */
487  if ((dx != 0) || (dy != 0)) {
488    G4double a,b,c,d;
489   
490    a = dx*dx + dy*dy ;
491    b = 2*(x*dx + y*dy) ;
492    c = x*x + y*y - (Rmax - Rmin)*(Rmax - Rmin) ;
493    d = b*b - 4*a*c ;
494   
495    if (d < 0) {
496      valid[4] = 0;
497      valid[5] = 0;
498      NewL[4] = -1.0;
499      NewL[5] = -1.0;
500    } else {
501      d = sqrt(d) ;
502      NewL[4] = (d - b)/(2*a);
503      NewL[5] = (-d - b)/(2*a);
504      if (NewL[4] < 0.0) valid[4] = 0;
505      if (fabs(z + NewL[4]*dz) - Rmin > EPSILON) valid[4] = 0;
506      if (NewL[5] < 0.0) valid[5] = 0;
507      if (fabs(z + NewL[5]*dz) - Rmin > EPSILON) valid[5] = 0;
508    }
509  } else {
510    /* only dz != 0 so we could know the exact solution */
511    /* OK but same as above .. */
512    valid[4] = 0;
513    valid[5] = 0;
514    NewL[4] = -1.0;
515    NewL[5] = -1.0;
516  }
517}
518
519void SortIntervals (int NbElem,G4double *SortL,G4double *NewL,int *valid,int *NbIntersection)
520{
521  int i,j;
522  G4double swap;
523       
524  (*NbIntersection) = 0;
525  SortL[0] = -INFINITY;
526       
527  for (i=0;i<NbElem;i++) {
528    if (valid[i] != 0) {
529      SortL[(*NbIntersection)] = NewL[i] ;
530      for (j=(*NbIntersection);j>0;j--) {
531        if (SortL[j] < SortL[j-1]) {
532          swap = SortL[j-1] ;
533          SortL[j-1] = SortL[j];
534          SortL[j] = swap;
535        }
536      }
537               
538      (*NbIntersection) ++;
539    }
540  }
541  /* Delete double value */
542  /* When the ray hits a corner we have a double value */
543  for (i=0;i<(*NbIntersection)-1;i++) {
544    if (SortL[i+1] - SortL[i] < EPSILON) {
545      if (((*NbIntersection) & (1)) == 1) {
546        /* If the NbIntersection is odd then we keep one value */
547        for (j=i+1;j<(*NbIntersection);j++) {
548          SortL[j-1] = SortL[j] ;
549        }
550        (*NbIntersection) --;
551      } else {
552        /* If it is even we delete the 2 values */
553        for (j=i+2;j<(*NbIntersection);j++) {
554          SortL[j-2] = SortL[j] ;
555        }
556        (*NbIntersection) -= 2;
557      }
558    }
559  }
560}
561
562
563/* TODO:
564  check if the root is entering the torus (with gradient)
565  clean problems when Rmin ~ Rmax (BVM is not good when near Z axis)
566 */
567
568/** Now the interesting part .. **/
569
570int SafeNewton(G4double x, G4double y, G4double z,
571                    G4double dx, G4double dy, G4double dz,
572                    G4double Rmax, G4double Rmin,
573                    G4double *Lmin,G4double *Lmax)
574{
575  /** SafeNewton is a clipping interval Newton method **/
576  G4double P[5][2],D[2] ;
577  G4double Lx,Ly,Lz ;
578  G4double NewMin,NewMax;
579 
580  int IntervalIsVoid = 1;
581  int NewtonIsSafe = 0;
582 
583  /*** Calculating Control Points  ***/
584 
585  /*
586    0           p0 = F((*Lmin))
587    1/4         p1 = F((*Lmin)) + ((*Lmax) - (*Lmin))/4 * F'((*Lmin))
588    2/4         p2 = 1/6 * (32*F(((*Lmax) + (*Lmin))/2) - (p0 + 4*p1 + 4*p3 + p4)) 
589    3/4         p3 = F((*Lmax)) - ((*Lmax) - (*Lmin))/4 * F'((*Lmax))
590    1           p4 = F((*Lmax))
591  */
592
593 
594  Lx = x + (*Lmin)*dx;
595  Ly = y + (*Lmin)*dy;
596  Lz = z + (*Lmin)*dz;
597
598  D[0] = dx*HyperboloidDerX(Lx,Ly,Lz,Rmax,Rmin);
599  D[0] += dy*HyperboloidDerY(Lx,Ly,Lz,Rmax,Rmin);
600  D[0] += dz*HyperboloidDerZ(Lx,Ly,Lz,Rmax,Rmin);
601
602  P[0][0] = (*Lmin);
603  P[0][1] = HyperboloidEquation(Lx,Ly,Lz,Rmax,Rmin);
604
605  if (fabs(P[0][1]) < TORUSPRECISION) {
606    NewtonIsSafe = 1;
607    //fprintf(stderr,"(fabs(P[0][1]) < TORUSPRECISION)\n");
608    return NewtonIsSafe;
609  }
610 
611  if (((*Lmax) - (*Lmin)) < EPSILON) {
612    //fprintf(stderr,"(((*Lmax) - (*Lmin)) < EPSILON)\n");
613    return 1;
614  }
615
616  P[1][0] = (*Lmin) + ((*Lmax) - (*Lmin))/4;
617  P[1][1] = P[0][1] + (((*Lmax) - (*Lmin))/4.0) * D[0];
618 
619  Lx = x + (*Lmax)*dx;
620  Ly = y + (*Lmax)*dy;
621  Lz = z + (*Lmax)*dz;
622
623  D[1] = dx*HyperboloidDerX(Lx,Ly,Lz,Rmax,Rmin);
624  D[1] += dy*HyperboloidDerY(Lx,Ly,Lz,Rmax,Rmin);
625  D[1] += dz*HyperboloidDerZ(Lx,Ly,Lz,Rmax,Rmin);
626
627  P[4][0] = (*Lmax);
628  P[4][1] = HyperboloidEquation(Lx,Ly,Lz,Rmax,Rmin);
629  P[3][0] = (*Lmax) - ((*Lmax) - (*Lmin))/4;
630  P[3][1] = P[4][1] - ((*Lmax) - (*Lmin))/4 * D[1];
631
632  Lx = x + ((*Lmax)+(*Lmin))/2*dx;
633  Ly = y + ((*Lmax)+(*Lmin))/2*dy;
634  Lz = z + ((*Lmax)+(*Lmin))/2*dz;
635
636  P[2][0] = ((*Lmax) + (*Lmin))/2;
637  P[2][1] = (16*HyperboloidEquation(Lx,Ly,Lz,Rmax,Rmin) - (P[0][1] + 4*P[1][1] + 4*P[3][1] + P[4][1]))/6 ;
638
639 
640 
641  //fprintf(stderr,"\n");
642  //fprintf(stderr,"Lmin = %14f\n",(*Lmin));
643  //fprintf(stderr,"Lmax = %14f\n",(*Lmax));
644  //fprintf(stderr,"P[0] = %14f\n",P[0][1]);
645  //fprintf(stderr,"P[1] = %14f\n",P[1][1]);
646  //fprintf(stderr,"P[2] = %14f\n",P[2][1]);
647  //fprintf(stderr,"P[3] = %14f\n",P[3][1]);
648  //fprintf(stderr,"P[4] = %14f\n",P[4][1]);
649
650#if DEBUGTORUS
651  G4cout << "G4Torus::SafeNewton    Lmin = " << (*Lmin) << G4endl ;
652  G4cout << "G4Torus::SafeNewton    Lmax = " << (*Lmax) << G4endl ;
653  G4cout << "G4Torus::SafeNewton    P[0] = " << P[0][1] << G4endl ;
654  G4cout << "G4Torus::SafeNewton    P[1] = " << P[1][1] << G4endl ;
655  G4cout << "G4Torus::SafeNewton    P[2] = " << P[2][1] << G4endl ;
656  G4cout << "G4Torus::SafeNewton    P[3] = " << P[3][1] << G4endl ;
657  G4cout << "G4Torus::SafeNewton    P[4] = " << P[4][1] << G4endl ;
658#endif
659
660  /** Ok now we have all control points, we could compute the convex area **/
661  /** Problems:
662      - if there is one point with a ~ 0 coordinate and all the other the same sign we
663      miss the value
664      - if there are more than a root in the interval then the interval length does not
665      decrease to 0. A solution may be to split intervals in the middle but how to
666      know that we must split ?
667      - we have to compute convex area of the control point before applying intersection
668      with y=0
669  **/
670
671  /*** For each points make 2 sets. A set of positive points and a set of negative points ***/
672  /*** Note: could be better done with scalar product .. ***/
673
674  /* there is an intersection only if each have different signs */
675  /* PROBLEM : If a control point have a 0.00 value the sign check is wrong */
676  {
677    G4double Intersection ;
678    int i,j;
679
680    NewMin = (*Lmax) ;
681    NewMax = (*Lmin) ;
682
683    for (i=0;i<5;i++)
684      for (j=i+1;j<5;j++)
685        {
686          /* there is an intersection only if each have different signs */
687          if (((P[j][1] > -TORUSPRECISION) && (P[i][1] < TORUSPRECISION)) ||
688              ((P[j][1] < TORUSPRECISION) && (P[i][1] > -TORUSPRECISION))) {
689            IntervalIsVoid  = 0;
690            Intersection = P[j][0] - P[j][1]*((P[i][0] - P[j][0])/(P[i][1] - P[j][1]));
691            if (Intersection < NewMin) {
692              NewMin = Intersection;
693            }
694            if (Intersection > NewMax) {
695              NewMax = Intersection;
696            }
697          }
698        }
699    if (IntervalIsVoid != 1) {
700      (*Lmax) = NewMax;
701      (*Lmin) = NewMin;
702    }
703  }
704 
705  if (IntervalIsVoid == 1) {
706    //fprintf(stderr,"(IntervalIsVoid == 1)\n");
707    return -1;
708  }
709 
710  //fprintf(stderr,"NewMin = %f  NewMax = %f\n",NewMin,NewMax);
711  /** Now we have each Extrema point of the new interval **/
712 
713  return NewtonIsSafe;
714}
715
716
717G4double Newton (G4double guess,
718                 G4double x, G4double y, G4double z,
719                 G4double dx, G4double dy, G4double dz,
720                 G4double Rmax, G4double Rmin,
721                 G4double Lmin,G4double Lmax)
722{
723  /* So now we have a good guess and an interval where if there are an intersection the root must be */
724
725  G4double Lx = 0;
726  G4double Ly = 0;
727  G4double Lz = 0;
728  G4double Value = 0;
729  G4double Gradient = 0;
730  G4double Lambda ;
731
732  int i=0;
733
734  /* Reduce interval before applying Newton Method */
735  {
736    int NewtonIsSafe ;
737
738    while ((NewtonIsSafe = SafeNewton(x,y,z,dx,dy,dz,Rmax,Rmin,&Lmin,&Lmax)) == 0) ;
739
740    guess = Lmin;
741  }
742
743  /*** BEWARE ***/
744  /* A typical problem is when Gradient is zero */
745  /* This is due to some 0 values in point or direction */
746  /* To solve that we move a little the guess
747  if ((((x == 0) || (y == 0)) || (z == 0)) ||
748      (((dx == 0) || (dy == 0)) || (dz == 0)))
749      guess += EPSILON;*/
750 
751  Lambda = guess;
752  Value = HyperboloidEquation(x + Lambda*dx,y + Lambda*dy,z + Lambda*dz,Rmax,Rmin);
753
754  //fprintf(stderr,"NEWTON begin with L = %f and V = %f\n",Lambda,Value);
755 
756  /*** Beware: we must eliminate case with no root ***/
757  /*** Beware: In some rare case we converge to the false root (internal border)***/
758  /***
759   {
760    FILE *fi;
761    int i;
762    fi = fopen("GNUplot.out","w+");
763    //fprintf(fi,"# Newton plot\n");
764         
765    for (i = 0; i < 1000 ; i ++) {
766      Lx = x + (Lmin + i*(Lmax - Lmin)/1000.0)*dx;
767      Ly = y + (Lmin + i*(Lmax - Lmin)/1000.0)*dy;
768      Lz = z + (Lmin + i*(Lmax - Lmin)/1000.0)*dz;
769      Value = HyperboloidEquation(Lx,Ly,Lz,Rmax,Rmin);
770      //fprintf(fi," %f %f\n",Lmin + i*(Lmax - Lmin)/1000.0,Value );
771    }
772         
773    fclose(fi);
774  }
775   
776  ***/
777     
778  /* In fact The Torus Equation give big number so TORUS PRECISION is not EPSILON */     
779  while (/* ?? (fabs(Value/Gradient) > 1e-2) ||*/ (fabs(Value) > TORUSPRECISION)) {
780         
781    //  do {
782    Lx = x + Lambda*dx;
783    Ly = y + Lambda*dy;
784    Lz = z + Lambda*dz;
785    Value = HyperboloidEquation(Lx,Ly,Lz,Rmax,Rmin);
786
787    Gradient = dx*HyperboloidDerX(Lx,Ly,Lz,Rmax,Rmin);
788    Gradient += dy*HyperboloidDerY(Lx,Ly,Lz,Rmax,Rmin);
789    Gradient += dz*HyperboloidDerZ(Lx,Ly,Lz,Rmax,Rmin);
790
791        /*
792        if (Gradient > -EPSILON)
793          return Lmin;
794        */
795       
796    /***
797        if ((beware != 0) && (Gradient > -EPSILON)) {
798    ***/
799
800    /** Newton does not go to the root because interval is too big **/
801    /** In fact Newton is known to converge if |f.f''/(f'^2)| < 1 **/
802    /** There is two cases: ray hits or not **/
803    /** If ray hits we must search for a better intervals **/
804    /** but if there are no hits then we could not .. **/
805    /** So the easier way the best: if Newton encounter a problem
806        it says to the BVM that the guess is no good
807        then the BVM search for a better intervals, possibly none
808        in this case no intersection, else we go back to Newton
809    **/
810
811    /**
812       Perhaps we have not to break Newton at the beginning because we could converge after some move
813       May be not: If we are here this means that the root we want is rejecting. We could converge to
814       another root.
815       PROBLEMS
816    **/
817    /* root is repulsive from this guess could you give me another guess ?
818       Note: that it may be no root in this area ..
819       Note: Lmin and Lmax are always outside the torus as a part of the BVM.
820       We just want a point in this direction with a gradient < 0
821                                 
822       guess = FindABetterGuess(Rmax,Rmin,guess,Lmin,Lmax);
823    */
824    Lambda = Lambda - Value/Gradient ;
825
826#if DEBUGTORUS
827    G4cout << "Newton     Iteration " << i << G4endl ;
828    G4cout << "Newton     Lambda = " << Lambda << " Value = " << Value << " Grad = " << Gradient << G4endl;
829    G4cout << "Newton     Lmin = " << Lmin << " Lmax = " << Lmax << G4endl ;
830#endif
831    //fprintf(stderr,"Newton    Iteration %d\n",i);
832    //fprintf(stderr,"Newton    Lambda = %f  Value = %f  Grad = %f\n",Lambda,Value,Gradient);
833   
834    i ++;
835
836    if (i > ITERATION) 
837      return NOINTERSECTION; //no convergency ??
838
839  } //while (/* ?? (fabs(Value/Gradient) > 1e-2) ||*/ (fabs(Value) > TORUSPRECISION));
840
841
842#if DEBUGTORUS
843  G4cout << "Newton    Exiting with Lambda = " << Lambda << G4endl ;
844  G4cout << "Newton    Exiting with Value = " << Value << G4endl ;
845#endif
846
847  //just a check
848  if (Lambda < 0.0) {
849        //fprintf(stderr,"Newton end with a negative solution ..\n");
850        return NOINTERSECTION;
851  }
852  //fprintf(stderr,"NEWTON: Lamdba = %f\n",Lambda);
853  return Lambda ;
854}
855
856/*
857G4double DistanceToTorus (G4double x,G4double y,G4double z,
858                                   G4double dx,G4double dy,G4double dz,
859                                   G4double Rmax,G4double Rmin)
860*/
861double DistanceToTorus (Intersect * Inter)
862{
863  static int Vstatic = 0;
864  G4double Lmin,Lmax;
865  G4double guess;
866  G4double SortL[4];
867   
868  int NbIntersection = 0;
869
870  G4double NewL[NBPOINT];
871  int valid[] = {1,1,1,1,1,1} ;
872  int j;
873
874                double x,y,z,dx,dy,dz;
875                double Rmax,Rmin;
876                double phi,deltaphi;
877
878                j = 0;
879
880
881                dx = Inter->dx;
882                dy = Inter->dy;
883                dz = Inter->dz;
884                x = Inter->x;
885                y = Inter->y;
886                z = Inter->z;
887                Rmax = Inter->R0 ;
888                Rmin = Inter->R1 ;
889                phi = Inter->phi;
890                deltaphi = Inter->deltaphi;
891
892
893  /*** Compute Intervals  from Bounding Volume ***/
894
895                //BVMIntersection(x,y,z,dx,dy,dz,Rmax,Rmin,NewL,valid);
896                BVMHyperboloidIntersection(x,y,z,dx,dy,dz,Rmax,Rmin,NewL,valid);
897
898  /*
899    We could compute intervals value
900    Sort all valid NewL to SortL.
901    There must be 4 values at max and
902    odd one if point is inside
903  */
904
905  SortIntervals(6,SortL,NewL,valid,&NbIntersection);
906  if (BVM_ONLY == 1)
907    return SortL[0] ; 
908
909#if 0
910  // Torus Only
911  {
912    /*** Length check ***/
913    G4double LengthMin = 0.82842712*Rmin;
914                               
915    switch(NbIntersection) {
916    case 1:
917      if (SortL[0] < EPSILON) {
918        if (fabs(HyperboloidEquation(x,y,z,Rmax,Rmin)) < TORUSPRECISION) {
919          return 0.0;
920        } else {
921          return NOINTERSECTION;
922        }
923      }
924      break;
925    case 2:
926      if ((SortL[1] - SortL[0]) < LengthMin) NbIntersection = 0;
927      break;
928    case 3:
929      if (SortL[0] < EPSILON) {
930        if (fabs(HyperboloidEquation(x,y,z,Rmax,Rmin)) < TORUSPRECISION) {
931          return 0.0;
932        } else {
933          NbIntersection --;
934          SortL[0] = SortL[1] ;
935          SortL[1] = SortL[2] ;
936          if ((SortL[1] - SortL[0]) < LengthMin) NbIntersection = 0;
937        }
938      } else {
939        if ((SortL[2] - SortL[1]) < LengthMin) NbIntersection -= 2;
940      }
941      break;
942    case 4:
943      if ((SortL[1] - SortL[0]) < LengthMin) {
944        NbIntersection -= 2;
945        SortL[0] = SortL[2];
946        SortL[1] = SortL[3];
947        if ((SortL[1] - SortL[0]) < LengthMin) NbIntersection -= 2;     
948      }
949      break;
950    }
951  }
952#endif
953 
954#if DEBUGTORUS
955  {
956    int i;
957    G4cout.precision(16);
958    G4cout << "DistanceToTorus    INTERVALS" << G4endl ;
959    for (i=0;i<NbIntersection;i++) {
960      G4cout << "DistanceToTorus    " << SortL[i] << G4endl ;
961    }
962  }
963#endif
964
965  Vstatic ++;
966   
967  //if  ((Vstatic % 2) == 0) return SortL[0];
968  //printf("NbIntersection = %d\n",NbIntersection);
969 
970 
971  /* BVM Test
972
973     switch(NbIntersection) {
974     case 0:
975     return -1.0;
976     break;
977     case 1:
978     return -1.0;
979     break;
980     case 2:
981     return -1.0;
982     break;
983     case 3:
984     return -1.0;
985     break;
986     case 4:
987     return -1.0;
988     break;
989     }
990  */
991                       
992  /*** If the ray intersects the torus it necessary intersects the BVMax ***/
993  /*** So it is necessary into *an* interval from the BVM ***/
994
995  /** Note : In general there are only 2 intersections so computing the second interval
996      could be done only if the first one does not contain any root */
997
998  /* NOW there is 2 possibilities */
999  /* If inside the BVM (or Torus instead), take "0, SortL[0] .." */
1000  /* If outside the BVM, we have intervals where if there is an intersection the root must be */
1001  /* Now Lmin1 <= Lambda <= Lmax and there is a *unique* root */
1002  /* Newton Methods in this interval from the guess */
1003
1004  /*** Beware The first interval could be the bad one and we have to see other one ***/
1005  /*** We must have a way to decide if an interval contains root or not .. ***/
1006
1007  /***
1008       Beware: If the original point is near the torus (into the BVM not the torus)
1009       we have serious precision issue (bad guess value) try it with a big Rmin
1010  ***/
1011
1012  /* We are Inside the BVM if the number of intersection is odd */
1013  /* Not necessary an intersection with Torus if point outside Torus and Inside BVM ! */
1014
1015  if (((NbIntersection) & (1)) != 0) {
1016    /*** If we are Inside the BVM Lmin = 0. Lmax is the point ***/
1017    /***    there is necessary an intersection if the point is inside the Torus ***/
1018    int InsideTorus = 0;
1019
1020    Lmin = 0.0 ;
1021    Lmax  = SortL[0] ;
1022
1023    if (HyperboloidEquation(x,y,z,Rmax,Rmin) < 0.0) {
1024     
1025      InsideTorus = 1;
1026      /* As we are inside the torus it must have an intersection */
1027      /* To have a good guess we take Lmax - Rmin/8.0 */
1028      /*(What is the best value for a square to be like a circle ?) */
1029      /* If we are inside the torus the upper bound is better */
1030          //return 1000.0;
1031      guess =  Lmax - Rmin*0.125;
1032      //printf("DistanceToTorus    Inside the torus\n");
1033     
1034#if DEBUGTORUS
1035      G4cout << "DistanceToTorus    Inside the torus" << G4endl ;
1036      G4cout << "DistanceToTorus    Initial Guess is " << guess << G4endl ;
1037#endif
1038     
1039    } else {
1040          //      return 1000.0;
1041      //printf("DistanceToTorus    Outside the torus\n");
1042#if DEBUGTORUS
1043      G4cout.precision(16);
1044      G4cout << "DistanceToTorus    point " << x << ", " << y << ", " << z << ", "  << " is outside the torus "
1045             << " Rmax = " << Rmax << " Rmin = " << Rmin << " Teq = " << HyperboloidEquation(x,y,z,Rmax,Rmin) << G4endl ;
1046#endif
1047      InsideTorus = 0;
1048      /* PROBLEMS what to choose ? 0.0 ? */
1049      /* 0.0 is generally a good guess, but there is case that it is very bad (hit center torus when inside BVM) */
1050
1051      if (Lmax > Rmin) {
1052        /* we are in the case where we hit center torus */
1053       
1054        //return 100000.0;
1055        guess = Lmax;
1056       
1057      } else {
1058        /* general case */
1059        guess = 0.0;
1060      }
1061    }
1062
1063    /* Ready to do Newton */
1064    guess = Newton(guess,x,y,z,dx,dy,dz,Rmax,Rmin,Lmin,Lmax);
1065
1066#if DEBUGTORUS
1067    G4cout << "DistanceToTorus    First Newton guess = " << guess << G4endl ;
1068    G4cout << "DistanceToTorus    Lmin = " << Lmin << "  Lmax = " << Lmax << G4endl ;
1069#endif
1070
1071    /* In case we are the origin point is just in the surface
1072       the NbIntersection will be odd and guess will be zero
1073       Anyway, it is correct to say that distance is zero but
1074       we want to return +inf if we are exiting the solid
1075       So ..
1076    */
1077
1078    /* Check here is the root found is into interval */
1079
1080    if ((guess >= (Lmin - EPSILON)) && (guess <= (Lmax + EPSILON))) {
1081      return guess ;
1082    } else {
1083      if (NbIntersection == 3) {
1084                                /** OK we are in the small part around the BVM **/
1085                                /** So we check the second interval **/
1086        Lmin = SortL[1];
1087        Lmax = SortL[2];
1088        guess = Lmin;
1089       
1090        guess = Newton(guess,x,y,z,dx,dy,dz,Rmax,Rmin,Lmin,Lmax);
1091#if DEBUGTORUS
1092        G4cout << "DistanceToTorus    Second Newton guess = " << guess << G4endl ;
1093        G4cout << "DistanceToTorus    Lmin = " << Lmin << "  Lmax = " << Lmax << G4endl ;
1094#endif
1095        if ((guess >= (Lmin - EPSILON)) && (guess <= (Lmax + EPSILON))) {
1096          return guess;
1097        } else {
1098          return NOINTERSECTION;
1099        }
1100      } else {
1101        if (InsideTorus == 1) {
1102          /* Incredible : sometimes precisions errors bring us here
1103             with guess = SortL[0]
1104             So we return guess ..
1105
1106             PROBLEMS 99%
1107         
1108                                                                               
1109          printf("Torus: Root not found final (guess - Limit) = %f\n"
1110                 ,guess - SortL[0]);
1111          printf("point: %f %f %f\n",x,y,z);
1112          printf("dir  : %f %f %f\n",dx,dy,dz);
1113          */
1114         
1115          return 100000.0;//guess;
1116          exit(1);
1117                                                                               
1118        }
1119        return NOINTERSECTION;
1120      }
1121    }
1122       
1123       
1124
1125  } else { // Outside
1126    /*** If we are Out then we need more to know if intersection exists ***/
1127    /***  there is 2 intersection points at least (perhaps the same) with BVMax ***/
1128
1129    /*** Return if no intersection with BVMax ***/
1130
1131    if (NbIntersection == 0) 
1132      return NOINTERSECTION ;
1133                               
1134
1135    Lmin = SortL[0] ;
1136    Lmax = SortL[1] ;
1137    /** Lmin because it is probably near the BVM entry point **/
1138    /** PROBLEM if the ray hits the top of BVM with a small angle
1139        then the interval is too big and the guess is bad **/
1140
1141    guess = Lmin ; 
1142                               
1143
1144    /*** We know only that if there is a solution, it is between Lmin and Lmax ***/
1145    /*** But we are not sure that there is one ... ***/
1146       
1147    /* Ready to do Newton */
1148    guess = Newton(guess,x,y,z,dx,dy,dz,Rmax,Rmin,Lmin,Lmax);
1149
1150#if DEBUGTORUS
1151    G4cout << "DistanceToTorus    Newton with 2 or 4 points : " << guess << G4endl ;
1152#endif   
1153
1154    /* Check here is the root found is into interval */
1155    if ((guess >= (Lmin - EPSILON)) && (guess <= (Lmax + EPSILON))) {
1156#if DEBUGTORUS
1157    G4cout << "DistanceToTorus    Newton gives a point into interval (Ok)" << G4endl ;
1158#endif   
1159      return guess;
1160    } else {                           
1161#if DEBUGTORUS
1162    G4cout << "DistanceToTorus    Newton does not give a point into interval (Ko)" << G4endl ;
1163#endif   
1164      if (NbIntersection == 4) {
1165        /* Well if that does not converge with the first interval try with the other one */
1166        Lmin = SortL[2] ;
1167        Lmax = SortL[3] ;
1168
1169        guess = Lmin;
1170        guess = Newton(guess,x,y,z,dx,dy,dz,Rmax,Rmin,Lmin,Lmax);
1171       
1172        if ((guess >= (Lmin - EPSILON)) && (guess <= (Lmax + EPSILON))) {
1173          return guess;
1174        } else {
1175          return NOINTERSECTION;
1176        }
1177      } else {
1178        /* Certainly this is due to the BVM part that is not in Torus */
1179
1180        return NOINTERSECTION ;
1181      }
1182    }
1183  }
1184}
1185
1186inline G4double TorusGradient(G4double dx,
1187                                       G4double dy,
1188                                       G4double dz,
1189                                       G4double x,
1190                                       G4double y,
1191                                       G4double z,
1192                                       G4double Rmax,
1193                                       G4double Rmin)
1194{
1195  /* This tell the normal at a surface point */
1196  G4double result;
1197  result = 0;
1198  result += dx*HyperboloidDerX(x,y,z,Rmax,Rmin); 
1199  result += dy*HyperboloidDerY(x,y,z,Rmax,Rmin); 
1200  result += dz*HyperboloidDerZ(x,y,z,Rmax,Rmin); 
1201
1202  return result;
1203}
1204
1205
1206inline G4double ParaboloidGradient(G4double dx,
1207                                       G4double dy,
1208                                       G4double dz,
1209                                       G4double x,
1210                                       G4double y,
1211                                       G4double z,
1212                                       G4double Rmax,
1213                                       G4double Rmin)
1214{
1215  /* This tell the normal at a surface point */
1216  G4double result;
1217  result = 0;
1218  result += dx*ParaboloidDerX(x,y,z,Rmax,Rmin); 
1219  result += dy*ParaboloidDerY(x,y,z,Rmax,Rmin); 
1220  result += dz*ParaboloidDerZ(x,y,z,Rmax,Rmin); 
1221
1222  return result;
1223}
1224
1225inline G4double HyperboloidGradient(G4double dx,
1226                                       G4double dy,
1227                                       G4double dz,
1228                                       G4double x,
1229                                       G4double y,
1230                                       G4double z,
1231                                       G4double Rmax,
1232                                       G4double Rmin)
1233{
1234  /* This tell the normal at a surface point */
1235  G4double result;
1236  result = 0;
1237  result += dx*HyperboloidDerX(x,y,z,Rmax,Rmin); 
1238  result += dy*HyperboloidDerY(x,y,z,Rmax,Rmin); 
1239  result += dz*HyperboloidDerZ(x,y,z,Rmax,Rmin); 
1240
1241  return result;
1242}
1243
Note: See TracBrowser for help on using the repository browser.