C++ VariablesGrid::addMatrix方法代码示例

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;
}

// 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;
}