curve.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 
00033 #include <acado/curve/curve.hpp>
00034 
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 Curve::Curve( ){
00046 
00047     nIntervals       = 0;
00048     dim              = 0;
00049     parameterization = 0;
00050     grid             = 0;
00051 }
00052 
00053 Curve::Curve( const Curve& arg ){
00054 
00055     uint run1;
00056 
00057     nIntervals       = arg.nIntervals;
00058     dim              = arg.dim       ;
00059 
00060     parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
00061 
00062     for( run1 = 0; run1 < nIntervals; run1++ ){
00063         if( arg.parameterization[run1] != 0 ) parameterization[run1] = new Function(*arg.parameterization[run1]);
00064         else                                  parameterization[run1] = 0;
00065     }
00066 
00067     if( arg.grid != 0 )  grid = new Grid(*arg.grid);
00068     else                 grid = 0;
00069 }
00070 
00071 
00072 Curve::~Curve( ){
00073 
00074     uint run1;
00075 
00076     for( run1 = 0; run1 < nIntervals; run1++ )
00077         if( parameterization[run1] != 0 )
00078             delete parameterization[run1];
00079 
00080      if( parameterization != 0 ) free(parameterization);
00081      if( grid != 0 ) delete grid;
00082 }
00083 
00084 
00085 Curve& Curve::operator=( const Curve& arg ){
00086 
00087     uint run1;
00088 
00089     if ( this != &arg ){
00090 
00091         for( run1 = 0; run1 < nIntervals; run1++ )
00092             if( parameterization[run1] != 0 )
00093                 delete parameterization[run1];
00094 
00095         if( parameterization != 0 ) free(parameterization);
00096         if( grid != 0 ) delete grid;
00097 
00098         nIntervals       = arg.nIntervals;
00099         dim              = arg.dim       ;
00100         parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
00101 
00102         for( run1 = 0; run1 < nIntervals; run1++ ){
00103             if( arg.parameterization[run1] != 0 ) parameterization[run1] = new Function(*arg.parameterization[run1]);
00104             else                                  parameterization[run1] = 0;
00105         }
00106 
00107         if( arg.grid != 0 )  grid = new Grid(*arg.grid);
00108         else                 grid = 0;
00109     }
00110     return *this;
00111 }
00112 
00113 
00114 Curve Curve::operator()(        uint idx
00115                                                         ) const
00116 {
00117         Curve tmp;
00118 
00119         if ( (int)idx >= getDim() )
00120         {
00121                 ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00122                 return tmp;
00123         }
00124 
00125     uint run1;
00126 
00127         tmp.nIntervals       = nIntervals;
00128         tmp.dim              = dim         ; // -> 1!
00129         tmp.parameterization = (Function**)calloc(nIntervals,sizeof(Function*));
00130 
00131         for( run1 = 0; run1 < nIntervals; run1++ ){
00132                 if( parameterization[run1] != 0 ) tmp.parameterization[run1] = new Function( parameterization[run1]->operator()( idx ) );
00133                 else                              tmp.parameterization[run1] = 0;
00134         }
00135 
00136         if( grid != 0 )  tmp.grid = new Grid(*grid);
00137         else             tmp.grid = 0;
00138 
00139         return tmp;
00140 }
00141 
00142 
00143 returnValue Curve::add( double tStart, double tEnd, const DVector constant ){
00144 
00145     uint     run1;
00146     Function tmp ;
00147 
00148     // CONSTRUCT SIMPLY A CONSTANT FUNCTION AND ADD IT:
00149     // ------------------------------------------------
00150 
00151     for( run1 = 0; run1 < constant.getDim(); run1++ )
00152          tmp << constant(run1);
00153 
00154     return add( tStart, tEnd, tmp );
00155 }
00156 
00157 
00158 returnValue Curve::add( const VariablesGrid& sampledData, InterpolationMode mode ){
00159 
00160     uint         run1,run2  ;
00161     returnValue  returnvalue;
00162     Function    *tmp        ;
00163     TIME         t          ;
00164     double       m,b        ;
00165 
00166     switch( mode ){
00167 
00168         case IM_CONSTANT:
00169              for( run1 = 0; run1 < sampledData.getNumPoints()-1; run1++ ){
00170                  tmp = new Function();
00171                  for( run2 = 0; run2 < sampledData.getNumRows(); run2++ )
00172                      tmp->operator<<( sampledData(run1,run2) );
00173                  returnvalue = add( sampledData.getTime(run1), sampledData.getTime(run1+1), *tmp );
00174                  if( returnvalue != SUCCESSFUL_RETURN )
00175                      ACADOERROR(returnvalue);
00176                  delete tmp;
00177              }
00178              return SUCCESSFUL_RETURN;
00179 
00180 
00181         case IM_LINEAR:
00182              for( run1 = 0; run1 < sampledData.getNumPoints()-1; run1++ ){
00183                  tmp = new Function();
00184                  for( run2 = 0; run2 < sampledData.getNumRows(); run2++ ){
00185 
00186                      m = (sampledData(run1+1,run2)    - sampledData(run1,run2)   )/
00187                          (sampledData.getTime(run1+1) - sampledData.getTime(run1));
00188 
00189                      b = sampledData(run1,run2) - m*sampledData.getTime(run1);
00190 
00191                      tmp->operator<<( m*t + b );
00192                  }
00193                  returnvalue = add( sampledData.getTime(run1), sampledData.getTime(run1+1), *tmp );
00194                  if( returnvalue != SUCCESSFUL_RETURN )
00195                      ACADOERROR(returnvalue);
00196                  delete tmp;
00197              }
00198              return SUCCESSFUL_RETURN;
00199 
00200 
00201         case IM_QUADRATIC:
00202              return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00203 
00204         case IM_CUBIC:
00205              return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00206 
00207         default:
00208              return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00209     }
00210     return SUCCESSFUL_RETURN;
00211 }
00212 
00213 
00214 
00215 returnValue Curve::add( double tStart, double tEnd, const Function &parameterization_ ){
00216 
00217     uint run1;
00218 
00219     // CHECK WHETHER  "tStart < tEnd": 
00220     // ------------------------------------------------
00221     if( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE )
00222         return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID);
00223 
00224 
00225     // CHECK WHETHER THE INPUT VECTOR IS EMPTY: 
00226     // ------------------------------------------------
00227     if( parameterization_.getDim() == 0 )
00228         return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
00229 
00230 
00231 
00232     if( isEmpty() == BT_FALSE ){
00233 
00234         // IF THE CURVE IS NOT EMPTY THE DIMENSIONS MUST BE CHECKED:
00235         // ---------------------------------------------------------
00236         if( getDim() != (int) parameterization_.getDim() )
00237             return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
00238 
00239         // CHECK WHETHER THE CURVE HAS NO GAP's:
00240         // ---------------------------------------------------------
00241         if( acadoIsEqual( tStart,grid->getLastTime() ) == BT_FALSE )
00242                 {
00243                         ASSERT(1==0);
00244             return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID);
00245                 }
00246 
00247         // APPEND THE NEW TIME INTERVAL TO THE GRID:
00248         // ---------------------------------------------------------
00249 
00250         double *times = new double[nIntervals+2];
00251         for( run1 = 0; run1 < nIntervals+1; run1++ )        
00252             times[run1] = grid->getTime(run1);
00253         times[nIntervals+1] = tEnd;
00254 
00255         delete grid;
00256         grid = new Grid( nIntervals+2, times );
00257 
00258         nIntervals++;
00259         delete[] times;
00260 
00261         // ---------------------------------------------------------
00262     }
00263     else{
00264 
00265         // SETUP A NEW GRID:
00266         // ----------------------------------------------------
00267 
00268         grid = new Grid( tStart, tEnd, 2 );
00269         nIntervals++;
00270 
00271         // ----------------------------------------------------
00272     }
00273 
00274 
00275     // CHECK WHETHER THE FUNCTION ITSELF IS VALID:
00276     // -------------------------------------------
00277     if( parameterization_.getNX () != 0 ||
00278         parameterization_.getNXA() != 0 ||
00279         parameterization_.getNP () != 0 ||
00280         parameterization_.getNPI() != 0 ||
00281         parameterization_.getNU () != 0 ||
00282         parameterization_.getNUI() != 0 ||
00283         parameterization_.getNW()  != 0 ){
00284             return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
00285     }
00286 
00287 
00288     // SET THE DIMENSION OF THE CURVE:
00289     // (the correctness of the dimension has already been cecked)
00290     // ----------------------------------------------------------
00291 
00292     dim = parameterization_.getDim();
00293 
00294 
00295     // ALLOCATE MEMORY FOR THE NEW PIECE OF CURVE:
00296     // -------------------------------------------
00297 
00298     parameterization = (Function**)realloc(parameterization,nIntervals*sizeof(Function*));
00299 
00300 
00301     // MAKE A DEEP COPY OF THE PARAMETERIZATION:
00302     // -----------------------------------------
00303 
00304     parameterization[nIntervals-1] = new Function(parameterization_);
00305 
00306 
00307     // RETURN:
00308     // -----------------------------------------
00309 
00310     return SUCCESSFUL_RETURN;
00311 }
00312 
00313 
00314 returnValue Curve::evaluate( double t, double *result ) const{
00315 
00316     uint        idx        ;
00317     returnValue returnvalue;
00318 
00319     // CHECK WHETHER THE CURVE IS EMPTY:
00320     // ---------------------------------
00321     if( isEmpty() == BT_TRUE )
00322         return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00323 
00324 
00325     // CHECK WHETHER THE TIME  t  IS IN THE DOMAIN OF THE CURVE: 
00326     // ---------------------------------------------------------
00327     if( (t > grid->getLastTime() + 100.0*EPS) || (t < grid->getFirstTime() - 100.0*EPS) )
00328         return ACADOERROR(RET_INVALID_TIME_POINT);
00329 
00330 
00331     // CHECK WHETHER THE ARGUMENT IS THE NULL POINTER:
00332     // -----------------------------------------------
00333     if( result == 0 )
00334         return ACADOERROR(RET_INVALID_ARGUMENTS);
00335 
00336 
00337     // OBTAIN THE INTERVAL INDEX:
00338     // --------------------------
00339 
00340     idx = grid->getFloorIndex(t);
00341     if( idx == nIntervals ) idx--;
00342 
00343 
00344     // EVALUATE THE FUNCTION ASSOCIATED WITH THIS INTERVAL:
00345     // ----------------------------------------------------
00346     double tt[1] = { t };
00347     returnvalue = parameterization[idx]->evaluate(0,tt,result);
00348 
00349 
00350     if( returnvalue != SUCCESSFUL_RETURN )
00351         return ACADOERROR(returnvalue);
00352 
00353     return SUCCESSFUL_RETURN;
00354 }
00355 
00356 
00357 returnValue Curve::evaluate( double t, DVector &result ) const{
00358 
00359     uint        run1       ;
00360     returnValue returnvalue;
00361 
00362     double *tmp = new double[dim];
00363 
00364     returnvalue = evaluate( t, tmp );
00365 
00366     if( returnvalue != SUCCESSFUL_RETURN )
00367         return returnvalue;
00368 
00369     result.init(dim);
00370     for( run1 = 0; run1 < dim; run1++ )
00371         result(run1) = tmp[run1];
00372 
00373      delete[] tmp;
00374 
00375     return SUCCESSFUL_RETURN;
00376 }
00377 
00378 
00379 returnValue Curve::evaluate( double tStart, double tEnd, VariablesGrid &result ) const
00380 {
00381         // determine sub grid of intervals with given time horizon [tStart,tEnd]
00382         Grid intervalsSubGrid;
00383 
00384         if ( grid->getSubGrid( tStart,tEnd,intervalsSubGrid ) != SUCCESSFUL_RETURN )
00385                 return ACADOERROR( RET_UNKNOWN_BUG );
00386 
00387         return discretize( intervalsSubGrid,result );
00388 }
00389 
00390 
00391 
00392 returnValue Curve::discretize( const Grid &discretizationGrid, VariablesGrid &result ) const{
00393 
00394     uint        run1       ;
00395     returnValue returnvalue;
00396     DVector      tmp        ;
00397 
00398     result.init( dim, discretizationGrid );
00399 
00400     for( run1 = 0; run1 < discretizationGrid.getNumPoints(); run1++ ){
00401         returnvalue = evaluate( discretizationGrid.getTime(run1), tmp );
00402         if( returnvalue != SUCCESSFUL_RETURN )
00403             return returnvalue;
00404         result.setVector(run1,tmp);
00405     }
00406     return SUCCESSFUL_RETURN;
00407 }
00408 
00409 
00410 returnValue Curve::getTimeDomain( double tStart, double tEnd ) const
00411 {
00412     // CHECK WHETHER THE CURVE IS EMPTY:
00413     // ---------------------------------
00414     if( isEmpty() == BT_TRUE )
00415         return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00416 
00417     tStart = grid->getFirstTime();
00418     tEnd   = grid->getLastTime() ;
00419 
00420     return SUCCESSFUL_RETURN;
00421 }
00422 
00423 
00424 returnValue Curve::getTimeDomain( const uint &idx, double tStart, double tEnd ) const
00425 {
00426     // CHECK WHETHER THE CURVE IS EMPTY:
00427     // ---------------------------------
00428     if( isEmpty() == BT_TRUE )
00429         return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00430 
00431 
00432     // CHECK WHETHER THE INDEX IS IN THE PERMISSIBLE RANGE:
00433     // ----------------------------------------------------
00434     if( idx >= grid->getNumIntervals() )
00435         return ACADOERROR(RET_INDEX_OUT_OF_BOUNDS);
00436 
00437 
00438     tStart = grid->getTime(idx  );
00439     tEnd   = grid->getTime(idx+1);
00440 
00441     return SUCCESSFUL_RETURN;
00442 }
00443 
00444 
00445 BooleanType Curve::isInTimeDomain(      double t
00446                                                                         ) const
00447 {
00448         if( isEmpty( ) == BT_TRUE )
00449                 return BT_FALSE;
00450 
00451         if ( ( acadoIsGreater( t,grid->getFirstTime() ) == BT_TRUE ) &&
00452                  ( acadoIsSmaller( t,grid->getLastTime()  ) == BT_TRUE ) )
00453                 return BT_TRUE;
00454         else
00455                 return BT_FALSE;
00456 }
00457 
00458 
00459 
00460 //
00461 // PROTECTED MEMBER FUNCTIONS:
00462 //
00463 
00464 
00465 CLOSE_NAMESPACE_ACADO
00466 
00467 /*
00468  *    end of file
00469  */


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