grid.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
35 #include <iomanip>
36 
37 using namespace std;
38 
40 
41 
42 
43 //
44 // PUBLIC MEMBER FUNCTIONS:
45 //
46 
48 {
49  nPoints = 0;
50  times = 0;
51 }
52 
53 
54 Grid::Grid( uint nPoints_,
55  double *times_
56  )
57 {
58  times = 0;
59  init( nPoints_,times_ );
60 }
61 
62 
63 Grid::Grid( const DVector& times_
64  )
65 {
66  times = 0;
67  init( times_ );
68 }
69 
70 
71 Grid::Grid( double _firstTime,
72  double _lastTime,
73  uint _nPoints
74  )
75 {
76  times = 0;
77  init( _firstTime,_lastTime,_nPoints );
78 }
79 
80 
81 
82 Grid::Grid( const Grid& rhs )
83 {
84  nPoints = rhs.nPoints;
85 
86  if ( rhs.times != 0 )
87  {
88  times = (double*) calloc( nPoints,sizeof(double) );
89 
90  for( uint i=0; i<nPoints; ++i )
91  times[i] = rhs.times[i];
92  }
93  else
94  {
95  times = 0;
96  }
97 }
98 
99 
101 {
102  if ( times != 0 )
103  free( times );
104 }
105 
106 
108  const double* const _times
109  )
110 {
111  nPoints = _nPoints;
112 
113  if ( times != 0 )
114  free( times );
115 
116  if ( nPoints > 0 )
117  times = (double*) calloc( nPoints,sizeof(double) );
118  else
119  times = 0;
120 
121  if ( _times != 0 )
122  {
123  for( uint i=0; i<nPoints; ++i )
124  times[i] = _times[i];
125  }
126  else
127  {
128  for( uint i=0; i<nPoints; ++i )
129  times[i] = -INFTY;
130  }
131 
132  return SUCCESSFUL_RETURN;
133 }
134 
135 
137  )
138 {
139  nPoints = times_.getDim();
140 
141  if ( times != 0 )
142  free( times );
143 
144  if ( nPoints > 0 )
145  times = (double*) calloc( nPoints,sizeof(double) );
146  else
147  times = 0;
148 
149  for( uint i=0; i<nPoints; ++i )
150  times[i] = times_(i);
151 
152  return SUCCESSFUL_RETURN;
153 }
154 
155 
156 returnValue Grid::init( double _firstTime,
157  double _lastTime,
158  uint _nPoints
159  )
160 {
161  nPoints = _nPoints;
162 
163  if ( times != 0 )
164  free( times );
165 
166  if ( nPoints > 0 )
167  times = (double*) calloc( nPoints,sizeof(double) );
168  else
169  times = 0;
170 
171  return setupEquidistant( _firstTime,_lastTime );
172 }
173 
174 
176  )
177 {
178  operator=( rhs );
179 
180  return SUCCESSFUL_RETURN;
181 }
182 
183 
184 
186 {
187  if ( this != &rhs )
188  {
189  if ( times != 0 )
190  free( times );
191 
192 
193  nPoints = rhs.nPoints;
194 
195  if ( rhs.times != 0 )
196  {
197  times = (double*) calloc( nPoints,sizeof(double) );
198 
199  for( uint i=0; i<nPoints; ++i )
200  times[i] = rhs.times[i];
201  }
202  else
203  {
204  times = 0;
205  }
206  }
207 
208  return *this;
209 }
210 
211 
212 Grid& Grid::operator&( const Grid& arg )
213 {
214  merge( arg,MM_KEEP,BT_TRUE );
215 
216 // if( times == 0 ) return Grid::operator=( arg );
217 // if( arg.times == 0 ) return *this;
218 //
219 // uint max_n = nPoints + arg.nPoints;
220 // uint count = 0;
221 // uint count1 = 0;
222 // uint count2 = 0;
223 //
224 // double* new_times = (double*) calloc( max_n,sizeof(double) );
225 // double tt = getTime(0);
226 //
227 // if( arg.getTime(0) < tt ){
228 // tt = arg.getTime(0);
229 // }
230 //
231 // double tt2 = tt;
232 //
233 // new_times[count] = tt2;
234 // count++;
235 //
236 // while( count < max_n ){
237 //
238 // count1 = 0;
239 // while( count1 < nPoints ){
240 //
241 // if( acadoIsStrictlyGreater( getTime(count1) , tt ) == BT_TRUE ){
242 // tt2 = getTime(count1);
243 // break;
244 // }
245 // count1++;
246 // }
247 //
248 // if( count1 < nPoints ){
249 // count2 = 0;
250 // while( count2 < arg.nPoints ){
251 // if( acadoIsStrictlyGreater( arg.getTime(count2) , tt ) == BT_TRUE &&
252 // acadoIsStrictlySmaller( arg.getTime(count2) , tt2 ) == BT_TRUE ){
253 // tt2 = arg.getTime(count2);
254 // break;
255 // }
256 // count2++;
257 // }
258 // }
259 // else{
260 // count2 = 0;
261 // while( count2 < arg.nPoints ){
262 // if( acadoIsStrictlyGreater( arg.getTime(count2), tt ) == BT_TRUE ){
263 // tt2 = arg.getTime(count2);
264 // break;
265 // }
266 // count2++;
267 // }
268 // }
269 // if( count1 == nPoints && count2 == arg.nPoints ){
270 // break;
271 // }
272 //
273 // new_times[count] = tt2;
274 // tt = tt2;
275 //
276 // count++;
277 // }
278 //
279 // free( times );
280 //
281 // nPoints = count;
282 // times = (double*) calloc( count,sizeof(double) );
283 // for( count = 0; count < nPoints; count++ ){
284 // times[count] = new_times[count];
285 // }
286 //
287 // free( new_times );
288 
289  return *this;
290 }
291 
292 
294  )
295 {
296  if ( *this == arg )
297  return SUCCESSFUL_RETURN;
298 
299  // construct common grid and assigns it to both grids
300  *this & arg;
301  arg = *this;
302 
303  return SUCCESSFUL_RETURN;
304 }
305 
306 
308  )
309 {
310  int idx = findNextIndex( );
311 
312  if ( idx < 0 )
314 
315  if ( idx > 0 )
316  {
317  if ( acadoIsStrictlyGreater( times[idx-1] , _time ) == BT_TRUE )
319  }
320 
321  times[idx] = _time;
322  return SUCCESSFUL_RETURN;
323 }
324 
325 
327  double _time
328  )
329 {
330  if ( pointIdx >= getNumPoints( ) )
332 
333  times[pointIdx] = _time;
334 
335  return SUCCESSFUL_RETURN;
336 }
337 
338 
340  )
341 {
342  if ( ( getNumPoints( ) > 0 ) && ( acadoIsGreater( _time,getLastTime() ) == BT_FALSE ) )
343  {
344  //ASSERT( 1==0 );
346  }
347 
348  ++nPoints;
349 
350  times = (double*) realloc( times,nPoints*sizeof(double) );
351  times[nPoints-1] = _time;
352 
353  return SUCCESSFUL_RETURN;
354 }
355 
356 
357 // uses a simple O(n^2) algorithm for sorting
359  MergeMethod _mergeMethod,
360  BooleanType keepOverlap
361  )
362 {
363  if ( ( keepOverlap == BT_FALSE ) && ( _mergeMethod == MM_DUPLICATE ) )
365 
366  // nothing to do if arg or object itself is empty
367  if ( arg.getNumPoints( ) == 0 )
368  return SUCCESSFUL_RETURN;
369 
370  if ( getNumPoints( ) == 0 )
371  {
372  *this = arg;
373  return SUCCESSFUL_RETURN;
374  }
375 
376 
377  // construct merged grid
378  Grid mergedGrid;
379  uint j = 0;
380  BooleanType overlapping = BT_FALSE;
381 
382  for( uint i=0; i<getNumPoints( ); ++i )
383  {
384  if ( keepOverlap == BT_FALSE )
385  overlapping = arg.isInInterval( getTime(i) );
386 
387  // add all grid points of argument grid that are smaller
388  // then current one of original grid
389  while ( ( j < arg.getNumPoints( ) ) &&
390  ( acadoIsStrictlySmaller( arg.getTime( j ),getTime( i ) ) == BT_TRUE ) )
391  {
392  if ( ( overlapping == BT_FALSE ) ||
393  ( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_REPLACE ) ) )
394  {
395  mergedGrid.addTime( arg.getTime( j ) );
396  }
397 
398  ++j;
399  }
400 
401  // merge current grid points if they are at equal times
402  if ( acadoIsEqual( arg.getTime( j ),getTime( i ) ) == BT_TRUE )
403  {
404  switch ( _mergeMethod )
405  {
406  case MM_KEEP:
407  mergedGrid.addTime( getTime( i ) );
408  break;
409 
410  case MM_REPLACE:
411  mergedGrid.addTime( arg.getTime( j ) );
412  break;
413 
414  case MM_DUPLICATE:
415  mergedGrid.addTime( getTime( i ) );
416  mergedGrid.addTime( arg.getTime( j ) );
417  break;
418  }
419  ++j;
420  }
421  else
422  {
423  // add current grid point of original grid
424  if ( ( overlapping == BT_FALSE ) ||
425  ( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_KEEP ) ) )
426  {
427  mergedGrid.addTime( getTime( i ) );
428  }
429  }
430  }
431 
432  // add all remaining grid points of argument grid
433  while ( j < arg.getNumPoints( ) )
434  {
435  if ( acadoIsStrictlyGreater( arg.getTime(j),getLastTime() ) == BT_TRUE )
436  mergedGrid.addTime( arg.getTime( j ) );
437 
438  ++j;
439  }
440 
441  // merged grid becomes current grid
442  *this = mergedGrid;
443 
444  return SUCCESSFUL_RETURN;
445 }
446 
447 
448 
449 Grid& Grid::shiftTimes( double timeShift
450  )
451 {
452  for( uint i=0; i<nPoints; ++i )
453  times[i] += timeShift;
454 
455  return *this;
456 }
457 
458 
459 
461  )
462 {
463  if ( acadoIsStrictlyGreater( scaling,0.0 ) == BT_FALSE )
464  {
466  scaling = 1.0;
467  }
468 
469  for( uint i=0; i<nPoints; ++i )
470  times[i] *= scaling;
471 
472  return SUCCESSFUL_RETURN;
473 }
474 
475 
476 
478  )
479 {
480  if ( factor == 0 )
482 
483  /* nothing to do */
484  if ( ( factor == 1 ) || ( getNumIntervals( ) == 0 ) )
485  return SUCCESSFUL_RETURN;
486 
487  /* setup <factor>-times finer grid */
488  double* newTimes = (double*) calloc( getNumIntervals( )*factor+1,sizeof(double) );
489 
490  for( uint i=0; i<getNumIntervals( ); ++i )
491  for( uint j=0; j<factor; ++j )
492  newTimes[i*factor + j] = getTime( i ) + ((double) j) / ((double) factor) * getIntervalLength( i );
493  newTimes[ getNumIntervals( )*factor ] = getLastTime( );
494 
495  /* assign new time array and deallocate old one */
496  nPoints = getNumIntervals( )*factor + 1;
497 
498  double* tmp = times;
499  times = newTimes;
500  free( tmp );
501 
502  return SUCCESSFUL_RETURN;
503 }
504 
505 
507  )
508 {
509  if ( factor == 0 )
511 
512  /* nothing to do */
513  if ( ( factor == 1 ) || ( getNumIntervals( ) == 0 ) )
514  return SUCCESSFUL_RETURN;
515 
517 }
518 
519 
521  ) const
522 {
523  if ( findTime( _time ) < 0 )
524  return BT_FALSE;
525  else
526  return BT_TRUE;
527 }
528 
529 
530 
531 int Grid::findTime( double _time,
532  uint startIdx
533  ) const
534 {
535  return findFirstTime( _time,startIdx );
536 }
537 
538 
539 int Grid::findFirstTime( double _time,
540  uint startIdx
541  ) const
542 {
543  if ( times == 0 )
544  return -1;
545 
546  for( uint i=startIdx; i<getNumPoints( ); ++i )
547  {
548  if ( acadoIsEqual( times[i] ,_time ) == BT_TRUE )
549  return i;
550 
551  /* stop here as grid point times are ordered! */
552  if ( times[i] > _time )
553  return -1;
554  }
555 
556  /* no grid point with given time found */
557  return -1;
558 }
559 
560 
561 int Grid::findLastTime( double _time,
562  uint startIdx
563  ) const
564 {
565  if ( times == 0 )
566  return -1;
567 
568  for( uint i=startIdx; i<getNumPoints( ); ++i )
569  {
570  if ( acadoIsEqual( times[i] ,_time ) == BT_TRUE )
571  {
572  uint j = i;
573 
574  while( ( j<getNumPoints( ) ) && ( acadoIsEqual( times[j] , _time ) == BT_TRUE ) )
575  {
576  ++j;
577  }
578 
579  return j-1;
580  }
581 
582  /* stop here as grid point times are ordered! */
583  if ( times[i] > _time )
584  return -1;
585  }
586 
587  /* no grid point with given time found */
588  return -1;
589 }
590 
591 
592 
594  ) const
595 {
596  uint lowerIdx = 0;
597  uint upperIdx = getLastIndex( );
598  uint idx = 0;
599 
600  /* ensure that time lies within range */
601  if ( acadoIsGreater( getTime( lowerIdx ) , time_ ) == BT_TRUE )
602  return lowerIdx;
603 
604  if ( acadoIsSmaller( getTime( upperIdx ) , time_ ) == BT_TRUE )
605  return upperIdx;
606 
607  /* if so, perform binary search */
608  while ( lowerIdx < upperIdx )
609  {
610  idx = (uint)floor( 0.5*( (double)(upperIdx + lowerIdx) ) );
611 
612  if ( isInUpperHalfOpenInterval( idx,time_ ) == BT_TRUE )
613  break;
614 
615  if ( acadoIsStrictlyGreater( getTime( idx ) , time_ ) == BT_TRUE )
616  upperIdx = idx;
617  else
618  lowerIdx = idx;
619  }
620 
621  return idx;
622 }
623 
624 
625 uint Grid::getCeilIndex ( double time_ ) const
626 {
627  uint lowerIdx = 0;
628  uint upperIdx = getLastIndex( );
629  uint idx = 0;
630 
631  /* ensure that time lies within range */
632  if ( acadoIsGreater( getTime( lowerIdx ) , time_ ) == BT_TRUE )
633  return lowerIdx;
634 
635  if ( acadoIsSmaller( getTime( upperIdx ) , time_ ) == BT_TRUE )
636  return upperIdx;
637 
638  /* if so, perform binary search */
639  while ( lowerIdx < upperIdx )
640  {
641  idx = (uint)ceil(0.5*( (double)( upperIdx + lowerIdx) ) );
642 
643  if ( isInLowerHalfOpenInterval( idx,time_ ) == BT_TRUE )
644  break;
645 
646  if ( acadoIsGreater( getTime( idx ) , time_ ) == BT_TRUE )
647  upperIdx = idx;
648  else
649  lowerIdx = idx;
650  }
651  return idx;
652 }
653 
654 
656  double tEnd,
657  Grid& _subGrid
658  ) const
659 {
660  if ( ( isInInterval( tStart ) == BT_FALSE ) ||
661  ( isInInterval( tEnd ) == BT_FALSE ) )
663 
664 
665  // determine number of subpoints
666  uint nSubPoints = 0;
667 
668  if ( hasTime( tStart ) == BT_FALSE )
669  ++nSubPoints;
670 
671  for( uint i=0; i<getNumPoints( ); ++i )
672  {
673  if ( acadoIsGreater( getTime( i ) , tStart ) == BT_TRUE &&
674  acadoIsSmaller( getTime( i ) , tEnd ) == BT_TRUE ) ++nSubPoints;
675  }
676 
677  if ( hasTime( tEnd ) == BT_FALSE )
678  ++nSubPoints;
679 
680  // setup subgrid with subpoints
681  _subGrid.init( nSubPoints );
682 
683  if ( hasTime( tStart ) == BT_FALSE )
684  _subGrid.setTime( tStart );
685 
686  for( uint i=0; i<getNumPoints( ); ++i )
687  {
688  if ( acadoIsGreater( getTime( i ) , tStart ) == BT_TRUE &&
689  acadoIsSmaller( getTime( i ) , tEnd ) == BT_TRUE )
690  _subGrid.setTime( getTime( i ) );
691  }
692 
693  if ( hasTime( tEnd ) == BT_FALSE )
694  _subGrid.setTime( tEnd );
695 
696  return SUCCESSFUL_RETURN;
697 }
698 
699 
701 {
702  for (unsigned t = 0; t < getNumPoints(); t++)
703  cout << setprecision( 8 ) << getTime( t ) << " ";
704  cout << endl;
705 
706  return SUCCESSFUL_RETURN;
707 }
708 
709 
710 
711 //
712 // PROTECTED MEMBER FUNCTIONS:
713 //
714 
716  double _lastTime
717  )
718 {
719  if ( getNumPoints( ) == 0 )
720  return SUCCESSFUL_RETURN;
721 
722  if ( getNumPoints( ) == 1 )
723  {
724  times[0] = _firstTime;
725  return SUCCESSFUL_RETURN;
726  }
727 
728  if ( _firstTime > _lastTime )
730 
731 
732  double horizon = _lastTime - _firstTime;
733 
734  for( uint i=0; i<nPoints; ++i )
735  times[i] = _firstTime + ((double) i) / ((double) getNumPoints( )-1) * horizon;
736 
737  return SUCCESSFUL_RETURN;
738 }
739 
740 
742 {
743  if ( times == 0 )
744  return -1;
745 
746  for( uint i=0; i<getNumPoints( ); ++i )
747  if ( times[i] < -INFTY+1.0 )
748  return i;
749 
750  /* no uninitialised grid point found */
751  return -1;
752 }
753 
754 
755 
757 
758 
759 /*
760  * end of file
761  */
returnValue setTime(double _time)
Definition: grid.cpp:307
int findLastTime(double _time, uint startIdx=0) const
Definition: grid.cpp:561
returnValue merge(const Grid &arg, MergeMethod _mergeMethod=MM_DUPLICATE, BooleanType keepOverlap=BT_TRUE)
Definition: grid.cpp:358
double getTime(uint pointIdx) const
int findTime(double _time, uint startIdx=0) const
Definition: grid.cpp:531
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
const double INFTY
uint getCeilIndex(double time) const
Definition: grid.cpp:625
Allows to pass back messages to the calling function.
Grid()
Definition: grid.cpp:47
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
BooleanType acadoIsGreater(double x, double y, double TOL)
#define CLOSE_NAMESPACE_ACADO
returnValue equalizeGrids(Grid &arg)
Definition: grid.cpp:293
BooleanType isInInterval(double _time) const
returnValue getSubGrid(double tStart, double tEnd, Grid &_subGrid) const
Definition: grid.cpp:655
returnValue print() const
Definition: grid.cpp:700
uint getFloorIndex(double time) const
Definition: grid.cpp:593
double * times
Definition: grid.hpp:614
BooleanType acadoIsStrictlySmaller(double x, double y, double TOL)
Grid & operator&(const Grid &arg)
Definition: grid.cpp:212
uint nPoints
Definition: grid.hpp:613
returnValue coarsenGrid(uint factor)
Definition: grid.cpp:506
Grid & operator=(const Grid &rhs)
Definition: grid.cpp:185
unsigned getDim() const
Definition: vector.hpp:172
returnValue refineGrid(uint factor)
Definition: grid.cpp:477
returnValue init(uint _nPoints=0, const double *const _times=0)
Definition: grid.cpp:107
BooleanType acadoIsSmaller(double x, double y, double TOL)
void rhs(const real_t *x, real_t *f)
returnValue scaleTimes(double scaling)
Definition: grid.cpp:460
#define BT_TRUE
Definition: acado_types.hpp:47
returnValue setupEquidistant(double _firstTime, double _lastTime)
Definition: grid.cpp:715
Grid & shiftTimes(double timeShift)
Definition: grid.cpp:449
returnValue addTime(double _time)
Definition: grid.cpp:339
int findFirstTime(double _time, uint startIdx=0) const
Definition: grid.cpp:539
uint getNumPoints() const
BooleanType hasTime(double _time) const
Definition: grid.cpp:520
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
MergeMethod
BooleanType acadoIsStrictlyGreater(double x, double y, double TOL)
void init(int nV, int nC, SymmetricMatrix *H, real_t *g, Matrix *A, const real_t *const lb, const real_t *const ub, const real_t *const lbA, const real_t *const ubA, int nWSR, const real_t *const x0, Options *options, int nOutputs, mxArray *plhs[])
~Grid()
Definition: grid.cpp:100
#define ACADOERROR(retval)
int findNextIndex() const
Definition: grid.cpp:741


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