ipopt-src 0.2.3+3.14.16

// Copyright (C) 2004, 2007 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-08-13

#include "IpPDFullSpaceSolver.hpp"
#include "IpDebug.hpp"

#include <cmath>

namespace Ipopt
{

#if IPOPT_VERBOSITY > 0
static const Index dbg_verbosity = 0;
#endif

PDFullSpaceSolver::PDFullSpaceSolver(
   AugSystemSolver&       augSysSolver,
   PDPerturbationHandler& perturbHandler
)
   : PDSystemSolver(),
     augSysSolver_(&augSysSolver),
     perturbHandler_(&perturbHandler),
     dummy_cache_(1)
{
   DBG_START_METH("PDFullSpaceSolver::PDFullSpaceSolver", dbg_verbosity);
}

PDFullSpaceSolver::~PDFullSpaceSolver()
{
   DBG_START_METH("PDFullSpaceSolver::~PDFullSpaceSolver()", dbg_verbosity);
}

void PDFullSpaceSolver::RegisterOptions(
   SmartPtr<RegisteredOptions> roptions
)
{
   roptions->AddLowerBoundedIntegerOption(
      "min_refinement_steps",
      "Minimum number of iterative refinement steps per linear system solve.",
      0,
      1,
      "Iterative refinement (on the full unsymmetric system) is performed for each right hand side. "
      "This option determines the minimum number of iterative refinements "
      "(i.e. at least \"min_refinement_steps\" iterative refinement steps are enforced per right hand side.)");
   roptions->AddLowerBoundedIntegerOption(
      "max_refinement_steps",
      "Maximum number of iterative refinement steps per linear system solve.",
      0,
      10,
      "Iterative refinement (on the full unsymmetric system) is performed for each right hand side. "
      "This option determines the maximum number of iterative refinement steps.");
   roptions->AddLowerBoundedNumberOption(
      "residual_ratio_max",
      "Iterative refinement tolerance",
      0., true,
      1e-10,
      "Iterative refinement is performed until the residual test ratio is less than this tolerance "
      "(or until \"max_refinement_steps\" refinement steps are performed).",
      true);
   roptions->AddLowerBoundedNumberOption(
      "residual_ratio_singular",
      "Threshold for declaring linear system singular after failed iterative refinement.",
      0., true,
      1e-5,
      "If the residual test ratio is larger than this value after failed iterative refinement, "
      "the algorithm pretends that the linear system is singular.",
      true);
   // ToDo Think about following option - are the correct norms used?
   roptions->AddLowerBoundedNumberOption(
      "residual_improvement_factor",
      "Minimal required reduction of residual test ratio in iterative refinement.",
      0., true,
      0.999999999,
      "If the improvement of the residual test ratio made by one iterative refinement step is not better than this factor, "
      "iterative refinement is aborted.",
      true);
   roptions->AddLowerBoundedNumberOption(
      "neg_curv_test_tol",
      "Tolerance for heuristic to ignore wrong inertia.",
      0.0, false,
      0.0,
      "If nonzero, incorrect inertia in the augmented system is ignored, and "
      "Ipopt tests if the direction is a direction of positive curvature. "
      "This tolerance is alpha_n in the paper by Zavala and Chiang (2014) and "
      "it determines when the direction is considered to be sufficiently positive. "
      "A value in the range of [1e-12, 1e-11] is recommended.");
   roptions->AddStringOption2(
      "neg_curv_test_reg",
      "Whether to do the curvature test with the primal regularization (see Zavala and Chiang, 2014).",
      "yes",
      "yes", "use primal regularization with the inertia-free curvature test",
      "no",  "use original IPOPT approach, in which the primal regularization is ignored");
}

bool PDFullSpaceSolver::InitializeImpl(
   const OptionsList& options,
   const std::string& prefix
)
{
   // Check for the algorithm options
   options.GetIntegerValue("min_refinement_steps", min_refinement_steps_, prefix);
   options.GetIntegerValue("max_refinement_steps", max_refinement_steps_, prefix);
   ASSERT_EXCEPTION(max_refinement_steps_ >= min_refinement_steps_, OPTION_INVALID,
                    "Option \"max_refinement_steps\": This value must be larger than or equal to min_refinement_steps (default 1)");

   options.GetNumericValue("residual_ratio_max", residual_ratio_max_, prefix);
   options.GetNumericValue("residual_ratio_singular", residual_ratio_singular_, prefix);
   ASSERT_EXCEPTION(residual_ratio_singular_ >= residual_ratio_max_, OPTION_INVALID,
                    "Option \"residual_ratio_singular\": This value must be not smaller than residual_ratio_max.");
   options.GetNumericValue("residual_improvement_factor", residual_improvement_factor_, prefix);
   options.GetNumericValue("neg_curv_test_tol", neg_curv_test_tol_, prefix);
   options.GetBoolValue("neg_curv_test_reg", neg_curv_test_reg_, prefix);

   // Reset internal flags and data
   augsys_improved_ = false;

   if( !augSysSolver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(), options, prefix) )
   {
      return false;
   }

