source: TRACY3/trunk/tracy/tracy/src/physlib.cc @ 11

Last change on this file since 11 was 11, checked in by zhangj, 11 years ago
  • Property svn:executable set to *
File size: 98.9 KB
Line 
1/* Tracy-2
2
3 J. Bengtsson, CBP, LBL      1990 - 1994   Pascal version
4 SLS, PSI      1995 - 1997
5 M. Boege      SLS, PSI      1998          C translation
6 L. Nadolski   SOLEIL        2002          Link to NAFF, Radia field maps
7 J. Bengtsson  NSLS-II, BNL  2004 -
8 */
9/* Current revision $Revision: 1.20 $
10 On branch $Name:  $
11 Latest change by $Author: zhang $
12*/
13
14/**************************/
15/* Routines for printing  */
16/**************************/
17
18/*******************************************************************************
19 *
20 *
21 *
22 *
23 ******************************************************************************/
24/**** same as asctime in C without the \n at the end****/
25char *asctime2(const struct tm *timeptr) {
26    // terminated with \0.
27    static char wday_name[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri",
28            "Sat" };
29    // terminated with \0.
30    static char mon_name[12][4] = { "Jan", "Feb", "Mar", "Apr", "May", "Jun",
31            "Jul", "Aug", "Sep", "Oct", "Nov", "Dec" };
32    static char result[26];
33
34    sprintf(result, "%.3s %.3s%3d %.2d:%.2d:%.2d %d",
35            wday_name[timeptr->tm_wday], mon_name[timeptr->tm_mon],
36            timeptr->tm_mday, timeptr->tm_hour, timeptr->tm_min,
37            timeptr->tm_sec, 1900 + timeptr->tm_year);
38    return result;
39}
40/*******************************************************************************
41 *
42 *
43 *
44 *
45 ******************************************************************************/
46/** Get time and date **/
47struct tm* GetTime() {
48    struct tm *whattime;
49    /* Get time and date */
50    time_t aclock;
51    time(&aclock); /* Get time in seconds */
52    whattime = localtime(&aclock); /* Convert time to struct */
53    return whattime;
54}
55/****************************************************************************
56 * uint32_t stampstart()
57
58
59 Purpose: record time in millliseconds
60
61 Input:
62 none
63
64 Output:
65 non
66
67 Return:
68 time in milliseconds
69
70 Global variables:
71 none
72
73 specific functions:
74 none
75
76 Comments:
77 to be used with stampstop()
78
79 ****************************************************************************/
80
81uint32_t stampstart(void) {
82    struct timeval tv;
83    struct timezone tz;
84    struct tm *tm;
85    uint32_t start;
86    const bool timedebug = false;
87
88    // get the time
89    gettimeofday(&tv, &tz);
90    tm = localtime(&tv.tv_sec);
91
92    // print detailed time in milliseconds
93    if (timedebug)
94        printf("TIMESTAMP-START\t  %d:%02d:%02d:%d (~%d ms)\n", tm->tm_hour,
95                tm->tm_min, tm->tm_sec, tv.tv_usec, tm->tm_hour * 3600 * 1000
96                        + tm->tm_min * 60 * 1000 + tm->tm_sec * 1000
97                        + tv.tv_usec / 1000);
98
99    start = tm->tm_hour * 3600 * 1000 + tm->tm_min * 60 * 1000 + tm->tm_sec
100            * 1000 + tv.tv_usec / 1000;
101
102    return (start);
103
104}
105
106// compute time elapsed since start
107/****************************************************************************
108 * uint32_t stampstop(uint32_t start)
109
110 Purpose: compute time elapsed since start time
111
112 Input:
113 start starting time i millisecond
114
115 Output:
116 none
117
118 Return:
119 none
120
121 Global variables:
122 none
123
124 specific functions:
125 none
126
127 Comments:
128 to be used with stampstart
129
130 ****************************************************************************/
131uint32_t stampstop(uint32_t start) {
132
133    struct timeval tv;
134    struct timezone tz;
135    struct tm *tm;
136    uint32_t stop;
137    const bool timedebug = false;
138    bool prt = true;
139    // get the time
140    gettimeofday(&tv, &tz);
141    tm = localtime(&tv.tv_sec);
142
143    stop = tm->tm_hour * 3600 * 1000 + tm->tm_min * 60 * 1000 + tm->tm_sec
144            * 1000 + tv.tv_usec / 1000;
145
146    // print detailed time in milliseconds
147    if (timedebug) {
148        printf("TIMESTAMP-END\t  %d:%02d:%02d:%d (~%d ms) \n", tm->tm_hour,
149                tm->tm_min, tm->tm_sec, tv.tv_usec, tm->tm_hour * 3600 * 1000
150                        + tm->tm_min * 60 * 1000 + tm->tm_sec * 1000
151                        + tv.tv_usec / 1000);
152        printf("ELAPSED\t  %d ms\n", stop - start);
153    }
154
155    uint32_t delta, hour, minute, second, millisecond;
156    delta = stop - start;
157    hour = delta / 3600000;
158    minute = (delta - 3600000 * hour) / 60000;
159    second = (delta - 3600000 * hour - minute * 60000) / 1000;
160    millisecond = delta - 3600000 * hour - minute * 60000 - second * 1000;
161
162    if (prt)
163        printf("ELAPSED\t  %d h %d min %d s %d ms\n", hour, minute, second,
164                millisecond);
165
166    return (stop);
167
168}
169/****************************************************************************/
170/* void printglob(void)
171
172 Purpose:
173 Print global variables on screen
174 Print tunes and chromaticities
175 Print Oneturn matrix
176
177 Input:
178 none
179
180 Output:
181 output on the screen
182
183 Return:
184 none
185
186 Global variables:
187 globval
188
189 Specific functions:
190 none
191
192 Comments:
193 26/03/03 Oneturn matrix added
194 26/03/03 RF acceptance added
195 10/05/03 Momentum compaction factor added
196 16/05/03 Correction for a asymmetrical vaccum vessel
197 20/06/03 Add corrector, skew quad and bpm number
198 27/10/03 Add flag for radiation and chambre
199
200 Comments copied from Tracy 2.7(soleil),Written by L.Nadolski.
201 
202 03/06/2013 Add feature to print the summary on the sreen and an external file
203
204 ****************************************************************************/
205void printglob(void) {
206    printf("\n***************************************************************"
207        "***************\n");
208    printf("\n");
209    printf("  dPcommon     =  %9.3e  dPparticle   =  %9.3e"
210        "  Energy [GeV] = %.3f\n", globval.dPcommon, globval.dPparticle,
211            globval.Energy);
212    printf("  MaxAmplx [m] = %9.3e   MaxAmply [m] = %9.3e"
213        "   RFAccept [%%] = +/- %4.2f\n", Cell[0].maxampl[X_][1],
214            Cell[0].maxampl[Y_][1], globval.delta_RF * 1e2);
215    printf("  MatMeth      =  %s     ", globval.MatMeth ? "TRUE " : "FALSE");
216    printf(" Cavity_On    =  %s    ", globval.Cavity_on ? "TRUE " : "FALSE");
217    printf("  Radiation_On = %s     \n", globval.radiation ? "TRUE " : "FALSE");
218   
219    if(globval.bpm == 0)
220      printf("  bpm          =  0"); 
221    else
222      printf("  bpm          =  %3d", GetnKid(globval.bpm));
223    if(globval.qt == 0)
224      printf("                             qt           = 0           \n"); 
225    else
226      printf("                             qt           = %3d         \n", GetnKid(globval.qt));   
227    if(globval.hcorr == 0)
228      printf("  hcorr         =  0"); 
229    else
230      printf("  hcorr         = %3d", GetnKid(globval.hcorr));
231    if(globval.vcorr == 0)
232      printf("                             vcorr        =  0        \n"); 
233    else
234      printf("                             vcorr        = %3d      \n", GetnKid(globval.vcorr));
235                                           
236    printf(" Chambre_On   = %s     \n", globval.Aperture_on ? "TRUE " : "FALSE");
237 
238    printf("  alphac       =   %8.4e\n", globval.Alphac);
239    printf("  nux          =   %8.6f      nuy  =  %8.6f",
240            globval.TotalTune[X_], globval.TotalTune[Y_]);
241    if (globval.Cavity_on)
242        printf("  omega  = %13.9f\n", globval.Omega);
243    else {
244        printf("\n");
245        printf("  ksix         =    %8.6f      ksiy  =   %8.6f\n",
246                globval.Chrom[X_], globval.Chrom[Y_]);
247    }
248    printf("\n");
249    printf("  OneTurn matrix:\n");
250    printf("\n");
251    prtmat(ss_dim, globval.OneTurnMat);
252
253    printf("\nTwiss parameters at entrance:\n");
254    printf(
255            "   Betax [m] = % 9.3e  Alphax = % 9.3e Etax [m] = % 9.3e Etaxp = % 9.3e\n"
256                "   Betay [m] = % 9.3e  Alphay = % 9.3e Etay [m] = % 9.3e Etayp = % 9.3e\n\n",
257            Cell[1].Beta[X_], Cell[1].Alpha[X_], Cell[1].Eta[X_],
258            Cell[1].Etap[X_], Cell[1].Beta[Y_], Cell[1].Alpha[Y_],
259            Cell[1].Eta[Y_], Cell[1].Etap[Y_]);
260
261    fflush( stdout);
262}
263
264/****************************************************************************/
265/* void printglob2file(void)
266
267 Purpose:
268 Print global variables on screen
269 Print tunes and chromaticities
270 Print Oneturn matrix
271
272 Input:
273 none
274
275 Output:
276 output on the screen
277
278 Return:
279 none
280
281 Global variables:
282 globval
283
284 Specific functions:
285 none
286
287 Comments:
288 03/06/2013  by Jianfeng Zhang @ LAL
289 The same feature as the file printglob(void), but
290 added feature to print the lattice summary to an external file
291
292 
293 ****************************************************************************/
294void printglob2file(const char fic[]) {
295  FILE *fout;
296  int i=0,j=0;
297 
298  fout = fopen(fic,"w");
299 
300    fprintf(fout, "\n***************************************************************"
301        "***************\n");
302    fprintf(fout,"\n");
303    fprintf(fout,"  dPcommon     =  %9.3e  dPparticle   =  %9.3e"
304        "  Energy [GeV] = %.3f\n", globval.dPcommon, globval.dPparticle,
305            globval.Energy);
306    fprintf(fout,"  MaxAmplx [m] = %9.3e   MaxAmply [m] = %9.3e"
307        "   RFAccept [%%] = +/- %4.2f\n", Cell[0].maxampl[X_][1],
308            Cell[0].maxampl[Y_][1], globval.delta_RF * 1e2);
309    fprintf(fout,"  MatMeth      =  %s     ", globval.MatMeth ? "TRUE " : "FALSE");
310    fprintf(fout," Cavity_On    =  %s    ", globval.Cavity_on ? "TRUE " : "FALSE");
311    fprintf(fout,"  Radiation_On = %s     \n", globval.radiation ? "TRUE " : "FALSE");
312   
313    if(globval.bpm == 0)
314      fprintf(fout,"  bpm          =  0"); 
315    else
316      fprintf(fout,"  bpm          =  %3d", GetnKid(globval.bpm));
317    if(globval.qt == 0)
318      fprintf(fout,"                             qt           = 0           \n"); 
319    else
320      fprintf(fout,"                             qt           = %3d         \n", GetnKid(globval.qt));   
321    if(globval.hcorr == 0)
322      fprintf(fout,"  hcorr         =  0"); 
323    else
324      fprintf(fout,"  hcorr         = %3d", GetnKid(globval.hcorr));
325    if(globval.vcorr == 0)
326      fprintf(fout,"                             vcorr        =  0        \n"); 
327    else
328      fprintf(fout,"                             vcorr        = %3d      \n", GetnKid(globval.vcorr));
329                                           
330    fprintf(fout," Chambre_On   = %s     \n", globval.Aperture_on ? "TRUE " : "FALSE");
331 
332    fprintf(fout,"  alphac       =   %8.4e\n", globval.Alphac);
333    fprintf(fout,"  nux          =   %8.6f      nuy  =  %8.6f",
334            globval.TotalTune[X_], globval.TotalTune[Y_]);
335    if (globval.Cavity_on)
336        fprintf(fout,"  omega  = %13.9f\n", globval.Omega);
337    else {
338        fprintf(fout,"\n");
339        fprintf(fout,"  ksix         =    %8.6f      ksiy  =   %8.6f\n",
340                globval.Chrom[X_], globval.Chrom[Y_]);
341    }
342    fprintf(fout,"\n");
343   
344    fprintf(fout,"  OneTurn matrix:\n");
345    for(i=0;i<ss_dim;i++){
346      fprintf(fout,"\n");
347      for(j=0;j<ss_dim;j++)
348        fprintf(fout,"%14.6e",globval.OneTurnMat[i][j]);
349    }
350
351    fprintf(fout,"\n\nTwiss parameters at entrance:\n");
352    fprintf(fout,
353            "   Betax [m] = % 9.3e  Alphax = % 9.3e Etax [m] = % 9.3e Etaxp = % 9.3e\n"
354                "   Betay [m] = % 9.3e  Alphay = % 9.3e Etay [m] = % 9.3e Etayp = % 9.3e\n\n",
355            Cell[1].Beta[X_], Cell[1].Alpha[X_], Cell[1].Eta[X_],
356            Cell[1].Etap[X_], Cell[1].Beta[Y_], Cell[1].Alpha[Y_],
357            Cell[1].Eta[Y_], Cell[1].Etap[Y_]);
358
359    fclose(fout);
360}
361
362/****************************************************************************/
363/* void printlatt(const char fic[])
364
365 Purpose:
366 Print twiss parameters into file linlat.out
367 name, position, alpha, beta, phase, dispersion and  its derivative
368
369 Input:
370 none
371
372 Output:
373 none
374
375 Return:
376 none
377
378 Global variables:
379 globval
380
381 Specific functions:
382 getelem
383
384 Comments:
385 28/04/03 outfile end with .out extension instead of .dat
386 Twiss parameters are computed at the end of elements
387
388 ****************************************************************************/
389//void printlatt(void)
390void printlatt(const char fic[]) {
391    long int i = 0;
392    FILE *outf;
393    struct tm *newtime;
394
395    /* Get time and date */
396    newtime = GetTime();
397   
398    if ((outf = fopen(fic, "w")) == NULL) {
399        fprintf(stdout, "printlatt: Error while opening file %s \n", fic);
400        exit(1);
401    }
402
403    fprintf(outf, "# TRACY III  -- %s -- %s \n", fic,
404            asctime2(newtime));
405    fprintf(
406            outf,
407            "#       name           s      alphax  betax   nux   etax   etapx  alphay  betay   nuy     etay        etapy\n");
408    fprintf(
409            outf,
410            "#                     [m]              [m]           [m]                   [m]             [m]\n");
411    fprintf(outf, "#                     exit        \n");
412
413    for (i = 1L; i <= globval.Cell_nLoc; i++) {
414        fprintf(
415                outf,
416                "%4ld:%.*s% 8.4f% 8.3f% 7.3f% 7.3f% 7.3f% 7.3f% 8.3f% 7.3f% 7.3f% 12.3e% 12.3e\n",
417                i, SymbolLength, Cell[i].Elem.PName, Cell[i].S,
418                Cell[i].Alpha[X_], Cell[i].Beta[X_], Cell[i].Nu[X_],
419                Cell[i].Eta[X_], Cell[i].Etap[X_], Cell[i].Alpha[Y_],
420                Cell[i].Beta[Y_], Cell[i].Nu[Y_], Cell[i].Eta[Y_],
421                Cell[i].Etap[Y_]);
422    }
423    fclose(outf);
424}
425/*******************************************************************************
426 *
427 *
428 *
429 *
430 ******************************************************************************/
431double int_curly_H1(long int n) {
432    /* Integration with Simpson's Rule */
433
434    double curly_H;
435    Vector2 alpha[3], beta[3], nu[3], eta[3], etap[3];
436
437    // only works for matrix style calculations
438    get_twiss3(n, alpha, beta, nu, eta, etap);
439
440    curly_H = (get_curly_H(alpha[0][X_], beta[0][X_], eta[0][X_], etap[0][X_])
441            + 4.0 * get_curly_H(alpha[1][X_], beta[1][X_], eta[1][X_],
442                    etap[1][X_]) + get_curly_H(alpha[2][X_], beta[2][X_],
443            eta[2][X_], etap[2][X_])) / 6.0;
444
445    return curly_H;
446}
447/*******************************************************************************
448 *
449 *
450 *
451 *
452 ******************************************************************************/
453void prt_sigma(void) {
454    long int i;
455    double code = 0.0;
456    FILE *outf;
457
458    outf = file_write("../out/sigma.out");
459
460    fprintf(outf, "#  name     s   sqrt(sx)   sqrt(sx')  sqrt(sy)  sqrt(sy')\n");
461    fprintf(outf, "#           [m]   [mm]       [mrad]     [mm]     [mrad]\n");
462    fprintf(outf, "#\n");
463
464    for (i = 0; i <= globval.Cell_nLoc; i++) {
465        switch (Cell[i].Elem.Pkind) {
466        case drift:
467            code = 0.0;
468            break;
469        case Mpole:
470            if (Cell[i].Elem.M->Pirho != 0)
471                code = 0.5;
472            else if (Cell[i].Elem.M->PBpar[Quad + HOMmax] != 0)
473                code = sgn(Cell[i].Elem.M->PBpar[Quad + HOMmax]);
474            else if (Cell[i].Elem.M->PBpar[Sext + HOMmax] != 0)
475                code = 1.5 * sgn(Cell[i].Elem.M->PBpar[Sext + HOMmax]);
476            else if (Cell[i].Fnum == globval.bpm)
477                code = 2.0;
478            else
479                code = 0.0;
480            break;
481        default:
482            code = 0.0;
483            break;
484        }
485        fprintf(outf, "%4ld %.*s %6.2f %4.1f %9.3e %9.3e %9.3e %9.3e\n", i,
486                SymbolLength, Cell[i].Elem.PName, Cell[i].S, code, 1e3 * sqrt(
487                        Cell[i].sigma[x_][x_]), 1e3 * sqrt(fabs(
488                        Cell[i].sigma[x_][px_])), 1e3 * sqrt(
489                        Cell[i].sigma[y_][y_]), 1e3 * sqrt(fabs(
490                        Cell[i].sigma[y_][py_])));
491    }
492
493    fclose(outf);
494}
495
496void recalc_S(void) {
497    long int k;
498    double S_tot;
499
500    S_tot = 0.0;
501    for (k = 0; k <= globval.Cell_nLoc; k++) {
502        S_tot += Cell[k].Elem.PL;
503        Cell[k].S = S_tot;
504    }
505}
506/**************************************************************************
507getcod(double dP, long &lastpos)
508
509   Purpose:
510       Find the closed orbit for the particle with energy offset dP
511       
512   Input:
513       imax number of iteration for cod search
514       dP   particle energy offset
515       eps  accuracy for cod search
516       
517   Output:
518   
519   Return:
520   
521   
522   Comments:
523     
524***************************************************************************/       
525bool getcod(double dP, long &lastpos) {
526    return GetCOD(globval.CODimax, globval.CODeps, dP, lastpos);
527}
528
529void get_twiss3(long int loc, Vector2 alpha[], Vector2 beta[], Vector2 nu[],
530        Vector2 eta[], Vector2 etap[]) {
531    /* Get Twiss functions at magnet entrance-, center-, and exit. */
532
533    long int j, k;
534    Vector2 dnu;
535    Matrix Ascr0, Ascr1;
536
537    // Lattice functions at the magnet entrance
538    for (k = 0; k <= 1; k++) {
539        alpha[0][k] = Cell[loc - 1].Alpha[k];
540        beta[0][k] = Cell[loc - 1].Beta[k];
541        nu[0][k] = Cell[loc - 1].Nu[k];
542        eta[0][k] = Cell[loc - 1].Eta[k];
543        etap[0][k] = Cell[loc - 1].Etap[k];
544    }
545
546    UnitMat(5, Ascr0);
547    for (k = 0; k <= 1; k++) {
548        Ascr0[2 * k][2 * k] = sqrt(Cell[loc - 1].Beta[k]);
549        Ascr0[2 * k][2 * k + 1] = 0.0;
550        Ascr0[2 * k + 1][2 * k] = -Cell[loc - 1].Alpha[k] / Ascr0[2 * k][2 * k];
551        Ascr0[2 * k + 1][2 * k + 1] = 1 / Ascr0[2 * k][2 * k];
552    }
553    Ascr0[0][4] = eta[0][X_];
554    Ascr0[1][4] = etap[0][X_];
555    Ascr0[2][4] = eta[1][Y_];
556    Ascr0[3][4] = etap[1][Y_];
557    CopyMat(5, Ascr0, Ascr1);
558    MulLMat(5, Cell[loc].Elem.M->AU55, Ascr1);
559    dnu[0]
560            = (atan(Ascr1[0][1] / Ascr1[0][0])
561                    - atan(Ascr0[0][1] / Ascr0[0][0])) / (2.0 * M_PI);
562    dnu[1]
563            = (atan(Ascr1[2][3] / Ascr1[2][2])
564                    - atan(Ascr0[2][3] / Ascr0[2][2])) / (2.0 * M_PI);
565
566    // Lattice functions at the magnet center
567    for (k = 0; k <= 1; k++) {
568        alpha[1][k] = -Ascr1[2 * k][2 * k] * Ascr1[2 * k + 1][2 * k] - Ascr1[2
569                * k][2 * k + 1] * Ascr1[2 * k + 1][2 * k + 1];
570        beta[1][k] = pow(Ascr1[2 * k][2 * k], 2.0) + pow(
571                Ascr1[2 * k][2 * k + 1], 2);
572        nu[1][k] = nu[0][k] + dnu[k];
573        j = 2 * k + 1;
574        eta[1][k] = Ascr1[j - 1][4];
575        etap[1][k] = Ascr1[j][4];
576    }
577
578    CopyMat(5, Ascr1, Ascr0);
579    MulLMat(5, Cell[loc].Elem.M->AD55, Ascr1);
580    dnu[0]
581            = (atan(Ascr1[0][1] / Ascr1[0][0])
582                    - atan(Ascr0[0][1] / Ascr0[0][0])) / (2.0 * M_PI);
583    dnu[1]
584            = (atan(Ascr1[2][3] / Ascr1[2][2])
585                    - atan(Ascr0[2][3] / Ascr0[2][2])) / (2.0 * M_PI);
586
587    // Lattice functions at the magnet exit
588    for (k = 0; k <= 1; k++) {
589        alpha[2][k] = -Ascr1[2 * k][2 * k] * Ascr1[2 * k + 1][2 * k] - Ascr1[2
590                * k][2 * k + 1] * Ascr1[2 * k + 1][2 * k + 1];
591        beta[2][k] = pow(Ascr1[2 * k][2 * k], 2.0) + pow(
592                Ascr1[2 * k][2 * k + 1], 2);
593        nu[2][k] = nu[1][k] + dnu[k];
594        j = 2 * k + 1;
595        eta[2][k] = Ascr1[j - 1][4];
596        etap[2][k] = Ascr1[j][4];
597    }
598}
599/*******************************************************************************
600 *
601 *
602 *
603 *
604 ******************************************************************************/
605void getabn(Vector2 &alpha, Vector2 &beta, Vector2 &nu) {
606    Vector2 gamma;
607    Cell_GetABGN(globval.OneTurnMat, alpha, beta, gamma, nu);
608}
609/*******************************************************************************
610 *
611 *
612 *
613 *
614 ******************************************************************************/
615void TraceABN(long i0, long i1, const Vector2 &alpha, const Vector2 &beta,
616        const Vector2 &eta, const Vector2 &etap, const double dP) {
617    long i, j;
618    double sb;
619    ss_vect<tps> Ascr;
620
621    UnitMat(6, globval.Ascr);
622    for (i = 1; i <= 2; i++) {
623        sb = sqrt(beta[i - 1]);
624        j = i * 2 - 1;
625        globval.Ascr[j - 1][j - 1] = sb;
626        globval.Ascr[j - 1][j] = 0.0;
627        globval.Ascr[j][j - 1] = -(alpha[i - 1] / sb);
628        globval.Ascr[j][j] = 1 / sb;
629    }
630    globval.Ascr[0][4] = eta[0];
631    globval.Ascr[1][4] = etap[0];
632    globval.Ascr[2][4] = eta[1];
633    globval.Ascr[3][4] = etap[1];
634
635    for (i = 0; i < 6; i++)
636        globval.CODvect[i] = 0.0;
637    globval.CODvect[4] = dP;
638
639    if (globval.MatMeth)
640        Cell_Twiss_M(i0, i1, globval.Ascr, false, false, dP);
641    else {
642        for (i = 0; i <= 5; i++) {
643            Ascr[i] = tps(globval.CODvect[i]);
644            for (j = 0; j <= 5; j++)
645                Ascr[i] += globval.Ascr[i][j] * tps(0.0, j + 1);
646        }
647        Cell_Twiss(i0, i1, Ascr, false, false, dP);
648    }
649
650}
651
652/****************************************************************************/
653/* void FitTune(long qf, long qd, double nux, double nuy)
654
655 Purpose:
656 Fit tunes to the target values using quadrupoles "qf" and "qd"
657 Input:
658 qf : tuned quadrupole
659 qd : tuned quadrupole
660 nux: target horizontal tune
661 nuy: target vertical tune
662 Output:
663 none
664
665 Return:
666 none
667
668 Global variables:
669
670 specific functions:
671
672 Comments:
673
674 ****************************************************************************/
675void FitTune(long qf, long qd, double nux, double nuy) {
676    long i;
677    iVector2 nq = { 0, 0 };
678    Vector2 nu = { 0.0, 0.0 };
679    fitvect qfbuf, qdbuf;
680
681    /* Get elements for the first quadrupole family */
682    nq[X_] = GetnKid(qf);
683    for (i = 1; i <= nq[X_]; i++)
684        qfbuf[i - 1] = Elem_GetPos(qf, i);
685
686    /* Get elements for the second quadrupole family */
687    nq[Y_] = GetnKid(qd);
688    for (i = 1; i <= nq[Y_]; i++)
689        qdbuf[i - 1] = Elem_GetPos(qd, i);
690
691    nu[X_] = nux;
692    nu[Y_] = nuy;
693
694    /* fit tunes */
695    Ring_Fittune(nu, nueps, nq, qfbuf, qdbuf, nudkL, nuimax);
696}
697
698/****************************************************************************/
699/* void FitChrom(long sf, long sd, double ksix, double ksiy)
700
701 Purpose:
702 Fit chromaticities to the target values using sextupoles "sf" and "sd"
703 Input:
704 sf  : tuned sextupole
705 sd  : tuned sextupole
706 ksix: target horizontal chromaticity
707 ksiy: target vertical chromaticity
708 Output:
709 none
710
711 Return:
712 none
713
714 Global variables:
715
716 specific functions:
717
718 Comments:
719
720 ****************************************************************************/
721
722void FitChrom(long sf, long sd, double ksix, double ksiy) {
723    long i;
724    iVector2 ns = { 0, 0 };
725    fitvect sfbuf, sdbuf;
726    Vector2 ksi = { 0.0, 0.0 };
727
728    /* Get elements for the first sextupole family */
729    ns[X_] = GetnKid(sf);
730    for (i = 1; i <= ns[X_]; i++)
731        sfbuf[i - 1] = Elem_GetPos(sf, i);
732
733    /* Get elements for the second sextupole family */
734    ns[Y_] = GetnKid(sd);
735    for (i = 1; i <= ns[Y_]; i++)
736        sdbuf[i - 1] = Elem_GetPos(sd, i);
737
738    ksi[X_] = ksix;
739    ksi[Y_] = ksiy;
740
741    /* Fit chromaticities */
742    /*    Ring_Fitchrom(ksi, ksieps, ns, sfbuf, sdbuf, 1.0, 1);*/
743    Ring_Fitchrom(ksi, ksieps, ns, sfbuf, sdbuf, ksidkpL, ksiimax);
744}
745/*******************************************************************************
746 *
747 *
748 *
749 *
750 ******************************************************************************/
751void FitDisp(long q, long pos, double eta) {
752    long i=0L, nq=0L;
753    fitvect qbuf;
754
755    /* Get elements for the quadrupole family */
756    nq = GetnKid(q);
757    for (i = 1; i <= nq; i++)
758        qbuf[i - 1] = Elem_GetPos(q, i);
759
760    Ring_FitDisp(pos, eta, dispeps, nq, qbuf, dispdkL, dispimax);
761}
762/*******************************************************************************
763 *
764 *
765 *
766 *
767 ******************************************************************************/
768#define nfloq           4
769
770void getfloqs(Vector &x) {
771    // Transform to Floquet space
772    LinTrans(nfloq, globval.Ascrinv, x);
773}
774
775#undef nfloq
776
777/*******************************************************************************
778 *
779 *
780 *
781 *
782 ******************************************************************************/
783#define ntrack          4
784
785// 4D tracking in normal or Floquet space over nmax turns
786
787void track(const char *file_name, double ic1, double ic2, double ic3,
788        double ic4, double dp, long int nmax, long int &lastn,
789        long int &lastpos, int floqs, double f_rf) {
790    /* Single particle tracking around closed orbit:
791
792     Output                floqs
793
794     Phase Space               0     [x, px, y, py, delta, ct]
795     Floquet Space             1     [x^, px^, y^, py^, delta, ct]
796     Action-Angle Variables    2     [2Jx, phx, 2Jy, phiy, delta, ct]
797
798     */
799    long int i=0L;
800    double twoJx=0.0, twoJy=0.0, phix=0.0, phiy=0.0, scl_1 = 1.0, scl_2 = 1.0;
801    Vector x0, x1, x2, xf;
802    FILE *outf;
803
804    bool prt = false;
805
806    if ((floqs == 0)) {
807        scl_1 = 1e3;
808        scl_2 = 1e3;
809        x0[x_] = ic1;
810        x0[px_] = ic2;
811        x0[y_] = ic3;
812        x0[py_] = ic4;
813    } else if ((floqs == 1)) {
814        scl_1 = 1.0;
815        scl_2 = 1.0;
816        x0[x_] = ic1;
817        x0[px_] = ic2;
818        x0[y_] = ic3;
819        x0[py_] = ic4;
820        LinTrans(4, globval.Ascr, x0);
821    } else if (floqs == 2) {
822        scl_1 = 1e6;
823        scl_2 = 1.0;
824        x0[x_] = sqrt(ic1) * cos(ic2);
825        x0[px_] = -sqrt(ic1) * sin(ic2);
826        x0[y_] = sqrt(ic3) * cos(ic4);
827        x0[py_] = -sqrt(ic3) * sin(ic4);
828        LinTrans(4, globval.Ascr, x0);
829    }
830
831    outf = file_write(file_name);
832    fprintf(outf, "# Tracking with TRACY");
833    getcod(dp, lastpos);
834    if (floqs == 0)
835        fprintf(outf, "\n");
836    else if (floqs == 1) {
837        Ring_GetTwiss(false, dp);
838        fprintf(outf, "# (Floquet space)\n");
839    } else if (floqs == 2) {
840        Ring_GetTwiss(false, dp);
841        fprintf(outf, "# (Action-Angle variables)\n");
842    }
843    fprintf(outf, "#\n");
844    fprintf(outf, "#%3d%6ld% .1E% .1E% .1E% .1E% 7.5f% 7.5f\n", 1, nmax, 1e0,
845            1e0, 0e0, 0e0, globval.TotalTune[0], globval.TotalTune[1]);
846    if (floqs == 0) {
847        fprintf(outf,
848                "#    N       x            p_x            y            p_y");
849        fprintf(outf, "          delta          cdt\n");
850        fprintf(outf, "#           [mm]         [mrad]"
851            "         [mm]         [mrad]");
852    } else if (floqs == 1) {
853        fprintf(outf, "#    N     x^          px^          y^          py^");
854        fprintf(outf, "          delta          cdt");
855        fprintf(outf, "#                              "
856            "                            ");
857    } else if (floqs == 2) {
858        fprintf(outf,
859                "#    N     2Jx          phi_x          2Jy          phi_y");
860        fprintf(outf, "          delta          cdt\n");
861        fprintf(outf, "#                              "
862            "                            ");
863    }
864    if (f_rf == 0.0) {
865        fprintf(outf, "         [%%]           [mm]\n");
866        fprintf(outf, "#\n");
867        fprintf(outf, "%4d %23.16e %23.16e %23.16e %23.16e %23.16e %23.16e\n",
868                0, scl_1 * ic1, scl_2 * ic2, scl_1 * ic3, scl_2 * ic4,
869                1e2 * dp, 1e3 * globval.CODvect[ct_]);
870    } else {
871        fprintf(outf, "         [%%]           [deg]\n");
872        fprintf(outf, "#\n");
873        fprintf(outf, "%4d %23.16e %23.16e %23.16e %23.16e %23.16e %23.16e\n",
874                0, scl_1 * ic1, scl_2 * ic2, scl_1 * ic3, scl_2 * ic4,
875                1e2 * dp, 2.0 * f_rf * 180.0 * globval.CODvect[ct_] / c0);
876    }
877    x2[x_] = x0[x_] + globval.CODvect[x_];
878    x2[px_] = x0[px_] + globval.CODvect[px_];
879    x2[y_] = x0[y_] + globval.CODvect[y_];
880    x2[py_] = x0[py_] + globval.CODvect[py_];
881    if (globval.Cavity_on) {
882        x2[delta_] = dp + globval.CODvect[delta_];
883        x2[ct_] = globval.CODvect[ct_];
884    } else {
885        x2[delta_] = dp;
886        x2[ct_] = 0.0;
887    }
888
889    lastn = 0;
890
891    if (prt) {
892        printf("\n");
893        printf("track:\n");
894        printf("%4ld %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", lastn, 1e3
895                * x2[x_], 1e3 * x2[px_], 1e3 * x2[y_], 1e3 * x2[py_], 1e2
896                * x2[delta_], 1e3 * x2[ct_]);
897    }
898
899    if (globval.MatMeth)
900        Cell_Concat(dp);
901
902    do {
903        (lastn)++;
904        for (i = 0; i < nv_; i++)
905            x1[i] = x2[i];
906
907        if (globval.MatMeth) {
908            Cell_fPass(x2, lastpos);
909        } else {
910            Cell_Pass(0, globval.Cell_nLoc, x2, lastpos);
911        }
912
913        for (i = x_; i <= py_; i++)
914            xf[i] = x2[i] - globval.CODvect[i];
915
916        for (i = delta_; i <= ct_; i++)
917            if (globval.Cavity_on && (i != ct_))
918                xf[i] = x2[i] - globval.CODvect[i];
919            else
920                xf[i] = x2[i];
921
922        if (floqs == 1)
923            getfloqs(xf);
924        else if (floqs == 2) {
925            getfloqs(xf);
926            twoJx = pow(xf[x_], 2) + pow(xf[px_], 2);
927            twoJy = pow(xf[y_], 2) + pow(xf[py_], 2);
928            phix = atan2(xf[px_], xf[x_]);
929            phiy = atan2(xf[py_], xf[y_]);
930            xf[x_] = twoJx;
931            xf[px_] = phix;
932            xf[y_] = twoJy;
933            xf[py_] = phiy;
934        }
935        if (f_rf == 0.0)
936            fprintf(
937                    outf,
938                    "%4ld %23.16le %23.16le %23.16le %23.16le %23.16le %23.16le\n",
939                    lastn, scl_1 * xf[0], scl_2 * xf[1], scl_1 * xf[2], scl_2
940                            * xf[3], 1e2 * xf[4], 1e3 * xf[5]);
941        else
942            fprintf(
943                    outf,
944                    "%4ld %23.16le %23.16le %23.16le %23.16le %23.16le %23.16le\n",
945                    lastn, scl_1 * xf[0], scl_2 * xf[1], scl_1 * xf[2], scl_2
946                            * xf[3], 1e2 * xf[4], 2.0 * f_rf * 180.0 * xf[5]
947                            / c0);
948    } while ((lastn != nmax) && (lastpos == globval.Cell_nLoc));
949
950    if (globval.MatMeth)
951        Cell_Pass(0, globval.Cell_nLoc, x1, lastpos);
952
953    fclose(outf);
954}
955
956#undef ntrack
957
958/*******************************************************************************
959 *
960 *
961 *
962 *
963 ******************************************************************************/
964#define step            0.1
965#define px              0.0
966#define py              0.0
967void track_(double r, struct LOC_getdynap *LINK) {
968    long i=0L, lastn=0L, lastpos=0L;
969    Vector x;
970
971    x[0] = r * cos(LINK->phi);
972    x[1] = px;
973    x[2] = r * sin(LINK->phi);
974    x[3] = py;
975    x[4] = LINK->delta;
976    x[5] = 0.0;
977    /* transform to phase space */
978    if (LINK->floqs) {
979        LinTrans(5, globval.Ascr, x);
980    }
981    for (i = 0; i <= 3; i++)
982        x[i] += globval.CODvect[i];
983    lastn = 0;
984    do {
985        lastn++;
986        if (globval.MatMeth) {
987            Cell_fPass(x, lastpos);
988        } else {
989            Cell_Pass(0, globval.Cell_nLoc, x, lastpos);
990        }
991    } while (lastn != LINK->nturn && lastpos == globval.Cell_nLoc);
992    LINK->lost = (lastn != LINK->nturn);
993}
994#undef step
995#undef px
996#undef py
997
998/****************************************************************************/
999/* void Trac(double x, double px, double y, double py, double dp, double ctau,
1000 long nmax, long pos, long &lastn, long &lastpos, FILE *outf1)
1001
1002 Purpose:
1003 Single particle tracking w/ respect to the chamber centrum
1004 Based on the version of tracy 2 in soleillib.c
1005 Input:
1006 x, px, y, py 4 transverses coordinates
1007 ctau         c*tau
1008 dp           energy offset
1009 nmax         number of turns
1010 pos          starting position for tracking
1011 aperture     global physical aperture
1012
1013 Output:
1014 lastn       last n (should be nmax if  not lost)
1015 lastpos     last position in the ring
1016 outf1       output file with 6D phase at different pos
1017
1018 Return:
1019 lastn:  last tracking turn that particle is not lost
1020 lastpos:   last element position that particle is not lost
1021
1022 Global variables:
1023 globval
1024
1025 specific functions:
1026 Cell_Pass
1027
1028 Comments:
1029 Absolute TRACKING with respect to the center of the vacuum vessel
1030 BUG: last printout is wrong because not at pos but at the end of
1031 the ring
1032 26/04/03 print output for phase space is for position pos now
1033 01/12/03 tracking from 0 to pos -1L instead of 0 to pos
1034 (wrong observation point)
1035
1036 23/07/10 change the call variable in Cell_Pass( ): pos-1L --> pos (L704, L717).
1037 since the Cell_Pass( ) is tracking from element i0 to i1(tracy 3), and
1038 the Cell_Pass( ) is tracking from element i0+1L to i1(tracy 2).
1039
1040 ****************************************************************************/
1041void Trac(double x, double px, double y, double py, double dp, double ctau,
1042        long nmax, long pos, long &lastn, long &lastpos, FILE *outf1) {
1043 
1044    bool lostF = true; /* Lost particle Flag */
1045    Vector x1; /* tracking coordinates */
1046    Vector2 aperture;
1047
1048    /* Compute closed orbit: useful if insertion devices */
1049
1050    aperture[0] = 1e0;
1051    aperture[1] = 1e0;
1052
1053    x1[0] = x;
1054    x1[1] = px;
1055    x1[2] = y;
1056    x1[3] = py;
1057    x1[4] = dp;
1058    x1[5] = ctau;
1059
1060    lastn = 0L;
1061    (lastpos) = pos;
1062
1063    if (trace)
1064        fprintf(outf1, "\n");
1065    if (trace)
1066        fprintf(outf1,
1067                "%6ld %+10.5e %+10.5e %+10.5e %+10.5e %+10.5e %+10.5e \n",
1068                lastn, x1[0], x1[1], x1[2], x1[3], x1[4], x1[5]);
1069
1070    //  Cell_Pass(pos -1L, globval.Cell_nLoc, x1, lastpos);
1071    Cell_Pass(pos, globval.Cell_nLoc, x1, lastpos);
1072
1073    if (lastpos == globval.Cell_nLoc)
1074        do {
1075            (lastn)++; /* 3) continue tracking for nmax turns*/
1076            Cell_Pass(0L, pos - 1L, x1, lastpos); /* 1) check whether particle is stable at element from 0 to pos-1L*/
1077
1078            if (trace)
1079                fprintf(
1080                        outf1,
1081                        "%6ld %+10.5e %+10.5e %+10.5e %+10.5e %+10.5e %+10.5e \n",
1082                        lastn, x1[0], x1[1], x1[2], x1[3], x1[4], x1[5]);
1083
1084            if (lastpos == pos - 1L)
1085                Cell_Pass(pos, globval.Cell_nLoc, x1, lastpos); /* 2) check particle is stable at element from pos to end*/
1086            //   Cell_Pass(pos-1L,globval.Cell_nLoc, x1, lastpos);
1087
1088        } while (((lastn) < nmax) && ((lastpos) == globval.Cell_nLoc));
1089
1090    if (lastpos == globval.Cell_nLoc)
1091        Cell_Pass(0L, pos, x1, lastpos);
1092
1093    if (lastpos != pos) {
1094        printf("Trac: Particle lost \n");
1095        fprintf(stdout, "turn:%6ld plane: %1d"
1096            " %+10.5g %+10.5g %+10.5g %+10.5g %+10.5g %+10.5g \n", lastn,
1097                status.lossplane, x1[0], x1[1], x1[2], x1[3], x1[4], x1[5]);
1098    }
1099}
1100
1101/****************************************************************************/
1102/*bool chk_if_lost(double x0, double y0, double delta,
1103 long int nturn, bool floqs)
1104
1105 Purpose:
1106 Binary search for dynamical aperture in Floquet space.
1107
1108 Input:
1109 none
1110
1111 Output:
1112 none
1113
1114 Return:
1115 none
1116
1117 Global variables:
1118 px_0, py_0
1119
1120 Specific functions:
1121 chk_if_lost
1122
1123 Comments:
1124 none
1125
1126 ****************************************************************************/
1127
1128#define nfloq     4
1129bool chk_if_lost(double x0, double y0, double delta, long int nturn, bool floqs) {
1130 
1131    long int i=0L, lastn=0L, lastpos=0L;
1132    Vector x;
1133
1134    bool prt = false;
1135
1136    x[x_] = x0;
1137    x[px_] = px_0;
1138    x[y_] = y0;
1139    x[py_] = py_0;
1140    x[delta_] = delta;
1141    x[ct_] = 0.0;
1142    if (floqs)
1143        // transform to phase space
1144        LinTrans(nfloq, globval.Ascr, x);
1145    for (i = 0; i <= 3; i++)
1146        x[i] += globval.CODvect[i];
1147
1148    lastn = 0;
1149    if (prt) {
1150        printf("\n");
1151        printf("chk_if_lost:\n");
1152        printf("%4ld %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", lastn,
1153                1e3 * x[x_], 1e3 * x[px_], 1e3 * x[y_], 1e3 * x[py_], 1e2
1154                        * x[delta_], 1e3 * x[ct_]);
1155    }
1156    do {
1157        lastn++;
1158        if (globval.MatMeth)
1159            Cell_fPass(x, lastpos);
1160        else
1161            Cell_Pass(0, globval.Cell_nLoc, x, lastpos);
1162        if (prt)
1163            printf("%4ld %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f\n", lastn, 1e3
1164                    * x[x_], 1e3 * x[px_], 1e3 * x[y_], 1e3 * x[py_], 1e2
1165                    * x[delta_], 1e3 * x[ct_]);
1166    } while ((lastn != nturn) && (lastpos == globval.Cell_nLoc));
1167    return (lastn != nturn);
1168}
1169#undef nfloq
1170
1171/****************************************************************************/
1172/* void getdynap(double *r, double phi, double delta, double eps,
1173 int nturn, bool floqs)
1174
1175 Purpose:
1176 Binary search for dynamical aperture in Floquet space.
1177
1178
1179 Input:
1180 none
1181
1182 Output:
1183 none
1184
1185 Return:
1186 none
1187
1188 Global variables:
1189 none
1190
1191 Specific functions:
1192 chk_if_lost
1193
1194 Comments:
1195 none
1196
1197 ****************************************************************************/
1198void getdynap(double &r, double phi, double delta, double eps, int nturn,
1199        bool floqs) {
1200    /* Determine dynamical aperture by binary search. */
1201    double rmin = 0.0, rmax = r;
1202
1203    const bool prt = false;
1204    const double r_reset = 1e-3, r0 = 10e-3;
1205
1206    if (prt)
1207        printf("\n");
1208
1209    if (globval.MatMeth)
1210        Cell_Concat(delta);
1211    while (!chk_if_lost(rmax * cos(phi), rmax * sin(phi), delta, nturn, floqs)) {
1212        if (rmax < r_reset)
1213            rmax = r0;
1214        rmax *= 2.0;
1215    }
1216    while (rmax - rmin >= eps) {
1217        r = rmin + (rmax - rmin) / 2.0;
1218        if (prt)
1219            printf("getdynap: %6.3f %6.3f %6.3f\n", 1e3 * rmin, 1e3 * rmax, 1e3
1220                    * r);
1221        if (!chk_if_lost(r * cos(phi), r * sin(phi), delta, nturn, floqs))
1222            rmin = r;
1223        else
1224            rmax = r;
1225        if (rmin > rmax) {
1226            printf("getdynap: rmin > rmax\n");
1227            exit_(0);
1228        }
1229
1230    }
1231    r = rmin;
1232}
1233
1234/****************************************************************************/
1235/* void getcsAscr(void)
1236
1237 Purpose:
1238 Get Courant-Snyder Ascr
1239
1240
1241 Input:
1242 none
1243
1244 Output:
1245 none
1246
1247 Return:
1248 none
1249
1250 Global variables:
1251 none
1252
1253 Specific functions:
1254 none
1255
1256 Comments:
1257 none
1258
1259 ****************************************************************************/
1260void getcsAscr(void) {
1261    long i=0L, j=0L;
1262    double phi=0.0;
1263    Matrix R;
1264
1265    UnitMat(6, R);
1266    for (i = 1; i <= 2; i++) {
1267        phi = -atan2(globval.Ascr[i * 2 - 2][i * 2 - 1],
1268                globval.Ascr[i * 2 - 2][i * 2 - 2]);
1269        R[i * 2 - 2][i * 2 - 2] = cos(phi);
1270        R[i * 2 - 1][i * 2 - 1] = R[i * 2 - 2][i * 2 - 2];
1271        R[i * 2 - 2][i * 2 - 1] = sin(phi);
1272        R[i * 2 - 1][i * 2 - 2] = -R[i * 2 - 2][i * 2 - 1];
1273    }
1274    MulRMat(6, globval.Ascr, R);
1275    for (i = 1; i <= 2; i++) {
1276        if (globval.Ascr[i * 2 - 2][i * 2 - 2] < 0.0) {
1277            for (j = 0; j <= 5; j++) {
1278                globval.Ascr[j][i * 2 - 2] = -globval.Ascr[j][i * 2 - 2];
1279                globval.Ascr[j][i * 2 - 1] = -globval.Ascr[j][i * 2 - 1];
1280            }
1281        }
1282    }
1283    if (!InvMat(6, globval.Ascrinv))
1284        printf("  *** Ascr is singular\n");
1285}
1286
1287/****************************************************************************
1288 void dynap(FILE *fp, double r, const double delta, const double eps,
1289        const int npoint, const int nturn, double x[], double y[],
1290        const bool floqs, const bool print)
1291
1292 Purpose:
1293 Determine the dynamical aperture by tracking using polar coordinates,
1294 and sampling in phase.
1295 Assumes mid-plane symmetry
1296 Write the dynamic aperture to file "fp"
1297 
1298 Input:
1299 r initial guess
1300 delta     off momentum energy
1301 eps       precision for binary search
1302 npoint    sample number for phase coordinate
1303 nturn     number of turn for computing da
1304 x[]       horizontal dynamics aperture
1305 y[]       vertical dynamics aperture
1306 floqs     true means Floquet space
1307 print     true means Print out to the screen
1308
1309 Output:
1310
1311
1312 Return:
1313 none
1314
1315 Global variables:
1316 none
1317
1318 Specific functions:
1319 getdynap
1320
1321 Comments:
1322 none
1323
1324 ****************************************************************************/
1325void dynap(FILE *fp, double r, const double delta, const double eps,
1326        const int npoint, const int nturn, double x[], double y[],
1327        const bool floqs, const bool print)
1328
1329{
1330    /* Determine the dynamical aperture by tracking.
1331     Assumes mid-plane symmetry.                    */
1332
1333    long int i=0L, lastpos=0L;
1334    double phi=0.0, x_min=0.0, x_max=0.0, y_min=0.0, y_max=0.0;
1335
1336    getcod(delta, lastpos);
1337    if (floqs) {
1338        Ring_GetTwiss(false, delta);
1339        if (print) {
1340            printf("\n");
1341            printf("Dynamical Aperture (Floquet space):\n");
1342            printf("     x^         y^\n");
1343            printf("\n");
1344        }
1345        fprintf(fp, "# Dynamical Aperture (Floquet space):\n");
1346        fprintf(fp, "#      x^         y^\n");
1347        fprintf(fp, "#\n");
1348    } else {
1349        if (print) {
1350            printf("\n");
1351            printf("Dynamical Aperture:\n");
1352            printf("     x      y\n");
1353            printf("    [mm]   [mm]\n");
1354            printf("\n");
1355        }
1356        fprintf(fp, "# Dynamical Aperture:\n");
1357        fprintf(fp, "#    x      y\n");
1358        fprintf(fp, "#   [mm]   [mm]\n");
1359        fprintf(fp, "#\n");
1360    }
1361
1362    x_min = 0.0;
1363    x_max = 0.0;
1364    y_min = 0.0;
1365    y_max = 0.0;
1366    for (i = 0; i < npoint; i++) {
1367        phi = i * M_PI / (npoint - 1);
1368        if (i == 0)
1369            phi = 1e-3;
1370        else if (i == npoint - 1)
1371            phi -= 1e-3;
1372        getdynap(r, phi, delta, eps, nturn, floqs);
1373        x[i] = r * cos(phi);
1374        y[i] = r * sin(phi);
1375        x_min = min(x[i], x_min);
1376        x_max = max(x[i], x_max);
1377        y_min = min(y[i], y_min);
1378        y_max = max(y[i], y_max);
1379        if (!floqs) {
1380            if (print)
1381                printf("  %6.2f %6.2f\n", 1e3 * x[i], 1e3 * y[i]);
1382            fprintf(fp, "  %6.2f %6.2f\n", 1e3 * x[i], 1e3 * y[i]);
1383        } else {
1384            if (print)
1385                printf("  %10.3e %10.3e\n", x[i], y[i]);
1386            fprintf(fp, "  %10.3e %10.3e\n", x[i], y[i]);
1387        }
1388        fflush(fp);
1389    }
1390
1391    if (print) {
1392        printf("\n");
1393        printf("  x^: %6.2f - %5.2f y^: %6.2f - %5.2f mm\n", 1e3 * x_min, 1e3
1394                * x_max, 1e3 * y_min, 1e3 * y_max);
1395    }
1396}
1397
1398/****************************************************************************/
1399/* double get_aper(int n, double x[], double y[])
1400
1401 Purpose:
1402
1403
1404 Input:
1405 none
1406
1407 Output:
1408 none
1409
1410 Return:
1411 none
1412
1413 Global variables:
1414 none
1415
1416 Specific functions:
1417 none
1418
1419 Comments:
1420 none
1421
1422 ****************************************************************************/
1423double get_aper(int n, double x[], double y[]) {
1424    int i=0;
1425    double A=0.0;
1426
1427    A = 0.0;
1428    for (i = 2; i <= n; i++)
1429        A += x[i - 2] * y[i - 1] - x[i - 1] * y[i - 2];
1430    A += x[n - 1] * y[0] - x[0] * y[n - 1];
1431    // x2 from mid-plane symmetry
1432    A = fabs(A);
1433    //  printf("\n");
1434    //  printf("  Dyn. Aper.: %5.1f mm^2\n", 1e6*A);
1435    return (A);
1436}
1437
1438/**************************************************************************
1439 * void GetTrack(const char *file_name, long *n, double x[], double px[],
1440        double y[], double py[])
1441 *
1442 *
1443 *
1444 **************************************************************************/
1445void GetTrack(const char *file_name, long *n, double x[], double px[],
1446        double y[], double py[]) {
1447    int k=0;
1448    char line[200];
1449    FILE *inf;
1450
1451    inf = file_read(file_name);
1452
1453    do {
1454        fgets(line, 200, inf);
1455    } while (strstr(line, "#") != NULL);
1456
1457    // skip initial conditions
1458    fgets(line, 200, inf);
1459
1460    do {
1461        sscanf(line, "%d", &k);
1462        sscanf(line, "%*d %lf %lf %lf %lf", &x[k - 1], &px[k - 1], &y[k - 1],
1463                &py[k - 1]);
1464    } while (fgets(line, 200, inf) != NULL);
1465
1466    *n = k;
1467
1468    fclose(inf);
1469}
1470
1471/****************************************************************************/
1472/* void Getj(long n, double *x, double *px, double *y, double *py)
1473
1474 Purpose:
1475 Calculates the linear invariant
1476
1477 Input:
1478 none
1479
1480 Output:
1481 none
1482
1483 Return:
1484 none
1485
1486 Global variables:
1487 none
1488
1489 Specific functions:
1490 none
1491
1492 Comments:
1493 none
1494
1495 ****************************************************************************/
1496void Getj(long n, double *x, double *px, double *y, double *py) {
1497    long int i=0L;
1498
1499    for (i = 0; i < n; i++) {
1500        x[i] = (pow(x[i], 2) + pow(px[i], 2)) / 2.0;
1501        y[i] = (pow(y[i], 2) + pow(py[i], 2)) / 2.0;
1502    }
1503}
1504
1505/****************************************************************************/
1506/* double GetArg(double x, double px, double nu)
1507
1508 Purpose:
1509 get argument of x
1510
1511 Input:
1512 none
1513
1514 Output:
1515 none
1516
1517 Return:
1518 none
1519
1520 Global variables:
1521 none
1522
1523 Specific functions:
1524 none
1525
1526 Comments:
1527 17/07/03 use M_PI instead of pi
1528
1529 ****************************************************************************/
1530double GetArg(double x, double px, double nu) {
1531   
1532  double phi=0.0, val=0.0;
1533
1534    phi = GetAngle(x, px);
1535    if (phi < 0.0)
1536        phi += 2.0 * M_PI;
1537    val = phi + Fract(nu) * 2.0 * M_PI;
1538    if (val < 0.0)
1539        val += 2.0 * M_PI;
1540    return val;
1541}
1542
1543/****************************************************************************/
1544/* void GetPhi(long n, double *x, double *px, double *y, double *py)
1545
1546 Purpose:
1547 get linear phases
1548
1549 Input:
1550 none
1551
1552 Output:
1553 none
1554
1555 Return:
1556 none
1557
1558 Global variables:
1559 none
1560
1561 Specific functions:
1562 none
1563
1564 Comments:
1565 none
1566
1567 ****************************************************************************/
1568void GetPhi(long n, double *x, double *px, double *y, double *py) {
1569    /* Calculates the linear phase */
1570    long i=0L;
1571
1572    for (i = 1; i <= n; i++) {
1573        x[i - 1] = GetArg(x[i - 1], px[i - 1], i * globval.TotalTune[0]);
1574        y[i - 1] = GetArg(y[i - 1], py[i - 1], i * globval.TotalTune[1]);
1575    }
1576}
1577
1578/*********************************/
1579/* Routines for Fourier analysis */
1580/*********************************/
1581
1582void Sinfft(int n, double xr[]) {
1583    /* DFT with sine window */
1584    int i=0;
1585    double xi[n];
1586
1587    for (i = 0; i < n; i++) {
1588        xr[i] = sin((double) i / (double) n * M_PI) * xr[i];
1589        xi[i] = 0.0;
1590    }
1591    FFT(n, xr, xi);
1592    for (i = 0; i < n; i++)
1593        xr[i] = sqrt(xr[i] * xr[i] + xi[i] * xi[i]);
1594}
1595
1596void sin_FFT(int n, double xr[]) {
1597    /* DFT with sine window */
1598    int i;
1599    double *xi;
1600
1601    xi = dvector(1, 2 * n);
1602
1603    for (i = 1; i <= n; i++) {
1604        xi[2 * i - 1] = sin((double) i / n * M_PI) * xr[i - 1];
1605        xi[2 * i] = 0.0;
1606    }
1607    dfour1(xi, (unsigned long) n, 1);
1608    for (i = 1; i <= n; i++)
1609        xr[i - 1] = sqrt(pow(xi[2 * i - 1], 2) + pow(xi[2 * i], 2)) * 2.0 / n;
1610
1611    free_dvector(xi, 1, 2 * n);
1612}
1613/*******************************************************************************
1614 *
1615 *
1616 *
1617 *
1618 ******************************************************************************/
1619void sin_FFT(int n, double xr[], double xi[]) {
1620    /* DFT with sine window */
1621    int i=0;
1622    double *xri;
1623
1624    xri = dvector(1, 2 * n);
1625
1626    for (i = 1; i <= n; i++) {
1627        xri[2 * i - 1] = sin((double) i / n * M_PI) * xr[i - 1];
1628        xri[2 * i] = sin((double) i / n * M_PI) * xi[i - 1];
1629    }
1630    dfour1(xri, (unsigned long) n, 1);
1631    for (i = 1; i <= n; i++) {
1632        xr[i - 1] = sqrt(pow(xri[2 * i - 1], 2) + pow(xri[2 * i], 2)) * 2.0 / n;
1633        xi[i - 1] = atan2(xri[2 * i], xri[2 * i - 1]);
1634    }
1635
1636    free_dvector(xri, 1, 2 * n);
1637}
1638/*******************************************************************************
1639 *
1640 *
1641 *
1642 *
1643 ******************************************************************************/
1644void GetInd(int n, int k, int *ind1, int *ind3) {
1645    if (k == 1) {
1646        *ind1 = 2;
1647        *ind3 = 2;
1648    } else if (k == n / 2 + 1) {
1649        *ind1 = n / 2;
1650        *ind3 = n / 2;
1651    } else {
1652        *ind1 = k - 1;
1653        *ind3 = k + 1;
1654    }
1655}
1656/*******************************************************************************
1657 *
1658 *
1659 *
1660 *
1661 ******************************************************************************/
1662void GetInd1(int n, int k, int *ind1, int *ind3) {
1663    if (k == 1) {
1664        *ind1 = 2;
1665        *ind3 = 2;
1666    } else if (k == n) {
1667        *ind1 = n - 1;
1668        *ind3 = n - 1;
1669    } else {
1670        *ind1 = k - 1;
1671        *ind3 = k + 1;
1672    }
1673}
1674/*******************************************************************************
1675 *
1676 *
1677 *
1678 *
1679 ******************************************************************************/
1680void GetPeak(int n, double *x, int *k) {
1681    /* Locate peak in DFT spectrum */
1682    int ind1=0, ind2=0, ind3=0;
1683    double peak=0.0;
1684
1685    *k = 1;
1686    for (ind2 = 1; ind2 <= n / 2 + 1; ind2++) {
1687        GetInd(n, ind2, &ind1, &ind3);
1688        if (x[ind2 - 1] > peak && x[ind1 - 1] < x[ind2 - 1] && x[ind3 - 1]
1689                < x[ind2 - 1]) {
1690            peak = x[ind2 - 1];
1691            *k = ind2;
1692        }
1693    }
1694}
1695/*******************************************************************************
1696 *
1697 *
1698 *
1699 *
1700 ******************************************************************************/
1701void GetPeak1(int n, double *x, int *k) {
1702    /* Locate peak in DFT spectrum */
1703    int ind1=0, ind2=0, ind3=0;
1704    double peak=0.0;
1705
1706    *k = 1;
1707    for (ind2 = 1; ind2 <= n; ind2++) {
1708        GetInd1(n, ind2, &ind1, &ind3);
1709        if (x[ind2 - 1] > peak && x[ind1 - 1] < x[ind2 - 1] && x[ind3 - 1]
1710                < x[ind2 - 1]) {
1711            peak = x[ind2 - 1];
1712            *k = ind2;
1713        }
1714    }
1715}
1716/*******************************************************************************
1717 *
1718 *
1719 *
1720 *
1721 ******************************************************************************/
1722double Int2snu(int n, double *x, int k) {
1723    /* Get frequency by nonlinear interpolation with two samples
1724     for sine window. The interpolation is:
1725
1726     1              2 A(k)       1
1727     nu = - [ k - 1 + ------------- - - ] ,      k-1 <= N nu <= k
1728     N           A(k-1) + A(k)   2
1729     */
1730    int ind=0, ind1=0, ind3=0;
1731    double ampl1=0.0, ampl2=0.0;
1732
1733    GetInd(n, k, &ind1, &ind3);
1734    if (x[ind3 - 1] > x[ind1 - 1]) {
1735        ampl1 = x[k - 1];
1736        ampl2 = x[ind3 - 1];
1737        ind = k;
1738    } else {
1739        ampl1 = x[ind1 - 1];
1740        ampl2 = x[k - 1];
1741        /* Interpolate in right direction for 0 frequency */
1742        if (k != 1)
1743            ind = ind1;
1744        else
1745            ind = 0;
1746    }
1747    /* Avoid division by zero */
1748    if (ampl1 + ampl2 != 0.0)
1749        return ((ind - 1 + 2 * ampl2 / (ampl1 + ampl2) - 0.5) / n);
1750    else
1751        return 0.0;
1752}
1753
1754/****************************************************************************/
1755/* double Sinc(double omega)
1756
1757 Purpose:
1758
1759
1760 Input:
1761 none
1762
1763 Output:
1764 none
1765
1766 Return:
1767 none
1768
1769 Global variables:
1770 none
1771
1772 Specific functions:
1773 none
1774
1775 Comments:
1776 zero test to be changed numericallywise
1777
1778 ****************************************************************************/
1779double Sinc(double omega) {
1780    /*  Function to calculate:
1781
1782     sin( omega )
1783     ------------
1784     omega
1785     */
1786    if (omega != 0.0)
1787        return (sin(omega) / omega);
1788    else
1789        return 1.0;
1790}
1791
1792/****************************************************************************/
1793/* double intsampl(long n, double *x, double nu, long k)
1794
1795 Purpose:
1796
1797
1798 Input:
1799 none
1800
1801 Output:
1802 none
1803
1804 Return:
1805 none
1806
1807 Global variables:
1808 none
1809
1810 Specific functions:
1811 none
1812
1813 Comments:
1814 17/07/03 use M_PI instead of pi
1815
1816 ****************************************************************************/
1817double intsampl(int n, double *x, double nu, int k) {
1818    /* Get amplitude by nonlinear interpolation for sine window. The
1819     distribution is given by:
1820
1821     1    sin pi ( k + 1/2 )     sin pi ( k - 1/2 )
1822     F(k) =  - ( -------------------- + -------------------- )
1823     2      pi ( k + 1/2 )          pi ( k - 1/2 )
1824     */
1825    double corr=0.0;
1826
1827    corr = (Sinc(M_PI * (k - 1 + 0.5 - nu * n)) + Sinc(M_PI * (k - 1 - 0.5 - nu
1828            * n))) / 2;
1829    return (x[k - 1] / corr);
1830}
1831
1832/****************************************************************************/
1833/* double linint(long n, long k, double nu, double *x)
1834
1835 Purpose:
1836
1837
1838 Input:
1839 none
1840
1841 Output:
1842 none
1843
1844 Return:
1845 none
1846
1847 Global variables:
1848 none
1849
1850 Specific functions:
1851 none
1852
1853 Comments:
1854 17/07/03 use M_PI instead of pi
1855
1856 ****************************************************************************/
1857double linint(int n, int k, double nu, double *x) {
1858    /* Get phase by linear interpolation for rectangular window
1859     with -pi <= phi <= pi */
1860    int i=0;
1861    double phi=0.0;
1862    double xr[n], xi[n];
1863
1864    for (i = 0; i < n; i++) {
1865        xr[i] = x[i];
1866        xi[i] = 0.0;
1867    }
1868    FFT(n, xr, xi);
1869    phi = GetAngle(xr[k - 1], xi[k - 1]) - (n * nu - k + 1) * M_PI;
1870    if (phi > M_PI)
1871        phi -= 2.0 * M_PI;
1872    else if (phi < -M_PI)
1873        phi += 2.0 * M_PI;
1874    return phi;
1875}
1876
1877/****************************************************************************/
1878/* void FndRes(struct LOC_findres *LINK)
1879
1880 Purpose:
1881
1882
1883 Input:
1884 none
1885
1886 Output:
1887 none
1888
1889 Return:
1890 none
1891
1892 Global variables:
1893 none
1894
1895 Specific functions:
1896 none
1897
1898 Comments:
1899 none
1900
1901 ****************************************************************************/
1902void FndRes(struct LOC_findres *LINK) {
1903    int i=0, j=0, FORLIM=0, FORLIM1=0;
1904    double delta=0.0;
1905
1906    FORLIM = LINK->n;
1907    for (i = 0; i <= FORLIM; i++) {
1908        FORLIM1 = LINK->n;
1909        for (j = -LINK->n; j <= FORLIM1; j++) {
1910            delta = fabs(i * LINK->nux + j * LINK->nuy);
1911            delta -= (int) delta;
1912            if (delta > 0.5)
1913                delta = 1 - delta;
1914            delta = fabs(delta - LINK->f);
1915            delta -= (int) delta;
1916            if (delta > 0.5)
1917                delta = 1 - delta;
1918            if (delta < LINK->eps) {
1919                if (abs(i) + abs(j) < LINK->n && (i != 0 || j >= 0)) {
1920                    LINK->found = true;
1921                    *LINK->nx = i;
1922                    *LINK->ny = j;
1923                }
1924            }
1925        }
1926    }
1927}
1928
1929/****************************************************************************/
1930/* void FindRes(long n_, double nux_, double nuy_, double f_,
1931 long *nx_, long *ny_)
1932
1933 Purpose:
1934
1935
1936 Input:
1937 none
1938
1939 Output:
1940 none
1941
1942 Return:
1943 none
1944
1945 Global variables:
1946 none
1947
1948 Specific functions:
1949 none
1950
1951 Comments:
1952 none
1953
1954 ****************************************************************************/
1955void FindRes(int n_, double nux_, double nuy_, double f_, int *nx_, int *ny_) {
1956    /* Match f by a linear combination of nux and nuy */
1957    struct LOC_findres V;
1958
1959    V.n = n_;
1960    V.nux = nux_;
1961    V.nuy = nuy_;
1962    V.f = f_;
1963    V.nx = nx_;
1964    V.ny = ny_;
1965    V.found = false;
1966    V.eps = 0.5e-6;
1967    do {
1968        V.eps = 10 * V.eps;
1969        FndRes(&V);
1970    } while (!V.found);
1971}
1972/*******************************************************************************
1973 *
1974 *
1975 *
1976 *
1977 ******************************************************************************/
1978void GetPeaks(int n, double *x, int nf, double *nu, double *A) {
1979    int i=0, k=0, ind1=0, ind3=0;
1980
1981    for (i = 0; i < nf; i++) {
1982        GetPeak(n, x, &k);
1983        nu[i] = Int2snu(n, x, k);
1984        A[i] = intsampl(n, x, nu[i], k);
1985        /* Make peak flat to allow for new call */
1986        GetInd(n, k, &ind1, &ind3);
1987        if (x[ind1 - 1] > x[ind3 - 1])
1988            x[k - 1] = x[ind1 - 1];
1989        else
1990            x[k - 1] = x[ind3 - 1];
1991    }
1992}
1993/*******************************************************************************
1994 *
1995 *
1996 *
1997 *
1998 ******************************************************************************/
1999void GetPeaks1(int n, double *x, int nf, double *nu, double *A) {
2000    int i=0, k=0, ind1=0, ind3=0;
2001
2002    for (i = 0; i < nf; i++) {
2003        GetPeak1(n, x, &k);
2004        nu[i] = Int2snu(n, x, k);
2005        A[i] = intsampl(n, x, nu[i], k);
2006        /* Make peak flat to allow for new call */
2007        GetInd1(n, k, &ind1, &ind3);
2008        if (x[ind1 - 1] > x[ind3 - 1])
2009            x[k - 1] = x[ind1 - 1];
2010        else
2011            x[k - 1] = x[ind3 - 1];
2012    }
2013}
2014
2015/*******************************/
2016/* Routines for magnetic error */
2017/*******************************/
2018
2019void SetTol(int Fnum, double dxrms, double dyrms, double drrms) {
2020    int i=0;
2021    long k=0L;
2022
2023    for (i = 1; i <= GetnKid(Fnum); i++) {
2024        k = Elem_GetPos(Fnum, i);
2025        Cell[k].Elem.M->PdSrms[X_] = dxrms;
2026        Cell[k].Elem.M->PdSrnd[X_] = normranf();
2027        Cell[k].Elem.M->PdSrms[Y_] = dyrms;
2028        Cell[k].Elem.M->PdSrnd[Y_] = normranf();
2029        Cell[k].Elem.M->PdTrms = drrms;
2030        Cell[k].Elem.M->PdTrnd = normranf();
2031        Mpole_SetdS(Fnum, i);
2032        Mpole_SetdT(Fnum, i);
2033    }
2034}
2035/*******************************************************************************
2036 *
2037 *
2038 *
2039 *
2040 ******************************************************************************/
2041void Scale_Tol(int Fnum, double dxrms, double dyrms, double drrms) {
2042    int Knum=0;
2043    long int loc=0L;
2044
2045    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++) {
2046        loc = Elem_GetPos(Fnum, Knum);
2047        Cell[loc].Elem.M->PdSrms[X_] = dxrms;
2048        Cell[loc].Elem.M->PdSrms[Y_] = dyrms;
2049        Cell[loc].Elem.M->PdTrms = drrms;
2050        Mpole_SetdS(Fnum, Knum);
2051        Mpole_SetdT(Fnum, Knum);
2052    }
2053}
2054
2055/****************************************************************************/
2056/* void SetaTol(int Fnum, int Knum, double dx, double dy, double dr)
2057
2058 Purpose:
2059 Set a known random multipole displacement error
2060
2061 Input:
2062 none
2063
2064 Output:
2065 none
2066
2067 Return:
2068 none
2069
2070 Global variables:
2071 none
2072
2073 Specific functions:
2074 none
2075
2076 Comments:
2077 none
2078
2079 ****************************************************************************/
2080void SetaTol(int Fnum, int Knum, double dx, double dy, double dr) {
2081    long int loc=0L;
2082
2083    loc = Elem_GetPos(Fnum, Knum);
2084    Cell[loc].Elem.M->PdSrms[0] = dx;
2085    Cell[loc].Elem.M->PdSrnd[0] = 1e0;
2086    Cell[loc].Elem.M->PdSrms[1] = dy;
2087    Cell[loc].Elem.M->PdSrnd[1] = 1e0;
2088    Cell[loc].Elem.M->PdTrms = dr;
2089    Cell[loc].Elem.M->PdTrnd = 1e0;
2090    Mpole_SetdS(Fnum, Knum);
2091    Mpole_SetdT(Fnum, Knum);
2092}
2093/*******************************************************************************
2094 *
2095 *
2096 *
2097 *
2098 ******************************************************************************/
2099void ini_aper(const double Dxmin, const double Dxmax, const double Dymin,
2100        const double Dymax) {
2101    int k=0;
2102
2103    for (k = 0; k <= globval.Cell_nLoc; k++) {
2104        Cell[k].maxampl[X_][0] = Dxmin;
2105        Cell[k].maxampl[X_][1] = Dxmax;
2106        Cell[k].maxampl[Y_][0] = Dymin;
2107        Cell[k].maxampl[Y_][1] = Dymax;
2108    }
2109}
2110/*******************************************************************************
2111 *
2112 *
2113 *
2114 *
2115 ******************************************************************************/
2116void set_aper(const int Fnum, const double Dxmin, const double Dxmax,
2117        const double Dymin, const double Dymax) {
2118    int i=0;
2119    long int loc=0L;
2120
2121    for (i = 1; i <= GetnKid(Fnum); i++) {
2122        loc = Elem_GetPos(Fnum, i);
2123        Cell[loc].maxampl[X_][0] = Dxmin;
2124        Cell[loc].maxampl[X_][1] = Dxmax;
2125        Cell[loc].maxampl[Y_][0] = Dymin;
2126        Cell[loc].maxampl[Y_][1] = Dymax;
2127    }
2128}
2129/*************************************************
2130 * void LoadApertures(const char *ChamberFileName)
2131 *
2132 *
2133 *
2134 **************************************************/
2135void LoadApertures(const char *ChamberFileName) {
2136    char line[128], FamName[32];
2137    long Fnum=0L;
2138    double Xmin=0.0, Xmax=0.0, Ymin=0.0, Ymax=0.0;
2139    FILE *ChamberFile;
2140
2141    ChamberFile = file_read(ChamberFileName);
2142
2143    do
2144        fgets(line, 128, ChamberFile);
2145    while (strstr(line, "#") != NULL);
2146
2147    do {
2148        sscanf(line, "%s %lf %lf %lf %lf", FamName, &Xmin, &Xmax, &Ymin, &Ymax);
2149        Fnum = ElemIndex(FamName);
2150        if (Fnum > 0)
2151            set_aper(Fnum, Xmin, Xmax, Ymin, Ymax);
2152    } while (fgets(line, 128, ChamberFile) != NULL);
2153
2154    fclose(ChamberFile);
2155}
2156/**********************************************************************************
2157 * void LoadTolerances(const char *TolFileName)
2158 *
2159 *
2160 *
2161 **********************************************************************************/
2162// Load tolerances from the file
2163void LoadTolerances(const char *TolFileName) {
2164    char line[128], FamName[32];
2165    int Fnum=0;
2166    double dx=0.0, dy=0.0, dr=0.0;
2167    FILE *tolfile;
2168
2169    tolfile = file_read(TolFileName);
2170    if(tolfile == NULL){
2171      printf("LoadTolerances(): Error! Failure to read file: %s \n",TolFileName);
2172      exit_(1);
2173    }
2174
2175    do
2176        fgets(line, 128, tolfile);
2177    while (strstr(line, "#") != NULL);
2178
2179    do {
2180        if (strstr(line, "#") == NULL) {
2181            sscanf(line, "%s %lf %lf %lf", FamName, &dx, &dy, &dr);
2182            Fnum = ElemIndex(FamName);
2183            if (Fnum > 0) {
2184                SetTol(Fnum, dx, dy, dr);
2185            } else {
2186                printf("LoadTolerances: undefined element %s\n", FamName);
2187                exit_(1);
2188            }
2189        }
2190    } while (fgets(line, 128, tolfile) != NULL);
2191
2192    fclose(tolfile);
2193}
2194
2195/**************************************************************************************
2196 * void ScaleTolerances(const char *TolFileName, const double scl)
2197 *
2198 *
2199 *
2200 * ************************************************************************************/
2201// Load tolerances from the file
2202void ScaleTolerances(const char *TolFileName, const double scl) {
2203    char line[128], FamName[32];
2204    int Fnum=0;
2205    double dx=0.0, dy=0.0, dr=0.0;
2206    FILE *tolfile;
2207
2208    tolfile = file_read(TolFileName);
2209    if(tolfile == NULL){
2210      printf("LoadTolerances(): Error! Failure to read file: %s \n",TolFileName);
2211      exit_(1);
2212    }
2213   
2214    do
2215        fgets(line, 128, tolfile);
2216    while (strstr(line, "#") != NULL);
2217
2218    do {
2219        if (strstr(line, "#") == NULL) {
2220            sscanf(line, "%s %lf %lf %lf", FamName, &dx, &dy, &dr);
2221            Fnum = ElemIndex(FamName);
2222            if (Fnum > 0) {
2223                Scale_Tol(Fnum, scl * dx, scl * dy, scl * dr);
2224            } else {
2225                printf("ScaleTolerances: undefined element %s\n", FamName);
2226                exit_(1);
2227            }
2228        }
2229    } while (fgets(line, 128, tolfile) != NULL);
2230    fclose(tolfile);
2231}
2232/*******************************************************************************
2233 *
2234 *
2235 *
2236 *
2237 ******************************************************************************/
2238void SetKpar(int Fnum, int Knum, int Order, double k) {
2239
2240    Cell[Elem_GetPos(Fnum, Knum)].Elem.M->PBpar[Order + HOMmax] = k;
2241    Mpole_SetPB(Fnum, Knum, Order);
2242}
2243/*******************************************************************************
2244 *
2245 *
2246 *
2247 *
2248 ******************************************************************************/
2249void SetL(int Fnum, int Knum, double L) {
2250
2251    Cell[Elem_GetPos(Fnum, Knum)].Elem.PL = L;
2252}
2253/*******************************************************************************
2254 *
2255 *
2256 *
2257 *
2258 ******************************************************************************/
2259void SetL(int Fnum, double L) {
2260    int i=0;
2261
2262    for (i = 1; i <= GetnKid(Fnum); i++)
2263        Cell[Elem_GetPos(Fnum, i)].Elem.PL = L;
2264}
2265/*******************************************************************************
2266 *
2267 *
2268 *
2269 *
2270 ******************************************************************************/
2271void SetdKpar(int Fnum, int Knum, int Order, double dk) {
2272
2273    Cell[Elem_GetPos(Fnum, Knum)].Elem.M->PBpar[Order + HOMmax] += dk;
2274    Mpole_SetPB(Fnum, Knum, Order);
2275}
2276/*******************************************************************************
2277 *
2278 *
2279 *
2280 *
2281 ******************************************************************************/
2282void SetKLpar(int Fnum, int Knum, int Order, double kL) {
2283    long int loc=0L;
2284
2285    if (abs(Order) > HOMmax) {
2286        printf("SetKLPar: Error!....Multipole Order %d  > HOMmax %d\n", Order,
2287                HOMmax);
2288        exit_(1);
2289    }
2290
2291    loc = Elem_GetPos(Fnum, Knum);
2292    if (Cell[loc].Elem.PL != 0e0)
2293        Cell[loc].Elem.M->PBpar[Order + HOMmax] = kL / Cell[loc].Elem.PL;
2294    else
2295        Cell[loc].Elem.M->PBpar[Order + HOMmax] = kL;
2296    Mpole_SetPB(Fnum, Knum, Order);
2297}
2298/*******************************************************************************
2299 *
2300 *
2301 *
2302 *
2303 ******************************************************************************/
2304void SetdKLpar(int Fnum, int Knum, int Order, double dkL) {
2305    long int loc=0L;
2306
2307    loc = Elem_GetPos(Fnum, Knum);
2308    if (Cell[loc].Elem.PL != 0e0)
2309        Cell[loc].Elem.M->PBpar[Order + HOMmax] += dkL / Cell[loc].Elem.PL;
2310    else
2311        Cell[loc].Elem.M->PBpar[Order + HOMmax] += dkL;
2312    Mpole_SetPB(Fnum, Knum, Order);
2313}
2314
2315/*************************************************************
2316 void SetdKrpar(int Fnum, int Knum, int Order, double dkrel)
2317 
2318   Purpose:
2319     Increase or reduce the strength of element by a certain scale.
2320     
2321   Input:
2322     Fnum           family number
2323     Knum           kid number
2324     order          field strength order to be modified
2325     bnr            scale of the field strength with order "order" 
2326   
2327   Output:
2328   
2329   Comments:
2330   
2331     
2332**************************************************************/
2333void SetdKrpar(int Fnum, int Knum, int Order, double dkrel) {
2334    long int loc=0L;
2335
2336    loc = Elem_GetPos(Fnum, Knum);
2337    if (Order == Dip && Cell[loc].Elem.M->Pthick == thick)
2338        Cell[loc].Elem.M->PBpar[Dip + HOMmax] += dkrel
2339                * Cell[loc].Elem.M->Pirho;
2340    else
2341        Cell[loc].Elem.M->PBpar[Order + HOMmax] += dkrel
2342                * Cell[loc].Elem.M->PBpar[Order + HOMmax];
2343    Mpole_SetPB(Fnum, Knum, Order);
2344}
2345/*******************************************************************************
2346 *
2347 *
2348 *
2349 *
2350 ******************************************************************************/
2351void Setbn(int Fnum, int order, double bn) {
2352    int i=0;
2353
2354    for (i = 1; i <= GetnKid(Fnum); i++)
2355        SetKpar(Fnum, i, order, bn);
2356}
2357/*******************************************************************************
2358 *
2359 *
2360 *
2361 *
2362 ******************************************************************************/
2363void SetbnL(int Fnum, int order, double bnL) {
2364    int i=0;
2365
2366    for (i = 1; i <= GetnKid(Fnum); i++)
2367        SetKLpar(Fnum, i, order, bnL);
2368}
2369/*******************************************************************************
2370 *
2371 *
2372 *
2373 *
2374 ******************************************************************************/
2375void Setdbn(int Fnum, int order, double dbn) {
2376    int i=0;
2377
2378    for (i = 1; i <= GetnKid(Fnum); i++)
2379        SetdKpar(Fnum, i, order, dbn);
2380}
2381/*******************************************************************************
2382 *
2383 *
2384 *
2385 *
2386 ******************************************************************************/
2387void SetdbnL(int Fnum, int order, double dbnL) {
2388    int i=0;
2389
2390    for (i = 1; i <= GetnKid(Fnum); i++) {
2391        SetdKLpar(Fnum, i, order, dbnL);
2392    }
2393}
2394
2395/*************************************************************
2396 void Setbnr(int Fnum, int order, double bnr)
2397 
2398   Purpose:
2399     Increase or reduce the strength of element family by a certain
2400     scale.
2401     
2402   Input:
2403     Fnum           family number
2404     order          field strength order to be modified
2405     bnr            scale of the field strength with order "order" 
2406   
2407   Output:
2408   
2409   Comments:
2410     Jianfeng Zhang  07/04/2011
2411     Fix the bug in the type definition of the function parameter 'order'
2412     from 'long' to 'int'.
2413     
2414**************************************************************/
2415//void Setbnr(int Fnum, long order, double bnr) {
2416void Setbnr(int Fnum, int order, double bnr) {
2417    int i=0;
2418
2419    for (i = 1; i <= GetnKid(Fnum); i++)
2420        SetdKrpar(Fnum, i, order, bnr);
2421}
2422/*******************************************************************************
2423 *
2424 *
2425 *
2426 *
2427 ******************************************************************************/
2428void SetbnL_sys(int Fnum, int Order, double bnL_sys) {
2429    int Knum=0;
2430    long int loc=0L;
2431
2432    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++) {
2433        loc = Elem_GetPos(Fnum, Knum);
2434        if (Cell[loc].Elem.PL != 0.0)
2435            Cell[loc].Elem.M->PBsys[Order + HOMmax] = bnL_sys
2436                    / Cell[loc].Elem.PL;
2437        else
2438            Cell[loc].Elem.M->PBsys[Order + HOMmax] = bnL_sys;
2439        Mpole_SetPB(Fnum, Knum, Order);
2440    }
2441}
2442/*******************************************************************************
2443 *
2444 *
2445 *
2446 *
2447 ******************************************************************************/
2448void set_dbn_rel(const int type, const int n, const double dbn_rel) {
2449    long int j=0L;
2450    double dbn=0.0;
2451
2452    printf("\n");
2453    printf("Setting Db_%d/b_%d = %6.1e for:\n", n, type, dbn_rel);
2454    printf("\n");
2455    for (j = 0; j <= globval.Cell_nLoc; j++)
2456        if ((Cell[j].Elem.Pkind == Mpole) && (Cell[j].Elem.M->n_design == type)) {
2457            printf("%s\n", Cell[j].Elem.PName);
2458            dbn = dbn_rel * Cell[j].Elem.M->PBpar[type + HOMmax];
2459            Cell[j].Elem.M->PBrms[n + HOMmax] = dbn;
2460            Cell[j].Elem.M->PBrnd[n + HOMmax] = normranf();
2461            Mpole_SetPB(Cell[j].Fnum, Cell[j].Knum, n);
2462        }
2463}
2464
2465/********************************************************************
2466 double GetKpar(int Fnum, int Knum, int Order)
2467
2468 Purpose:
2469 Return the n-th order design field strength of the element
2470
2471 Input:
2472 Fnum     family index
2473 Knum     kid index
2474 Order     design field strength
2475
2476 Ouput:
2477 None
2478
2479 Return:
2480 n-th order design field strength
2481
2482 *********************************************************************/
2483double GetKpar(int Fnum, int Knum, int Order) {
2484    return (Cell[Elem_GetPos(Fnum, Knum)].Elem.M->PBpar[Order + HOMmax]);
2485}
2486
2487/********************************************************************
2488 double GetL(int Fnum, int Knum)
2489
2490 Purpose:
2491 Return the length of the element with "Fnum" and "Knum"
2492
2493 Input:
2494 Fnum     family index
2495 Knum     kid index
2496
2497
2498 Ouput:
2499 None
2500
2501 Return:
2502
2503
2504 *********************************************************************/
2505double GetL(int Fnum, int Knum) {
2506    return (Cell[Elem_GetPos(Fnum, Knum)].Elem.PL);
2507}
2508
2509/********************************************************************
2510 double GetKLpar(int Fnum, int Knum, int Order)
2511
2512 Purpose:
2513 Return the n-th order design integrated field strength of the element
2514
2515 Input:
2516 Fnum     family index
2517 Knum     kid index
2518 Order     design field strength
2519
2520 Ouput:
2521 None
2522
2523 Return:
2524 n-th order design integrated field strength
2525
2526 *********************************************************************/
2527
2528double GetKLpar(int Fnum, int Knum, int Order) {
2529    long int loc = 0L;
2530
2531    loc = Elem_GetPos(Fnum, Knum);
2532    if (Cell[loc].Elem.PL != 0e0)
2533        return (Cell[loc].Elem.M->PBpar[Order + HOMmax] * Cell[loc].Elem.PL);
2534    else
2535        return (Cell[loc].Elem.M->PBpar[Order + HOMmax]);
2536}
2537/*******************************************************************************
2538 *
2539 *
2540 *
2541 *
2542 ******************************************************************************/
2543void SetdKLrms(int Fnum, int Order, double dkLrms) {
2544    long int Knum=0L, loc=0L;
2545
2546    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++) {
2547        loc = Elem_GetPos(Fnum, Knum);
2548        if (Cell[loc].Elem.PL != 0e0)
2549            Cell[loc].Elem.M->PBrms[Order + HOMmax] = dkLrms
2550                    / Cell[loc].Elem.PL;
2551        else
2552            Cell[loc].Elem.M->PBrms[Order + HOMmax] = dkLrms;
2553        Cell[loc].Elem.M->PBrnd[Order + HOMmax] = normranf();
2554        Mpole_SetPB(Fnum, Knum, Order);
2555    }
2556}
2557/*******************************************************************************
2558 *
2559 *
2560 *
2561 *
2562 ******************************************************************************/
2563void Setdkrrms(int Fnum, int Order, double dkrrms) {
2564    long int Knum=0L, loc=0L;
2565
2566    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++) {
2567        loc = Elem_GetPos(Fnum, Knum);
2568        if (Order == Dip && Cell[loc].Elem.M->Pthick == thick)
2569            Cell[loc].Elem.M->PBrms[Dip + HOMmax] = dkrrms
2570                    * Cell[loc].Elem.M->Pirho;
2571        else
2572            Cell[loc].Elem.M->PBrms[Order + HOMmax] = dkrrms
2573                    * Cell[loc].Elem.M->PBpar[Order + HOMmax];
2574        Cell[loc].Elem.M->PBrnd[Order + HOMmax] = normranf();
2575        Mpole_SetPB(Fnum, Knum, Order);
2576    }
2577}
2578/*******************************************************************************
2579 *
2580 *
2581 *
2582 *
2583 ******************************************************************************/
2584void SetKL(int Fnum, int Order) {
2585    long int Knum=0L;
2586
2587    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++)
2588        Mpole_SetPB(Fnum, Knum, Order);
2589}
2590/*******************************************************************************
2591 *
2592 *
2593 *
2594 *
2595 ******************************************************************************/
2596void set_dx(const int type, const double sigma_x, const double sigma_y) {
2597    long int j=0L;
2598
2599    printf("\n");
2600    printf("Setting sigma_x,y = (%6.1e, %6.1e) for b_%d:\n", sigma_x, sigma_y,
2601            type);
2602    printf("\n");
2603    for (j = 0; j <= globval.Cell_nLoc; j++)
2604        if ((Cell[j].Elem.Pkind == Mpole) && (Cell[j].Elem.M->n_design == type)) {
2605            printf("%s\n", Cell[j].Elem.PName);
2606            Cell[j].Elem.M->PdSrms[X_] = sigma_x;
2607            Cell[j].Elem.M->PdSrms[Y_] = sigma_y;
2608            Cell[j].Elem.M->PdSrnd[X_] = normranf();
2609            Cell[j].Elem.M->PdSrnd[Y_] = normranf();
2610            Mpole_SetdS(Cell[j].Fnum, Cell[j].Knum);
2611        }
2612}
2613/*******************************************************************************
2614 *
2615 *
2616 *
2617 *
2618 ******************************************************************************/
2619void SetBpmdS(int Fnum, double dxrms, double dyrms) {
2620    long int Knum, loc=0L;
2621
2622    for (Knum = 1; Knum <= GetnKid(Fnum); Knum++) {
2623        loc = Elem_GetPos(Fnum, Knum);
2624        Cell[loc].dS[X_] = normranf() * dxrms;
2625        Cell[loc].dS[Y_] = normranf() * dyrms;
2626    }
2627}
2628
2629/******************************************************************************
2630void codstat(double *mean, double *sigma, double *xmax, long lastpos, bool all)
2631
2632  Purpose:
2633     Routines for closed orbit correction
2634     Return the mean orbit, rms orbit and maximum orbit, based on the orbits at
2635     all lattice elements or all bpm postion.
2636 
2637  Input:
2638     mean           mean value of the orbit, horizontal or vertical
2639     sigma          rms value of the orbit, horizontal or vertical
2640     xmax           maximum value of the orbit, horizontal or vertical
2641     lastpos        last element index in the lattice
2642     all            true, then do statistics on the orbit at all elements
2643                    false, ...............................at all bpm
2644****************************************************************************/
2645void codstat(double *mean, double *sigma, double *xmax, long lastpos, bool all) {
2646    long i=0L, j=0L, n=0L;
2647    Vector2 sum, sum2;
2648    double TEMP=0.0;
2649
2650    n = 0;
2651    for (j = 0; j <= 1; j++) {
2652        sum[j] = 0.0;
2653        sum2[j] = 0.0;
2654        xmax[j] = 0.0;
2655    }
2656    for (i = 0; i <= lastpos; i++) {
2657        if (all || Cell[i].Fnum == globval.bpm) {//get the sum and max orbit at all elements or all bpm
2658            n++;
2659            for (j = 1; j <= 2; j++) {
2660                sum[j - 1] += Cell[i].BeamPos[j * 2 - 2];
2661                TEMP = Cell[i].BeamPos[j * 2 - 2];
2662                sum2[j - 1] += TEMP * TEMP;
2663                xmax[j - 1]
2664                        = max(xmax[j - 1], fabs(Cell[i].BeamPos[j * 2 - 2]));
2665            }
2666        }
2667    }
2668    for (j = 0; j <= 1; j++) {
2669        if (n != 0)
2670            mean[j] = sum[j] / n; //mean value of the orbit
2671        else
2672            mean[j] = 0.0;
2673        if (n != 0 && n != 1) {
2674            TEMP = sum[j];
2675            sigma[j] = (n * sum2[j] - TEMP * TEMP) / (n * (n - 1.0));
2676        } else
2677            sigma[j] = 0.0;
2678        if (sigma[j] >= 0.0)
2679            sigma[j] = sqrt(sigma[j]);
2680        else
2681            sigma[j] = 0.0;
2682    }
2683}
2684
2685/****************************************************************************/
2686/* void CodStatBpm(double *mean, double *sigma, double *xmax, long lastpos,
2687 long bpmdis[mnp])
2688
2689 Purpose:
2690 Get statistics for  closed orbit
2691
2692 Input:
2693 none
2694
2695 Output:
2696 none
2697
2698 Return:
2699 none
2700
2701 Global variables:
2702 none
2703
2704 Specific functions:
2705 none
2706
2707 Comments:
2708 none
2709
2710 ****************************************************************************/
2711void CodStatBpm(double *mean, double *sigma, double *xmax, long lastpos,
2712        long bpmdis[mnp]) {
2713    long i=0L, j=0L, m=0L, n=0L;
2714    Vector2 sum, sum2;
2715    double TEMP=0.0;
2716
2717    m = n = 0;
2718    for (j = 0; j <= 1; j++) {
2719        sum[j] = 0.0;
2720        sum2[j] = 0.0;
2721        xmax[j] = 0.0;
2722    }
2723
2724    for (i = 0; i <= lastpos; i++) {
2725        if (Cell[i].Fnum == globval.bpm) {
2726            if (!bpmdis[m]) {
2727                for (j = 1; j <= 2; j++) {
2728                    sum[j - 1] += Cell[i].BeamPos[j * 2 - 2];
2729                    TEMP = Cell[i].BeamPos[j * 2 - 2];
2730                    sum2[j - 1] += TEMP * TEMP;
2731                    xmax[j - 1] = max(xmax[j - 1], fabs(Cell[i].BeamPos[j * 2
2732                            - 2]));
2733                }
2734                n++;
2735            }
2736            m++;
2737        }
2738    }
2739    for (j = 0; j <= 1; j++) {
2740        if (n != 0)
2741            mean[j] = sum[j] / n;
2742        else
2743            mean[j] = 0.0;
2744        if (n != 0 && n != 1) {
2745            TEMP = sum[j];
2746            sigma[j] = (n * sum2[j] - TEMP * TEMP) / (n * (n - 1.0));
2747        } else
2748            sigma[j] = 0.0;
2749        if (sigma[j] >= 0.0)
2750            sigma[j] = sqrt(sigma[j]);
2751        else
2752            sigma[j] = 0.0;
2753    }
2754}
2755
2756/****************************************************************************/
2757/* double digitize(double x, double maxkick, double maxsamp)
2758
2759 Purpose:
2760 Map x onto the integer interval (-maxsamp ... maxsamp) where maxsamp
2761 corresponds maxkick.
2762
2763
2764 Input:
2765 none
2766
2767 Output:
2768 none
2769
2770 Return:
2771 none
2772
2773 Global variables:
2774 none
2775
2776 Specific functions:
2777 none
2778
2779 Comments:
2780 none
2781
2782 ****************************************************************************/
2783double digitize(double x, double maxkick, double maxsamp) {
2784    if (maxkick > 0.)
2785        if (maxsamp > 1.)
2786            return Sgn(x) * maxkick / maxsamp * min(floor(fabs(x) / maxkick
2787                    * maxsamp), maxsamp - 1.);
2788        else {
2789            return Sgn(x) * min(fabs(x), maxkick);
2790        }
2791    else
2792        return x;
2793}
2794
2795/****************************************************************************/
2796/* double digitize2(long plane, long inum, double x, double maxkick, double maxsamp)
2797
2798 Purpose:
2799
2800
2801 Input:
2802 none
2803
2804 Output:
2805 none
2806
2807 Return:
2808 none
2809
2810 Global variables:
2811 none
2812
2813 Specific functions:
2814 none
2815
2816 Comments:
2817 none
2818
2819 ****************************************************************************/
2820svdarray xmemo[2];
2821
2822double digitize2(long plane, long inum, double x, double maxkick,
2823        double maxsamp) {
2824    double xint;
2825
2826    if (maxkick > 0.)
2827        if (maxsamp > 1.) {
2828            xint = min(floor(fabs(x) / maxkick * maxsamp), maxsamp - 1.);
2829
2830            if (fabs(xint - xmemo[inum][plane]) >= 1.) {
2831                xmemo[inum][plane] = xint;
2832            } else {
2833                xmemo[inum][plane] += 0.1;
2834                xint = min(xmemo[inum][plane], maxsamp - 1.);
2835            }
2836            return Sgn(x) * maxkick / maxsamp * xint;
2837        } else {
2838            return Sgn(x) * min(fabs(x), maxkick);
2839        }
2840    else
2841        return x;
2842}
2843
2844// MATH ROUTINE a mettre dans mathlib.c
2845
2846/****************************************************************************/
2847/*  void GetMean(n, x)
2848
2849 Purpose:
2850 Get out the mean value of vector x
2851
2852 Input:
2853 n vector size
2854 x vector to get out the mean value
2855
2856 Output:
2857 none
2858
2859 Return:
2860 none
2861
2862 Global variables:
2863 none
2864
2865 Specific functions:
2866 none
2867
2868 Comments:
2869 to be moved in mathlib
2870
2871 ****************************************************************************/
2872void GetMean(long n, double *x) {
2873    long i=0L;
2874    double mean = 0e0;
2875
2876    if (n < 1) {
2877        fprintf(stdout, "GetMean: error wrong vector size n=%ld\n", n);
2878        exit_(1);
2879    }
2880    for (i = 0; i < n; i++)
2881        mean += x[i];
2882    mean /= n;
2883    for (i = 0; i < n; i++)
2884        x[i] = x[i] - mean;
2885}
2886
2887/****************************************************************************/
2888/* double Fract(double x)
2889
2890 Purpose:
2891 Gets fractional part of x
2892
2893 Input:
2894 none
2895
2896 Output:
2897 none
2898
2899 Return:
2900 none
2901
2902 Global variables:
2903 none
2904
2905 Specific functions:
2906 none
2907
2908 Comments:
2909 none
2910
2911 ****************************************************************************/
2912double Fract(double x) {
2913    return (x - (long int) x);
2914}
2915
2916/****************************************************************************/
2917/* double Sgn (double x)
2918
2919 Purpose:
2920 Gets sign of x
2921
2922 Input:
2923 none
2924
2925 Output:
2926 0  if zero
2927 1  if positive
2928 -1 if negative
2929
2930 Return:
2931 none
2932
2933 Global variables:
2934 none
2935
2936 Specific functions:
2937 none
2938
2939 Comments:
2940 none
2941
2942 ****************************************************************************/
2943double Sgn(double x) {
2944    return (x == 0.0 ? 0.0 : (x > 0.0 ? 1.0 : -1.0));
2945}
2946
2947
2948
2949/*************************************************************************/
2950/*void PrintCh(void)
2951 
2952 Purpose:
2953 Output vacuum chamber limitation at each element to file "chambre.out"
2954
2955 Input:
2956 none
2957
2958 Output:
2959 none
2960
2961 Return:
2962 none
2963
2964 Global variables:
2965 none
2966
2967 Specific functions:
2968 none
2969
2970 Comments:
2971 none
2972
2973
2974 *************************************************************************/
2975void PrintCh(void) {
2976    long i = 0;
2977    struct tm *newtime;
2978    FILE *f;
2979
2980    const char *fic = "chambre.out";
2981
2982    newtime = GetTime();
2983
2984    f = file_write(fic);
2985    fprintf(f, "# TRACY III Synchrotron SOLEIL -- %s -- %s \n", fic, asctime2(
2986            newtime));
2987    fprintf(f,
2988            "#  i  name               s    -xch(mm) +xch(mm)  -ych(mm) +ych(mm)\n#\n");
2989
2990    for (i = 1; i <= globval.Cell_nLoc; i++)
2991        fprintf(f, "%4ld  %15s %6.2f  %7.3f  %7.3f  %7.3f %7.3f\n", i,
2992                Cell[i].Elem.PName, Cell[i].S, Cell[i].maxampl[X_][0] * 1E3,
2993                Cell[i].maxampl[X_][1] * 1E3, Cell[i].maxampl[Y_][0] * 1E3,
2994                Cell[i].maxampl[Y_][1] * 1E3);
2995
2996    fclose(f);
2997}
2998
2999
3000/****************************************************************************/
3001/* void GetChromTrac(long Nb, long Nbtour, double emax,
3002 double *xix, double *xiz)
3003
3004 Purpose:
3005 Computes chromaticities by tracking
3006
3007 Input:
3008 Nb      point number
3009 Nbtour  turn number
3010 emax    energy step
3011
3012 Output:
3013 xix horizontal chromaticity
3014 xiz vertical chromaticity
3015
3016 Return:
3017 none
3018
3019 Global variables:
3020 trace
3021
3022 Specific functions:
3023 Trac_Simple, Get_NAFF
3024
3025 Comments:
3026 27/04/03 chromaticities are now output arguments
3027 07/10/10 add test if unstable
3028
3029 ****************************************************************************/
3030#define nterm  2
3031#define ZERO 1E-8
3032
3033void GetChromTrac(long Nb, long Nbtour, double emax, double *xix, double *xiy) {
3034    bool status = true;
3035    int nb_freq[2] = { 0, 0 }; /* frequency number to look for */
3036    int i = 0;
3037    double Tab[6][NTURN], fx[nterm], fy[nterm], nux1, nux2, nuy1, nuy2;
3038
3039    double x  = 1e-6, xp  = 0.0, y  = 1e-6, yp  = 0.0;
3040    double x0 = 1e-6, xp0 = 0.0, y0 = 1e-6, yp0 = 0.0;
3041    //double xixExtra = 0.0, xizExtra= 0.0, xixhalf= 0.0, xizhalf= 0.0;
3042    //double nux3 = 0.0, nux4 = 0.0, nuz3 = 0.0, nuz4 = 0.0;
3043
3044    /* initializations */
3045    for (i = 0; i < nterm; i++) {
3046        fx[i] = 0.0;
3047        fy[i] = 0.0;
3048    }
3049    /* end init */
3050
3051    /* Tracking for delta = emax and computing tunes */
3052    x  = x0;
3053    xp = xp0;
3054    y  = y0;
3055    yp = yp0;
3056
3057    Trac_Simple4DCOD(x, xp, y, yp, emax, 0.0, Nbtour, Tab, &status);
3058    if (status){
3059    Get_NAFF(nterm, Nbtour, Tab, fx, fy, nb_freq);
3060    nux1 = (fabs(fx[0]) > ZERO ? fx[0] : fx[1]);
3061    nuy1 = fy[0];}
3062    else{
3063        nux1=999; nuy1=999;
3064    }
3065
3066    if (trace)
3067        fprintf(stdout,
3068                "\nGetChromTrac: Entering routine for chroma using tracking\n");
3069    if (trace)
3070        fprintf(stdout, "emax= % 10.6e nux1=% 10.6e nuy1= % 10.6e\n", emax,
3071                nux1, nuy1);
3072
3073    /* Tracking for delta = -emax and computing tunes */
3074    x  = x0;
3075    xp = xp0;
3076    y  = y0;
3077    yp = yp0;
3078
3079    Trac_Simple4DCOD(x, xp, y, yp, -emax, 0.0, Nbtour, Tab, &status);
3080    if (status){
3081        Get_NAFF(nterm, Nbtour, Tab, fx, fy, nb_freq);
3082    if (trace)
3083        fprintf(stdout, "nturn=%6ld x=% 10.5g xp=% 10.5g z=% 10.5g zp=% 10.5g"
3084            " delta=% 10.5g ctau=% 10.5g \n", Nbtour, Tab[0][Nbtour - 1],
3085                Tab[1][Nbtour - 1], Tab[2][Nbtour - 1], Tab[3][Nbtour - 1],
3086                Tab[4][Nbtour - 1], Tab[5][Nbtour - 1]);
3087
3088    nux2 = (fabs(fx[0]) > ZERO ? fx[0] : fx[1]);
3089    nuy2 = fy[0];
3090
3091    if (trace)
3092        fprintf(stdout, "emax= % 10.6e nux2= % 10.6e nuy2= % 10.6e\n", -emax,
3093                nux2, nuy2);
3094
3095    /* Computing chromaticities */
3096    *xix = (nux2 - nux1) * 0.5 / emax;
3097    *xiy = (nuy2 - nuy1) * 0.5 / emax;
3098
3099    if (trace)
3100        fprintf(stdout,
3101                "GetChromTrac: Exiting routine for chroma using tracking\n\n");
3102    }
3103    else{ // unstable
3104        *xix = -99999;
3105        *xiy = -99999;
3106    }
3107
3108    /*
3109     // Compute for half a step to diagnose precision
3110     Trac_Simple(x, xp, z, zp, emax*0.5, 0.0, Nbtour, Tab, &status);
3111     Get_NAFF(nterm, Nbtour, Tab, fx, fz, nb_freq);
3112     nux3 = (fabs (fx[0]) > 1e-8 ? fx[0] : fx[1]); nuz3 = fz[0];
3113
3114     Trac_Simple(x, xp, z, zp, -emax*0.5, 0.0, Nbtour, Tab, &status);
3115     Get_NAFF(nterm, Nbtour, Tab, fx, fz, nb_freq);
3116     nux4 = (fabs(fx[0]) > 1e-8 ? fx[0] : fx[1]); nuz4 = fz[0];
3117
3118     xixhalf = (nux4-nux3)/emax; xizhalf = (nuz4-nuz3)/emax;
3119
3120     // Richardson extrapolation
3121     xixExtra = (4.0*xixhalf-*xix)/3.0;
3122     xizExtra = (4.0*xizhalf-*xiz)/3.0;
3123
3124     fprintf(stdout, "chroma evaluated at +/- %6.2g, xix = % f xiz = % f\n",
3125     emax, *xix, *xiz);
3126     fprintf(stdout, "chroma evaluated at +/- %6.2g, xix = % f xiz = % f\n",
3127     emax/2, xixhalf, xizhalf);
3128     fprintf(stdout, "chroma evaluated from Richardson Extrapolation, xix = % f xiz = % f\n",
3129     xixExtra, xizExtra);
3130     */
3131}
3132#undef nterm
3133#undef ZERO
3134
3135/****************************************************************************/
3136/* void GetTuneTrac(long Nbtour, double emax, double *nux, double *nuz)
3137
3138 Purpose:
3139 Computes tunes by tracking
3140
3141 Input:
3142 Nb      point number
3143 Nbtour  turn number
3144 emax    energy step
3145
3146 Output:
3147 nux horizontal tune (0.0 if unstable)
3148 nuz vertical tune (0.0 if unstable)
3149
3150 Return:
3151 none
3152
3153 Global variables:
3154 trace
3155
3156 Specific functions:
3157 Trac_Simple, Get_NAFF
3158
3159 Comments:
3160   Add test on stability
3161
3162 ****************************************************************************/
3163#define nterm  2
3164#define ZERO 1E-8
3165void GetTuneTrac(long Nbtour, double emax, double *nux, double *nuz) {
3166    double Tab[6][NTURN], fx[nterm], fz[nterm];
3167    int nb_freq[2];
3168    bool status;
3169
3170    double x = 1e-6, xp = 0.0, z = 1e-6, zp = 0.0;
3171
3172    Trac_Simple4DCOD(x, xp, z, zp, emax, 0.0, Nbtour, Tab, &status);
3173    if (status){
3174       Get_NAFF(nterm, Nbtour, Tab, fx, fz, nb_freq);
3175        *nux = (fabs(fx[0]) > ZERO ? fx[0] : fx[1]);
3176        *nuz = fz[0];}
3177    else{ // particle unstable
3178        *nux = 0.0;
3179        *nuz = 0.0;}
3180}
3181#undef nterm
3182#undef ZERO
3183
3184/****************************************************************************/
3185/* void TransTwiss(double *alpha, double *beta, double *eta, double *etap, double *codvect)
3186
3187 Purpose: high level application
3188 Calculate Twiss functions for a transport line
3189
3190 Input:
3191 alpha   alpha functions at the line entrance
3192 beta    beta functions at the line entrance
3193 eta     dispersion functions at the line entrance
3194 etap    dispersion derivatives functions at the line entrance
3195 codvect closed orbit functions at the line entrance
3196
3197 Output:
3198 none
3199
3200 Return:
3201 none
3202
3203 Global variables:
3204
3205
3206 Specific functions:
3207 TransTrace
3208
3209 Comments:
3210 redundant with ttwiss
3211
3212 ****************************************************************************/
3213void TransTwiss(Vector2 &alpha, Vector2 &beta, Vector2 &eta, Vector2 &etap,
3214        Vector &codvect) {
3215    TransTrace(0, globval.Cell_nLoc, alpha, beta, eta, etap, codvect);
3216}
3217
3218/****************************************************************************/
3219/* void ttwiss(double *alpha, double *beta, double *eta, double *etap, double dP)
3220
3221 Purpose:
3222 Calculate Twiss functions for transport line
3223
3224 Input:
3225 none
3226
3227 Output:
3228 none
3229
3230 Return:
3231 none
3232
3233 Global variables:
3234 none
3235
3236 Specific functions:
3237 none
3238
3239 Comments:
3240 redundant with TransTwiss
3241
3242 ****************************************************************************/
3243void ttwiss(const Vector2 &alpha, const Vector2 &beta, const Vector2 &eta,
3244        const Vector2 &etap, const double dP) {
3245    TraceABN(0, globval.Cell_nLoc, alpha, beta, eta, etap, dP);
3246}
3247
3248/****************************************************************************/
3249/* void findcodS(double dP)
3250
3251 Purpose:
3252 Search for the closed orbit using a numerical method
3253 Algo: Newton_Raphson method
3254 Quadratic convergence
3255 May need a guess starting point
3256 Simple precision algorithm
3257
3258 Input:
3259 dP energy offset
3260
3261 Output:
3262 none
3263
3264 Return:
3265 none
3266
3267 Global variables:
3268 none
3269
3270 specific functions:
3271 Newton_Raphson
3272
3273 Comments:
3274 Method introduced because of bad convergence of da for ID using RADIA maps
3275
3276 ****************************************************************************/
3277void findcodS(double dP) {
3278    double *vcod;
3279    Vector x0;
3280    const int ntrial = 40; // maximum number of trials for closed orbit
3281    const double tolx = 1e-8; // numerical precision
3282    int k=0;
3283    int dim=0; // 4D or 6D tracking
3284    long lastpos=0L;
3285
3286    vcod = dvector(1, 6);
3287
3288    // starting point
3289    for (k = 1; k <= 6; k++)
3290        vcod[k] = 0.0;
3291
3292    vcod[5] = dP; // energy offset
3293
3294    if (globval.Cavity_on) {
3295        dim = 6; /* 6D tracking*/
3296        fprintf(stdout, "Error looking for cod in 6D\n");
3297        exit_(1);
3298    } else {
3299        dim = 4; /* 4D tracking */
3300        vcod[1] = Cell[0].Eta[0] * dP;
3301        vcod[2] = Cell[0].Etap[0] * dP;
3302        vcod[3] = Cell[0].Eta[1] * dP;
3303        vcod[4] = Cell[0].Etap[1] * dP;
3304    }
3305
3306    Newton_RaphsonS(ntrial, vcod, dim, tolx);
3307
3308    if (status.codflag == false)
3309        fprintf(stdout, "Error No COD found\n");
3310    if (trace) {
3311        for (k = 1; k <= 6; k++)
3312            x0[k - 1] = vcod[k];
3313        fprintf(stdout, "Before cod % .5e % .5e % .5e % .5e % .5e % .5e \n",
3314                x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]);
3315        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos);
3316        fprintf(stdout, "After  cod % .5e % .5e % .5e % .5e % .5e % .5e \n",
3317                x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]);
3318        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos);
3319    }
3320    free_dvector(vcod, 1, 6);
3321}
3322
3323/****************************************************************************/
3324/* void findcod(double dP)
3325
3326 Purpose:
3327 Search for the closed orbit using a numerical method
3328 Algo: Newton_Raphson method
3329 Quadratic convergence
3330 May need a guess starting point
3331 Simple precision algorithm
3332 4D
3333 Starting point: linear closed orbit
3334
3335 6D
3336 Starting point: zero
3337 if radiation on : x[5] is the synchroneous phase (equilibrium RF phase)
3338 off: x[5] is zero
3339
3340 Input:
3341 dP energy offset
3342
3343 Output:
3344 none
3345
3346 Return:
3347 vcod:  6-D closed orbit
3348
3349 Global variables:
3350 none
3351
3352 specific functions:
3353 Newton_Raphson
3354
3355 Comments:
3356 Method introduced because of bad convergence of da for ID
3357 using RADIA maps
3358
3359 ****************************************************************************/
3360void findcod(double dP) {
3361    Vector vcod;
3362    const int ntrial = 40; // maximum number of trials for closed orbit
3363    const double tolx = 1e-10; // numerical precision
3364    int k=0, dim = 0;
3365    long lastpos=0L;
3366
3367    // initializations
3368    for (k = 0; k <= 5; k++)
3369        vcod[k] = 0.0;
3370
3371    if (globval.Cavity_on) {
3372        fprintf(stdout, "warning looking for cod in 6D\n");
3373        dim = 6;
3374    } else { // starting point linear closed orbit
3375        dim = 4;
3376        vcod[0] = Cell[0].Eta[0] * dP;
3377        vcod[1] = Cell[0].Etap[0] * dP;
3378        vcod[2] = Cell[0].Eta[1] * dP;
3379        vcod[3] = Cell[0].Etap[1] * dP;
3380        vcod[4] = dP; // energy offset
3381    }
3382
3383    Newton_Raphson(dim, vcod, ntrial, tolx);
3384
3385    if (status.codflag == false)
3386        fprintf(stdout, "Error No COD found\n");
3387
3388    CopyVec(6, vcod, globval.CODvect); // save closed orbit at the ring entrance
3389
3390    if (trace) {
3391        fprintf(stdout, "Before cod2 % .5e % .5e % .5e % .5e % .5e % .5e \n",
3392                vcod[0], vcod[1], vcod[2], vcod[3], vcod[4], vcod[5]);
3393        Cell_Pass(0, globval.Cell_nLoc, vcod, lastpos);
3394        fprintf(stdout, "After  cod2 % .5e % .5e % .5e % .5e % .5e % .5e \n",
3395                vcod[0], vcod[1], vcod[2], vcod[3], vcod[4], vcod[5]);
3396    }
3397}
3398/****************************************************************************/
3399/* void computeFandJS(double *x, int n, double **fjac, double *fvect)
3400
3401 Purpose:
3402 Simple precision algo
3403 Tracks x over one turn. And computes the Jacobian matrix of the
3404 transformation by numerical differentiation.
3405 using forward difference formula : faster but less accurate
3406 using symmetric difference formula
3407
3408 Input:
3409 x vector for evaluation
3410 n dimension 4 or 6
3411
3412 Output:
3413 fvect transport of x over one turn
3414 fjac  Associated jacobian matrix
3415
3416 Return:
3417 none
3418
3419 Global variables:
3420 none
3421
3422 specific functions:
3423 none
3424
3425 Comments:
3426 none
3427
3428 ****************************************************************************/
3429
3430void computeFandJS(double *x, int n, double **fjac, double *fvect) {
3431    int i=0, k=0;
3432    long lastpos = 0L;
3433    Vector x0, fx, fx1, fx2;
3434
3435    const double deps = 1e-8; //stepsize for numerical differentiation
3436
3437    for (i = 1; i <= 6; i++)
3438        x0[i - 1] = x[i];
3439
3440    Cell_Pass(0, globval.Cell_nLoc, x0, lastpos);
3441
3442    for (i = 1; i <= n; i++) {
3443        fvect[i] = x0[i - 1];
3444        fx[i - 1] = x0[i - 1];
3445    }
3446
3447    // compute Jacobian matrix by numerical differentiation
3448    for (k = 0; k < n; k++) {
3449        for (i = 1; i <= 6; i++)
3450            x0[i - 1] = x[i];
3451        x0[k] += deps; // differential step in coordinate k
3452
3453        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos); // tracking along the ring
3454        for (i = 1; i <= 6; i++)
3455            fx1[i - 1] = x0[i - 1];
3456
3457        for (i = 1; i <= 6; i++)
3458            x0[i - 1] = x[i];
3459        x0[5] = 0.0;
3460        x0[k] -= deps; // differential step in coordinate k
3461
3462        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos); // tracking along the ring
3463        for (i = 1; i <= 6; i++)
3464            fx2[i - 1] = x0[i - 1];
3465
3466        for (i = 1; i <= n; i++) // symmetric difference formula
3467            fjac[i][k + 1] = 0.5 * (fx1[i - 1] - fx2[i - 1]) / deps;
3468        //~ for (i = 1; i <= n; i++) // forward difference formula
3469        //~ fjac[i][k + 1] = (float) ((x0[i - 1] - fx[i - 1]) / deps);
3470    }
3471}
3472
3473/****************************************************************************/
3474/* void computeFand(int n, float *x, float **fjac, float *fvect)
3475
3476 Purpose:
3477 Tracks x over one turn. And computes the Jacobian matrix of the
3478 transformation by numerical differentiation.
3479 using symmetric difference formula
3480 double precision algorithm
3481
3482 Input:
3483 x vector for evaluation
3484
3485 Output:
3486 fvect transport of x over one turn
3487 fjac  Associated jacobian matrix
3488
3489 Return:
3490 none
3491
3492 Global variables:
3493 none
3494
3495 specific functions:
3496 none
3497
3498 Comments:
3499 none
3500
3501 ****************************************************************************/
3502void computeFandJ(int n, Vector &x, Matrix &fjac, Vector &fvect) {
3503    int i=0, k=0;
3504    long lastpos = 0;
3505    Vector x0, fx1, fx2;
3506
3507    const double deps = 1e-8; //stepsize for numerical differentiation
3508
3509    CopyVec(6, x, x0);
3510
3511    Cell_Pass(0, globval.Cell_nLoc, x0, lastpos);
3512    CopyVec(n, x0, fvect);
3513
3514    // compute Jacobian matrix by numerical differentiation
3515    for (k = 0; k < n; k++) {
3516        CopyVec(6L, x, x0);
3517        x0[k] += deps; // differential step in coordinate k
3518
3519        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos); // tracking along the ring
3520        CopyVec(6L, x0, fx1);
3521
3522        CopyVec(6L, x, x0);
3523        x0[k] -= deps; // differential step in coordinate k
3524
3525        Cell_Pass(0, globval.Cell_nLoc, x0, lastpos); // tracking along the ring
3526        CopyVec(6L, x0, fx2);
3527
3528        for (i = 0; i < n; i++) // symmetric difference formula
3529            fjac[i][k] = 0.5 * (fx1[i] - fx2[i]) / deps;
3530    }
3531}
3532
3533/****************************************************************************/
3534/* void Newton_RaphsonS(int ntrial,double x[],int n,double tolx, double tolf)
3535 
3536 Purpose:
3537 Newton_Rapson algorithm from Numerical Recipes
3538 single precision algorithm
3539 Robustess: quadratic convergence
3540 Hint: for n-dimensional problem, the algo can be stuck on local minimum
3541 In this case, it should be enough to provide a resonable starting
3542 point.
3543
3544 Method:
3545 look for closed orbit solution of f(x) = x
3546 This problems is equivalent to finding the zero of g(x)= f(x) - x
3547 g(x+h) ~= f(x) - x + (Jacobian(f) -Id) h + O(h*h)
3548 Then at first order we solve h:
3549 h = - inverse(Jacobian(f) -Id) * (f(x)-x)
3550 the new guess is then xnew = x + h
3551 By iteration, this converges quadratically.
3552
3553 The algo is stopped whenever  |x -xnew| < tolx
3554
3555 f(x) is computes by tracking over one turn
3556 Jacobian(f) is computed numerically by numerical differentiation
3557 These two operations are provided by the function computeFandJ
3558
3559 Input:
3560 ntrial number of iterations for closed zero search
3561 n number of dimension 4 or 6
3562 x intial guess for the closed orbit
3563 tolx tolerance over the solution x
3564 tolf tolerance over the evalution f(x)
3565
3566 Output:
3567 x closed orbit
3568
3569 Return:
3570 none
3571
3572 Global variables:
3573 status
3574
3575 specific functions:
3576 computeFandJS
3577 ludcmp,lubksb
3578
3579 Comments:
3580 none
3581
3582 ****************************************************************************/
3583
3584void Newton_RaphsonS(int ntrial, double x[], int n, double tolx) {
3585    int k=0, i=0, *indx;
3586    double errx=0.0, d=0.0, *bet, *fvect, **alpha;
3587
3588    errx = 0.0;
3589    // NR arrays start from 1 and not 0 !!!
3590    indx = ivector(1, n);
3591    bet = dvector(1, n);
3592    fvect = dvector(1, n);
3593    alpha = dmatrix(1, n, 1, n);
3594
3595    for (k = 1; k <= ntrial; k++) { // loop over number of iterations
3596        // supply function values at x in fvect and Jacobian matrix in fjac
3597        computeFandJS(x, n, alpha, fvect);
3598
3599        // Jacobian -Id
3600        for (i = 1; i <= n; i++)
3601            alpha[i][i] -= 1.0;
3602        for (i = 1; i <= n; i++)
3603            bet[i] = x[i] - fvect[i]; // right side of linear equation
3604        // solve linear equations using LU decomposition using NR routines
3605        dludcmp(alpha, n, indx, &d);
3606        dlubksb(alpha, n, indx, bet);
3607        errx = 0.0; // check root convergence
3608        for (i = 1; i <= n; i++) { // update solution
3609            errx += fabs(bet[i]);
3610            x[i] += bet[i];
3611        }
3612
3613        if (trace)
3614            fprintf(
3615                    stdout,
3616                    "%02d: cod % .5e % .5e % .5e % .5e % .5e % .5e  errx =% .5e\n",
3617                    k, x[1], x[2], x[3], x[4], x[5], x[6], errx);
3618        if (errx <= tolx) {
3619            status.codflag = true;
3620            break;
3621        }
3622    }
3623    // check whenver closed orbit found out
3624    if ((k >= ntrial) && (errx >= tolx * 100))
3625        status.codflag = false;
3626
3627    free_dmatrix(alpha, 1, n, 1, n);
3628    free_dvector(bet, 1, n);
3629    free_dvector(fvect, 1, n);
3630    free_ivector(indx, 1, n);
3631}
3632
3633/****************************************************************************/
3634/* int Newton_Raphson(int n, double x[], int ntrial, double tolx)
3635 
3636 Purpose:
3637 Newton_Rapson algorithm from Numerical Recipes
3638 double precision algorithm
3639 Robustess: quadratic convergence
3640 Hint: for n-dimensional problem, the algo can be stuck on local minimum
3641 In this case, it should be enough to provide a resonable starting
3642 point.
3643
3644 Method:
3645 look for closed orbit solution of f(x) = x
3646 This problems is equivalent to finding the zero of g(x)= f(x) - x
3647 g(x+h) ~= f(x) - x + (Jacobian(f) -Id) h + O(h*h)
3648 Then at first order we solve h:
3649 h = - inverse(Jacobian(f) -Id) * (f(x)-x)
3650 the new guess is then xnew = x + h
3651 By iteration, this converges quadratically.
3652
3653 The algo is stopped whenever  |x -xnew| < tolx
3654
3655 f(x) is computes by tracking over one turn
3656 Jacobian(f) is computed numerically by numerical differentiation
3657 These two operations are provided by the function computeFandJ
3658
3659 Input:
3660 ntrial number of iterations for closed zero search
3661 x intial guess for the closed orbit
3662 tolx tolerance over the solution x
3663 tolf tolerance over the evalution f(x)
3664
3665 Output:
3666 x closed orbit
3667
3668 Return:
3669 none
3670
3671 Global variables:
3672 status
3673
3674 specific functions:
3675 computeFandJ
3676 InvMat, LinTrans
3677
3678 Comments:
3679 none
3680
3681 ****************************************************************************/
3682int Newton_Raphson(int n, Vector &x, int ntrial, double tolx) {
3683    int k=0, i=0;
3684    double errx=0.0;
3685    Vector bet, fvect;
3686    Matrix alpha;
3687
3688    errx = 0.0;
3689
3690    for (k = 1; k <= ntrial; k++) { // loop over number of iterations
3691        // supply function values at x in fvect and Jacobian matrix in fjac
3692        computeFandJ(n, x, alpha, fvect);
3693
3694        // Jacobian - Id
3695        for (i = 0; i < n; i++)
3696            alpha[i][i] -= 1.0;
3697        for (i = 0; i < n; i++)
3698            bet[i] = x[i] - fvect[i]; // right side of linear equation
3699        // inverse matrix using gauss jordan method from Tracy (from NR)
3700        if (!InvMat((long) n, alpha))
3701            fprintf(stdout, "Matrix non inversible ...\n");
3702        LinTrans((long) n, alpha, bet); // bet = alpha*bet
3703        errx = 0.0; // check root convergence
3704        for (i = 0; i < n; i++) { // update solution
3705            errx += fabs(bet[i]);
3706            x[i] += bet[i];
3707        }
3708
3709        if (trace)
3710            fprintf(
3711                    stdout,
3712                    "%02d: cod2 % .5e % .5e % .5e % .5e % .5e % .5e  errx =% .5e\n",
3713                    k, x[0], x[1], x[2], x[3], x[4], x[5], errx);
3714        if (errx <= tolx) {
3715            status.codflag = true;
3716            return 1;
3717        }
3718    }
3719    // check whever closed orbit found out
3720    if ((k >= ntrial) && (errx >= tolx)) {
3721        status.codflag = false;
3722        return 1;
3723    }
3724    return 0;
3725}
3726/*******************************************************************************
3727 *
3728 *
3729 *
3730 *
3731 ******************************************************************************/
3732void rm_mean(long int n, double x[]) {
3733    long int i=0L;
3734    double mean=0.0;
3735
3736    for (i = 0; i < n; i++)
3737        mean += x[i];
3738    mean /= n;
3739    for (i = 0; i < n; i++)
3740        x[i] -= mean;
3741}
Note: See TracBrowser for help on using the repository browser.