Back to index

extremetuxracer  0.5beta
nmrcl.cpp
Go to the documentation of this file.
00001 /* 
00002  * PPRacer 
00003  * Copyright (C) 2004-2005 Volker Stroebel <volker@planetpenguin.de>
00004  *
00005  * Copyright (C) 1999-2001 Jasmin F. Patry
00006  * 
00007  * This program is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU General Public License
00009  * as published by the Free Software Foundation; either version 2
00010  * of the License, or (at your option) any later version.
00011  * 
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  * 
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program; if not, write to the Free Software
00019  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00020  */
00021 
00022 #include "etracer.h"
00023 #include "nmrcl.h"
00024 
00025 const double ode23_time_step_mat[] = { 0., 1./2., 3./4., 1. };
00026 const double ode23_coeff_mat[][4] = {
00027     { 0., 1./2.,    0.,  2./9. } ,
00028     { 0.,    0., 3./4.,  1./3. } ,
00029     { 0.,    0.,    0.,  4./9. } ,
00030     { 0.,    0.,    0.,     0. } 
00031 };
00032 const double ode23_error_mat[] = { -5./72., 1./12., 1./9., -1./8. };
00033 const double ode23_time_step_exp = 1./3.;
00034 
00035 const double ode45_time_step_mat[] = 
00036 { 0., 1./5., 3./10., 4./5., 8./9., 1., 1. };
00037 const double ode45_coeff_mat[][7] = {
00038     { 0, 1./5., 3./40.,  44./45.,  19372./6561.,   9017./3168.,     35./384. },
00039     { 0, 0.,    9./40., -56./15., -25360./2187.,     -355./33.,           0. },
00040     { 0, 0.,         0,   32./9.,  64448./6561.,  46732./5247.,   500./1113. },
00041     { 0, 0.,         0,        0,    -212./729.,      49./176.,    125./192. },
00042     { 0, 0.,         0,        0,             0, -5103./18656., -2187./6784. },
00043     { 0, 0.,         0,        0,             0,             0,      11./84. },
00044     { 0, 0.,         0,        0,             0,             0,           0. }
00045 };
00046 
00047 const double ode45_error_mat[] = 
00048 { 71./57600., 0., -71./16695., 71./1920., -17253./339200., 22./525., -1./40.};
00049 const double ode45_time_step_exp = 1./5.;
00050 
00051 ode_data_t* euler_new_ode_data()
00052 {
00053     euler_data_t *data;
00054     data = (euler_data_t*)malloc(sizeof(euler_data_t));
00055     return (ode_data_t*) data;
00056 }
00057 
00058 int euler_num_estimates()
00059 {
00060     return 2;
00061 }
00062 
00063 void euler_init_ode_data( ode_data_t *p, double init_val, double h )
00064 {
00065     euler_data_t *data = (euler_data_t*)p;
00066     data->init_val = init_val;
00067     data->h = h;
00068 }
00069 
00070 double euler_next_time(ode_data_t *p, int step)
00071 {
00072     euler_data_t *data = (euler_data_t*)p;
00073     check_assertion( 0 <= step && step < 2, 
00074                    "invalid step number in euler ode solver" );
00075     return step * data->h;
00076 }
00077 
00078 double euler_next_val( ode_data_t *p, int step )
00079 {
00080     euler_data_t *data = (euler_data_t*)p;
00081     double val = data->init_val;
00082 
00083     if ( step == 1 ) 
00084        val += data->k[0];
00085 
00086     return val;
00087 }
00088 
00089 void euler_update_estimate(ode_data_t *p, int step, double val)
00090 {
00091     euler_data_t *data = (euler_data_t*)p;
00092     data->k[step] = data->h * val;
00093 }
00094 
00095 double euler_final_estimate(ode_data_t *p)
00096 {
00097     euler_data_t *data = (euler_data_t*)p;
00098     double val = data->init_val;
00099 
00100     val += 0.5 * (data->k[0] + data->k[1] );
00101     return val;
00102 }
00103 
00104 ode_solver_t new_euler_solver()
00105 {
00106     ode_solver_t s; 
00107     s.new_ode_data = euler_new_ode_data;
00108     s.num_estimates = euler_num_estimates;
00109     s.init_ode_data = euler_init_ode_data;
00110     s.next_time = euler_next_time;
00111     s.next_val = euler_next_val;
00112     s.update_estimate = euler_update_estimate;
00113     s.final_estimate = euler_final_estimate;
00114     s.estimate_error = NULL;
00115     s.time_step_exponent = NULL;
00116     return s;
00117 }
00118 
00119 
00120 ode_data_t* ode23_new_ode_data()
00121 {
00122     ode23_data_t *data;
00123     data = (ode23_data_t*)malloc(sizeof(ode23_data_t));
00124     return (ode_data_t*) data;
00125 }
00126 
00127 int ode23_num_estimates()
00128 {
00129     return 4;
00130 }
00131 
00132 void ode23_init_ode_data( ode_data_t *p, double init_val, double h )
00133 {
00134     ode23_data_t *data = (ode23_data_t*)p;
00135     data->init_val = init_val;
00136     data->h = h;
00137 }
00138 
00139 double ode23_next_time(ode_data_t *p, int step)
00140 {
00141     ode23_data_t *data = (ode23_data_t*)p;
00142     check_assertion( 0 <= step && step < 4, "invalid step for ode23 solver" );
00143     return ode23_time_step_mat[step] * data->h;
00144 }
00145 
00146 double ode23_next_val( ode_data_t *p, int step )
00147 {
00148     ode23_data_t *data = (ode23_data_t*)p;
00149     double val = data->init_val;
00150     int i;
00151 
00152     for ( i=0; i<step; i++ ) {
00153        val += ode23_coeff_mat[i][step] * data->k[i];
00154     }
00155     return val;
00156 }
00157 
00158 void ode23_update_estimate(ode_data_t *p, int step, double val)
00159 {
00160     ode23_data_t *data = (ode23_data_t*)p;
00161     data->k[step] = data->h * val;
00162 }
00163 
00164 double ode23_final_estimate(ode_data_t *p)
00165 {
00166     ode23_data_t *data = (ode23_data_t*)p;
00167     double val = data->init_val;
00168     int i;
00169 
00170     for ( i=0; i<3; i++ ) {
00171        val += ode23_coeff_mat[i][3] * data->k[i];
00172     }
00173     return val;
00174 }
00175 
00176 double ode23_estimate_error(ode_data_t *p)
00177 {
00178     ode23_data_t *data = (ode23_data_t*)p;
00179     double err=0.;
00180     int i;
00181 
00182     for ( i=0; i<4; i++ ) {
00183        err += ode23_error_mat[i] * data->k[i];
00184     }
00185     return fabs(err);
00186 }
00187 
00188 double ode23_time_step_exponent()
00189 {
00190     return ode23_time_step_exp;
00191 }
00192 
00193 ode_solver_t new_ode23_solver()
00194 {
00195     ode_solver_t s; 
00196     s.new_ode_data = ode23_new_ode_data;
00197     s.num_estimates = ode23_num_estimates;
00198     s.init_ode_data = ode23_init_ode_data;
00199     s.next_time = ode23_next_time;
00200     s.next_val = ode23_next_val;
00201     s.update_estimate = ode23_update_estimate;
00202     s.final_estimate = ode23_final_estimate;
00203     s.estimate_error = ode23_estimate_error;
00204     s.time_step_exponent = ode23_time_step_exponent;
00205     return s;
00206 }
00207 
00208 ode_data_t* ode45_new_ode_data()
00209 {
00210     ode45_data_t *data;
00211     data = (ode45_data_t*)malloc(sizeof(ode45_data_t));
00212     return (ode_data_t*) data;
00213 }
00214 
00215 int ode45_num_estimates()
00216 {
00217     return 7;
00218 }
00219 
00220 void ode45_init_ode_data( ode_data_t *p, double init_val, double h )
00221 {
00222     ode45_data_t *data = (ode45_data_t*)p;
00223     data->init_val = init_val;
00224     data->h = h;
00225 }
00226 
00227 double ode45_next_time(ode_data_t *p, int step)
00228 {
00229     ode45_data_t *data = (ode45_data_t*)p;
00230     check_assertion( 0 <= step && step < 7, "invalid step for ode45 solver" );
00231     return ode45_time_step_mat[step] * data->h;
00232 }
00233 
00234 double ode45_next_val( ode_data_t *p, int step )
00235 {
00236     ode45_data_t *data = (ode45_data_t*)p;
00237     double val = data->init_val;
00238     int i;
00239 
00240     for ( i=0; i<step; i++ ) {
00241        val += ode45_coeff_mat[i][step] * data->k[i];
00242     }
00243     return val;
00244 }
00245 
00246 void ode45_update_estimate(ode_data_t *p, int step, double val)
00247 {
00248     ode45_data_t *data = (ode45_data_t*)p;
00249     data->k[step] = data->h * val;
00250 }
00251 
00252 double ode45_final_estimate(ode_data_t *p)
00253 {
00254     ode45_data_t *data = (ode45_data_t*)p;
00255     double val = data->init_val;
00256     int i;
00257 
00258     for ( i=0; i<6; i++ ) {
00259        val += ode45_coeff_mat[i][6] * data->k[i];
00260     }
00261     return val;
00262 }
00263 
00264 double ode45_estimate_error(ode_data_t *p)
00265 {
00266     ode45_data_t *data = (ode45_data_t*)p;
00267     double err=0.;
00268     int i;
00269 
00270     for ( i=0; i<7; i++ ) {
00271        err += ode45_error_mat[i] * data->k[i];
00272     }
00273     return fabs(err);
00274 }
00275 
00276 double ode45_time_step_exponent()
00277 {
00278     return ode45_time_step_exp;
00279 }
00280 
00281 ode_solver_t new_ode45_solver()
00282 {
00283     ode_solver_t s; 
00284     s.new_ode_data = ode45_new_ode_data;
00285     s.num_estimates = ode45_num_estimates;
00286     s.init_ode_data = ode45_init_ode_data;
00287     s.next_time = ode45_next_time;
00288     s.next_val = ode45_next_val;
00289     s.update_estimate = ode45_update_estimate;
00290     s.final_estimate = ode45_final_estimate;
00291     s.estimate_error = ode45_estimate_error;
00292     s.time_step_exponent = ode45_time_step_exponent;
00293     return s;
00294 }
00295 
00296 double lin_interp( const double x[], const double y[], double val, int n )
00297 {
00298     int i;
00299     double m, b;
00300 
00301     check_assertion( 
00302        n>=2, "linear interpolation requires at least two data points" );
00303 
00304     if ( val < x[0] ) {
00305        i = 0;
00306     } else if ( val >= x[n-1] ) {
00307        i = n-2;
00308     } else {
00309        /* should replace with binary search if we ever use large tables */
00310        for ( i=0; i<n-1; i++ ) {
00311            if ( val < x[i+1] ) break;
00312        }
00313     }
00314 
00315     m = ( y[i+1] - y[i] ) / ( x[i+1] - x[i] );
00316     b =  y[i] - m * x[i];
00317 
00318     return m * val + b;
00319 }