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

Last change on this file since 1353 was 1350, checked in by garnier, 15 years ago

update to last version 4.9.4
File size: 30.4 KB
RevLine 
[1350]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.