returnValue PeriodicReferenceTrajectory::getReference( double tStart,
double tEnd,
VariablesGrid& _yRef
) const
{
if ( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE )
return ACADOERROR( RET_INVALID_ARGUMENTS );
double T = yRef.getLastTime() - yRef.getFirstTime(); // cycle duration
int nStart = (int)floor( (double) (tStart/T+100.0*EPS) ); // cycle number at start
int nEnd = (int)floor( (double) (tEnd /T-100.0*EPS) ); // cycle number at end
if ( nStart == nEnd )
{
_yRef = (yRef.getTimeSubGrid( tStart-T*(double)nStart,tEnd-T*(double)nStart )).shiftTimes( T*(double)nStart );
}
else
{
_yRef = (yRef.getTimeSubGrid( tStart-T*(double)nStart,yRef.getLastTime() )).shiftTimes( T*(double)nStart );
for( int i=nStart+1; i<nEnd; ++i )
_yRef.appendTimes( VariablesGrid(yRef).shiftTimes( T*(double)i ),MM_KEEP );
_yRef.appendTimes( (yRef.getTimeSubGrid( yRef.getFirstTime(),tEnd-T*(double)nEnd )).shiftTimes( T*(double)nEnd ) );
}
return SUCCESSFUL_RETURN;
}
returnValue PlotWindow::getAutoScaleYLimits( const VariablesGrid& dataGridY,
PlotFormat plotFormat,
double& lowerLimit,
double& upperLimit
) const
{
double maxValue = dataGridY.getMax( );
double minValue = dataGridY.getMin( );
double meanValue = dataGridY.getMean( );
if ( ( acadoIsZero( maxValue ) == BT_TRUE ) && ( acadoIsZero( minValue ) == BT_TRUE ) )
meanValue = 0.1 / 0.025;
lowerLimit = minValue - 0.1*( maxValue-minValue ) - 0.025*meanValue - 1.0e-8;
upperLimit = maxValue + 0.1*( maxValue-minValue ) + 0.025*meanValue + 1.0e-8;
if ( ( plotFormat == PF_LOG ) || ( plotFormat == PF_LOG_LOG ) )
{
if ( ( acadoIsStrictlyGreater( minValue,0.0, ZERO ) == BT_TRUE ) && ( acadoIsGreater( lowerLimit,0.0, ZERO ) == BT_FALSE ) )
{
lowerLimit = ( minValue + acadoMin( minValue,EPS ) ) / sqrt(10.0);
upperLimit = upperLimit * sqrt(10.0);
}
}
else
{
if ( ( acadoIsStrictlyGreater( minValue,0.0, ZERO ) == BT_TRUE ) && ( acadoIsGreater( lowerLimit,0.0, ZERO ) == BT_FALSE ) )
lowerLimit = 0.0;
if ( ( acadoIsStrictlySmaller( maxValue,0.0 ) == BT_TRUE ) && ( acadoIsSmaller( upperLimit,0.0 ) == BT_FALSE ) )
upperLimit = 0.0;
}
return SUCCESSFUL_RETURN;
}
// uses a simple O(n^2) algorithm for sorting
returnValue VariablesGrid::merge( const VariablesGrid& arg,
MergeMethod _mergeMethod,
BooleanType keepOverlap
)
{
if ( ( keepOverlap == BT_FALSE ) && ( _mergeMethod == MM_DUPLICATE ) )
return ACADOERROR( RET_INVALID_ARGUMENTS );
// nothing to do if arg or object itself is empty
if ( arg.getNumPoints( ) == 0 )
return SUCCESSFUL_RETURN;
if ( getNumPoints( ) == 0 )
{
*this = arg;
return SUCCESSFUL_RETURN;
}
// use append if grids do not overlap
if ( acadoIsSmaller( getLastTime( ),arg.getFirstTime( ) ) == BT_TRUE )
return appendTimes( arg,_mergeMethod );
// construct merged grid
VariablesGrid mergedGrid;
uint j = 0;
BooleanType overlapping = BT_FALSE;
for( uint i=0; i<getNumPoints( ); ++i )
{
if ( keepOverlap == BT_FALSE )
overlapping = arg.isInInterval( getTime(i) );
// add all grid points of argument grid that are smaller
// then current one of original grid
while ( ( j < arg.getNumPoints( ) ) &&
( acadoIsStrictlySmaller( arg.getTime( j ),getTime( i ) ) == BT_TRUE ) )
{
if ( ( overlapping == BT_FALSE ) ||
( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_REPLACE ) ) )
{
mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) );
}
++j;
}
// merge current grid points if they are at equal times
if ( acadoIsEqual( arg.getTime( j ),getTime( i ) ) == BT_TRUE )
{
switch ( _mergeMethod )
{
case MM_KEEP:
mergedGrid.addMatrix( *(values[i]),getTime( i ) );
break;
case MM_REPLACE:
mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) );
break;
case MM_DUPLICATE:
mergedGrid.addMatrix( *(values[i]),getTime( i ) );
mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) );
break;
}
++j;
}
else
{
// add current grid point of original grid
if ( ( overlapping == BT_FALSE ) ||
( ( overlapping == BT_TRUE ) && ( _mergeMethod == MM_KEEP ) ) )
{
mergedGrid.addMatrix( *(values[i]),getTime( i ) );//arg.
}
}
}
// add all remaining grid points of argument grid
while ( j < arg.getNumPoints( ) )
{
if ( acadoIsStrictlyGreater( arg.getTime(j),getLastTime() ) == BT_TRUE )
mergedGrid.addMatrix( *(arg.values[j]),arg.getTime( j ) );
++j;
}
// merged grid becomes current grid
*this = mergedGrid;
return SUCCESSFUL_RETURN;
}
returnValue Curve::add( double tStart, double tEnd, const Function ¶meterization_ ){
uint run1;
// CHECK WHETHER "tStart < tEnd":
// ------------------------------------------------
if( acadoIsStrictlyGreater( tStart,tEnd ) == BT_TRUE )
return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID);
// CHECK WHETHER THE INPUT VECTOR IS EMPTY:
// ------------------------------------------------
if( parameterization_.getDim() == 0 )
return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
if( isEmpty() == BT_FALSE ){
// IF THE CURVE IS NOT EMPTY THE DIMENSIONS MUST BE CHECKED:
// ---------------------------------------------------------
if( getDim() != (int) parameterization_.getDim() )
return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
// CHECK WHETHER THE CURVE HAS NO GAP's:
// ---------------------------------------------------------
if( acadoIsEqual( tStart,grid->getLastTime() ) == BT_FALSE )
{
ASSERT(1==0);
return ACADOERROR(RET_TIME_INTERVAL_NOT_VALID);
}
// APPEND THE NEW TIME INTERVAL TO THE GRID:
// ---------------------------------------------------------
double *times = new double[nIntervals+2];
for( run1 = 0; run1 < nIntervals+1; run1++ )
times[run1] = grid->getTime(run1);
times[nIntervals+1] = tEnd;
delete grid;
grid = new Grid( nIntervals+2, times );
nIntervals++;
delete[] times;
// ---------------------------------------------------------
}
else{
// SETUP A NEW GRID:
// ----------------------------------------------------
grid = new Grid( tStart, tEnd, 2 );
nIntervals++;
// ----------------------------------------------------
}
// CHECK WHETHER THE FUNCTION ITSELF IS VALID:
// -------------------------------------------
if( parameterization_.getNX () != 0 ||
parameterization_.getNXA() != 0 ||
parameterization_.getNP () != 0 ||
parameterization_.getNPI() != 0 ||
parameterization_.getNU () != 0 ||
parameterization_.getNUI() != 0 ||
parameterization_.getNW() != 0 ){
return ACADOERROR(RET_INPUT_DIMENSION_MISMATCH);
}
// SET THE DIMENSION OF THE CURVE:
// (the correctness of the dimension has already been cecked)
// ----------------------------------------------------------
dim = parameterization_.getDim();
// ALLOCATE MEMORY FOR THE NEW PIECE OF CURVE:
// -------------------------------------------
parameterization = (Function**)realloc(parameterization,nIntervals*sizeof(Function*));
// MAKE A DEEP COPY OF THE PARAMETERIZATION:
// -----------------------------------------
parameterization[nIntervals-1] = new Function(parameterization_);
// RETURN:
// -----------------------------------------
return SUCCESSFUL_RETURN;
}