   return perturbHandler_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(), options, prefix);
}

bool PDFullSpaceSolver::Solve(
   Number                alpha,
   Number                beta,
   const IteratesVector& rhs,
   IteratesVector&       res,
   bool                  allow_inexact,
   bool                  improve_solution /* = false */
)
{
   DBG_START_METH("PDFullSpaceSolver::Solve", dbg_verbosity);
   DBG_ASSERT(!allow_inexact || !improve_solution);
   DBG_ASSERT(!improve_solution || beta == 0.);

   // Timing of PDSystem solver starts here
   IpData().TimingStats().PDSystemSolverTotal().Start();

   DBG_PRINT_VECTOR(2, "rhs_x", *rhs.x());
   DBG_PRINT_VECTOR(2, "rhs_s", *rhs.s());
   DBG_PRINT_VECTOR(2, "rhs_c", *rhs.y_c());
   DBG_PRINT_VECTOR(2, "rhs_d", *rhs.y_d());
   DBG_PRINT_VECTOR(2, "rhs_zL", *rhs.z_L());
   DBG_PRINT_VECTOR(2, "rhs_zU", *rhs.z_U());
   DBG_PRINT_VECTOR(2, "rhs_vL", *rhs.v_L());
   DBG_PRINT_VECTOR(2, "rhs_vU", *rhs.v_U());
   DBG_PRINT_VECTOR(2, "res_x in", *res.x());
   DBG_PRINT_VECTOR(2, "res_s in", *res.s());
   DBG_PRINT_VECTOR(2, "res_c in", *res.y_c());
   DBG_PRINT_VECTOR(2, "res_d in", *res.y_d());
   DBG_PRINT_VECTOR(2, "res_zL in", *res.z_L());
   DBG_PRINT_VECTOR(2, "res_zU in", *res.z_U());
   DBG_PRINT_VECTOR(2, "res_vL in", *res.v_L());
   DBG_PRINT_VECTOR(2, "res_vU in", *res.v_U());

   // if beta is nonzero, keep a copy of the incoming values in res_ */
   SmartPtr<IteratesVector> copy_res;
   if( beta != 0. )
   {
      copy_res = res.MakeNewIteratesVectorCopy();
   }

   // Receive data about matrix
//   SmartPtr<const Vector> x = IpData().curr()->x();
//   SmartPtr<const Vector> s = IpData().curr()->s();
   SmartPtr<const SymMatrix> W = IpData().W();
   SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
   SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();
   SmartPtr<const Matrix> Px_L = IpNLP().Px_L();
   SmartPtr<const Matrix> Px_U = IpNLP().Px_U();
   SmartPtr<const Matrix> Pd_L = IpNLP().Pd_L();
   SmartPtr<const Matrix> Pd_U = IpNLP().Pd_U();
   SmartPtr<const Vector> z_L = IpData().curr()->z_L();
   SmartPtr<const Vector> z_U = IpData().curr()->z_U();
   SmartPtr<const Vector> v_L = IpData().curr()->v_L();
   SmartPtr<const Vector> v_U = IpData().curr()->v_U();
   SmartPtr<const Vector> slack_x_L = IpCq().curr_slack_x_L();
   SmartPtr<const Vector> slack_x_U = IpCq().curr_slack_x_U();
   SmartPtr<const Vector> slack_s_L = IpCq().curr_slack_s_L();
   SmartPtr<const Vector> slack_s_U = IpCq().curr_slack_s_U();
   SmartPtr<const Vector> sigma_x = IpCq().curr_sigma_x();
   SmartPtr<const Vector> sigma_s = IpCq().curr_sigma_s();
   DBG_PRINT_VECTOR(2, "Sigma_x", *sigma_x);
   DBG_PRINT_VECTOR(2, "Sigma_s", *sigma_s);

   bool done = false;
   // The following flag is set to true, if we asked the linear
   // solver to improve the quality of the solution in
   // the next solve
   bool resolve_with_better_quality = false;
   // the following flag is set to true, if iterative refinement
   // failed and we want to try if a modified system is able to
   // remedy that problem by pretending the matrix is singular
   bool pretend_singular = false;
   bool pretend_singular_last_time = false;

   // Beginning of loop for solving the system (including all
   // modifications for the linear system to ensure good solution
   // quality)
   while( !done )
   {

      // if improve_solution is true, we are given already a solution
      // from the calling function, so we can skip the first solve
      bool solve_retval = true;
      if( !improve_solution )
      {
         solve_retval = SolveOnce(resolve_with_better_quality, pretend_singular, *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L,
                                  *Pd_U, *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U, *sigma_x, *sigma_s, 1., 0.,
                                  rhs, res);
         resolve_with_better_quality = false;
         pretend_singular = false;
      }
      improve_solution = false;

      if( !solve_retval )
      {
         // If system seems not to be solvable, we return with false
         // and let the calling routine deal with it.
         IpData().TimingStats().PDSystemSolverTotal().End();
         return false;
      }

      if( allow_inexact )
      {
         // no safety checks required
         if( Jnlst().ProduceOutput(J_MOREDETAILED, J_LINEAR_ALGEBRA) )
         {
            SmartPtr<IteratesVector> resid = res.MakeNewIteratesVector(true);
            ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                             *slack_s_L, *slack_s_U, *sigma_x, *sigma_s, alpha, beta, rhs, res, *resid);
         }
         break;
      }

      // Get space for the residual
      SmartPtr<IteratesVector> resid = res.MakeNewIteratesVector(true);

      // ToDo don't to that after max refinement?
      ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                       *slack_s_L, *slack_s_U, *sigma_x, *sigma_s, alpha, beta, rhs, res, *resid);

      Number residual_ratio = ComputeResidualRatio(rhs, res, *resid);
      Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                     "residual_ratio = %e\n", residual_ratio);
      Number residual_ratio_old = residual_ratio;

      // Beginning of loop for iterative refinement
      Index num_iter_ref = 0;
      bool quit_refinement = false;
      while( /* !allow_inexact &&*/ !quit_refinement   // allow_inexact is always false here
                                    && (num_iter_ref < min_refinement_steps_ || residual_ratio > residual_ratio_max_) )
      {

         // To the next back solve
         solve_retval = SolveOnce(resolve_with_better_quality, false, *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L,
                                  *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U, *sigma_x, *sigma_s, -1., 1., *resid, res);
         ASSERT_EXCEPTION(solve_retval, INTERNAL_ABORT, "SolveOnce returns false during iterative refinement.");

         ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
                          *slack_s_L, *slack_s_U, *sigma_x, *sigma_s, alpha, beta, rhs, res, *resid);

         residual_ratio = ComputeResidualRatio(rhs, res, *resid);
         Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                        "residual_ratio = %e\n", residual_ratio);

         num_iter_ref++;
         // Check if we have to give up on iterative refinement
         if( residual_ratio > residual_ratio_max_ && num_iter_ref > min_refinement_steps_
             && (num_iter_ref > max_refinement_steps_
                 || residual_ratio > residual_improvement_factor_ * residual_ratio_old) )
         {

            Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                           "Iterative refinement failed with residual_ratio = %e\n", residual_ratio);
            quit_refinement = true;

            // Pretend singularity only once - if it didn't help, we
            // have to live with what we got so far
            resolve_with_better_quality = false;
            DBG_PRINT((1, "pretend_singular = %d\n", pretend_singular));
            if( !pretend_singular_last_time )
            {
               // First try if we can ask the augmented system solver to
               // improve the quality of the solution (only if that hasn't
               // been done before for this linear system)
               if( !augsys_improved_ )
               {
                  Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                 "Asking augmented system solver to improve quality of its solutions.\n");
                  augsys_improved_ = augSysSolver_->IncreaseQuality();
                  if( augsys_improved_ )
                  {
                     IpData().Append_info_string("q");
                     resolve_with_better_quality = true;
                  }
                  else
                  {
                     // solver said it cannot improve quality, so let
                     // possibly conclude that the current modification is
                     // singular
                     pretend_singular = true;
                  }
               }
               else
               {
                  // we had already asked the solver before to improve the
                  // quality of the solution, so let's now pretend that the
                  // modification is possibly singular
                  pretend_singular = true;
               }
               pretend_singular_last_time = pretend_singular;
               if( pretend_singular )
               {
                  // let's only conclude that the current linear system
                  // including modifications is singular, if the residual is
                  // quite bad
                  if( residual_ratio < residual_ratio_singular_ )
                  {
                     pretend_singular = false;
                     IpData().Append_info_string("S");
                     Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                    "Just accept current solution.\n");
                  }
                  else
                  {
                     IpData().Append_info_string("s");
                     Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                    "Pretend that the current system (including modifications) is singular.\n");
                  }
               }
            }
            else
            {
               pretend_singular = false;
               DBG_PRINT((1, "Resetting pretend_singular to false.\n"));
            }
         }

         residual_ratio_old = residual_ratio;
      } // End of loop for iterative refinement

      done = !(resolve_with_better_quality) && !(pretend_singular);

   } // End of loop for solving the linear system (incl. modifications)

   // Finally let's assemble the res result vectors
   if( alpha != 0. )
   {
      res.Scal(alpha);
   }

   if( beta != 0. )
   {
      res.Axpy(beta, *copy_res);
   }

   DBG_PRINT_VECTOR(2, "res_x", *res.x());
   DBG_PRINT_VECTOR(2, "res_s", *res.s());
   DBG_PRINT_VECTOR(2, "res_c", *res.y_c());
   DBG_PRINT_VECTOR(2, "res_d", *res.y_d());
   DBG_PRINT_VECTOR(2, "res_zL", *res.z_L());
   DBG_PRINT_VECTOR(2, "res_zU", *res.z_U());
   DBG_PRINT_VECTOR(2, "res_vL", *res.v_L());
   DBG_PRINT_VECTOR(2, "res_vU", *res.v_U());

   IpData().TimingStats().PDSystemSolverTotal().End();

   return true;
}

