00001
00002
00003
00004 #include "cddefines.h"
00005 #include "thirdparty.h"
00006 #include "iter_track.h"
00007
00009
00010
00011
00012
00013
00014
00015
00017
00018 double iter_track::next_val()
00019 {
00020
00021
00022
00023
00024
00025
00026
00027 if( p_y(p_a) > 0.0 )
00028 {
00029
00030
00031
00032 if( (p_x(p_b)-p_x(p_a)) < p_tol )
00033 {
00034 if( p_y(p_b) > 0 )
00035 p_a = p_b;
00036 else
00037 p_set_root(p_x(p_b));
00038 return p_midpoint();
00039 }
00040
00041
00042
00043
00044 if( (p_x(p_c)-p_x(p_b)) < p_tol )
00045 {
00046 if( p_y(p_b) < 0 )
00047 p_c = p_b;
00048 else
00049 p_set_root(p_x(p_b));
00050 return p_midpoint();
00051 }
00052
00053
00054
00055 if( ( p_y(p_a) > p_y(p_b) ) && ( p_y(p_b) > p_y(p_c) ) )
00056 {
00057 double delta = p_denominator(p_y(p_a),p_y(p_b),p_y(p_c));
00058 if( delta != 0.0 )
00059 {
00060 double dab = p_x(p_a)-p_x(p_b);
00061 double dcb = p_x(p_c)-p_x(p_b);
00062 delta = safe_div( p_numerator(dab,dcb,p_y(p_a),p_y(p_b),p_y(p_c)), delta );
00063
00064
00065
00066
00067
00068 if( delta > dab && delta < dcb )
00069 {
00070 if( p_y(p_b) > 0.0 )
00071 p_a = p_b;
00072 else if( p_y(p_b) < 0.0 )
00073 p_c = p_b;
00074 else
00075 p_set_root(p_x(p_b));
00076 return p_x(p_b) + delta;
00077 }
00078 }
00079 }
00080
00081
00082
00083 if( p_y(p_b) > 0.0 )
00084 p_a = p_b;
00085 else
00086 p_c = p_b;
00087 return p_midpoint();
00088 }
00089 else
00090 {
00091
00092
00093
00094
00095
00096
00097
00098 if( (p_x(p_b)-p_x(p_a)) < p_tol )
00099 {
00100 if( p_y(p_b) < 0 )
00101 p_a = p_b;
00102 else
00103 p_set_root(p_x(p_b));
00104 return p_midpoint();
00105 }
00106
00107 if( (p_x(p_c)-p_x(p_b)) < p_tol )
00108 {
00109 if( p_y(p_b) > 0 )
00110 p_c = p_b;
00111 else
00112 p_set_root(p_x(p_b));
00113 return p_midpoint();
00114 }
00115
00116 if( ( p_y(p_a) < p_y(p_b) ) && ( p_y(p_b) < p_y(p_c) ) )
00117 {
00118 double delta = p_denominator(p_y(p_a),p_y(p_b),p_y(p_c));
00119 if( delta != 0.0 )
00120 {
00121 double dab = p_x(p_a)-p_x(p_b);
00122 double dcb = p_x(p_c)-p_x(p_b);
00123 delta = safe_div( p_numerator(dab,dcb,p_y(p_a),p_y(p_b),p_y(p_c)), delta );
00124 if( delta > dab && delta < dcb )
00125 {
00126 if( p_y(p_b) < 0.0 )
00127 p_a = p_b;
00128 else if( p_y(p_b) > 0.0 )
00129 p_c = p_b;
00130 else
00131 p_set_root(p_x(p_b));
00132 return p_x(p_b) + delta;
00133 }
00134 }
00135 }
00136
00137 if( p_y(p_b) < 0.0 )
00138 p_a = p_b;
00139 else
00140 p_c = p_b;
00141 return p_midpoint();
00142 }
00143 }
00144
00145 double iter_track::deriv(int n, double& sigma) const
00146 {
00147 n = min( n, p_history.size() );
00148 ASSERT( n >= 2 );
00149 double *x = new double[n];
00150 double *y = new double[n];
00151 for( int i=0; i < n; ++i )
00152 {
00153 x[i] = p_x(p_history.size() - n + i);
00154 y[i] = p_y(p_history.size() - n + i);
00155 }
00156 double a,b,siga,sigb;
00157 linfit( n, x, y, a, siga, b, sigb );
00158 delete[] y;
00159 delete[] x;
00160 sigma = sigb;
00161 return b;
00162 }
00163
00164 double iter_track::zero_fit(int n, double& sigma) const
00165 {
00166 n = min( n, p_history.size() );
00167 ASSERT( n >= 2 );
00168 double *x = new double[n];
00169 double *y = new double[n];
00170 for( int i=0; i < n; ++i )
00171 {
00172 x[i] = p_y(p_history.size() - n + i);
00173 y[i] = p_x(p_history.size() - n + i);
00174 }
00175 double a,b,siga,sigb;
00176 linfit( n, x, y, a, siga, b, sigb );
00177 delete[] y;
00178 delete[] x;
00179 sigma = siga;
00180 return a;
00181 }
00182
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00237
00238 double Amsterdam_Method( double (*f)(double), double a, double fa, double c, double fc,
00239 double tol, int max_iter, int& err )
00240 {
00241 iter_track track;
00242
00243 double result;
00244 set_NaN( result );
00245
00246 track.set_tol(tol);
00247
00248
00249 if( ( err = track.init_bracket( a, fa, c, fc ) ) < 0 )
00250 return result;
00251
00252 double b = 0.5*(a + c);
00253 for( int i = 0; i < max_iter && !track.lgConverged(); i++ )
00254 {
00255 track.add( b, (*f)(b) );
00256 b = track.next_val();
00257 }
00258
00259 if( track.lgConverged() )
00260 {
00261 err = 0;
00262 result = track.root();
00263 }
00264 else
00265 {
00266 err = -2;
00267 }
00268 return result;
00269 }