00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00054
00055
00056
00057 returnValue print( const real_t* const v, int n )
00058 {
00059 int i;
00060 char myPrintfString[160];
00061
00062
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
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
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
00100
00101 returnValue print( const real_t* const v, int n,
00102 const char* name
00103 )
00104 {
00105 char myPrintfString[160];
00106
00107
00108 sprintf( myPrintfString,"%s = ", name );
00109 myPrintf( myPrintfString );
00110
00111
00112 return print( v, n );
00113 }
00114
00115
00116
00117
00118
00119 returnValue print( const real_t* const M, int nrow, int ncol )
00120 {
00121 int i;
00122
00123
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
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
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
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
00160 sprintf( myPrintfString,"%s = ", name );
00161 myPrintf( myPrintfString );
00162
00163
00164 return print( M, nrow, ncol );
00165 }
00166
00167
00168
00169
00170
00171 returnValue print( const int* const index, int n )
00172 {
00173 int i;
00174 char myPrintfString[160];
00175
00176
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
00191
00192 returnValue print( const int* const index, int n,
00193 const char* name
00194 )
00195 {
00196 char myPrintfString[160];
00197
00198
00199 sprintf( myPrintfString,"%s = ", name );
00200 myPrintf( myPrintfString );
00201
00202
00203 return print( index, n );
00204 }
00205
00206
00207
00208
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
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
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
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
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
00271 fclose( datafile );
00272
00273 return SUCCESSFUL_RETURN;
00274 }
00275
00276
00277
00278
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
00291
00292 returnValue readFromFile( int* data, int n,
00293 const char* datafilename
00294 )
00295 {
00296 int i;
00297 myFILE* datafile;
00298
00299
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
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
00320 fclose( datafile );
00321
00322 return SUCCESSFUL_RETURN;
00323 }
00324
00325
00326
00327
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
00337 if ( append == BT_TRUE )
00338 {
00339
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
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
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
00368 fclose( datafile );
00369
00370 return SUCCESSFUL_RETURN;
00371 }
00372
00373
00374
00375
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
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
00397 if ( append == BT_TRUE )
00398 {
00399
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
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
00419 for( i=0; i<n; ++i )
00420 fprintf( datafile, "%d\n", data[i] );
00421
00422
00423 fclose( datafile );
00424
00425 return SUCCESSFUL_RETURN;
00426 }
00427 #endif
00428
00429
00430
00431
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
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
00471