bool PDFullSpaceSolver::SolveOnce(
   bool                  resolve_with_better_quality,
   bool                  pretend_singular,
   const SymMatrix&      W,
   const Matrix&         J_c,
   const Matrix&         J_d,
   const Matrix&         Px_L,
   const Matrix&         Px_U,
   const Matrix&         Pd_L,
   const Matrix&         Pd_U,
   const Vector&         z_L,
   const Vector&         z_U,
   const Vector&         v_L,
   const Vector&         v_U,
   const Vector&         slack_x_L,
   const Vector&         slack_x_U,
   const Vector&         slack_s_L,
   const Vector&         slack_s_U,
   const Vector&         sigma_x,
   const Vector&         sigma_s,
   Number                alpha,
   Number                beta,
   const IteratesVector& rhs,
   IteratesVector&       res
)
{
   // TO DO LIST:
   //
   // 1. decide for reasonable return codes (e.g. fatal error, too
   //    ill-conditioned...)
   // 2. Make constants parameters that can be set from the outside
   // 3. Get Information out of Ipopt structures
   // 4. add heuristic for structurally singular problems
   // 5. see if it makes sense to distinguish delta_x and delta_s,
   //    or delta_c and delta_d
   // 6. increase pivot tolerance if number of get evals so too small
   DBG_START_METH("PDFullSpaceSolver::SolveOnce", dbg_verbosity);

   IpData().TimingStats().PDSystemSolverSolveOnce().Start();

   // Compute the right hand side for the augmented system formulation
   SmartPtr<Vector> augRhs_x = rhs.x()->MakeNewCopy();
   Px_L.AddMSinvZ(1.0, slack_x_L, *rhs.z_L(), *augRhs_x);
   Px_U.AddMSinvZ(-1.0, slack_x_U, *rhs.z_U(), *augRhs_x);

   SmartPtr<Vector> augRhs_s = rhs.s()->MakeNewCopy();
   Pd_L.AddMSinvZ(1.0, slack_s_L, *rhs.v_L(), *augRhs_s);
   Pd_U.AddMSinvZ(-1.0, slack_s_U, *rhs.v_U(), *augRhs_s);

   // Get space into which we can put the solution of the augmented system
   SmartPtr<IteratesVector> sol = res.MakeNewIteratesVector(true);

   // Now check whether any data has changed
   std::vector<const TaggedObject*> deps(13);
   deps[0] = &W;
   deps[1] = &J_c;
   deps[2] = &J_d;
   deps[3] = &z_L;
   deps[4] = &z_U;
   deps[5] = &v_L;
   deps[6] = &v_U;
   deps[7] = &slack_x_L;
   deps[8] = &slack_x_U;
   deps[9] = &slack_s_L;
   deps[10] = &slack_s_U;
   deps[11] = &sigma_x;
   deps[12] = &sigma_s;
   void* dummy = NULL;
   bool uptodate = dummy_cache_.GetCachedResult(dummy, deps);
   if( !uptodate )
   {
      dummy_cache_.AddCachedResult(dummy, deps);
      augsys_improved_ = false;
   }
   // improve_current_solution can only be true, if that system has
   // been solved before
   DBG_ASSERT((!resolve_with_better_quality && !pretend_singular) || uptodate);
   (void) resolve_with_better_quality;

   ESymSolverStatus retval;
   if( uptodate && !pretend_singular )
   {

      // Get the perturbation values
      Number delta_x;
      Number delta_s;
      Number delta_c;
      Number delta_d;
      perturbHandler_->CurrentPerturbation(delta_x, delta_s, delta_c, delta_d);

      // No need to go through the pain of finding the appropriate
      // values for the deltas, because the matrix hasn't changed since
      // the last call.  So, just call the Solve Method
      //
      // Note: resolve_with_better_quality is true, then the Solve
      // method has already asked the augSysSolver to increase the
      // quality at the end solve, and we are now getting the solution
      // with that better quality
      retval = augSysSolver_->Solve(&W, 1.0, &sigma_x, delta_x, &sigma_s, delta_s, &J_c, NULL, delta_c, &J_d, NULL,
                                    delta_d, *augRhs_x, *augRhs_s, *rhs.y_c(), *rhs.y_d(), *sol->x_NonConst(), *sol->s_NonConst(),
                                    *sol->y_c_NonConst(), *sol->y_d_NonConst(), false, 0);
      if( retval != SYMSOLVER_SUCCESS )
      {
         IpData().TimingStats().PDSystemSolverSolveOnce().End();
         return false;
      }
   }
   else
   {
      const Index numberOfEVals = rhs.y_c()->Dim() + rhs.y_d()->Dim();
      // counter for the number of trial evaluations
      // ToDo is not at the correct place
      Index count = 0;

      // Get the very first perturbation values from the perturbation
      // Handler
      Number delta_x;
      Number delta_s;
      Number delta_c;
      Number delta_d;
      perturbHandler_->ConsiderNewSystem(delta_x, delta_s, delta_c, delta_d);

      retval = SYMSOLVER_SINGULAR;
      while( retval != SYMSOLVER_SUCCESS )
      {

         if( pretend_singular )
         {
            retval = SYMSOLVER_SINGULAR;
            pretend_singular = false;
         }
         else
         {
            count++;
            Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                           "Solving system with delta_x=%e delta_s=%e\n                    delta_c=%e delta_d=%e\n", delta_x,
                           delta_s, delta_c, delta_d);
            bool check_inertia = true;
            if( neg_curv_test_tol_ > 0. )
            {
               check_inertia = false;
            }
            retval = augSysSolver_->Solve(&W, 1.0, &sigma_x, delta_x, &sigma_s, delta_s, &J_c, NULL, delta_c, &J_d,
                                          NULL, delta_d, *augRhs_x, *augRhs_s, *rhs.y_c(), *rhs.y_d(), *sol->x_NonConst(), *sol->s_NonConst(),
                                          *sol->y_c_NonConst(), *sol->y_d_NonConst(), check_inertia, numberOfEVals);
         }
         if( retval == SYMSOLVER_FATAL_ERROR )
         {
            return false;
         }
         if( retval == SYMSOLVER_SINGULAR && (rhs.y_c()->Dim() + rhs.y_d()->Dim() > 0) )
         {

            // Get new perturbation factors from the perturbation
            // handlers for the singular case
            bool pert_return = perturbHandler_->PerturbForSingularity(delta_x, delta_s, delta_c, delta_d);
            if( !pert_return )
            {
               Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                              "PerturbForSingularity can't be done\n");
               IpData().TimingStats().PDSystemSolverSolveOnce().End();
               return false;
            }
         }
         else if( retval == SYMSOLVER_WRONG_INERTIA && augSysSolver_->NumberOfNegEVals() < numberOfEVals )
         {
            Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                           "Number of negative eigenvalues too small!\n");
            // If the number of negative eigenvalues is too small, then
            // we first try to remedy this by asking for better quality
            // solution (e.g. increasing pivot tolerance), and if that
            // doesn't help, we assume that the system is singular
            bool assume_singular = true;
            if( !augsys_improved_ )
            {
               Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                              "Asking augmented system solver to improve quality of its solutions.\n");
               augsys_improved_ = augSysSolver_->IncreaseQuality();
               if( augsys_improved_ )
               {
                  IpData().Append_info_string("q");
                  assume_singular = false;
               }
               else
               {
                  Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                 "Quality could not be improved\n");
               }
            }
            if( assume_singular )
            {
               bool pert_return = perturbHandler_->PerturbForSingularity(delta_x, delta_s, delta_c, delta_d);
               if( !pert_return )
               {
                  Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                 "PerturbForSingularity can't be done for assume singular.\n");
                  IpData().TimingStats().PDSystemSolverSolveOnce().End();
                  return false;
               }
               IpData().Append_info_string("a");
            }
         }
         else if( retval == SYMSOLVER_WRONG_INERTIA || retval == SYMSOLVER_SINGULAR )
         {
            // Get new perturbation factors from the perturbation
            // handlers for the case of wrong inertia
            bool pert_return = perturbHandler_->PerturbForWrongInertia(delta_x, delta_s, delta_c, delta_d);
            if( !pert_return )
            {
               Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                              "PerturbForWrongInertia can't be done for wrong interia or singular.\n");
               IpData().TimingStats().PDSystemSolverSolveOnce().End();
               return false;
            }
         }
         else if (neg_curv_test_tol_ > 0.)
         {
            DBG_ASSERT(augSysSolver_->ProvidesInertia());
            // we now check if the inertia is possible wrong
            Index neg_values = augSysSolver_->NumberOfNegEVals();
            if (neg_values != numberOfEVals)
            {
               // check if we have a direction of sufficient positive curvature
               SmartPtr<Vector> x_tmp = sol->x()->MakeNew();
               W.MultVector(1., *sol->x(), 0., *x_tmp);
               Number xWx = x_tmp->Dot(*sol->x());
               x_tmp->Copy(*sol->x());
               x_tmp->ElementWiseMultiply(sigma_x);
               xWx += x_tmp->Dot(*sol->x());
               SmartPtr<Vector> s_tmp = sol->s()->MakeNewCopy();
               s_tmp->ElementWiseMultiply(sigma_s);
               xWx += s_tmp->Dot(*sol->s());
               if (neg_curv_test_reg_)
               {
                  x_tmp->Copy(*sol->x());
                  x_tmp->Scal(delta_x);
                  xWx += x_tmp->Dot(*sol->x());

                  s_tmp->Copy(*sol->s());
                  s_tmp->Scal(delta_s);
                  xWx += s_tmp->Dot(*sol->s());
               }
               Number xs_nrmsq = std::pow(sol->x()->Nrm2(), 2) + std::pow(sol->s()->Nrm2(), 2);
               Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                              "In inertia heuristic: xWx = %e xx = %e\n",
                              xWx, xs_nrmsq);
               if (xWx < neg_curv_test_tol_ * xs_nrmsq)
               {
                  Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                 "    -> Redo with modified matrix.\n");
                  bool pert_return = perturbHandler_->PerturbForWrongInertia(delta_x, delta_s,
                                     delta_c, delta_d);
                  if (!pert_return)
                  {
                     Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                                    "PerturbForWrongInertia can't be done for inertia heuristic.\n");
                     IpData().TimingStats().PDSystemSolverSolveOnce().End();
                     return false;
                  }
                  retval = SYMSOLVER_WRONG_INERTIA;
               }
            }
         }
      } // while (retval!=SYMSOLVER_SUCCESS)

      // Some output
      Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                     "Number of trial factorizations performed: %" IPOPT_INDEX_FORMAT "\n", count);
      Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
                     "Perturbation parameters: delta_x=%e delta_s=%e\n                         delta_c=%e delta_d=%e\n", delta_x,
                     delta_s, delta_c, delta_d);
      // Set the perturbation values in the Data object
      IpData().setPDPert(delta_x, delta_s, delta_c, delta_d);
   }

   // Compute the remaining sol Vectors
   Px_L.SinvBlrmZMTdBr(-1., slack_x_L, *rhs.z_L(), z_L, *sol->x(), *sol->z_L_NonConst());
   Px_U.SinvBlrmZMTdBr(1., slack_x_U, *rhs.z_U(), z_U, *sol->x(), *sol->z_U_NonConst());
   Pd_L.SinvBlrmZMTdBr(-1., slack_s_L, *rhs.v_L(), v_L, *sol->s(), *sol->v_L_NonConst());
   Pd_U.SinvBlrmZMTdBr(1., slack_s_U, *rhs.v_U(), v_U, *sol->s(), *sol->v_U_NonConst());

   // Finally let's assemble the res result vectors
   res.AddOneVector(alpha, *sol, beta);

   IpData().TimingStats().PDSystemSolverSolveOnce().End();

   return true;
}

