VariablesGrid VariablesGrid::getTimeSubGrid( double startTime,
double endTime
) const
{
uint startIdx = getCeilIndex( startTime );
uint endIdx = getFloorIndex( endTime );
VariablesGrid newVariablesGrid;
if ( ( isInInterval( startTime ) == BT_FALSE ) || ( isInInterval( endTime ) == BT_FALSE ) )
return newVariablesGrid;
if ( ( startIdx >= getNumPoints( ) ) || ( endIdx >= getNumPoints( ) ) )
return newVariablesGrid;
// if ( startIdx > endIdx )
// return newVariablesGrid;
// add all matrices in interval (constant interpolation)
if ( ( hasTime( startTime ) == BT_FALSE ) && ( startIdx > 0 ) )
newVariablesGrid.addMatrix( *(values[ startIdx-1 ]),startTime );
for( uint i=startIdx; i<=endIdx; ++i )
newVariablesGrid.addMatrix( *(values[i]),getTime( i ) );
if ( hasTime( endTime ) == BT_FALSE )
newVariablesGrid.addMatrix( *(values[ endIdx ]),endTime );
return newVariablesGrid;
}
VariablesGrid VariablesGrid::operator[]( const uint pointIdx
) const
{
ASSERT( values != 0 );
if ( pointIdx >= getNumPoints( ) )
{
ACADOERROR( RET_INVALID_ARGUMENTS );
return VariablesGrid();
}
VariablesGrid pointGrid;
pointGrid.addMatrix( *(values[pointIdx]),getTime( pointIdx ) );
return pointGrid;
}
VariablesGrid VariablesGrid::getValuesSubGrid( uint startIdx,
uint endIdx
) const
{
VariablesGrid newVariablesGrid;
if ( ( startIdx >= getNumValues( ) ) || ( endIdx >= getNumValues( ) ) )
return newVariablesGrid;
if ( startIdx > endIdx )
return newVariablesGrid;
for( uint i=0; i<getNumPoints( ); ++i )
newVariablesGrid.addMatrix( values[i]->getRows( startIdx,endIdx ),getTime( i ) );
return newVariablesGrid;
}
VariablesGrid VariablesGrid::getTimeSubGrid( uint startIdx,
uint endIdx
) const
{
VariablesGrid newVariablesGrid;
if ( ( startIdx >= getNumPoints( ) ) || ( endIdx >= getNumPoints( ) ) )
return newVariablesGrid;
if ( startIdx > endIdx )
return newVariablesGrid;
for( uint i=startIdx; i<=endIdx; ++i )
newVariablesGrid.addMatrix( *(values[i]),getTime( i ) );
return newVariablesGrid;
}
// 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;
}
