external_packages/qpoases/SRC/Utils.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
6  *
7  * qpOASES is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * qpOASES is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with qpOASES; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  */
22 
23 
35 #include <math.h>
36 
37 #if defined(__WIN32__) || defined(WIN32)
38  #include <windows.h>
39 #elif defined(LINUX)
40  #include <sys/stat.h>
41  #include <sys/time.h>
42 #endif
43 
44 #ifdef __MATLAB__
45  #include <mex.h>
46 #endif
47 
48 
49 #include <Utils.hpp>
50 
51 
52 
53 #ifdef PC_DEBUG /* Define print functions only for debugging! */
54 /*
55  * p r i n t
56  */
57 returnValue print( const real_t* const v, int n )
58 {
59  int i;
60  char myPrintfString[160];
61 
62  /* Print a vector. */
63  myPrintf( "[\t" );
64  for( i=0; i<n; ++i )
65  {
66  sprintf( myPrintfString," %.16e\t", v[i] );
67  myPrintf( myPrintfString );
68  }
69  myPrintf( "]\n" );
70 
71  return SUCCESSFUL_RETURN;
72 }
73 
74 
75 /*
76  * p r i n t
77  */
78 returnValue print( const real_t* const v, int n,
79  const int* const V_idx
80  )
81 {
82  int i;
83  char myPrintfString[160];
84 
85  /* Print a permuted vector. */
86  myPrintf( "[\t" );
87  for( i=0; i<n; ++i )
88  {
89  sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
90  myPrintf( myPrintfString );
91  }
92  myPrintf( "]\n" );
93 
94  return SUCCESSFUL_RETURN;
95 }
96 
97 
98 /*
99  * p r i n t
100  */
101 returnValue print( const real_t* const v, int n,
102  const char* name
103  )
104 {
105  char myPrintfString[160];
106 
107  /* Print vector name ... */
108  sprintf( myPrintfString,"%s = ", name );
109  myPrintf( myPrintfString );
110 
111  /* ... and the vector itself. */
112  return print( v, n );
113 }
114 
115 
116 /*
117  * p r i n t
118  */
119 returnValue print( const real_t* const M, int nrow, int ncol )
120 {
121  int i;
122 
123  /* Print a matrix as a collection of row vectors. */
124  for( i=0; i<nrow; ++i )
125  print( &(M[i*ncol]), ncol );
126  myPrintf( "\n" );
127 
128  return SUCCESSFUL_RETURN;
129 }
130 
131 
132 /*
133  * p r i n t
134  */
135 returnValue print( const real_t* const M, int nrow, int ncol,
136  const int* const ROW_idx, const int* const COL_idx
137  )
138 {
139  int i;
140 
141  /* Print a permuted matrix as a collection of permuted row vectors. */
142  for( i=0; i<nrow; ++i )
143  print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
144  myPrintf( "\n" );
145 
146  return SUCCESSFUL_RETURN;
147 }
148 
149 
150 /*
151  * p r i n t
152  */
153 returnValue print( const real_t* const M, int nrow, int ncol,
154  const char* name
155  )
156 {
157  char myPrintfString[160];
158 
159  /* Print matrix name ... */
160  sprintf( myPrintfString,"%s = ", name );
161  myPrintf( myPrintfString );
162 
163  /* ... and the matrix itself. */
164  return print( M, nrow, ncol );
165 }
166 
167 
168 /*
169  * p r i n t
170  */
171 returnValue print( const int* const index, int n )
172 {
173  int i;
174  char myPrintfString[160];
175 
176  /* Print a indexlist. */
177  myPrintf( "[\t" );
178  for( i=0; i<n; ++i )
179  {
180  sprintf( myPrintfString," %d\t", index[i] );
181  myPrintf( myPrintfString );
182  }
183  myPrintf( "]\n" );
184 
185  return SUCCESSFUL_RETURN;
186 }
187 
188 
189 /*
190  * p r i n t
191  */
192 returnValue print( const int* const index, int n,
193  const char* name
194  )
195 {
196  char myPrintfString[160];
197 
198  /* Print indexlist name ... */
199  sprintf( myPrintfString,"%s = ", name );
200  myPrintf( myPrintfString );
201 
202  /* ... and the indexlist itself. */
203  return print( index, n );
204 }
205 
206 
207 /*
208  * m y P r i n t f
209  */
210 returnValue myPrintf( const char* s )
211 {
212  #ifdef __MATLAB__
213  mexPrintf( s );
214  #else
215  myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
216  if ( outputfile == 0 )
218 
219  fprintf( outputfile, "%s", s );
220  #endif
221 
222  return SUCCESSFUL_RETURN;
223 }
224 
225 
226 /*
227  * p r i n t C o p y r i g h t N o t i c e
228  */
230 {
231  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" );
232 }
233 
234 
235 /*
236  * r e a d F r o m F i l e
237  */
238 returnValue readFromFile( real_t* data, int nrow, int ncol,
239  const char* datafilename
240  )
241 {
242  int i, j;
243  float float_data;
244  myFILE* datafile;
245 
246  /* 1) Open file. */
247  if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
248  {
249  char errstr[80];
250  sprintf( errstr,"(%s)",datafilename );
252  }
253 
254  /* 2) Read data from file. */
255  for( i=0; i<nrow; ++i )
256  {
257  for( j=0; j<ncol; ++j )
258  {
259  if ( fscanf( datafile, "%f ", &float_data ) == 0 )
260  {
261  fclose( datafile );
262  char errstr[80];
263  sprintf( errstr,"(%s)",datafilename );
265  }
266  data[i*ncol + j] = ( (real_t) float_data );
267  }
268  }
269 
270  /* 3) Close file. */
271  fclose( datafile );
272 
273  return SUCCESSFUL_RETURN;
274 }
275 
276 
277 /*
278  * r e a d F r o m F i l e
279  */
280 returnValue readFromFile( real_t* data, int n,
281  const char* datafilename
282  )
283 {
284  return readFromFile( data, n, 1, datafilename );
285 }
286 
287 
288 
289 /*
290  * r e a d F r o m F i l e
291  */
292 returnValue readFromFile( int* data, int n,
293  const char* datafilename
294  )
295 {
296  int i;
297  myFILE* datafile;
298 
299  /* 1) Open file. */
300  if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
301  {
302  char errstr[80];
303  sprintf( errstr,"(%s)",datafilename );
305  }
306 
307  /* 2) Read data from file. */
308  for( i=0; i<n; ++i )
309  {
310  if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
311  {
312  fclose( datafile );
313  char errstr[80];
314  sprintf( errstr,"(%s)",datafilename );
316  }
317  }
318 
319  /* 3) Close file. */
320  fclose( datafile );
321 
322  return SUCCESSFUL_RETURN;
323 }
324 
325 
326 /*
327  * w r i t e I n t o F i l e
328  */
329 returnValue writeIntoFile( const real_t* const data, int nrow, int ncol,
330  const char* datafilename, BooleanType append
331  )
332 {
333  int i, j;
334  myFILE* datafile;
335 
336  /* 1) Open file. */
337  if ( append == BT_TRUE )
338  {
339  /* append data */
340  if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
341  {
342  char errstr[80];
343  sprintf( errstr,"(%s)",datafilename );
345  }
346  }
347  else
348  {
349  /* do not append data */
350  if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
351  {
352  char errstr[80];
353  sprintf( errstr,"(%s)",datafilename );
355  }
356  }
357 
358  /* 2) Write data into file. */
359  for( i=0; i<nrow; ++i )
360  {
361  for( j=0; j<ncol; ++j )
362  fprintf( datafile, "%.16e ", data[i*ncol+j] );
363 
364  fprintf( datafile, "\n" );
365  }
366 
367  /* 3) Close file. */
368  fclose( datafile );
369 
370  return SUCCESSFUL_RETURN;
371 }
372 
373 
374 /*
375  * w r i t e I n t o F i l e
376  */
377 returnValue writeIntoFile( const real_t* const data, int n,
378  const char* datafilename, BooleanType append
379  )
380 {
381  return writeIntoFile( data,1,n,datafilename,append );
382 }
383 
384 
385 /*
386  * w r i t e I n t o F i l e
387  */
388 returnValue writeIntoFile( const int* const data, int n,
389  const char* datafilename, BooleanType append
390  )
391 {
392  int i;
393 
394  myFILE* datafile;
395 
396  /* 1) Open file. */
397  if ( append == BT_TRUE )
398  {
399  /* append data */
400  if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
401  {
402  char errstr[80];
403  sprintf( errstr,"(%s)",datafilename );
405  }
406  }
407  else
408  {
409  /* do not append data */
410  if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
411  {
412  char errstr[80];
413  sprintf( errstr,"(%s)",datafilename );
415  }
416  }
417 
418  /* 2) Write data into file. */
419  for( i=0; i<n; ++i )
420  fprintf( datafile, "%d\n", data[i] );
421 
422  /* 3) Close file. */
423  fclose( datafile );
424 
425  return SUCCESSFUL_RETURN;
426 }
427 #endif /* PC_DEBUG */
428 
429 
430 /*
431  * g e t C P U t i m e
432  */
434 {
435  real_t current_time = -1.0;
436 
437  #if defined(__WIN32__) || defined(WIN32)
438  LARGE_INTEGER counter, frequency;
439  QueryPerformanceFrequency(&frequency);
440  QueryPerformanceCounter(&counter);
441  current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
442  #elif defined(LINUX)
443  struct timeval theclock;
444  gettimeofday( &theclock,0 );
445  current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
446  #endif
447 
448  return current_time;
449 }
450 
451 
452 /*
453  * g e t N o r m
454  */
455 real_t getNorm( const real_t* const v, int n )
456 {
457  int i;
458 
459  real_t norm = 0.0;
460 
461  for( i=0; i<n; ++i )
462  norm += v[i]*v[i];
463 
464  return sqrt( norm );
465 }
466 
467 
468 
469 /*
470  * end of file
471  */
int counter
Definition: powerkite_c.cpp:40
IntermediateState sqrt(const Expression &arg)
#define __LINE__
returnValue throwError(returnValue Enumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Allows to pass back messages to the calling function.
real_t getNorm(const real_t *const v, int n)
#define __FUNCTION__
returnValue readFromFile(real_t *data, int nrow, int ncol, const char *datafilename)
returnValue myPrintf(const char *s)
returnValue writeIntoFile(const real_t *const data, int nrow, int ncol, const char *datafilename, BooleanType append)
#define v
#define BT_TRUE
Definition: acado_types.hpp:47
myFILE * getOutputFile() const
BEGIN_NAMESPACE_QPOASES returnValue print(const real_t *const v, int n)
#define __FILE__
double real_t
Definition: AD_test.c:10


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:16