c_function.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00036 #include <acado/function/c_function.hpp>
00037 #include <acado/function/c_operator.hpp>
00038 
00039 
00040 BEGIN_NAMESPACE_ACADO
00041 
00042 
00043 
00044 //
00045 // PUBLIC MEMBER FUNCTIONS:
00046 //
00047 
00048 CFunction::CFunction( ){
00049 
00050     cFcn          = NULL;
00051     cFcnDForward  = NULL;
00052     cFcnDBackward = NULL;
00053     dim           = 0   ;
00054 
00055     user_data     = 0;
00056 
00057     initialize();
00058 }
00059 
00060 
00061 CFunction::CFunction( uint dim_, cFcnPtr cFcn_ ){
00062 
00063     cFcn          = cFcn_;
00064     cFcnDForward  = NULL ;
00065     cFcnDBackward = NULL ;
00066     dim           = dim_ ;
00067 
00068     user_data     = 0;
00069 
00070     initialize();
00071 }
00072 
00073 
00074 CFunction::CFunction( uint     dim_          , 
00075                       cFcnPtr  cFcn_         ,
00076                       cFcnDPtr cFcnDForward_ ,
00077                       cFcnDPtr cFcnDBackward_  ){
00078 
00079     cFcn          = cFcn_         ;
00080     cFcnDForward  = cFcnDForward_ ;
00081     cFcnDBackward = cFcnDBackward_;
00082     dim           = dim_          ;
00083 
00084     user_data     = 0;
00085 
00086     initialize();
00087 }
00088 
00089 
00090 CFunction:: CFunction( const CFunction& arg ){ copy     (arg); }
00091 CFunction::~CFunction(                      ){ deleteAll(   ); }
00092 
00093 CFunction& CFunction::operator=( const CFunction& arg ){
00094 
00095     if ( this != &arg ){
00096         deleteAll();
00097         copy(arg);
00098     }
00099     return *this;
00100 }
00101 
00102 
00103 returnValue CFunction::initialize(){
00104 
00105     nn            = 0;
00106     maxAlloc      = 1;
00107 
00108     xStore    = (double**)calloc(maxAlloc, sizeof(double*));
00109     seedStore = (double**)calloc(maxAlloc, sizeof(double*));
00110 
00111     xStore   [0]  = 0;
00112     seedStore[0]  = 0;
00113 
00114     return SUCCESSFUL_RETURN;
00115 }
00116 
00117 
00118 void CFunction::copy( const CFunction &arg ){
00119 
00120     uint run1, run2;
00121 
00122     if( arg.cFcn == 0          ) cFcn          = 0                ;
00123     else                         cFcn          = arg.cFcn         ;
00124     if( arg.cFcnDForward == 0  ) cFcnDForward  = 0                ;
00125     else                         cFcnDForward  = arg.cFcnDForward ;
00126     if( arg.cFcnDBackward == 0 ) cFcnDBackward = 0                ;
00127     else                         cFcnDBackward = arg.cFcnDBackward;
00128 
00129     dim = arg.dim;
00130     nn  = arg.nn ;
00131 
00132     user_data = arg.user_data;
00133 
00134     maxAlloc = arg.maxAlloc;
00135 
00136     xStore    = (double**)calloc(maxAlloc,sizeof(double*));
00137     seedStore = (double**)calloc(maxAlloc,sizeof(double*));
00138 
00139     for( run1 = 0; run1 < maxAlloc; run1++ ){
00140         if( nn != 0 ){
00141                xStore[run1] = new double[nn];
00142             seedStore[run1] = new double[nn];
00143             for( run2 = 0; run2 < nn; run2++ ){
00144                    xStore[run1][run2] = arg.xStore   [run1][run2];
00145                 seedStore[run1][run2] = arg.seedStore[run1][run2];
00146             }
00147         }
00148         else{
00149                xStore[run1] = 0;
00150             seedStore[run1] = 0;
00151         }
00152     }
00153 }
00154 
00155 
00156 void CFunction::deleteAll(){
00157 
00158     uint run1;
00159 
00160     for( run1 = 0; run1 < maxAlloc; run1++ ){
00161         if( xStore    != 0 ) delete[] xStore   [run1];
00162         if( seedStore != 0 ) delete[] seedStore[run1];
00163     }
00164     if( xStore    != 0 ) free(xStore   );
00165     if( seedStore != 0 ) free(seedStore);
00166 }
00167 
00168 
00169 Expression CFunction::operator()( const Expression &arg ){
00170 
00171     uint run1,run2;
00172 
00173     CFunction thisFunction(*this);
00174     thisFunction.nn = arg.getDim();
00175 
00176     for( run1 = 0; run1 < maxAlloc; run1++ ){
00177         thisFunction.xStore   [run1] = new double[thisFunction.nn];
00178         thisFunction.seedStore[run1] = new double[thisFunction.nn];
00179         for( run2 = 0; run2 < thisFunction.nn; run2++ ){
00180             thisFunction.xStore   [run1][run2] = 0.0;
00181             thisFunction.seedStore[run1][run2] = 0.0;
00182         }
00183     }
00184 
00185     Expression tmp(dim);
00186 
00187     COperator dummy;
00188     dummy.increaseID();
00189 
00190     for( run1 = 0; run1 < dim; run1++ ){
00191         delete tmp.element[run1];
00192         tmp.element[run1] = new COperator( thisFunction, arg, run1 );
00193     }
00194 
00195     return tmp;
00196 }
00197 
00198 
00199 uint CFunction::getDim () const{
00200 
00201     return dim;
00202 }
00203 
00204 
00205 returnValue CFunction::evaluate( double *x, double *result ){
00206 
00207     if( cFcn != 0 ) cFcn( x, result, user_data );
00208     else evaluateCFunction( x, result );
00209 
00210     return SUCCESSFUL_RETURN;
00211 }
00212 
00213 
00214 
00215 void CFunction::evaluateCFunction( double *x, double *result ){
00216 
00217     // DUMMY IMPLEMENTATION, CAN BE IMPLEMENTED BY THE USER
00218     // IN DERIVED CLASSES.
00219 }
00220 
00221 
00222 
00223 returnValue CFunction::evaluate( double *x, double *result, PrintLevel printL ){
00224 
00225     uint run1;
00226 
00227     evaluate(x,result);
00228 
00229         if (printL == MEDIUM || printL == HIGH)
00230         {
00231                 std::cout << "C-function evaluation: ";
00232                 for (run1 = 0; run1 < dim; run1++)
00233                         std::cout << "f[" << run1 << "] = " << std::scientific << result[ run1 ] << std::endl;
00234         }
00235     return SUCCESSFUL_RETURN;
00236 }
00237 
00238 
00239 returnValue CFunction::evaluate( int number, double *x, double *result ){
00240 
00241     uint run1;
00242     uint run2;
00243 
00244     evaluate(x,result);
00245 
00246     if( number >= (int) maxAlloc ){
00247 
00248         xStore= (double**)realloc(xStore, (number+1)*sizeof(double*));
00249         for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
00250            xStore[run1] = new double[nn];
00251         }
00252         seedStore = (double**)realloc(seedStore, (number+1)*sizeof(double*));
00253         for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
00254            seedStore[run1] = new double[nn];
00255         }
00256         maxAlloc = number+1;
00257     }
00258 
00259     for( run2 = 0; run2 < nn; run2++ ){
00260         xStore[number][run2] = x[run2];
00261     }
00262 
00263     return SUCCESSFUL_RETURN;
00264 }
00265 
00266 
00267 returnValue CFunction::AD_forward( double *x, double *seed, double *f, double *df ){
00268 
00269     if( cFcnDForward != NULL ){
00270         cFcnDForward( 0, x, seed, f, df, user_data );
00271         return SUCCESSFUL_RETURN;
00272     }
00273     else{
00274 
00275         uint run1;
00276 
00277         if( cFcn != 0 )
00278             cFcn( x, f, user_data );
00279         else evaluateCFunction( x, f );
00280 
00281         for( run1 = 0; run1 < nn; run1++ ){
00282             x[run1] = x[run1] + SQRT_EPS*seed[run1];
00283         }
00284 
00285         if( cFcn != 0 )
00286             cFcn( x, df, user_data );
00287         else evaluateCFunction( x, df );
00288 
00289         for( run1 = 0; run1 < dim; run1++ ){
00290             df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
00291         }
00292         for( run1 = 0; run1 < nn; run1++ ){
00293             x[run1] = x[run1] - SQRT_EPS*seed[run1];
00294         }
00295     }
00296     return SUCCESSFUL_RETURN;
00297 }
00298 
00299 
00300 returnValue CFunction::AD_forward( int number, double *x, double *seed, double *f, double *df ){
00301 
00302     uint run1;
00303 
00304     if( number >= (int) maxAlloc ){
00305 
00306         xStore= (double**)realloc(xStore, (number+1)*sizeof(double*));
00307         for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
00308            xStore[run1] = new double[nn];
00309         }
00310         seedStore = (double**)realloc(seedStore, (number+1)*sizeof(double*));
00311         for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
00312            seedStore[run1] = new double[nn];
00313         }
00314         maxAlloc = number+1;
00315     }
00316 
00317     if( cFcnDForward != NULL ){
00318         cFcnDForward( number, x, seed, f, df, user_data );
00319 
00320         for( run1 = 0; run1 < nn; run1++ ){
00321 
00322             xStore[number][run1]    = x   [run1];
00323             seedStore[number][run1] = seed[run1];
00324         }
00325         return SUCCESSFUL_RETURN;
00326     }
00327 
00328     if( cFcn != 0 )
00329         cFcn( x, f, user_data );
00330     else evaluateCFunction( x, f );
00331 
00332     for( run1 = 0; run1 < nn; run1++ ){
00333         xStore[number][run1] = x[run1];
00334         seedStore[number][run1] = seed[run1];
00335         x              [run1] = x[run1] + SQRT_EPS*seed[run1];
00336     }
00337     if( cFcn != 0 )
00338         cFcn( x, df, user_data );
00339     else evaluateCFunction( x, df );
00340     for( run1 = 0; run1 < dim; run1++ ){
00341         df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
00342     }
00343     for( run1 = 0; run1 < nn; run1++ ){
00344         x[run1] = xStore[number][run1];
00345     }
00346 
00347     return SUCCESSFUL_RETURN;
00348 }
00349 
00350 
00351 returnValue CFunction::AD_forward( int number, double *seed, double *df ){
00352 
00353     uint run1;
00354 
00355     ASSERT( number < (int) maxAlloc );
00356 
00357     double *f = new double[dim];
00358 
00359     if( cFcnDForward != NULL ){
00360         cFcnDForward( number, xStore[number], seed, f, df, user_data );
00361 
00362         for( run1 = 0; run1 < nn; run1++ )
00363             seedStore[number][run1] = seed[run1];
00364 
00365         delete[] f;
00366         return SUCCESSFUL_RETURN;
00367     }
00368 
00369     if( cFcn != 0 )
00370         cFcn( xStore[number], f, user_data );
00371     else evaluateCFunction( xStore[number], f );
00372 
00373     for( run1 = 0; run1 < nn; run1++ ){
00374         xStore[number][run1] = xStore[number][run1] + SQRT_EPS*seed[run1];
00375     }
00376 
00377     if( cFcn != 0 )
00378         cFcn( xStore[number], df, user_data );
00379     else evaluateCFunction( xStore[number], df );
00380     for( run1 = 0; run1 < dim; run1++ ){
00381         df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
00382     }
00383     for( run1 = 0; run1 < nn; run1++ ){
00384         xStore[number][run1] = xStore[number][run1] - SQRT_EPS*seed[run1];
00385     }
00386 
00387     for( run1 = 0; run1 < nn; run1++ ){
00388         seedStore[number][run1] = seed[run1];
00389     }
00390 
00391     delete[] f;
00392 
00393     return SUCCESSFUL_RETURN;
00394 }
00395 
00396 
00397 
00398 returnValue CFunction::AD_backward( double *seed, double  *df ){
00399 
00400     return AD_backward( 0, seed, df );
00401 }
00402 
00403 
00404 returnValue CFunction::AD_backward( int number, double *seed, double  *df ){
00405 
00406     uint run1;
00407 
00408     ASSERT( number < (int) maxAlloc );
00409 
00410     if( cFcnDBackward != NULL ){
00411 
00412         double *f = new double[dim];
00413 
00414         cFcnDBackward( number, xStore[number], seed, f, df, user_data );
00415 
00416         for( run1 = 0; run1 < nn; run1++ )
00417             seedStore[number][run1] = seed[run1];
00418 
00419         delete[] f;
00420         return SUCCESSFUL_RETURN;
00421     }
00422     return ACADOERROR(RET_INVALID_USE_OF_FUNCTION);
00423 }
00424 
00425 
00426 returnValue CFunction::AD_forward2( int number, double *seed, double *dseed, double *df, double *ddf ){
00427 
00428     uint run1;
00429 
00430     ASSERT( number < (int) maxAlloc );
00431 
00432     double *f1 = new double[dim];
00433     double *f2 = new double[dim];
00434     double *f3 = new double[dim];
00435     double *f4 = new double[dim];
00436 
00437 
00438     // FIRST ORDER DERIVATIVES:
00439     // ------------------------
00440 
00441     evaluate( xStore[number], f1 );
00442 
00443     for( run1 = 0; run1 < nn; run1++ ){
00444         xStore[number][run1] = xStore[number][run1] + SQRT_EPS*seed[run1];
00445     }
00446     evaluate( xStore[number], df );
00447     for( run1 = 0; run1 < dim; run1++ ){
00448         df[run1] = ( df[run1] - f1[run1] )/SQRT_EPS;
00449     }
00450     for( run1 = 0; run1 < nn; run1++ ){
00451         xStore[number][run1] = xStore[number][run1] - SQRT_EPS*seed[run1];
00452     }
00453 
00454     for( run1 = 0; run1 < nn; run1++ ){
00455         xStore[number][run1] = xStore[number][run1] + SQRT_EPS*dseed[run1];
00456     }
00457     evaluate( xStore[number], ddf );
00458     for( run1 = 0; run1 < dim; run1++ ){
00459         ddf[run1] = ( ddf[run1] - f1[run1] )/SQRT_EPS;
00460     }
00461     for( run1 = 0; run1 < nn; run1++ ){
00462         xStore[number][run1] = xStore[number][run1] - SQRT_EPS*dseed[run1];
00463     }
00464 
00465 
00466     // SECOND DERIVATIVES:
00467     // -------------------
00468 
00469     for( run1 = 0; run1 < nn; run1++ ){
00470         xStore[number][run1] = xStore[number][run1]
00471                                 + 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
00472     }
00473     evaluate( xStore[number], f1 );
00474 
00475     for( run1 = 0; run1 < nn; run1++ ){
00476         xStore[number][run1] = xStore[number][run1]
00477                                 - 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1])
00478                                 + 0.5*FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1]);
00479     }
00480     evaluate( xStore[number], f2 );
00481 
00482     for( run1 = 0; run1 < nn; run1++ ){
00483         xStore[number][run1] = xStore[number][run1]
00484                                 - FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1]);
00485     }
00486     evaluate( xStore[number], f3 );
00487 
00488     for( run1 = 0; run1 < nn; run1++ ){
00489         xStore[number][run1] = xStore[number][run1]
00490                                 + 0.5*FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1])
00491                                 - 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
00492     }
00493     evaluate( xStore[number], f4 );
00494 
00495     for( run1 = 0; run1 < nn; run1++ ){
00496         xStore[number][run1] = xStore[number][run1]
00497                                 + 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
00498     }
00499 
00500     for( run1 = 0; run1 < dim; run1++ ){
00501         ddf[run1] = ddf[run1] + ( f1[run1] - f2[run1] - f3[run1] + f4[run1] )/SQRT_EPS;
00502     }
00503 
00504     delete[] f1;
00505     delete[] f2;
00506     delete[] f3;
00507     delete[] f4;
00508 
00509     return SUCCESSFUL_RETURN;
00510 }
00511 
00512 
00513 returnValue CFunction::AD_backward2( int number, double *seed1, double *seed2, double *df, double *ddf ){
00514 
00515     return ACADOERROR(RET_INVALID_USE_OF_FUNCTION);
00516 }
00517 
00518 returnValue CFunction::clearBuffer(){
00519 
00520     uint run1;
00521 
00522     if( maxAlloc > 1 ){
00523 
00524         for( run1 = 1; run1 < maxAlloc; run1++ ){
00525             delete[] xStore[run1];
00526             delete[] seedStore[run1];
00527         }
00528 
00529         maxAlloc = 1;
00530 
00531         xStore= (double**)realloc( xStore, maxAlloc*sizeof(double*) );
00532         seedStore = (double**)realloc( seedStore, maxAlloc*sizeof(double*) );
00533     }
00534 
00535     return SUCCESSFUL_RETURN;
00536 }
00537 
00538 
00539 returnValue CFunction::setUserData( void * user_data_){
00540 
00541   user_data = user_data_;
00542 
00543   return SUCCESSFUL_RETURN;
00544 }
00545 
00546 
00547 
00548 CLOSE_NAMESPACE_ACADO
00549 
00550 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:48