void PDFullSpaceSolver::ComputeResiduals(
   const SymMatrix&      W,
   const Matrix&         J_c,
   const Matrix&         J_d,
   const Matrix&         Px_L,
   const Matrix&         Px_U,
   const Matrix&         Pd_L,
   const Matrix&         Pd_U,
   const Vector&         z_L,
   const Vector&         z_U,
   const Vector&         v_L,
   const Vector&         v_U,
   const Vector&         slack_x_L,
   const Vector&         slack_x_U,
   const Vector&         slack_s_L,
   const Vector&         slack_s_U,
   const Vector&         /*sigma_x*/,
   const Vector&         /*sigma_s*/,
   Number                /*alpha*/,
   Number                /*beta*/,
   const IteratesVector& rhs,
   const IteratesVector& res,
   IteratesVector&       resid
)
{
   DBG_START_METH("PDFullSpaceSolver::ComputeResiduals", dbg_verbosity);

   DBG_PRINT_VECTOR(2, "res", res);
   IpData().TimingStats().ComputeResiduals().Start();

   // Get the current sizes of the perturbation factors
   Number delta_x;
   Number delta_s;
   Number delta_c;
   Number delta_d;
   perturbHandler_->CurrentPerturbation(delta_x, delta_s, delta_c, delta_d);

   SmartPtr<Vector> tmp;

   // x
   W.MultVector(1., *res.x(), 0., *resid.x_NonConst());
   J_c.TransMultVector(1., *res.y_c(), 1., *resid.x_NonConst());
   J_d.TransMultVector(1., *res.y_d(), 1., *resid.x_NonConst());
   Px_L.MultVector(-1., *res.z_L(), 1., *resid.x_NonConst());
   Px_U.MultVector(1., *res.z_U(), 1., *resid.x_NonConst());
   resid.x_NonConst()->AddTwoVectors(delta_x, *res.x(), -1., *rhs.x(), 1.);

   // s
   Pd_U.MultVector(1., *res.v_U(), 0., *resid.s_NonConst());
   Pd_L.MultVector(-1., *res.v_L(), 1., *resid.s_NonConst());
   resid.s_NonConst()->AddTwoVectors(-1., *res.y_d(), -1., *rhs.s(), 1.);
   if( delta_s != 0. )
   {
      resid.s_NonConst()->Axpy(delta_s, *res.s());
   }

   // c
   J_c.MultVector(1., *res.x(), 0., *resid.y_c_NonConst());
   resid.y_c_NonConst()->AddTwoVectors(-delta_c, *res.y_c(), -1., *rhs.y_c(), 1.);

   // d
   J_d.MultVector(1., *res.x(), 0., *resid.y_d_NonConst());
   resid.y_d_NonConst()->AddTwoVectors(-1., *res.s(), -1., *rhs.y_d(), 1.);
   if( delta_d != 0. )
   {
      resid.y_d_NonConst()->Axpy(-delta_d, *res.y_d());
   }

   // zL
   resid.z_L_NonConst()->Copy(*res.z_L());
   resid.z_L_NonConst()->ElementWiseMultiply(slack_x_L);
   tmp = z_L.MakeNew();
   Px_L.TransMultVector(1., *res.x(), 0., *tmp);
   tmp->ElementWiseMultiply(z_L);
   resid.z_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.z_L(), 1.);

   // zU
   resid.z_U_NonConst()->Copy(*res.z_U());
   resid.z_U_NonConst()->ElementWiseMultiply(slack_x_U);
   tmp = z_U.MakeNew();
   Px_U.TransMultVector(1., *res.x(), 0., *tmp);
   tmp->ElementWiseMultiply(z_U);
   resid.z_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.z_U(), 1.);

   // vL
   resid.v_L_NonConst()->Copy(*res.v_L());
   resid.v_L_NonConst()->ElementWiseMultiply(slack_s_L);
   tmp = v_L.MakeNew();
   Pd_L.TransMultVector(1., *res.s(), 0., *tmp);
   tmp->ElementWiseMultiply(v_L);
   resid.v_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.v_L(), 1.);

   // vU
   resid.v_U_NonConst()->Copy(*res.v_U());
   resid.v_U_NonConst()->ElementWiseMultiply(slack_s_U);
   tmp = v_U.MakeNew();
   Pd_U.TransMultVector(1., *res.s(), 0., *tmp);
   tmp->ElementWiseMultiply(v_U);
   resid.v_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.v_U(), 1.);

   DBG_PRINT_VECTOR(2, "resid", resid);

   if( Jnlst().ProduceOutput(J_MOREVECTOR, J_LINEAR_ALGEBRA) )
   {
      resid.Print(Jnlst(), J_MOREVECTOR, J_LINEAR_ALGEBRA, "resid");
   }

   if( Jnlst().ProduceOutput(J_MOREDETAILED, J_LINEAR_ALGEBRA) )
   {
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_x  %e\n", resid.x()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_s  %e\n", resid.s()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_c  %e\n", resid.y_c()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_d  %e\n", resid.y_d()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_zL %e\n", resid.z_L()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_zU %e\n", resid.z_U()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_vL %e\n", resid.v_L()->Amax());
      Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                     "max-norm resid_vU %e\n", resid.v_U()->Amax());
   }
   IpData().TimingStats().ComputeResiduals().End();
}

Number PDFullSpaceSolver::ComputeResidualRatio(
   const IteratesVector& rhs,
   const IteratesVector& res,
   const IteratesVector& resid
)
{
   DBG_START_METH("PDFullSpaceSolver::ComputeResidualRatio", dbg_verbosity);

   Number nrm_rhs = rhs.Amax();
   Number nrm_res = res.Amax();
   Number nrm_resid = resid.Amax();
   Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
                  "nrm_rhs = %8.2e nrm_sol = %8.2e nrm_resid = %8.2e\n", nrm_rhs, nrm_res, nrm_resid);

   if( nrm_rhs + nrm_res == 0. )
   {
      return nrm_resid;  // this should be zero
   }
   else
   {
      // ToDo: determine how to include norm of matrix, and what
      // safeguard to use against incredibly large solution vectors
      Number max_cond = 1e6;
      return nrm_resid / (Min(nrm_res, max_cond * nrm_rhs) + nrm_rhs);
   }
}

} // namespace Ipopt