returnValue Constraint::add( const int index_, const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
ASSERT_RETURN( index_ < (int) grid.getNumPoints() ).addMessage("\n >>> The constraint component can not be set as the associated discretization point is not in the time horizon. <<< \n\n");
uint run1;
if( component.hasLBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getLB()).getDim() == 1 )
tmp_lb(run1) = (component.getLB()).operator()(0);
else {
if( (component.getLB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_lb(run1) = (component.getLB()).operator()(run1);
}
}
}
else {
VariablesGrid LBgrid = component.getLBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
tmp_lb(run1) = tmp(0);
}
}
if( component.hasUBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getUB()).getDim() == 1 )
tmp_ub(run1) = (component.getUB()).operator()(0);
else {
if( (component.getUB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_ub(run1) = (component.getUB()).operator()(run1);
}
}
}
else {
VariablesGrid UBgrid = component.getUBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
tmp_ub(run1) = tmp(0);
}
}
ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );
return SUCCESSFUL_RETURN;
}
returnValue Constraint::add( const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
uint run1;
if( component.hasLBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getLB()).getDim() == 1 )
tmp_lb(run1) = (component.getLB()).operator()(0);
else {
if( (component.getLB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_lb(run1) = (component.getLB()).operator()(run1);
}
}
}
else {
VariablesGrid LBgrid = component.getLBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
tmp_lb(run1) = tmp(0);
}
}
if( component.hasUBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getUB()).getDim() == 1 )
tmp_ub(run1) = (component.getUB()).operator()(0);
else {
if( (component.getUB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_ub(run1) = (component.getUB()).operator()(run1);
}
}
}
else {
VariablesGrid UBgrid = component.getUBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
tmp_ub(run1) = tmp(0);
}
}
return add( tmp_lb, component.getExpression(), tmp_ub );
}