bool DenseLinAlgPack::comp(const DVectorSlice& vs1, const DVectorSlice& vs2) {
DVectorSlice::const_iterator
vs1_itr = vs1.begin(),
vs2_itr = vs2.begin();
for(; vs1_itr != vs1.end() && vs2_itr != vs2.end(); ++vs1_itr, ++vs2_itr)
if( !_comp(*vs1_itr,*vs2_itr) ) return false;
return true;
}
void MatrixSymDiagStd::Vp_StMtV(
DVectorSlice* vs_lhs, value_type alpha, BLAS_Cpp::Transp trans_rhs1
, const DVectorSlice& vs_rhs2, value_type beta) const
{
const DVectorSlice diag = this->diag();
size_type n = diag.size();
//
// y = b*y + a * op(A) * x
//
DenseLinAlgPack::Vp_MtV_assert_sizes(
vs_lhs->size(), n, n, trans_rhs1, vs_rhs2.size() );
//
// A is symmetric and diagonal A = diag(diag) so:
//
// y(j) += a * diag(j) * x(j), for j = 1...n
//
if( vs_rhs2.stride() == 1 && vs_lhs->stride() == 1 ) {
// Optimized implementation
const value_type
*d_itr = diag.raw_ptr(),
*x_itr = vs_rhs2.raw_ptr();
value_type
*y_itr = vs_lhs->raw_ptr(),
*y_end = y_itr + vs_lhs->size();
if( beta == 0.0 ) {
while( y_itr != y_end )
*y_itr++ = alpha * (*d_itr++) * (*x_itr++);
}
else if( beta == 1.0 ) {
while( y_itr != y_end )
*y_itr++ += alpha * (*d_itr++) * (*x_itr++);
}
else {
for( ; y_itr != y_end; ++y_itr )
*y_itr = beta * (*y_itr) + alpha * (*d_itr++) * (*x_itr++);
}
}
else {
// Generic implementation
DVectorSlice::const_iterator
d_itr = diag.begin(),
x_itr = vs_rhs2.begin();
DVectorSlice::iterator
y_itr = vs_lhs->begin(),
y_end = vs_lhs->end();
for( ; y_itr != y_end; ++y_itr, ++d_itr, ++x_itr ) {
#ifdef LINALGPACK_CHECK_RANGE
TEST_FOR_EXCEPT( !( d_itr < diag.end() ) );
TEST_FOR_EXCEPT( !( x_itr < vs_rhs2.end() ) );
TEST_FOR_EXCEPT( !( y_itr < vs_lhs->end() ) );
#endif
*y_itr = beta * (*y_itr) + alpha * (*d_itr) * (*x_itr);
}
}
}
bool DenseLinAlgPack::assert_print_nan_inf( const DVectorSlice& v
, const std::string & name, bool throw_excpt, std::ostream* out )
{
bool has_nan_or_inf = false;
bool printed_header = false;
for( DVectorSlice::const_iterator v_itr = v.begin(); v_itr != v.end(); ++v_itr ) {
if( RTOp_is_nan_inf(*v_itr) ) {
if(out) {
if(!printed_header) {
*out
<< "The vector \"" << name
<< "\" has the following NaN or Inf entries\n";
printed_header = true;
}
*out
<< name << "(" << v_itr - v.begin() + 1 << ") = "
<< *v_itr << std::endl;
}
has_nan_or_inf = true;
}
}
if( has_nan_or_inf && throw_excpt ) {
if(out)
out->flush();
std::ostringstream omsg;
omsg
<< "assert_print_nan_inf(...) : Error, the vector named "
<< name << " has at least one element which is NaN or Inf";
throw NaNInfException( omsg.str() );
}
return !has_nan_or_inf;
}
bool DenseLinAlgPack::comp_less(const DVectorSlice& vs, value_type alpha)
{
DVectorSlice::const_iterator vs_itr = vs.begin();
const value_type denom = my_max( ::fabs(alpha), 1.0 );
for(; vs_itr != vs.end(); ++vs_itr)
if( *vs_itr > alpha ) return false;
return true;
}
void MatrixSymDiagStd::V_InvMtV(
DVectorSlice* vs_lhs, BLAS_Cpp::Transp trans_rhs1
, const DVectorSlice& vs_rhs2) const
{
const DVectorSlice diag = this->diag();
size_type n = diag.size();
// y = inv(op(A)) * x
//
// A is symmetric and diagonal (A = diag(diag)) so:
//
// y(j) = x(j) / diag(j), for j = 1...n
DenseLinAlgPack::Vp_MtV_assert_sizes( vs_lhs->size()
, n, n, trans_rhs1, vs_rhs2.size() );
if( vs_rhs2.stride() == 1 && vs_lhs->stride() == 1 ) {
// Optimized implementation
const value_type
*d_itr = diag.raw_ptr(),
*x_itr = vs_rhs2.raw_ptr();
value_type
*y_itr = vs_lhs->raw_ptr(),
*y_end = y_itr + vs_lhs->size();
while( y_itr != y_end )
*y_itr++ = (*x_itr++) / (*d_itr++);
}
else {
// Generic implementation
DVectorSlice::const_iterator
d_itr = diag.begin(),
x_itr = vs_rhs2.begin();
DVectorSlice::iterator
y_itr = vs_lhs->begin(),
y_end = vs_lhs->end();
for( ; y_itr != y_end; ++y_itr, ++d_itr, ++x_itr ) {
TEST_FOR_EXCEPT( !( d_itr < diag.end() ) );
TEST_FOR_EXCEPT( !( x_itr < vs_rhs2.end() ) );
TEST_FOR_EXCEPT( !( y_itr < vs_lhs->end() ) );
*y_itr = (*x_itr)/(*d_itr);
}
}
}
bool DenseLinAlgPack::comp(const DVectorSlice& vs, value_type alpha) {
DVectorSlice::const_iterator vs_itr = vs.begin();
for(; vs_itr != vs.end(); ++vs_itr)
if( !_comp(*vs_itr,alpha) ) return false;
return true;
}
bool MeritFunc_PenaltyParamsUpdateWithMult_AddedStep::do_step(Algorithm& _algo
, poss_type step_poss, IterationPack::EDoStepType type
, poss_type assoc_step_poss)
{
using DenseLinAlgPack::norm_inf;
NLPAlgo &algo = rsqp_algo(_algo);
NLPAlgoState &s = algo.rsqp_state();
EJournalOutputLevel olevel = algo.algo_cntr().journal_output_level();
std::ostream& out = algo.track().journal_out();
// print step header.
if( static_cast<int>(olevel) >= static_cast<int>(PRINT_ALGORITHM_STEPS) ) {
using IterationPack::print_algorithm_step;
print_algorithm_step( algo, step_poss, type, assoc_step_poss, out );
}
MeritFuncPenaltyParams
*params = dynamic_cast<MeritFuncPenaltyParams*>(&merit_func());
if( !params ) {
std::ostringstream omsg;
omsg
<< "MeritFunc_PenaltyParamsUpdateWithMult_AddedStep::do_step(...), Error "
<< "The class " << typeName(&merit_func()) << " does not support the "
<< "MeritFuncPenaltyParams iterface\n";
out << omsg.str();
throw std::logic_error( omsg.str() );
}
MeritFuncNLPDirecDeriv
*direc_deriv = dynamic_cast<MeritFuncNLPDirecDeriv*>(&merit_func());
if( !direc_deriv ) {
std::ostringstream omsg;
omsg
<< "MeritFunc_PenaltyParamsUpdateWithMult_AddedStep::do_step(...), Error "
<< "The class " << typeName(&merit_func()) << " does not support the "
<< "MeritFuncNLPDirecDeriv iterface\n";
out << omsg.str();
throw std::logic_error( omsg.str() );
}
bool perform_update = true;
if( s.mu().updated_k(0) ) {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nmu_k is already updated by someone else?\n";
}
const value_type mu_k = s.mu().get_k(0);
if( mu_k == norm_inf_mu_last_ ) {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nmu_k " << mu_k << " == norm_inf_mu_last = " << norm_inf_mu_last_
<< "\nso we will take this as a signal to skip the update.\n";
}
perform_update = false;
}
else {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nmu_k " << mu_k << " != norm_inf_mu_last = " << norm_inf_mu_last_
<< "\nso we will ignore this and perform the update anyway.\n";
}
}
}
if(perform_update) {
if ( s.lambda().updated_k(0) ) {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nUpdate the penalty parameter...\n";
}
const DVector
&lambda_k = s.lambda().get_k(0).cv();
if( params->mu().size() != lambda_k.size() )
params->resize( lambda_k.size() );
DVectorSlice
mu = params->mu();
const value_type
max_lambda = norm_inf( lambda_k() ),
mult_fact = (1.0 + mult_factor_);
if(near_solution_) {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nNear solution, forcing mu(j) >= mu_old(j)...\n";
}
DVector::const_iterator lb_itr = lambda_k.begin();
DVectorSlice::iterator mu_itr = mu.begin();
for( ; lb_itr != lambda_k.end(); ++mu_itr, ++ lb_itr )
*mu_itr = max( max( *mu_itr, mult_fact * ::fabs(*lb_itr) ), small_mu_ );
}
else {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nNot near solution, allowing reduction in mu(j) ...\n";
}
DVector::const_iterator lb_itr = lambda_k.begin();
DVectorSlice::iterator mu_itr = mu.begin();
for( ; lb_itr != lambda_k.end(); ++mu_itr, ++ lb_itr ) {
const value_type lb_j = ::fabs(*lb_itr);
*mu_itr = max(
(3.0 * (*mu_itr) + lb_j) / 4.0
, max( mult_fact * lb_j, small_mu_ )
);
}
value_type kkt_error = s.opt_kkt_err().get_k(0) + s.feas_kkt_err().get_k(0);
if(kkt_error <= kkt_near_sol_) {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nkkt_error = " << kkt_error << " <= kkt_near_sol = "
<< kkt_near_sol_ << std::endl
<< "Switching to forcing mu_k >= mu_km1 in the future\n";
}
near_solution_ = true;
}
}
// Force the ratio
const value_type
max_mu = norm_inf( mu() ),
min_mu = min_mu_ratio_ * max_mu;
for(DVectorSlice::iterator mu_itr = mu.begin(); mu_itr != mu.end(); ++mu_itr)
*mu_itr = max( (*mu_itr), min_mu );
s.mu().set_k(0) = norm_inf_mu_last_ = max_mu;
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nmax(|mu(j)|) = " << (*std::max_element( mu.begin(), mu.end() ))
<< "\nmin(|mu(j)|) = " << (*std::min_element( mu.begin(), mu.end() ))
<< std::endl;
}
if( (int)olevel >= (int)PRINT_VECTORS ) {
out << "\nmu = \n" << mu;
}
}
else {
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nDon't have the info to update penalty parameter so just use the last updated...\n";
}
}
}
// In addition also compute the directional derivative
direc_deriv->calc_deriv( s.Gf().get_k(0)(), s.c().get_k(0)(), s.d().get_k(0)() );
if( (int)olevel >= (int)PRINT_ALGORITHM_STEPS ) {
out << "\nmu_k = " << s.mu().get_k(0) << "\n";
}
return true;
}
