C++ (Cpp) VariablesGrid::appendTimes Examples

Example #1

File: periodic_reference_trajectory.cpp Project: OspreyX/acado

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