bool StdStepCalculator::Step(DenseVector& delta_u,
IteratesVector& sol)
{
DBG_START_METH("StdStepCalculator::Step", dbg_verbosity);
bool retval = false;
SmartPtr<IteratesVector> delta_u_long = IpData().trial()->MakeNewIteratesVector();
ift_data_->TransMultiply(delta_u, *delta_u_long);
SmartPtr<IteratesVector> r_s = IpData().trial()->MakeNewIteratesVector();
if (kkt_residuals_) {
/* This should be almost zero... */
r_s->Set_x_NonConst(*IpCq().curr_grad_lag_x()->MakeNewCopy());
r_s->Set_s_NonConst(*IpCq().curr_grad_lag_s()->MakeNewCopy());
r_s->Set_y_c_NonConst(*IpCq().curr_c()->MakeNewCopy());
r_s->Set_y_d_NonConst(*IpCq().curr_d_minus_s()->MakeNewCopy());
r_s->Set_z_L_NonConst(*IpCq().curr_compl_x_L()->MakeNewCopy());
r_s->Set_z_U_NonConst(*IpCq().curr_compl_x_U()->MakeNewCopy());
r_s->Set_v_L_NonConst(*IpCq().curr_compl_s_L()->MakeNewCopy());
r_s->Set_v_U_NonConst(*IpCq().curr_compl_s_U()->MakeNewCopy());
r_s->Print(Jnlst(),J_VECTOR,J_USER1,"r_s init");
delta_u.Print(Jnlst(),J_VECTOR,J_USER1,"delta_u init");
DBG_PRINT((dbg_verbosity,"r_s init Nrm2=%23.16e\n", r_s->Asum()));
delta_u_long->Axpy(-1.0, *r_s);
}
backsolver_->Solve(&sol, ConstPtr(delta_u_long));
SmartPtr<IteratesVector> Kr_s;
if (Do_Boundcheck()) {
Kr_s = sol.MakeNewIteratesVectorCopy();
}
sol.Axpy(1.0, *IpData().trial());
if (Do_Boundcheck()) {
DBG_PRINT((dbg_verbosity, "Entering boundcheck"));
// initialize
Index new_du_size =0;
Number* new_du_values;
std::vector<Index> x_bound_violations_idx;
std::vector<Number> x_bound_violations_du;
std::vector<Index> delta_u_sort;
bool bounds_violated;
SmartPtr<DenseVectorSpace> delta_u_space = new DenseVectorSpace(0);
SmartPtr<DenseVector> old_delta_u = new DenseVector(GetRawPtr(delta_u_space));
SmartPtr<DenseVector> new_delta_u;
bounds_violated = BoundCheck(sol, x_bound_violations_idx, x_bound_violations_du);
while (bounds_violated) {
Driver()->data_A()->Print(Jnlst(),J_VECTOR,J_USER1,"data_A_init");
Driver()->data_B()->Print(Jnlst(),J_VECTOR,J_USER1,"data_B_init");
// write new schurdata A
dynamic_cast<IndexSchurData*>(GetRawPtr(Driver()->data_A_nonconst()))->AddData_List(x_bound_violations_idx, delta_u_sort, new_du_size, 1);
// write new schurdata B
dynamic_cast<IndexSchurData*>(GetRawPtr(Driver()->data_B_nonconst()))->AddData_List(x_bound_violations_idx, delta_u_sort, new_du_size, 1);
Driver()->data_A()->Print(Jnlst(),J_VECTOR,J_USER1,"data_A");
Driver()->data_B()->Print(Jnlst(),J_VECTOR,J_USER1,"data_B");
Driver()->SchurBuild();
Driver()->SchurFactorize();
old_delta_u->Print(Jnlst(),J_VECTOR,J_USER1,"old_delta_u");
delta_u_space = NULL; // delete old delta_u space
delta_u_space = new DenseVectorSpace(new_du_size); // create new delta_u space
new_delta_u = new DenseVector(GetRawPtr(ConstPtr(delta_u_space)));
new_du_values = new_delta_u->Values();
IpBlasDcopy(old_delta_u->Dim(), old_delta_u->Values(), 1, new_du_values, 1);
for (UINT i=0; i<x_bound_violations_idx.size(); ++i) {
// printf("i=%d, delta_u_sort[i]=%d, x_bound_viol_du[i]=%f\n", i, delta_u_sort[i], x_bound_violations_du[i]);
new_du_values[delta_u_sort[i]] = x_bound_violations_du[i];
}
SmartPtr<IteratesVector> new_sol = sol.MakeNewIteratesVector();
new_delta_u->Print(Jnlst(),J_VECTOR,J_USER1,"new_delta_u");
// solve with new data_B and delta_u
retval = Driver()->SchurSolve(&sol, ConstPtr(delta_u_long), dynamic_cast<Vector*>(GetRawPtr(new_delta_u)), Kr_s);
sol.Axpy(1.0, *IpData().trial());
x_bound_violations_idx.clear();
x_bound_violations_du.clear();
delta_u_sort.clear();
bounds_violated = BoundCheck(sol, x_bound_violations_idx, x_bound_violations_du);
// copy new vector in old vector ->has to be done becpause otherwise only pointers will be copied and then it makes no sense
old_delta_u = new_delta_u->MakeNewDenseVector();
old_delta_u->Copy(*new_delta_u);
}
}
return retval;
}
