00001
00002
00003 #include "cddefines.h"
00004 #include "optimize.h"
00005
00006 static void calcc(long,float*,long,long,int,float[]);
00007 static double cdasum(long,float[],long);
00008 static void cdaxpy(long,double,float[],long,float[],long);
00009 static void cdcopy(long,float[],long,float[],long);
00010 static void csscal(long,double,float[],long);
00011 static double dist(long,float[],float[]);
00012 static void evalf(long,long[],float[],long,float[],float*,long*);
00013 static void fstats(double,long,int);
00014 static void newpt(long,double,float[],float[],int,float[],int*);
00015 static void order(long,float[],long*,long*,long*);
00016 static void partx(long,long[],float[],long*,long[]);
00017 static void setstp(long,long,float[],float[]);
00018 static void simplx(long,float[],long,long[],long,int,float[],
00019 float*,long*,float*,float[],long*);
00020 static void sortd(long,float[],long[]);
00021 static void start(long,float[],float[],long,long[],float*,int*);
00022 static void subopt(long);
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 struct t_usubc {
00038 float alpha,
00039 beta,
00040 gamma,
00041 delta,
00042 psi,
00043 omega;
00044 long int nsmin,
00045 nsmax,
00046 irepl,
00047 ifxsw;
00048 float bonus,
00049 fstop;
00050 long int nfstop,
00051 nfxe;
00052 float fxstat[4],
00053 ftest;
00054 int minf,
00055 initx,
00056 newx;
00057 } usubc;
00058 struct t_isubc {
00059 float fbonus,
00060 sfstop,
00061 sfbest;
00062 int IntNew;
00063 } isubc;
00064
00065 void optimize_subplex(long int n,
00066 double tol,
00067 long int maxnfe,
00068 long int mode,
00069 float scale[],
00070 float x[],
00071 float *fx,
00072 long int *nfe,
00073 float work[],
00074 long int iwork[],
00075 long int *iflag)
00076 {
00077 static int cmode;
00078 static long int i,
00079 i_,
00080 ifsptr,
00081 ins,
00082 insfnl,
00083 insptr,
00084 ipptr,
00085 isptr,
00086 istep,
00087 istptr,
00088 nnn,
00089 ns,
00090 nsubs;
00091 static float dum,
00092 scl[1],
00093 sfx,
00094 xpscl,
00095 xstop,
00096 xstop2;
00097
00098 static const float bnsfac[2][3] = { {-1.0f,-2.0f,0.0f}, {1.0f,0.0f,2.0f} };
00099
00100 DEBUG_ENTRY( "optimize_subplex()" );
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185 if( (mode%2) == 0 )
00186 {
00187
00188
00189
00190 if( n < 1 )
00191 goto L_120;
00192 if( tol < 0.0f )
00193 goto L_120;
00194 if( maxnfe < 1 )
00195 goto L_120;
00196 if( scale[0] >= 0.0f )
00197 {
00198 for( i=1; i <= n; i++ )
00199 {
00200 i_ = i - 1;
00201 xpscl = x[i_] + scale[i_];
00202
00203 if( xpscl == x[i_] )
00204
00205 goto L_120;
00206 }
00207 }
00208 else
00209 {
00210 scl[0] = (float)fabs(scale[0]);
00211 for( i=1; i <= n; i++ )
00212 {
00213 i_ = i - 1;
00214 xpscl = x[i_] + scl[0];
00215
00216 if( xpscl == x[i_] )
00217
00218 goto L_120;
00219 }
00220 }
00221 if( ((mode/2)%2) == 0 )
00222 {
00223 subopt(n);
00224 }
00225 else if( usubc.alpha <= 0.0f )
00226 {
00227 goto L_120;
00228 }
00229 else if( usubc.beta <= 0.0f || usubc.beta >= 1.0f )
00230 {
00231 goto L_120;
00232 }
00233 else if( usubc.gamma <= 1.0f )
00234 {
00235 goto L_120;
00236 }
00237 else if( usubc.delta <= 0.0f || usubc.delta >= 1.0f )
00238 {
00239 goto L_120;
00240 }
00241 else if( usubc.psi <= 0.0f || usubc.psi >= 1.0f )
00242 {
00243 goto L_120;
00244 }
00245 else if( usubc.omega <= 0.0f || usubc.omega >= 1.0f )
00246 {
00247 goto L_120;
00248 }
00249 else if( (usubc.nsmin < 1 || usubc.nsmax < usubc.nsmin) ||
00250 n < usubc.nsmax )
00251 {
00252 goto L_120;
00253 }
00254 else if( n < ((n - 1)/usubc.nsmax + 1)*usubc.nsmin )
00255 {
00256 goto L_120;
00257 }
00258 else if( usubc.irepl < 0 || usubc.irepl > 2 )
00259 {
00260 goto L_120;
00261 }
00262 else if( usubc.ifxsw < 1 || usubc.ifxsw > 3 )
00263 {
00264 goto L_120;
00265 }
00266 else if( usubc.bonus < 0.0f )
00267 {
00268 goto L_120;
00269 }
00270 else if( usubc.nfstop < 0 )
00271 {
00272 goto L_120;
00273 }
00274
00275
00276
00277 istptr = n + 1;
00278 isptr = istptr + n;
00279 ifsptr = isptr + usubc.nsmax*(usubc.nsmax + 3);
00280 insptr = n + 1;
00281 if( scale[0] > 0.0f )
00282 {
00283 cdcopy(n,scale,1,work,1);
00284 cdcopy(n,scale,1,&work[istptr-1],1);
00285 }
00286 else
00287 {
00288 cdcopy(n,scl,0,work,1);
00289 cdcopy(n,scl,0,&work[istptr-1],1);
00290 }
00291 for( i=1; i <= n; i++ )
00292 {
00293 i_ = i - 1;
00294 iwork[i_] = i;
00295 }
00296 *nfe = 0;
00297 usubc.nfxe = 1;
00298 if( usubc.irepl == 0 )
00299 {
00300 isubc.fbonus = 0.0f;
00301 }
00302 else if( usubc.minf )
00303 {
00304 isubc.fbonus = bnsfac[0][usubc.ifxsw-1]*usubc.bonus;
00305 }
00306 else
00307 {
00308 isubc.fbonus = bnsfac[1][usubc.ifxsw-1]*usubc.bonus;
00309 }
00310 if( usubc.nfstop == 0 )
00311 {
00312 isubc.sfstop = 0.0f;
00313 }
00314 else if( usubc.minf )
00315 {
00316 isubc.sfstop = usubc.fstop;
00317 }
00318 else
00319 {
00320 isubc.sfstop = -usubc.fstop;
00321 }
00322 usubc.ftest = 0.0f;
00323 cmode = false;
00324 isubc.IntNew = true;
00325 usubc.initx = true;
00326 nnn = 0;
00327 evalf(nnn,iwork,(float*)&dum,n,x,&sfx,nfe);
00328 usubc.initx = false;
00329
00330
00331
00332 }
00333 else if( *iflag == 2 )
00334 {
00335 if( usubc.minf )
00336 {
00337 isubc.sfstop = usubc.fstop;
00338 }
00339 else
00340 {
00341 isubc.sfstop = -usubc.fstop;
00342 }
00343 cmode = true;
00344 goto L_70;
00345 }
00346 else if( *iflag == -1 )
00347 {
00348 cmode = true;
00349 goto L_70;
00350 }
00351 else if( *iflag == 0 )
00352 {
00353 cmode = false;
00354 goto L_90;
00355 }
00356 else
00357 {
00358
00359 DEBUG_EXIT( "optimize_subplex()" );
00360 return;
00361 }
00362
00363
00364
00365 L_40:
00366
00367 for( i=0; i < n; i++ )
00368 {
00369 work[i] = (float)fabs(work[i]);
00370 }
00371
00372 sortd(n,work,iwork);
00373 partx(n,iwork,work,&nsubs,&iwork[insptr-1]);
00374 cdcopy(n,x,1,work,1);
00375 ins = insptr;
00376 insfnl = insptr + nsubs - 1;
00377 ipptr = 1;
00378
00379
00380
00381
00382 L_60:
00383 ns = iwork[ins-1];
00384
00385 L_70:
00386 simplx(n,&work[istptr-1],ns,&iwork[ipptr-1],maxnfe,cmode,x,&sfx,
00387 nfe,&work[isptr-1],&work[ifsptr-1],iflag);
00388
00389 cmode = false;
00390 if( *iflag != 0 )
00391 goto L_121;
00392
00393 if( ins < insfnl )
00394 {
00395 ins += 1;
00396 ipptr += ns;
00397 goto L_60;
00398 }
00399
00400
00401
00402 for( i=0; i < n; i++ )
00403 {
00404 work[i] = x[i] - work[i];
00405 }
00406
00407
00408
00409 L_90:
00410
00411
00412
00413 istep = istptr-1;
00414 xstop = 0.f;
00415 xstop2 = 0.f;
00416 for( i=0; i < n; i++ )
00417 {
00418 float ttemp;
00419 xstop += POW2(work[i]);
00420 ttemp = (float)fabs(work[istep]);
00421
00422
00423 xstop2 = MAX2( xstop2 , ttemp );
00424 istep++;
00425 }
00426
00427 if( sqrt(xstop) > tol || xstop2*usubc.psi > (float)tol )
00428 {
00429 setstp(nsubs,n,work,&work[istptr-1]);
00430 goto L_40;
00431 }
00432
00433
00434
00435
00436 *iflag = 0;
00437 L_121:
00438 if( usubc.minf )
00439 {
00440 *fx = sfx;
00441 }
00442 else
00443 {
00444 *fx = -sfx;
00445 }
00446
00447 DEBUG_EXIT( "optimize_subplex()" );
00448 return;
00449
00450
00451
00452 L_120:
00453
00454 *iflag = -2;
00455
00456 DEBUG_EXIT( "optimize_subplex()" );
00457 return;
00458 }
00459
00460 static void subopt(long int n)
00461 {
00462
00463 DEBUG_ENTRY( "subopt()" );
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494 usubc.alpha = 1.0f;
00495
00496
00497
00498
00499 usubc.beta = .50f;
00500
00501
00502
00503
00504 usubc.gamma = 2.0f;
00505
00506
00507
00508
00509 usubc.delta = .50f;
00510
00511
00512
00513
00514
00515
00516
00517
00518 usubc.psi = .250f;
00519
00520
00521
00522
00523 usubc.omega = .10f;
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535 usubc.nsmin = MIN2(2,n);
00536
00537
00538
00539 usubc.nsmax = MIN2(5,n);
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585 usubc.irepl = 0;
00586
00587
00588
00589
00590
00591
00592
00593 usubc.ifxsw = 1;
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610 usubc.bonus = 1.0f;
00611
00612
00613
00614
00615
00616
00617
00618
00619 usubc.nfstop = 0;
00620
00621
00622
00623
00624
00625
00626
00627
00628 usubc.minf = true;
00629
00630 DEBUG_EXIT( "subopt()" );
00631 return;
00632 }
00633
00634
00635 static void cdcopy(long int n,
00636 float dx[],
00637 long int incx,
00638 float dy[],
00639 long int incy)
00640 {
00641 long int i,
00642 ix,
00643 iy,
00644 m;
00645
00646 DEBUG_ENTRY( "cdcopy()" );
00647
00648
00649
00650
00651
00652
00653
00654 if( n > 0 )
00655 {
00656 if( incx == 1 && incy == 1 )
00657 {
00658
00659
00660
00661
00662 m = n%7;
00663 if( m != 0 )
00664 {
00665 for( i=0; i < m; i++ )
00666 {
00667 dy[i] = dx[i];
00668 }
00669 if( n < 7 )
00670 {
00671 DEBUG_EXIT( "cdcopy()" );
00672 return;
00673 }
00674 }
00675
00676 for( i=m; i < n; i += 7 )
00677 {
00678 dy[i] = dx[i];
00679 dy[i+1] = dx[i+1];
00680 dy[i+2] = dx[i+2];
00681 dy[i+3] = dx[i+3];
00682 dy[i+4] = dx[i+4];
00683 dy[i+5] = dx[i+5];
00684 dy[i+6] = dx[i+6];
00685 }
00686 }
00687 else
00688 {
00689
00690
00691
00692
00693 ix = 1;
00694 iy = 1;
00695 if( incx < 0 )
00696 ix = (-n + 1)*incx + 1;
00697 if( incy < 0 )
00698 iy = (-n + 1)*incy + 1;
00699 for( i=0; i < n; i++ )
00700 {
00701 dy[iy-1] = dx[ix-1];
00702 ix += incx;
00703 iy += incy;
00704 }
00705 }
00706 }
00707
00708 DEBUG_EXIT( "cdcopy()" );
00709 return;
00710 }
00711
00712 static void evalf(long int ns,
00713 long int ips[],
00714 float xs[],
00715 long int n,
00716 float x[],
00717 float *sfx,
00718 long int *nfe)
00719 {
00720 static int newbst;
00721 static long int i,
00722 i_;
00723 static float fx;
00724
00725
00726
00727 DEBUG_ENTRY( "evalf()" );
00728
00729 i_ = n;
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778 for( i=1; i <= ns; i++ )
00779 {
00780 i_ = i - 1;
00781 x[ips[i_]-1] = xs[i_];
00782 }
00783 usubc.newx = isubc.IntNew || usubc.irepl != 2;
00784 fx = (float)optimize_func(x);
00785
00786 if( usubc.irepl == 0 )
00787 {
00788 if( usubc.minf )
00789 {
00790 *sfx = fx;
00791 }
00792 else
00793 {
00794 *sfx = -fx;
00795 }
00796 }
00797 else if( isubc.IntNew )
00798 {
00799 if( usubc.minf )
00800 {
00801 *sfx = fx;
00802 newbst = fx < usubc.ftest;
00803 }
00804 else
00805 {
00806 *sfx = -fx;
00807 newbst = fx > usubc.ftest;
00808 }
00809 if( usubc.initx || newbst )
00810 {
00811 if( usubc.irepl == 1 )
00812 fstats(fx,1,true);
00813 usubc.ftest = fx;
00814 isubc.sfbest = *sfx;
00815 }
00816 }
00817 else
00818 {
00819 if( usubc.irepl == 1 )
00820 {
00821 fstats(fx,1,false);
00822 fx = usubc.fxstat[usubc.ifxsw-1];
00823 }
00824 usubc.ftest = fx + isubc.fbonus*usubc.fxstat[3];
00825 if( usubc.minf )
00826 {
00827 *sfx = usubc.ftest;
00828 isubc.sfbest = fx;
00829 }
00830 else
00831 {
00832 *sfx = -usubc.ftest;
00833 isubc.sfbest = -fx;
00834 }
00835 }
00836 *nfe += 1;
00837
00838 DEBUG_EXIT( "evalf()" );
00839 return;
00840 }
00841
00842
00843 static void setstp(long int nsubs,
00844 long int n,
00845 float deltax[],
00846 float step[])
00847 {
00848 static long int i,
00849 i_;
00850 static float stpfac;
00851
00852 DEBUG_ENTRY( "setstp()" );
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894 if( nsubs > 1 )
00895 {
00896 double a , b , c;
00897 a = cdasum(n,deltax,1);
00898 b = cdasum(n,step,1);
00899 c = MAX2(a/b ,usubc.omega);
00900 stpfac = (float)MIN2(c , 1.0f/usubc.omega);
00901 }
00902 else
00903 {
00904 stpfac = usubc.psi;
00905 }
00906 csscal(n,stpfac,step,1);
00907
00908
00909
00910 for( i=1; i <= n; i++ )
00911 {
00912 i_ = i - 1;
00913 if( deltax[i_] == 0.f )
00914 {
00915 step[i_] = -step[i_];
00916 }
00917 else
00918 {
00919 step[i_] = (float)sign(step[i_],deltax[i_]);
00920 }
00921 }
00922
00923 DEBUG_EXIT( "setstp()" );
00924 return;
00925 }
00926
00927
00928 static void sortd(long int n,
00929 float xkey[],
00930 long int ix[])
00931 {
00932 long int i,
00933 i_,
00934 ifirst,
00935 ilast,
00936 iswap,
00937 ixi,
00938 ixip1;
00939
00940 DEBUG_ENTRY( "sortd()" );
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973 ifirst = 1;
00974 iswap = 1;
00975 ilast = n - 1;
00976 while( ifirst <= ilast )
00977 {
00978 for( i=ifirst; i <= ilast; i++ )
00979 {
00980 i_ = i - 1;
00981 ixi = ix[i_];
00982 ixip1 = ix[i_+1];
00983 if( xkey[ixi-1] < xkey[ixip1-1] )
00984 {
00985 ix[i_] = ixip1;
00986 ix[i_+1] = ixi;
00987 iswap = i;
00988 }
00989 }
00990 ilast = iswap - 1;
00991 for( i=ilast; i >= ifirst; i-- )
00992 {
00993 i_ = i - 1;
00994 ixi = ix[i_];
00995 ixip1 = ix[i_+1];
00996 if( xkey[ixi-1] < xkey[ixip1-1] )
00997 {
00998 ix[i_] = ixip1;
00999 ix[i_+1] = ixi;
01000 iswap = i;
01001 }
01002 }
01003 ifirst = iswap + 1;
01004 }
01005
01006 DEBUG_EXIT( "sortd()" );
01007 return;
01008 }
01009
01010
01011 static void partx(long int n,
01012 long int ip[],
01013 float absdx[],
01014 long int *nsubs,
01015 long int nsvals[])
01016 {
01017 static long int i,
01018 limit,
01019 nleft,
01020 ns1,
01021 ns1_,
01022 ns2,
01023 nused;
01024 static float as1,
01025 as1max,
01026 as2,
01027 asleft,
01028 gap,
01029 gapmax;
01030
01031 DEBUG_ENTRY( "partx()" );
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070 *nsubs = 0;
01071 nused = 0;
01072 nleft = n;
01073 asleft = absdx[0];
01074 for( i=1; i < n; i++ )
01075 {
01076 asleft += absdx[i];
01077 }
01078
01079 while( nused < n )
01080 {
01081 *nsubs += 1;
01082 as1 = 0.0f;
01083 for( i=0; i < (usubc.nsmin - 1); i++ )
01084 {
01085 as1 += absdx[ip[nused+i]-1];
01086 }
01087
01088 gapmax = -1.0f;
01089 limit = MIN2(usubc.nsmax,nleft);
01090 for( ns1=usubc.nsmin; ns1 <= limit; ns1++ )
01091 {
01092 ns1_ = ns1 - 1;
01093 as1 += absdx[ip[nused+ns1_]-1];
01094 ns2 = nleft - ns1;
01095 if( ns2 > 0 )
01096 {
01097 if( ns2 >= ((ns2 - 1)/usubc.nsmax + 1)*usubc.nsmin )
01098 {
01099 as2 = asleft - as1;
01100 gap = as1/ns1 - as2/ns2;
01101 if( gap > gapmax )
01102 {
01103 gapmax = gap;
01104 nsvals[*nsubs-1] = ns1;
01105 as1max = as1;
01106 }
01107 }
01108 }
01109 else if( as1/ns1 > gapmax )
01110 {
01111 goto L_21;
01112 }
01113 }
01114 nused += nsvals[*nsubs-1];
01115 nleft = n - nused;
01116 asleft -= as1max;
01117 }
01118
01119 DEBUG_EXIT( "partx()" );
01120 return;
01121 L_21:
01122 nsvals[*nsubs-1] = ns1;
01123
01124 DEBUG_EXIT( "partx()" );
01125 return;
01126 }
01127
01128
01129
01130 #undef S
01131 #define S(I_,J_) (*(s+(I_)*(ns)+(J_)))
01132
01133 static void simplx(long int n,
01134 float step[],
01135 long int ns,
01136 long int ips[],
01137 long int maxnfe,
01138 int cmode,
01139 float x[],
01140 float *fx,
01141 long int *nfe,
01142 float *s,
01143 float fs[],
01144 long int *iflag)
01145 {
01146 static int small,
01147 updatc;
01148
01149 static long int i,
01150 icent,
01151 ih,
01152 il,
01153 inew = 0,
01154 is,
01155 itemp,
01156 j,
01157 j_,
01158 npts;
01159
01160 static float dum,
01161 fc,
01162 fe,
01163 fr,
01164 tol;
01165
01166 DEBUG_ENTRY( "simplx()" );
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221
01222
01223
01224
01225
01226
01227
01228
01229
01230
01231
01232
01233
01234
01235
01236
01237
01238
01239
01240 if( cmode )
01241 goto L_50;
01242 npts = ns + 1;
01243 icent = ns + 2;
01244 itemp = ns + 3;
01245 updatc = false;
01246 start(n,x,step,ns,ips,s,&small);
01247 if( small )
01248 {
01249 *iflag = 1;
01250
01251 DEBUG_EXIT( "simplx()" );
01252 return;
01253 }
01254 else
01255 {
01256 if( usubc.irepl > 0 )
01257 {
01258 isubc.IntNew = false;
01259 evalf(ns,ips,&S(0,0),n,x,&fs[0],nfe);
01260 }
01261 else
01262 {
01263 fs[0] = *fx;
01264 }
01265 isubc.IntNew = true;
01266 for( j=2; j <= npts; j++ )
01267 {
01268 j_ = j - 1;
01269 evalf(ns,ips,&S(j_,0),n,x,&fs[j_],nfe);
01270 }
01271 il = 1;
01272 order(npts,fs,&il,&is,&ih);
01273 tol = (float)(usubc.psi*dist(ns,&S(ih-1,0),&S(il-1,0)));
01274 }
01275
01276
01277
01278 L_20:
01279 calcc(ns,s,ih,inew,updatc,&S(icent-1,0));
01280 updatc = true;
01281 inew = ih;
01282
01283
01284
01285 newpt(ns,usubc.alpha,&S(icent-1,0),&S(ih-1,0),true,&S(itemp-1,0),
01286 &small);
01287 if( !small )
01288 {
01289 evalf(ns,ips,&S(itemp-1,0),n,x,&fr,nfe);
01290 if( fr < fs[il-1] )
01291 {
01292
01293
01294
01295 newpt(ns,-usubc.gamma,&S(icent-1,0),&S(itemp-1,0),true,
01296 &S(ih-1,0),&small);
01297 if( small )
01298 goto L_40;
01299 evalf(ns,ips,&S(ih-1,0),n,x,&fe,nfe);
01300 if( fe < fr )
01301 {
01302 fs[ih-1] = fe;
01303 }
01304 else
01305 {
01306 cdcopy(ns,&S(itemp-1,0),1,&S(ih-1,0),1);
01307 fs[ih-1] = fr;
01308 }
01309 }
01310 else if( fr < fs[is-1] )
01311 {
01312
01313
01314
01315 cdcopy(ns,&S(itemp-1,0),1,&S(ih-1,0),1);
01316 fs[ih-1] = fr;
01317 }
01318 else
01319 {
01320
01321
01322
01323 if( fr > fs[ih-1] )
01324 {
01325 newpt(ns,-usubc.beta,&S(icent-1,0),&S(ih-1,0),true,
01326 &S(itemp-1,0),&small);
01327 }
01328 else
01329 {
01330 newpt(ns,-usubc.beta,&S(icent-1,0),&S(itemp-1,0),false,
01331 (float*)&dum,&small);
01332 }
01333 if( small )
01334 goto L_40;
01335 evalf(ns,ips,&S(itemp-1,0),n,x,&fc,nfe);
01336 if( fc < (float)MIN2(fr,fs[ih-1]) )
01337 {
01338 cdcopy(ns,&S(itemp-1,0),1,&S(ih-1,0),1);
01339 fs[ih-1] = fc;
01340 }
01341 else
01342 {
01343
01344
01345
01346 for( j=1; j <= npts; j++ )
01347 {
01348 j_ = j - 1;
01349 if( j != il )
01350 {
01351 newpt(ns,-usubc.delta,&S(il-1,0),&S(j_,0),
01352 false,(float*)&dum,&small);
01353 if( small )
01354 goto L_40;
01355 evalf(ns,ips,&S(j_,0),n,x,&fs[j_],nfe);
01356 }
01357 }
01358 }
01359 updatc = false;
01360 }
01361 order(npts,fs,&il,&is,&ih);
01362 }
01363
01364
01365
01366
01367 L_40:
01368 if( usubc.irepl == 0 )
01369 {
01370 *fx = fs[il-1];
01371 }
01372 else
01373 {
01374 *fx = isubc.sfbest;
01375 }
01376
01377 L_50:
01378 if( (usubc.nfstop > 0 && *fx <= isubc.sfstop) && usubc.nfxe >=
01379 usubc.nfstop )
01380 goto L_51;
01381 if( *nfe >= maxnfe )
01382 goto L_52;
01383 if( !(dist(ns,&S(ih-1,0),&S(il-1,0)) <= tol || small) )
01384 goto L_20;
01385 *iflag = 0;
01386 goto L_53;
01387
01388 L_52:
01389 *iflag = -1;
01390 goto L_53;
01391
01392 L_51:
01393 *iflag = 2;
01394
01395
01396
01397
01398 L_53:
01399 for( i=0; i < ns; i++ )
01400 {
01401 x[ips[i]-1] = S(il-1,i);
01402 }
01403
01404 DEBUG_EXIT( "simplx()" );
01405 return;
01406 }
01407
01408
01409 static void fstats(double fx,
01410 long int ifxwt,
01411 int reset)
01412 {
01413 static long int nsv;
01414 static float f1sv,
01415 fscale;
01416
01417 DEBUG_ENTRY( "fstats()" );
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427
01428
01429
01430
01431
01432
01433
01434
01435
01436
01437
01438
01439
01440
01441
01442
01443
01444
01445
01446
01447
01448
01449 if( reset )
01450 {
01451 usubc.nfxe = ifxwt;
01452 usubc.fxstat[0] = (float)fx;
01453 usubc.fxstat[1] = (float)fx;
01454 usubc.fxstat[2] = (float)fx;
01455 usubc.fxstat[3] = 0.0f;
01456 }
01457 else
01458 {
01459 nsv = usubc.nfxe;
01460 f1sv = usubc.fxstat[0];
01461 usubc.nfxe += ifxwt;
01462 usubc.fxstat[0] += (float)(ifxwt*(fx - usubc.fxstat[0])/usubc.nfxe);
01463 usubc.fxstat[1] = MAX2(usubc.fxstat[1],(float)fx);
01464 usubc.fxstat[2] = MIN2(usubc.fxstat[2],(float)fx);
01465 fscale = (float)MAX3(fabs(usubc.fxstat[1]),fabs(usubc.fxstat[2]),1.);
01466 usubc.fxstat[3] = (float)(fscale*sqrt(((nsv-1)*POW2(usubc.fxstat[3]/
01467 fscale)+nsv*POW2((usubc.fxstat[0]-f1sv)/fscale)+ifxwt*
01468 POW2((fx-usubc.fxstat[0])/fscale))/(usubc.nfxe-1)));
01469 }
01470
01471 DEBUG_EXIT( "fstats()" );
01472 return;
01473 }
01474
01475 static double cdasum(long int n,
01476 float dx[],
01477 long int incx)
01478 {
01479
01480
01481
01482
01483
01484
01485
01486
01487 long int i,
01488 ix,
01489 m;
01490 float cdasum_v,
01491 dtemp;
01492
01493 DEBUG_ENTRY( "cdasum()" );
01494
01495 cdasum_v = 0.00f;
01496 dtemp = 0.00f;
01497 if( n > 0 )
01498 {
01499 if( incx == 1 )
01500 {
01501
01502
01503
01504
01505
01506
01507 m = n%6;
01508 if( m != 0 )
01509 {
01510 for( i=0; i < m; i++ )
01511 {
01512 dtemp += (float)fabs(dx[i]);
01513 }
01514 if( n < 6 )
01515 goto L_60;
01516 }
01517
01518 for( i=m; i < n; i += 6 )
01519 {
01520 dtemp += (float)(fabs(dx[i]) + fabs(dx[i+1]) + fabs(dx[i+2]) +
01521 fabs(dx[i+3]) + fabs(dx[i+4]) + fabs(dx[i+5]));
01522 }
01523 L_60:
01524 cdasum_v = dtemp;
01525 }
01526 else
01527 {
01528
01529
01530
01531 ix = 1;
01532
01533 if( incx < 0 )
01534 ix = (-n + 1)*incx + 1;
01535
01536 for( i=0; i < n; i++ )
01537 {
01538 dtemp += (float)fabs(dx[ix-1]);
01539 ix += incx;
01540 }
01541 cdasum_v = dtemp;
01542 }
01543 }
01544
01545 DEBUG_EXIT( "cdasum()" );
01546 return( cdasum_v );
01547 }
01548
01549 static void csscal(long int n,
01550 double da,
01551 float dx[],
01552 long int incx)
01553 {
01554 long int
01555 i,
01556 i_,
01557 m,
01558 mp1,
01559 nincx;
01560
01561 DEBUG_ENTRY( "csscal()" );
01562
01563
01564
01565
01566
01567
01568
01569
01570
01571 if( !(n <= 0 || incx <= 0) )
01572 {
01573 if( incx == 1 )
01574 {
01575
01576
01577
01578
01579
01580
01581 m = n%5;
01582 if( m != 0 )
01583 {
01584 for( i=1; i <= m; i++ )
01585 {
01586 i_ = i - 1;
01587 dx[i_] *= (float)da;
01588 }
01589 if( n < 5 )
01590 {
01591 DEBUG_EXIT( "csscal()" );
01592 return;
01593 }
01594 }
01595 mp1 = m + 1;
01596 for( i=mp1; i <= n; i += 5 )
01597 {
01598 i_ = i - 1;
01599 dx[i_] *= (float)da;
01600 dx[i_+1] *= (float)da;
01601 dx[i_+2] *= (float)da;
01602 dx[i_+3] *= (float)da;
01603 dx[i_+4] *= (float)da;
01604 }
01605 }
01606 else
01607 {
01608
01609
01610
01611 nincx = n*incx;
01612
01613
01614 for( i=0; i<nincx; i=i+incx)
01615
01616 {
01617 dx[i] *= (float)da;
01618 }
01619 }
01620 }
01621
01622 DEBUG_EXIT( "csscal()" );
01623 return;
01624 }
01625
01626
01627
01628 #undef S
01629 #define S(I_,J_) (*(s+(I_)*(ns)+(J_)))
01630
01631 static void start(long int n,
01632 float x[],
01633 float step[],
01634 long int ns,
01635 long int ips[],
01636 float *s,
01637 int *small)
01638 {
01639 long int i,
01640 i_,
01641 j,
01642 j_;
01643
01644 DEBUG_ENTRY( "start()" );
01645
01646
01647 i_ = n;
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687 for( i=1; i <= ns; i++ )
01688 {
01689 i_ = i - 1;
01690 S(0,i_) = x[ips[i_]-1];
01691 }
01692
01693 for( j=2; j <= (ns + 1); j++ )
01694 {
01695 j_ = j - 1;
01696 cdcopy(ns,&S(0,0),1,&S(j_,0),1);
01697 S(j_,j_-1) = S(0,j_-1) + step[ips[j_-1]-1];
01698 }
01699
01700
01701
01702 for( j=2; j <= (ns + 1); j++ )
01703 {
01704 j_ = j - 1;
01705 if( (double)(S(j_,j_-1)) == (double)(S(0,j_-1)) )
01706 goto L_40;
01707 }
01708 *small = false;
01709
01710 DEBUG_EXIT( "start()" );
01711 return;
01712
01713
01714
01715 L_40:
01716 *small = true;
01717
01718 DEBUG_EXIT( "start()" );
01719 return;
01720 }
01721
01722
01723 static void order(long int npts,
01724 float fs[],
01725 long int *il,
01726 long int *is,
01727 long int *ih)
01728 {
01729 long int i,
01730 il0,
01731 j;
01732
01733 DEBUG_ENTRY( "order()" );
01734
01735
01736
01737
01738
01739
01740
01741
01742
01743
01744
01745
01746
01747
01748
01749
01750
01751
01752
01753
01754
01755
01756
01757
01758
01759
01760
01761
01762
01763
01764
01765
01766
01767
01768
01769 il0 = *il;
01770 j = (il0%npts) + 1;
01771
01772 if( fs[j-1] >= fs[*il-1] )
01773 {
01774 *ih = j;
01775 *is = il0;
01776 }
01777 else
01778 {
01779 *ih = il0;
01780 *is = j;
01781 *il = j;
01782 }
01783
01784 for( i=il0 + 1; i <= (il0 + npts - 2); i++ )
01785 {
01786 j = (i%npts) + 1;
01787 if( fs[j-1] >= fs[*ih-1] )
01788 {
01789 *is = *ih;
01790 *ih = j;
01791 }
01792 else if( fs[j-1] > fs[*is-1] )
01793 {
01794 *is = j;
01795 }
01796 else if( fs[j-1] < fs[*il-1] )
01797 {
01798 *il = j;
01799 }
01800 }
01801
01802 DEBUG_EXIT( "order()" );
01803 return;
01804 }
01805
01806 static double dist(long int n,
01807 float x[],
01808 float y[])
01809 {
01810
01811
01812
01813 long int i,
01814 i_;
01815 float absxmy,
01816 dist_v,
01817 scale,
01818 sum;
01819
01820 DEBUG_ENTRY( "dist()" );
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839
01840
01841
01842
01843
01844
01845 absxmy = (float)fabs(x[0]-y[0]);
01846 if( absxmy <= 1.0f )
01847 {
01848 sum = absxmy*absxmy;
01849 scale = 1.0f;
01850 }
01851 else
01852 {
01853 sum = 1.0f;
01854 scale = absxmy;
01855 }
01856
01857 for( i=2; i <= n; i++ )
01858 {
01859 i_ = i - 1;
01860 absxmy = (float)fabs(x[i_]-y[i_]);
01861 if( absxmy <= scale )
01862 {
01863 sum += POW2(absxmy/scale);
01864 }
01865 else
01866 {
01867 sum = 1.0f + sum*POW2(scale/absxmy);
01868 scale = absxmy;
01869 }
01870 }
01871 dist_v = (float)(scale*sqrt(sum));
01872
01873 DEBUG_EXIT( "dist()" );
01874 return( dist_v );
01875 }
01876
01877
01878
01879 #undef S
01880 #define S(I_,J_) (*(s+(I_)*(ns)+(J_)))
01881
01882 static void calcc(long int ns,
01883 float *s,
01884 long int ih,
01885 long int inew,
01886 int updatc,
01887 float c[])
01888 {
01889 long int i,
01890 j,
01891 j_;
01892
01893 float xNothing[1] = { 0.0f };
01894
01895 DEBUG_ENTRY( "calcc()" );
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908
01909
01910
01911
01912
01913
01914
01915
01916
01917
01918
01919
01920
01921
01922
01923
01924
01925
01926
01927
01928
01929
01930
01931
01932
01933
01934
01935
01936
01937 if( !updatc )
01938 {
01939
01940
01941 cdcopy(ns,xNothing,0,c,1);
01942 for( j=1; j <= (ns + 1); j++ )
01943 {
01944 j_ = j - 1;
01945 if( j != ih )
01946 cdaxpy(ns,1.0f,&S(j_,0),1,c,1);
01947 }
01948 csscal(ns,1.0f/ns,c,1);
01949 }
01950 else if( ih != inew )
01951 {
01952 for( i=0; i < ns; i++ )
01953 {
01954 c[i] += (S(inew-1,i) - S(ih-1,i))/ns;
01955 }
01956 }
01957
01958 DEBUG_EXIT( "calcc()" );
01959 return;
01960 }
01961
01962
01963 static void newpt(long int ns,
01964 double coef,
01965 float xbase[],
01966 float xold[],
01967 int IntNew,
01968 float xnew[],
01969 int *small)
01970 {
01971 int eqbase,
01972 eqold;
01973 long int i;
01974 float xoldi;
01975
01976 DEBUG_ENTRY( "newpt()" );
01977
01978
01979
01980
01981
01982
01983
01984
01985
01986
01987
01988
01989
01990
01991
01992
01993
01994
01995
01996
01997
01998
01999
02000
02001
02002
02003
02004
02005
02006
02007
02008
02009
02010
02011
02012
02013
02014
02015
02016
02017
02018
02019
02020
02021
02022
02023
02024
02025
02026
02027
02028
02029
02030
02031
02032
02033 eqbase = true;
02034 eqold = true;
02035 if( IntNew )
02036 {
02037 for( i=0; i < ns; i++ )
02038 {
02039 xnew[i] = (float)(xbase[i] + coef*(xbase[i] - xold[i]));
02040 eqbase = eqbase && ((double)(xnew[i]) == (double)(xbase[i]));
02041 eqold = eqold && ((double)(xnew[i]) == (double)(xold[i]));
02042 }
02043 }
02044 else
02045 {
02046 for( i=0; i < ns; i++ )
02047 {
02048 xoldi = xold[i];
02049 xold[i] = (float)(xbase[i] + coef*(xbase[i] - xold[i]));
02050 eqbase = eqbase && ((double)(xold[i]) == (double)(xbase[i]));
02051 eqold = eqold && ((double)(xold[i]) == (double)(xoldi));
02052 }
02053 }
02054 *small = eqbase || eqold;
02055
02056 DEBUG_EXIT( "newpt()" );
02057 return;
02058 }
02059
02060 static void cdaxpy(long int n,
02061 double da,
02062 float dx[],
02063 long int incx,
02064 float dy[],
02065 long int incy)
02066 {
02067 long int i,
02068 i_,
02069 ix,
02070 iy,
02071 m;
02072
02073 DEBUG_ENTRY( "cdaxpy()" );
02074
02075
02076
02077
02078
02079
02080 if( n > 0 )
02081 {
02082 if( da != 0.00f )
02083 {
02084 if( incx == 1 && incy == 1 )
02085 {
02086
02087
02088
02089
02090
02091
02092 m = n%4;
02093 if( m != 0 )
02094 {
02095 for( i=1; i <= m; i++ )
02096 {
02097 i_ = i - 1;
02098 dy[i_] += (float)(da*dx[i_]);
02099 }
02100 if( n < 4 )
02101 {
02102 DEBUG_EXIT( "cdaxpy()" );
02103 return;
02104 }
02105 }
02106
02107 for( i=m; i < n; i += 4 )
02108 {
02109 dy[i] += (float)(da*dx[i]);
02110 dy[i+1] += (float)(da*dx[i+1]);
02111 dy[i+2] += (float)(da*dx[i+2]);
02112 dy[i+3] += (float)(da*dx[i+3]);
02113 }
02114 }
02115 else
02116 {
02117
02118
02119
02120
02121 ix = 1;
02122 iy = 1;
02123 if( incx < 0 )
02124 ix = (-n + 1)*incx + 1;
02125 if( incy < 0 )
02126 iy = (-n + 1)*incy + 1;
02127 for( i=0; i < n; i++ )
02128 {
02129 dy[iy-1] += (float)(da*dx[ix-1]);
02130 ix += incx;
02131 iy += incy;
02132 }
02133 }
02134 }
02135 }
02136
02137 DEBUG_EXIT( "cdaxpy()" );
02138 return;
02139 }
02140
02141
02142
02143