Utils.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
00006  *
00007  *      qpOASES is free software; you can redistribute it and/or
00008  *      modify it under the terms of the GNU Lesser General Public
00009  *      License as published by the Free Software Foundation; either
00010  *      version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *      qpOASES 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 GNU
00015  *      Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with qpOASES; if not, write to the Free Software
00019  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020  *
00021  */
00022 
00023 
00035 #include <math.h>
00036 
00037 #if defined(__WIN32__) || defined(WIN32)
00038   #include <windows.h>
00039 #elif defined(LINUX)
00040   #include <sys/stat.h>
00041   #include <sys/time.h>
00042 #endif
00043 
00044 #ifdef __MATLAB__
00045   #include <mex.h>
00046 #endif
00047 
00048 
00049 #include <Utils.hpp>
00050 
00051 
00052 
00053 #ifdef PC_DEBUG  /* Define print functions only for debugging! */
00054 /*
00055  *      p r i n t
00056  */
00057 returnValue print( const real_t* const v, int n )
00058 {
00059         int i;
00060         char myPrintfString[160];
00061 
00062         /* Print a vector. */
00063         myPrintf( "[\t" );
00064         for( i=0; i<n; ++i )
00065         {
00066                 sprintf( myPrintfString," %.16e\t", v[i] );
00067                 myPrintf( myPrintfString );
00068         }
00069         myPrintf( "]\n" );
00070 
00071         return SUCCESSFUL_RETURN;
00072 }
00073 
00074 
00075 /*
00076  *      p r i n t
00077  */
00078 returnValue print(      const real_t* const v, int n,
00079                                         const int* const V_idx
00080                                         )
00081 {
00082         int i;
00083         char myPrintfString[160];
00084 
00085         /* Print a permuted vector. */
00086         myPrintf( "[\t" );
00087         for( i=0; i<n; ++i )
00088         {
00089                 sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
00090                 myPrintf( myPrintfString );
00091         }
00092         myPrintf( "]\n" );
00093 
00094         return SUCCESSFUL_RETURN;
00095 }
00096 
00097 
00098 /*
00099  *      p r i n t
00100  */
00101 returnValue print(      const real_t* const v, int n,
00102                                         const char* name
00103                                         )
00104 {
00105         char myPrintfString[160];
00106 
00107         /* Print vector name ... */
00108         sprintf( myPrintfString,"%s = ", name );
00109         myPrintf( myPrintfString );
00110 
00111         /* ... and the vector itself. */
00112         return print( v, n );
00113 }
00114 
00115 
00116 /*
00117  *      p r i n t
00118  */
00119 returnValue print( const real_t* const M, int nrow, int ncol )
00120 {
00121         int i;
00122 
00123         /* Print a matrix as a collection of row vectors. */
00124         for( i=0; i<nrow; ++i )
00125                 print( &(M[i*ncol]), ncol );
00126         myPrintf( "\n" );
00127 
00128         return SUCCESSFUL_RETURN;
00129 }
00130 
00131 
00132 /*
00133  *      p r i n t
00134  */
00135 returnValue print(      const real_t* const M, int nrow, int ncol,
00136                                         const int* const ROW_idx, const int* const COL_idx
00137                                         )
00138 {
00139         int i;
00140 
00141         /* Print a permuted matrix as a collection of permuted row vectors. */
00142         for( i=0; i<nrow; ++i )
00143                 print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
00144         myPrintf( "\n" );
00145 
00146         return SUCCESSFUL_RETURN;
00147 }
00148 
00149 
00150 /*
00151  *      p r i n t
00152  */
00153 returnValue print(      const real_t* const M, int nrow, int ncol,
00154                                         const char* name
00155                                         )
00156 {
00157         char myPrintfString[160];
00158 
00159         /* Print matrix name ... */
00160         sprintf( myPrintfString,"%s = ", name );
00161         myPrintf( myPrintfString );
00162 
00163         /* ... and the matrix itself. */
00164         return print( M, nrow, ncol );
00165 }
00166 
00167 
00168 /*
00169  *      p r i n t
00170  */
00171 returnValue print( const int* const index, int n )
00172 {
00173         int i;
00174         char myPrintfString[160];
00175 
00176         /* Print a indexlist. */
00177         myPrintf( "[\t" );
00178         for( i=0; i<n; ++i )
00179         {
00180                 sprintf( myPrintfString," %d\t", index[i] );
00181                 myPrintf( myPrintfString );
00182         }
00183         myPrintf( "]\n" );
00184 
00185         return SUCCESSFUL_RETURN;
00186 }
00187 
00188 
00189 /*
00190  *      p r i n t
00191  */
00192 returnValue print(      const int* const index, int n,
00193                                         const char* name
00194                                         )
00195 {
00196         char myPrintfString[160];
00197 
00198         /* Print indexlist name ... */
00199         sprintf( myPrintfString,"%s = ", name );
00200         myPrintf( myPrintfString );
00201 
00202         /* ... and the indexlist itself. */
00203         return print( index, n );
00204 }
00205 
00206 
00207 /*
00208  *      m y P r i n t f
00209  */
00210 returnValue myPrintf( const char* s )
00211 {
00212         #ifdef __MATLAB__
00213         mexPrintf( s );
00214         #else
00215         myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
00216         if ( outputfile == 0 )
00217                 return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
00218 
00219         fprintf( outputfile, "%s", s );
00220         #endif
00221 
00222         return SUCCESSFUL_RETURN;
00223 }
00224 
00225 
00226 /*
00227  *      p r i n t C o p y r i g h t N o t i c e
00228  */
00229 returnValue printCopyrightNotice( )
00230 {
00231         return myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
00232 }
00233 
00234 
00235 /*
00236  *      r e a d F r o m F i l e
00237  */
00238 returnValue readFromFile(       real_t* data, int nrow, int ncol,
00239                                                         const char* datafilename
00240                                                         )
00241 {
00242         int i, j;
00243         float float_data;
00244         myFILE* datafile;
00245 
00246         /* 1) Open file. */
00247         if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
00248         {
00249                 char errstr[80];
00250                 sprintf( errstr,"(%s)",datafilename );
00251                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00252         }
00253 
00254         /* 2) Read data from file. */
00255         for( i=0; i<nrow; ++i )
00256         {
00257                 for( j=0; j<ncol; ++j )
00258                 {
00259                         if ( fscanf( datafile, "%f ", &float_data ) == 0 )
00260                         {
00261                                 fclose( datafile );
00262                                 char errstr[80];
00263                                 sprintf( errstr,"(%s)",datafilename );
00264                                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00265                         }
00266                         data[i*ncol + j] = ( (real_t) float_data );
00267                 }
00268         }
00269 
00270         /* 3) Close file. */
00271         fclose( datafile );
00272 
00273         return SUCCESSFUL_RETURN;
00274 }
00275 
00276 
00277 /*
00278  *      r e a d F r o m F i l e
00279  */
00280 returnValue readFromFile(       real_t* data, int n,
00281                                                         const char* datafilename
00282                                                         )
00283 {
00284         return readFromFile( data, n, 1, datafilename );
00285 }
00286 
00287 
00288 
00289 /*
00290  *      r e a d F r o m F i l e
00291  */
00292 returnValue readFromFile(       int* data, int n,
00293                                                         const char* datafilename
00294                                                         )
00295 {
00296         int i;
00297         myFILE* datafile;
00298 
00299         /* 1) Open file. */
00300         if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
00301         {
00302                 char errstr[80];
00303                 sprintf( errstr,"(%s)",datafilename );
00304                 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00305         }
00306 
00307         /* 2) Read data from file. */
00308         for( i=0; i<n; ++i )
00309         {
00310                 if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
00311                 {
00312                         fclose( datafile );
00313                         char errstr[80];
00314                         sprintf( errstr,"(%s)",datafilename );
00315                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00316                 }
00317         }
00318 
00319         /* 3) Close file. */
00320         fclose( datafile );
00321 
00322         return SUCCESSFUL_RETURN;
00323 }
00324 
00325 
00326 /*
00327  *      w r i t e I n t o F i l e
00328  */
00329 returnValue writeIntoFile(      const real_t* const data, int nrow, int ncol,
00330                                                         const char* datafilename, BooleanType append
00331                                                         )
00332 {
00333         int i, j;
00334         myFILE* datafile;
00335 
00336         /* 1) Open file. */
00337         if ( append == BT_TRUE )
00338         {
00339                 /* append data */
00340                 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
00341                 {
00342                         char errstr[80];
00343                         sprintf( errstr,"(%s)",datafilename );
00344                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00345                 }
00346         }
00347         else
00348         {
00349                 /* do not append data */
00350                 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
00351                 {
00352                         char errstr[80];
00353                         sprintf( errstr,"(%s)",datafilename );
00354                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00355                 }
00356         }
00357 
00358         /* 2) Write data into file. */
00359         for( i=0; i<nrow; ++i )
00360         {
00361                 for( j=0; j<ncol; ++j )
00362                         fprintf( datafile, "%.16e ", data[i*ncol+j] );
00363 
00364                 fprintf( datafile, "\n" );
00365         }
00366 
00367         /* 3) Close file. */
00368         fclose( datafile );
00369 
00370         return SUCCESSFUL_RETURN;
00371 }
00372 
00373 
00374 /*
00375  *      w r i t e I n t o F i l e
00376  */
00377 returnValue writeIntoFile(      const real_t* const data, int n,
00378                                                         const char* datafilename, BooleanType append
00379                                                         )
00380 {
00381         return writeIntoFile( data,1,n,datafilename,append );
00382 }
00383 
00384 
00385 /*
00386  *      w r i t e I n t o F i l e
00387  */
00388 returnValue writeIntoFile(      const int* const data, int n,
00389                                                         const char* datafilename, BooleanType append
00390                                                         )
00391 {
00392         int i;
00393 
00394         myFILE* datafile;
00395 
00396         /* 1) Open file. */
00397         if ( append == BT_TRUE )
00398         {
00399                 /* append data */
00400                 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
00401                 {
00402                         char errstr[80];
00403                         sprintf( errstr,"(%s)",datafilename );
00404                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00405                 }
00406         }
00407         else
00408         {
00409                 /* do not append data */
00410                 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
00411                 {
00412                         char errstr[80];
00413                         sprintf( errstr,"(%s)",datafilename );
00414                         return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00415                 }
00416         }
00417 
00418         /* 2) Write data into file. */
00419         for( i=0; i<n; ++i )
00420                 fprintf( datafile, "%d\n", data[i] );
00421 
00422         /* 3) Close file. */
00423         fclose( datafile );
00424 
00425         return SUCCESSFUL_RETURN;
00426 }
00427 #endif  /* PC_DEBUG */
00428 
00429 
00430 /*
00431  *      g e t C P U t i m e
00432  */
00433 real_t getCPUtime( )
00434 {
00435         real_t current_time = -1.0;
00436 
00437         #if defined(__WIN32__) || defined(WIN32)
00438         LARGE_INTEGER counter, frequency;
00439         QueryPerformanceFrequency(&frequency);
00440         QueryPerformanceCounter(&counter);
00441         current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
00442         #elif defined(LINUX)
00443         struct timeval theclock;
00444         gettimeofday( &theclock,0 );
00445         current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
00446         #endif
00447 
00448         return current_time;
00449 }
00450 
00451 
00452 /*
00453  *      g e t N o r m
00454  */
00455 real_t getNorm( const real_t* const v, int n )
00456 {
00457         int i;
00458 
00459         real_t norm = 0.0;
00460 
00461         for( i=0; i<n; ++i )
00462                 norm += v[i]*v[i];
00463 
00464         return sqrt( norm );
00465 }
00466 
00467 
00468 
00469 /*
00470  *      end of file
00471  */


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