ConstrainedOptPack_QPSchur.cpp

Go to the documentation of this file.
00001 // @HEADER
00002 // ***********************************************************************
00003 // 
00004 // Moocho: Multi-functional Object-Oriented arCHitecture for Optimization
00005 //                  Copyright (2003) Sandia Corporation
00006 // 
00007 // Under terms of Contract DE-AC04-94AL85000, there is a non-exclusive
00008 // license for use of this work by or on behalf of the U.S. Government.
00009 // 
00010 // This library is free software; you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as
00012 // published by the Free Software Foundation; either version 2.1 of the
00013 // License, or (at your option) any later version.
00014 //  
00015 // This library is distributed in the hope that it will be useful, but
00016 // WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018 // Lesser General Public License for more details.
00019 //  
00020 // You should have received a copy of the GNU Lesser General Public
00021 // License along with this library; if not, write to the Free Software
00022 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
00023 // USA
00024 // Questions? Contact Roscoe A. Bartlett (rabartl@sandia.gov) 
00025 // 
00026 // ***********************************************************************
00027 // @HEADER
00028 
00029 //
00030 // 7/4/2002: RAB: I was able to update this class using the
00031 // functions in AbstractLinAlgPack_LinAlgOpPackHack.hpp.  This is wastefull in that
00032 // I am creating temporaries every time any operation is performed
00033 // but this was the easiest way to get things going.
00034 //
00035 // 7/4/2002: RAB : ToDo:  In the future it would be good to create
00036 // some type of temporary vector server so that I could avoid
00037 // creating all of these temporaries.  This will take some thought
00038 // and may not be worth it for now.
00039 //
00040 
00041 #include <assert.h>
00042 
00043 #include <ostream>
00044 #include <iomanip>
00045 #include <limits>
00046 
00047 #include "ConstrainedOptPack_QPSchur.hpp"
00048 #include "ConstrainedOptPack_ComputeMinMult.hpp"
00049 #include "AbstractLinAlgPack_MatrixSymPosDefCholFactor.hpp"
00050 #include "AbstractLinAlgPack_SpVectorOp.hpp"
00051 #include "AbstractLinAlgPack_GenPermMatrixSliceOp.hpp"
00052 #include "AbstractLinAlgPack_LinAlgOpPackHack.hpp"
00053 #include "AbstractLinAlgPack_GenPermMatrixSliceOut.hpp"
00054 #include "AbstractLinAlgPack_SpVectorOut.hpp"
00055 #include "AbstractLinAlgPack_MatrixOpNonsing.hpp"
00056 #include "AbstractLinAlgPack_MatrixOpOut.hpp"
00057 #include "AbstractLinAlgPack_VectorStdOps.hpp"
00058 #include "AbstractLinAlgPack_EtaVector.hpp"
00059 #include "DenseLinAlgPack_AssertOp.hpp"
00060 #include "DenseLinAlgPack_DVectorClass.hpp"
00061 #include "DenseLinAlgPack_DVectorClassExt.hpp"
00062 #include "DenseLinAlgPack_DVectorOp.hpp"
00063 #include "DenseLinAlgPack_LinAlgOpPack.hpp"
00064 #include "DenseLinAlgPack_DVectorOut.hpp"
00065 #include "DenseLinAlgPack_DMatrixOut.hpp"
00066 #include "Teuchos_Workspace.hpp"
00067 #include "Teuchos_TestForException.hpp"
00068 
00069 namespace LinAlgOpPack {
00070 using AbstractLinAlgPack::Vp_StV;
00071 using AbstractLinAlgPack::Vp_StMtV;
00072 using AbstractLinAlgPack::Vp_StV;
00073 using AbstractLinAlgPack::Vp_StMtV;
00074 }
00075 
00076 namespace {
00077 
00078 // Some local helper functions.
00079 
00080 template< class T >
00081 inline
00082 T my_max( const T& v1, const T& v2 ) { return v1 > v2 ? v1 : v2; }
00083 
00084 //
00085 // Print a bnd as a string
00086 //
00087 inline
00088 const char* bnd_str( ConstrainedOptPack::EBounds bnd )
00089 {
00090   switch(bnd) {
00091     case ConstrainedOptPack::FREE:
00092       return "FREE";
00093     case ConstrainedOptPack::UPPER:
00094       return "UPPER";
00095     case ConstrainedOptPack::LOWER:
00096       return "LOWER";
00097     case ConstrainedOptPack::EQUALITY:
00098       return "EQUALITY";
00099   }
00100   TEST_FOR_EXCEPT(true);  // should never be executed
00101   return 0;
00102 }
00103 
00104 //
00105 // print a bool
00106 //
00107 inline
00108 const char* bool_str( bool b )
00109 {
00110   return b ? "true" : "false";
00111 }
00112 
00113 //
00114 // Return a std::string that has a file name, line number and
00115 // error message.
00116 //
00117 std::string error_msg(
00118   const char file_name[], const int line_num, const char err_msg[]
00119   )
00120 {
00121   std::ostringstream  omsg;
00122   omsg
00123     << file_name << ":" << line_num << ":" << err_msg;
00124   return omsg.str();
00125 }
00126 
00127 //
00128 // Deincrement all indices less that k_remove
00129 //
00130 void deincrement_indices(
00131   DenseLinAlgPack::size_type                k_remove
00132   ,std::vector<DenseLinAlgPack::size_type>  *indice_vector
00133   ,size_t                              len_vector
00134   )
00135 {
00136   typedef DenseLinAlgPack::size_type        size_type;
00137   typedef std::vector<DenseLinAlgPack::size_type> vec_t;
00138   TEST_FOR_EXCEPT( !(  len_vector <= indice_vector->size()  ) );
00139   for( vec_t::iterator itr = indice_vector->begin(); itr != indice_vector->begin() + len_vector; ++itr ) {
00140     if( *itr > k_remove )
00141       --(*itr);
00142   }
00143 }
00144 
00145 //
00146 // Insert the element (r_v,c_v) into r[] and c[] sorted by r[]
00147 //
00148 void insert_pair_sorted(
00149   DenseLinAlgPack::size_type                r_v
00150   ,DenseLinAlgPack::size_type               c_v
00151   ,size_t                              len_vector  // length of the new vector
00152   ,std::vector<DenseLinAlgPack::size_type>  *r
00153   ,std::vector<DenseLinAlgPack::size_type>  *c
00154   )
00155 {
00156   typedef std::vector<DenseLinAlgPack::size_type> rc_t;
00157   TEST_FOR_EXCEPT( !(  r->size() >= len_vector && c->size() >= len_vector  ) );
00158   // find the insertion point in r[]
00159   rc_t::iterator
00160     itr = std::lower_bound( r->begin(), r->begin() + len_vector-1, r_v );
00161   const DenseLinAlgPack::size_type p = itr - r->begin();
00162   // Shift all of the stuff out of the way to make room for the insert
00163   {for( rc_t::iterator itr_last = r->begin() + len_vector-1;
00164       itr_last > r->begin() + p; --itr_last )
00165   {
00166     *itr_last = *(itr_last-1);
00167   }}
00168   {for( rc_t::iterator itr_last = c->begin() + len_vector-1;
00169       itr_last > c->begin() + p; --itr_last )
00170   {
00171     *itr_last = *(itr_last-1);
00172   }}
00173   // Insert the new elements
00174   (*r)[p] = r_v;
00175   (*c)[p] = c_v;
00176 }
00177 
00178 //
00179 // z_hat = inv(S_hat) * ( d_hat - U_hat'*vo )
00180 //
00181 void calc_z(
00182   const AbstractLinAlgPack::MatrixSymOpNonsing   &S_hat
00183   ,const DenseLinAlgPack::DVectorSlice           &d_hat
00184   ,const AbstractLinAlgPack::MatrixOp            &U_hat
00185   ,const DenseLinAlgPack::DVectorSlice           *vo       // If NULL then assumed zero
00186   ,DenseLinAlgPack::DVectorSlice                 *z_hat
00187   )
00188 {
00189   using LinAlgOpPack::Vp_StMtV;
00190   using LinAlgOpPack::V_InvMtV;
00191   using Teuchos::Workspace;
00192   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00193 
00194   Workspace<DenseLinAlgPack::value_type> t_ws(wss,d_hat.dim());
00195   DenseLinAlgPack::DVectorSlice                t(&t_ws[0],t_ws.size());
00196   t = d_hat;
00197   if(vo)
00198     Vp_StMtV( &t, -1.0, U_hat, BLAS_Cpp::trans, *vo );
00199   V_InvMtV( z_hat, S_hat, BLAS_Cpp::no_trans, t );
00200 }
00201 
00202 //
00203 // v = inv(Ko) * ( fo - U_hat * z_hat )
00204 //
00205 void calc_v(
00206   const AbstractLinAlgPack::MatrixSymOpNonsing   &Ko
00207   ,const DenseLinAlgPack::DVectorSlice                         *fo    // If NULL then assumed to be zero
00208   ,const AbstractLinAlgPack::MatrixOp                &U_hat
00209   ,const DenseLinAlgPack::DVectorSlice                         &z_hat // Only accessed if U_hat.cols() > 0
00210   ,DenseLinAlgPack::DVectorSlice                               *v
00211   )
00212 {
00213   using DenseLinAlgPack::norm_inf;
00214   using LinAlgOpPack::Vp_StMtV;
00215   using LinAlgOpPack::V_InvMtV;
00216   using Teuchos::Workspace;
00217   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00218 
00219   Workspace<DenseLinAlgPack::value_type> t_ws(wss,v->dim());
00220   DenseLinAlgPack::DVectorSlice                t(&t_ws[0],t_ws.size());
00221   if(fo) {  
00222     t = *fo;
00223   }
00224   else {
00225     t = 0.0;
00226   }
00227   if( U_hat.cols() )
00228     Vp_StMtV( &t, -1.0, U_hat, BLAS_Cpp::no_trans, z_hat );
00229   if( norm_inf(t) > 0.0 )
00230     V_InvMtV( v, Ko, BLAS_Cpp::no_trans, t );
00231   else
00232     *v = 0.0;
00233 }
00234 
00235 //
00236 // mu_D_hat =
00237 //    - Q_XD_hat' * g
00238 //    - Q_XD_hat' * G * x
00239 //    - Q_XD_hat' * A * v(n_R+1:n_R+m)
00240 //    - Q_XD_hat' * A_bar * P_plus_hat * z_hat
00241 //
00242 void calc_mu_D(
00243   const ConstrainedOptPack::QPSchur::ActiveSet  &act_set
00244   ,const DenseLinAlgPack::DVectorSlice                         &x
00245   ,const DenseLinAlgPack::DVectorSlice                         &v
00246   ,DenseLinAlgPack::DVectorSlice                               *mu_D
00247   )
00248 {
00249   using BLAS_Cpp::no_trans;
00250   using BLAS_Cpp::trans;
00251   using LinAlgOpPack::V_MtV;
00252   using LinAlgOpPack::V_StMtV;
00253   using LinAlgOpPack::Vp_StPtMtV;
00254   using AbstractLinAlgPack::V_MtV;
00255   using AbstractLinAlgPack::Vp_MtV;
00256   using Teuchos::Workspace;
00257   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00258 
00259   const ConstrainedOptPack::QPSchurPack::QP
00260     &qp = act_set.qp();
00261   const DenseLinAlgPack::size_type
00262     n = qp.n(),
00263     n_R = qp.n_R(),
00264     m = qp.m();
00265 
00266   const AbstractLinAlgPack::GenPermMatrixSlice &Q_XD_hat = act_set.Q_XD_hat();
00267   const DenseLinAlgPack::DVectorSlice      g = qp.g();
00268   const AbstractLinAlgPack::MatrixSymOp &G = qp.G();
00269   // mu_D_hat = - Q_XD_hat' * g
00270   V_StMtV( mu_D, -1.0, Q_XD_hat, trans, g ); 
00271   // mu_D_hat += - Q_XD_hat' * G * x
00272   Vp_StPtMtV( mu_D, -1.0, Q_XD_hat, trans, G, no_trans, x );
00273   // mu_D_hat += - Q_XD_hat' * A * v(n_R+1:n_R+m)
00274   if( m ) {
00275     Vp_StPtMtV( mu_D, -1.0, Q_XD_hat, trans, qp.A(), no_trans, v(n_R+1,n_R+m) );
00276   }
00277   // p_mu_D_hat += - Q_XD_hat' * A_bar * P_plus_hat * z_hat
00278   if( act_set.q_plus_hat() && act_set.q_hat() ) {
00279     const DenseLinAlgPack::DVectorSlice z_hat = act_set.z_hat();
00280     AbstractLinAlgPack::SpVector P_plus_hat_z_hat;
00281     V_MtV( &P_plus_hat_z_hat, act_set.P_plus_hat(), no_trans, z_hat ); 
00282     Vp_StPtMtV( mu_D, -1.0, Q_XD_hat, trans
00283       , qp.constraints().A_bar(), no_trans, P_plus_hat_z_hat() );
00284   }
00285 }
00286 
00287 //
00288 // p_mu_D_hat =
00289 //    - Q_XD_hat' * G * Q_R * p_v(1:n_R)
00290 //    - Q_XD_hat' * G * P_XF_hat * p_z_hat
00291 //    - Q_XD_hat' * A * p_v(n_R+1:n_R+m)
00292 //    - Q_XD_hat' * A_bar * (P_plus_hat * p_z_hat + e(ja))
00293 //
00294 void calc_p_mu_D(
00295   const ConstrainedOptPack::QPSchur::ActiveSet  &act_set
00296   ,const DenseLinAlgPack::DVectorSlice                         &p_v
00297   ,const DenseLinAlgPack::DVectorSlice                         &p_z_hat
00298   ,const DenseLinAlgPack::size_type                           *ja       // If != NULL then we will include the term e(ja)
00299   ,DenseLinAlgPack::DVectorSlice                               *p_mu_D
00300   )
00301 {
00302   using BLAS_Cpp::no_trans;
00303   using BLAS_Cpp::trans;
00304   using AbstractLinAlgPack::V_MtV;
00305   using AbstractLinAlgPack::Vp_MtV;
00306   using LinAlgOpPack::Vp_StPtMtV;
00307   using Teuchos::Workspace;
00308   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00309 
00310   const ConstrainedOptPack::QPSchurPack::QP
00311     &qp = act_set.qp();
00312   const ConstrainedOptPack::QPSchurPack::Constraints
00313     &constraints = qp.constraints();
00314   const DenseLinAlgPack::size_type
00315     n = qp.n(),
00316     n_R = qp.n_R(),
00317     m = qp.m();
00318 
00319   const AbstractLinAlgPack::GenPermMatrixSlice &Q_XD_hat = act_set.Q_XD_hat();
00320   const AbstractLinAlgPack::MatrixSymOp &G = qp.G();
00321   // p_mu_D_hat = - Q_XD_hat' * G * Q_R * p_v(1:n_R)
00322   {
00323     AbstractLinAlgPack::SpVector Q_R_p_v1;
00324     V_MtV( &Q_R_p_v1, qp.Q_R(), no_trans, p_v(1,n_R) ); 
00325     Vp_StPtMtV( p_mu_D, -1.0, Q_XD_hat, trans, G, no_trans, Q_R_p_v1(), 0.0 );
00326   }
00327   // p_mu_D_hat += - Q_XD_hat' * G * P_XF_hat * p_z_hat
00328   if( act_set.q_F_hat() ) {
00329     AbstractLinAlgPack::SpVector P_XF_hat_p_z_hat;
00330     V_MtV( &P_XF_hat_p_z_hat, act_set.P_XF_hat(), no_trans, p_z_hat ); 
00331     Vp_StPtMtV( p_mu_D, -1.0, Q_XD_hat, trans, G, no_trans, P_XF_hat_p_z_hat() );
00332   }
00333   // p_mu_D_hat += - Q_XD_hat' * A * p_v(n_R+1:n_R+m)
00334   if( m ) {
00335     Vp_StPtMtV( p_mu_D, -1.0, Q_XD_hat, trans, qp.A(), no_trans, p_v(n_R+1,n_R+m) );
00336   }
00337   // p_mu_D_hat += - Q_XD_hat' * A_bar * ( P_plus_hat * p_z_hat + e(ja) )
00338   if( act_set.q_plus_hat() || ja ) {
00339     AbstractLinAlgPack::SpVector p_lambda_bar(
00340       n+constraints.m_breve(), act_set.q_plus_hat() + (ja ? 1 : 0) );
00341     if( act_set.q_plus_hat() ) // p_lambda_bar =  P_plus_hat * p_z_hat
00342       Vp_MtV( &p_lambda_bar, act_set.P_plus_hat(), no_trans, p_z_hat );
00343     if( ja ) // p_lambda_bar += e(ja) (non-duplicate indices?)
00344       p_lambda_bar.insert_element(AbstractLinAlgPack::SpVector::element_type(*ja,1.0));
00345     Vp_StPtMtV( p_mu_D, -1.0, Q_XD_hat, trans
00346       , constraints.A_bar(), no_trans, p_lambda_bar() );
00347   }
00348 }
00349 
00350 //
00351 // Calculate the residual of the augmented KKT system:
00352 //
00353 // [ ro ] = [   Ko     U_hat ] [   v   ] + [  ao * bo ]
00354 // [ ra ]   [ U_hat'   V_hat ] [ z_hat ]   [  aa * ba ]
00355 //
00356 // Expanding this out we have:
00357 //
00358 // ro = Ko * v + U_hat * z_hat + ao * bo
00359 //
00360 //    = [ Q_R'*G*Q_R   Q_R'*A ] * [   x_R  ] + [ Q_R'*G*P_XF_hat + Q_R'*A_bar*P_plus_hat ] * z_hat + [ ao*boR ]
00361 //      [  A'*Q_R             ]   [ lambda ]   [          A'*P_XF_hat                    ]           [ ao*bom ]
00362 //
00363 //    = [ Q_R'*G*(Q_R*x_R + P_XF_hat*z_hat) + Q_R'*A*lambda + Q_R'*A_bar*P_plus_hat*z_hat + ao*boR ]   
00364 //      [      A'*(Q_R*x_R + P_XF_hat*z_hat) + ao*bom                                              ]
00365 //
00366 // ra = [ U_hat' * v + V_hat * z_hat + aa*ba
00367 //
00368 //    = [ P_XF_hat'*G*Q_R + P_plus_hat'*A_bar'*Q_R , P_XF_hat'*A ] * [ x_R ; lambda ]
00369 //      + [ P_XF_hat'*G*P_XF_hat + P_XF_hat'*A_bar*P_plus_hat + P_plus_hat'*A_bar'*P_XF_hat
00370 //          + P_F_tilde'*P_C_hat + P_C_hat'*P_F_tilde ] * z_hat + aa*ba
00371 //
00372 //    = P_XF_hat'*G*(Q_R*x_R + P_XF_hat*z_hat) + P_plus_hat'*A_bar'*(Q_R*x_R + P_XF_hat*z_hat)
00373 //      + P_XF_hat'*A*lambda + P_XF_hat'*A_bar*P_plus_hat*z_hat
00374 //      + (P_F_tilde'*P_C_hat + P_C_hat'*P_F_tilde)*z_hat + aa*ba
00375 //
00376 // Given the QP matrices G, A, and A_bar and the active set mapping matrices Q_R, P_XF_hat,
00377 // P_plus_hat and P_FC_hat = P_F_tilde'*P_C_hat, we can compute the residual efficiently
00378 // as follows:
00379 //
00380 // x_free = Q_R*x_R + P_XF_hat*z_hat (sparse)
00381 // 
00382 // lambda_bar = P_plus_hat*z_hat (sparse)
00383 //
00384 // t1 = G*x_free
00385 //
00386 // t2 = A*lambda
00387 //
00388 // t3 = A_bar*lambda_bar
00389 //
00390 // roR = Q_R'*t1 + Q_R'*t2 + Q_R'*t3 + ao*boR
00391 //
00392 // rom = A'*t1 + ao*bom
00393 //
00394 // ra = P_XF_hat'*t1 + P_plus_hat'*A_bar'*x_free + P_XF_hat'*t2 + P_XF_hat'*t3
00395 //      + (P_FC_hat + P_FC_hat')*z_hat  + aa*ba
00396 //
00397 // On output we will have set:
00398 //
00399 //   roR_scaling = ||Q_R'*t1||inf + ||Q_R'*t2||inf + ||Q_R'*t3||inf + ||ao*boR||inf
00400 //
00401 //   rom_scaling = ||A'*t1||inf + ||ao*bom||inf
00402 //
00403 //   ra_scaling  = ||P_XF_hat'*t1||inf + ||P_plus_hat'*A_bar'*t1||inf + ||P_XF_hat'*t2||inf
00404 //                 + ||P_XF_hat'*t3||inf + ||(P_FC_hat + P_FC_hat')*z_hat|| + ||aa*ba||inf
00405 // 
00406 // Note: In the future we could make this a little more efficent by combining (Q_R + P_XF_hat) into
00407 // an single permulation matrix and then we could leave out the terms for the variables initially 
00408 // fixed and still fixed rather than computing the terms then throwing them away.
00409 //
00410 // Also, in the future, this could really be implemented for extended precision data types but
00411 // it would require some real work!
00412 //
00413 template<class val_type>
00414 void calc_resid(
00415   const ConstrainedOptPack::QPSchur::ActiveSet     &act_set
00416   ,const DenseLinAlgPack::DVectorSlice                            &v
00417   ,const DenseLinAlgPack::DVectorSlice                            &z_hat        // Only accessed if q_hat > 0
00418   ,const DenseLinAlgPack::value_type                             ao            // Only accessed if bo != NULL
00419   ,const DenseLinAlgPack::DVectorSlice                            *bo           // If NULL then considered 0
00420   ,DenseLinAlgPack::VectorSliceTmpl<val_type>                    *ro
00421   ,DenseLinAlgPack::value_type                                   *roR_scaling
00422   ,DenseLinAlgPack::value_type                                   *rom_scaling  // Only set if m > 0
00423   ,const DenseLinAlgPack::value_type                             aa            // Only accessed if q_hat > 0
00424   ,const DenseLinAlgPack::DVectorSlice                            *ba           // If NULL then considered 0, Only accessed if q_hat > 0
00425   ,DenseLinAlgPack::VectorSliceTmpl<val_type>                    *ra           // Only set if q_hat > 0
00426   ,DenseLinAlgPack::value_type                                   *ra_scaling   // Only set if q_hat > 0
00427   )
00428 {
00429   using BLAS_Cpp::no_trans;
00430   using BLAS_Cpp::trans;
00431   using DenseLinAlgPack::norm_inf;
00432   using DenseLinAlgPack::DVectorSlice;
00433   using DenseLinAlgPack::Vp_StV;
00434   using AbstractLinAlgPack::V_MtV;
00435   using AbstractLinAlgPack::SpVector;
00436   using AbstractLinAlgPack::GenPermMatrixSlice;
00437   using LinAlgOpPack::Vp_V;
00438   using LinAlgOpPack::V_StV;
00439   using LinAlgOpPack::V_MtV;
00440   using LinAlgOpPack::Vp_MtV;
00441   using LinAlgOpPack::Vp_StMtV;
00442   using LinAlgOpPack::Vp_StPtMtV;
00443   namespace COP = ConstrainedOptPack;
00444   using Teuchos::Workspace;
00445   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00446   
00447   const COP::QPSchurPack::QP
00448     &qp = act_set.qp();
00449   const COP::QPSchurPack::Constraints
00450     &constraints = qp.constraints();
00451   const GenPermMatrixSlice
00452     &Q_R        = qp.Q_R(),
00453     &P_XF_hat   = act_set.P_XF_hat(),
00454     &P_plus_hat = act_set.P_plus_hat();
00455   const DenseLinAlgPack::size_type
00456     n          = qp.n(),
00457     n_R        = qp.n_R(),
00458     m          = qp.m(),
00459     m_breve    = constraints.m_breve(),
00460     q_hat      = act_set.q_hat(),
00461     q_F_hat    = act_set.q_F_hat(),
00462     q_C_hat    = act_set.q_C_hat(),
00463     q_plus_hat = act_set.q_plus_hat();
00464   
00465   const DVectorSlice
00466     x_R = v(1,n_R),
00467     lambda = ( m ? v(n_R+1,n_R+m): DVectorSlice() ),
00468     boR = ( bo ? (*bo)(1,n_R) : DVectorSlice() ),
00469     bom = ( bo && m ? (*bo)(n_R+1,n_R+m) : DVectorSlice() );
00470 
00471   DenseLinAlgPack::VectorSliceTmpl<val_type>
00472     roR = (*ro)(1,n_R),
00473     rom = ( m ? (*ro)(n_R+1,n_R+m) : DenseLinAlgPack::VectorSliceTmpl<val_type>() );
00474 
00475   Workspace<DenseLinAlgPack::value_type>
00476     x_free_ws(wss,n);
00477   DenseLinAlgPack::DVectorSlice
00478     x_free(&x_free_ws[0],x_free_ws.size());
00479   Workspace<val_type>
00480     t1_ws(wss,n),
00481     t2_ws(wss,n),
00482     t3_ws(wss,n),
00483     tR_ws(wss,n_R),
00484     tm_ws(wss,m),
00485     ta_ws(wss,q_hat);
00486   DenseLinAlgPack::VectorSliceTmpl<val_type>
00487     t1(&t1_ws[0],t1_ws.size()),
00488     t2(&t2_ws[0],t2_ws.size()),
00489     t3(&t3_ws[0],t3_ws.size()),
00490     tR(&tR_ws[0],tR_ws.size()),
00491     tm(tm_ws.size()?&tm_ws[0]:NULL,tm_ws.size()),
00492     ta(ta_ws.size()?&ta_ws[0]:NULL,ta_ws.size());
00493 
00494   *roR_scaling = 0.0;
00495   if( m )
00496     *rom_scaling = 0.0;
00497   if( q_hat )
00498     *ra_scaling  = 0.0;
00499   
00500   // x_free = Q_R*x_R + P_XF_hat*z_hat (dense for now)
00501   LinAlgOpPack::V_MtV( &x_free, Q_R, no_trans, x_R );
00502   if( q_F_hat )
00503     LinAlgOpPack::Vp_MtV( &x_free, P_XF_hat, no_trans, z_hat );
00504   // lambda_bar = P_plus_hat*z_hat
00505   SpVector lambda_bar;
00506   if( q_plus_hat )
00507     V_MtV( &lambda_bar, P_plus_hat, no_trans, z_hat );
00508   // t1 = G*x_free
00509   Vp_StMtV( &t1, 1.0, qp.G(), no_trans, x_free, 0.0 );
00510   // t2 = A*lambda
00511   if( m )
00512     Vp_StMtV( &t2, 1.0, qp.A(), no_trans, lambda, 0.0 );
00513   // t3 = A_bar*lambda_bar
00514   if( q_plus_hat )
00515     Vp_StMtV( &t3, 1.0, constraints.A_bar(), no_trans, lambda_bar(), 0.0 );
00516   // roR = Q_R'*t1 + Q_R'*t2 + Q_R'*t3 + ao*boR
00517   LinAlgOpPack::V_MtV( &tR, Q_R, trans, t1 );  // roR = Q_R'*t1
00518   *roR_scaling += norm_inf(tR);
00519   roR = tR;
00520   if( m ) {                     // roR += Q_R'*t2
00521     LinAlgOpPack::V_MtV( &tR, Q_R, trans, t2 );
00522     *roR_scaling += norm_inf(tR);
00523     LinAlgOpPack::Vp_V(&roR,tR);
00524   }
00525   if( q_plus_hat ) {           // roR += Q_R'*t3
00526     LinAlgOpPack::V_MtV( &tR, Q_R, trans, t3 );
00527     *roR_scaling += norm_inf(tR);
00528     LinAlgOpPack::Vp_V(&roR,tR);
00529   }
00530   if( bo ) {
00531     LinAlgOpPack::Vp_StV( &roR, ao, boR );    // roR += ao*boR
00532     *roR_scaling += std::fabs(ao) * norm_inf(boR);
00533   }
00534   // rom = A'*t1 + ao*bom
00535   if( m ) {
00536     Vp_StMtV( &tm, 1.0, qp.A(), trans, t1, 0.0 );   // A'*t1
00537     *rom_scaling += norm_inf(tm);
00538     LinAlgOpPack::Vp_V(&rom,tm);
00539     if(bo) {
00540       LinAlgOpPack::Vp_StV( &rom, ao, bom );        // rom += ao*bom
00541       *rom_scaling += std::fabs(ao)*norm_inf(bom);
00542     }
00543   }
00544   // ra = P_XF_hat'*t1 + P_plus_hat'*A_bar'*x_free + P_XF_hat'*t2 + P_XF_hat'*t3
00545   //      +(P_FC_hat + P_FC_hat')*z_hat + aa*ba
00546   if( q_hat ) {
00547     if(ba) {              // ra = aa*ba
00548       V_StV( ra, aa, *ba );
00549       *ra_scaling += std::fabs(aa) * norm_inf(*ba);
00550     }
00551     else {
00552       *ra = 0.0;
00553     }
00554     if( q_F_hat ) {       // ra +=  P_XF_hat'*t1
00555       LinAlgOpPack::V_MtV( &ta, P_XF_hat, trans, t1 );
00556       *ra_scaling += norm_inf(ta);
00557       LinAlgOpPack::Vp_V(ra,ta);
00558     }
00559     if( q_plus_hat ) {    // ra += P_plus_hat'*A_bar'*x_free
00560       Vp_StPtMtV( &ta, 1.0, P_plus_hat, trans, constraints.A_bar(), trans, x_free, 0.0 );
00561       *ra_scaling += norm_inf(ta);
00562       LinAlgOpPack::Vp_V(ra,ta);
00563     }
00564     if( q_F_hat && m ) {  // ra += P_XF_hat'*t2
00565       LinAlgOpPack::V_MtV( &ta, P_XF_hat, trans, t2 );
00566       *ra_scaling += norm_inf(ta);
00567       LinAlgOpPack::Vp_V(ra,ta);
00568     }
00569     if( q_F_hat && q_plus_hat ) { // ra += P_XF_hat'*t3
00570       LinAlgOpPack::V_MtV( &ta, P_XF_hat, trans, t3 );
00571       *ra_scaling += norm_inf(ta);
00572       LinAlgOpPack::Vp_V(ra,ta);
00573     }
00574     if( q_C_hat ) {       // ra += (P_FC_hat + P_FC_hat')*z_hat
00575       const GenPermMatrixSlice
00576         &P_FC_hat = act_set.P_FC_hat();
00577       ta = 0.0;
00578       for( GenPermMatrixSlice::const_iterator itr = P_FC_hat.begin(); itr != P_FC_hat.end(); ++itr ) {
00579         ta(itr->row_i()) = z_hat(itr->col_j());
00580         ta(itr->col_j()) = z_hat(itr->row_i());
00581       }
00582       *ra_scaling += norm_inf(ta);
00583       LinAlgOpPack::Vp_V(ra,ta);
00584     }
00585   }
00586 }
00587 
00588 //
00589 // Correct a nearly degenerate Lagrange multiplier
00590 //
00591 // If < 0 is returned it means that the multiplier could not
00592 // be corrected and this should be considered to be dual infeasible
00593 // In this case the error output is sent to *out if print_dual_infeas
00594 // == true.
00595 //
00596 // If 0 is returned then the multiplier was near degenerate and
00597 // was corrected.  In this case a warning message is printed to
00598 // *out.
00599 //
00600 // If > 0 is returned then the multiplier's sign
00601 // is just fine and no corrections were needed (no output).
00602 //
00603 int correct_dual_infeas(
00604   const DenseLinAlgPack::size_type                                  j                // for output info only
00605   ,const ConstrainedOptPack::EBounds                  bnd_j
00606   ,const DenseLinAlgPack::value_type                                t_P              // (> 0) full step length
00607   ,const DenseLinAlgPack::value_type                                scale            // (> 0) scaling value
00608   ,const DenseLinAlgPack::value_type                                dual_infeas_tol
00609   ,const DenseLinAlgPack::value_type                                degen_mult_val
00610   ,std::ostream                                                *out             // Can be NULL
00611   ,const ConstrainedOptPack::QPSchur::EOutputLevel    olevel
00612   ,const bool                                                  print_dual_infeas
00613   ,const char                                                  nu_j_n[]         // Name of nu_j
00614   ,DenseLinAlgPack::value_type                                      *nu_j            // required
00615   ,DenseLinAlgPack::value_type                                      *scaled_viol     // = scale*nu_j*(bnd_j==UPPER ? 1.0: -1.0 ) (after output)
00616   ,const char                                                  p_nu_j_n[]    = NULL // Name of p_nu_j (can be NULL if p_nu_j==NULL)
00617   ,DenseLinAlgPack::value_type                                      *p_nu_j       = NULL // optional (can be NULL)
00618   ,const char                                                  nu_j_plus_n[] = NULL // Name of nu_j_plus (can be NULL if p_nu_j==NULL)
00619   ,DenseLinAlgPack::value_type                                      *nu_j_plus    = NULL // optional (can be NULL)
00620   )
00621 {
00622   typedef DenseLinAlgPack::value_type value_type;
00623   namespace COP = ConstrainedOptPack;
00624 
00625   value_type nu_j_max = (*scaled_viol) = scale * (*nu_j) * (bnd_j == COP::UPPER ? +1.0 : -1.0);
00626   if( nu_j_max > 0.0 || bnd_j == COP::EQUALITY ) // Leave any multiplier value with the correct sign alone!
00627     return +1; // No correction needed
00628   // See if we need to correct the multiplier
00629   nu_j_max = std::fabs(nu_j_max);
00630   if( nu_j_max < dual_infeas_tol ) {
00631     // This is a near degenerate multiplier so adjust it
00632     value_type degen_val = degen_mult_val * ( bnd_j == COP::UPPER ? +1.0 : -1.0 );
00633     if( out && (int)olevel >= (int)COP::QPSchur::OUTPUT_BASIC_INFO ) {
00634       *out
00635         << "\nWarning, the constriant a(" << j << ") currently at its "
00636         << (bnd_j == COP::UPPER ? "UPPER" : "LOWER") << " bound"
00637         << " has the wrong Lagrange multiplier value but\n"
00638         << "scale*|"<<nu_j_n<<"| = " << scale << " * |" << (*nu_j)
00639         << "| = " << nu_j_max  << " < dual_infeas_tol = " << dual_infeas_tol
00640         << "\nTherefore, this is considered a degenerate constraint and this "
00641         << "multiplier is set to " << degen_val << std::endl;
00642     }
00643     if(p_nu_j) {
00644       nu_j_max += std::fabs( t_P * (*p_nu_j) ) * scale;
00645       if( nu_j_max < dual_infeas_tol ) {
00646         // The full step is also degenerate so adjust it also
00647         if( out && (int)olevel >= (int)COP::QPSchur::OUTPUT_BASIC_INFO ) {
00648           *out
00649             << "Also, the maximum full step scale*(|"<<nu_j_n<<"|+|t_P*"<<p_nu_j_n<<"|) = "
00650             << scale << " * (|" << (*nu_j) << "| +  |" << t_P << " * " << (*p_nu_j) << "|) = "
00651             << nu_j_max << " < dual_infeas_tol = " << dual_infeas_tol
00652             << "\nTherefore, this is considered degenerate and therefore "
00653             << "seting "<<p_nu_j_n<<" = 0";
00654           if(nu_j_plus)
00655             *out << " and "<< nu_j_plus_n <<" = " << degen_val;
00656           *out << std::endl;
00657         }
00658         *p_nu_j = 0.0; // Don't let it limit the step length
00659         if(nu_j_plus) {
00660           *nu_j_plus = degen_val;
00661         }
00662       }
00663     }
00664     *nu_j = degen_val;  // Now set it
00665     *scaled_viol = scale * (*nu_j) * (bnd_j == COP::UPPER ? +1.0 : -1.0); // Not violated!
00666     return 0;
00667   }
00668   else {
00669     if( print_dual_infeas && out && (int)olevel >= (int)COP::QPSchur::OUTPUT_BASIC_INFO ) {
00670       *out
00671         << "\nError, the constriant a(" << j << ") currently at its "
00672         << (bnd_j == COP::UPPER ? "UPPER" : "LOWER") << " bound"
00673         << " has the wrong Lagrange multiplier value and\n"
00674         << "scale*|"<<nu_j_n<<"| = " << scale << " * |" << (*nu_j)
00675         << "| = " << nu_j_max  << " > dual_infeas_tol = " << dual_infeas_tol
00676         << "\nThis is an indication of instability in the calculations.\n"
00677         << "The QP algorithm is terminated!\n";
00678     }
00679     return -1;
00680   }
00681   return 0; // Will never be executed!
00682 }
00683 
00684 //
00685 // Calculate the QP objective if it has not been already
00686 //
00687 // qp_grad_norm_inf = ||g + G*x||inf
00688 //
00689 // On input, if qp_grad_norm_inf != 0, then it will be assumed
00690 // that this value has already been computed and the computation will
00691 // be skipped.
00692 //
00693 void calc_obj_grad_norm_inf(
00694   const ConstrainedOptPack::QPSchurPack::QP     &qp
00695   ,const DenseLinAlgPack::DVectorSlice                         &x
00696   ,DenseLinAlgPack::value_type                                *qp_grad_norm_inf
00697   )
00698 {
00699   TEST_FOR_EXCEPT(true); // ToDo: Implement this?
00700 }
00701 
00702 } // end namespace
00703 
00704 namespace ConstrainedOptPack {
00705 
00706 // public member functions for QPSchur::U_hat_t
00707 
00708 QPSchur::U_hat_t::U_hat_t()
00709   :G_(NULL)
00710   ,A_(NULL)
00711   ,A_bar_(NULL)
00712   ,Q_R_(NULL)
00713   ,P_XF_hat_(NULL)
00714   ,P_plus_hat_(NULL)
00715 {}
00716 
00717 void QPSchur::U_hat_t::initialize( 
00718   const MatrixSymOp       *G
00719   ,const MatrixOp         *A
00720   ,const MatrixOp         *A_bar
00721   ,const GenPermMatrixSlice   *Q_R
00722   ,const GenPermMatrixSlice   *P_XF_hat
00723   ,const GenPermMatrixSlice   *P_plus_hat
00724   )
00725 {
00726   G_            = G;
00727   A_            = A;
00728   A_bar_        = A_bar;
00729   Q_R_          = Q_R;
00730   P_XF_hat_     = P_XF_hat;
00731   P_plus_hat_   = P_plus_hat;
00732 }
00733 
00734 size_type QPSchur::U_hat_t::rows() const
00735 {
00736   return Q_R_->cols() + ( A_ ? A_->cols() : 0 );
00737 }
00738 
00739 size_type QPSchur::U_hat_t::cols() const
00740 {
00741   return P_plus_hat_->cols();
00742 }
00743 
00744 /* 10/25/00: I don't think we need this function!
00745 void QPSchur::U_hat_t::Mp_StM(DMatrixSlice* C, value_type a
00746   , BLAS_Cpp::Transp M_trans ) const
00747 {
00748   using BLAS_Cpp::no_trans;
00749   using BLAS_Cpp::trans;
00750   using LinAlgOpPack::Mp_StMtP;
00751   using LinAlgOpPack::Mp_StPtMtP;
00752 
00753   // C += a * op(U_hat)
00754 
00755   LinAlgOpPack::Mp_M_assert_sizes( C->rows(), C->cols(), no_trans
00756     , rows(), cols(), M_trans );
00757 
00758   const size_type
00759     n_R = Q_R_->cols(),
00760     m   = A() ? A()->cols()  : 0;
00761 
00762   if( M_trans == no_trans ) {
00763     //
00764     // C += a * op(U_hat)
00765     // 
00766     //    = a * [  Q_R' * G * P_XF_hat + Q_R' * A_bar * P_plus_hat ]
00767     //          [                  A' * P_XF_hat                   ]
00768     //          
00769     // C1 += a * Q_R' * G * P_XF_hat + a * Q_R' * A_bar * P_plus_hat
00770     // 
00771     // C2 += a * A' * P_XF_hat
00772     //
00773     DMatrixSlice
00774       C1 = (*C)(1,n_R,1,C->cols()),
00775       C2 = m ? (*C)(n_R+1,n_R+m,1,C->cols()) : DMatrixSlice();
00776     // C1 += a * Q_R' * G * P_XF_hat
00777     if( P_XF_hat().nz() )
00778       Mp_StPtMtP( &C1, a, Q_R(), trans, G(), no_trans, P_XF_hat(), no_trans );
00779     // C1 += a * Q_R' * A_bar * P_plus_hat
00780     if( P_plus_hat().nz() )
00781       Mp_StPtMtP( &C1, a, Q_R(), trans, A_bar(), no_trans, P_plus_hat(), no_trans );
00782     // C2 += a * A' * P_XF_hat
00783     if(m && P_XF_hat().nz())
00784       Mp_StMtP( &C2, a, *A(), trans, P_plus_hat(), no_trans );
00785   }
00786   else {
00787     TEST_FOR_EXCEPT(true);  // Implement this!
00788   }
00789 }
00790 */
00791 
00792 void QPSchur::U_hat_t::Vp_StMtV(
00793   DVectorSlice* y, value_type a, BLAS_Cpp::Transp M_trans
00794   ,const DVectorSlice& x, value_type b
00795   ) const
00796 {
00797   using BLAS_Cpp::no_trans;
00798   using BLAS_Cpp::trans;
00799   using DenseLinAlgPack::Vt_S;
00800   using AbstractLinAlgPack::V_MtV;
00801   using AbstractLinAlgPack::Vp_StMtV;
00802   using LinAlgOpPack::V_MtV;
00803   using LinAlgOpPack::Vp_StMtV;
00804   using LinAlgOpPack::Vp_StPtMtV;
00805   using Teuchos::Workspace;
00806   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00807 
00808   LinAlgOpPack::Vp_MtV_assert_sizes(y->dim(),rows(),cols(),M_trans,x.dim());
00809 
00810   //
00811   // U_hat = [  Q_R' * G * P_XF_hat + Q_R' * A_bar * P_plus_hat ]
00812   //         [                  A' * P_XF_hat                   ]
00813   //         
00814 
00815   const size_type
00816     n_R = Q_R_->cols(),
00817     m   = A() ? A()->cols()  : 0;
00818 
00819   if( M_trans == BLAS_Cpp::no_trans ) {
00820     //
00821     // y =  b*y + a * U_hat * x
00822     // 
00823     //   = b*y + a * [  Q_R' * G * P_XF_hat + Q_R' * A_bar * P_plus_hat ] * x
00824     //               [                  A' * P_XF_hat                   ]
00825     // 
00826     //  =>
00827     // 
00828     // y1 = b * y1 + a * Q_R' * G * P_XF_hat * x + a * Q_R' * A_bar * P_plus_hat * x
00829     // y2 = b * y2 + a * A' * P_XF_hat * x
00830     // 
00831     DVectorSlice
00832       y1 = (*y)(1,n_R),
00833       y2 = m ? (*y)(n_R+1,n_R+m) : DVectorSlice();
00834     SpVector
00835       P_XF_hat_x,
00836       P_plus_hat_x;
00837     // P_XF_hat_x = P_XF_hat * x
00838     if( P_XF_hat().nz() )
00839       V_MtV( &P_XF_hat_x, P_XF_hat(), no_trans, x );
00840     // P_plus_hat_x = P_plus_hat * x
00841     if(P_plus_hat().nz())
00842       V_MtV( &P_plus_hat_x, P_plus_hat(), no_trans, x );
00843     // y1 = b * y1
00844     if(b==0.0)      y1=0.0;
00845     else if(b!=1.0) Vt_S(&y1,b);
00846     // y1 += a * Q_R' * G * P_XF_hat_x
00847     if(P_XF_hat().nz())
00848       Vp_StPtMtV( &y1, a, Q_R(), trans, G(), no_trans, P_XF_hat_x() );
00849     // y1 += a * Q_R' * A_bar * P_plus_hat_x
00850     if(P_plus_hat().nz())
00851       Vp_StPtMtV( &y1, a, Q_R(), trans, A_bar(), no_trans, P_plus_hat_x() );
00852     if(m) {
00853       // y2 = b * y2
00854       if(b==0.0)      y2=0.0;
00855       else if(b!=1.0) Vt_S(&y2,b);
00856       // y2 +=  a * A' * P_XF_hat_x
00857       if( P_XF_hat().nz() )
00858         Vp_StMtV( &y2, a, *A(), trans, P_XF_hat_x() );
00859     }
00860   }
00861   else if( M_trans == BLAS_Cpp::trans ) {
00862     //
00863     // y =  b*y + a * U_hat' * x
00864     // 
00865     //   = b*y + a * [  P_XF_hat' * G * Q_R + P_plus_hat' * A_bar' * Q_R, P_XF_hat' * A ] * [ x1 ]
00866     //                                                                                      [ x2 ]
00867     //  =>
00868     // 
00869     // y = b * y + a * P_XF_hat' * G * Q_R * x1 + a * P_plus_hat' * A_bar' * Q_R * x1
00870     //     + a * P_XF_hat' * A * x2
00871     // 
00872     const DVectorSlice
00873       x1 = x(1,n_R),
00874       x2 = m ? x(n_R+1,n_R+m) : DVectorSlice();
00875     SpVector
00876       Q_R_x1;
00877     // Q_R_x1 = Q_R * x1
00878     V_MtV( &Q_R_x1, Q_R(), no_trans, x1 );
00879     // y = b*y
00880     if(b==0.0)      *y = 0.0;
00881     else if(b!=1.0) Vt_S( y, b );
00882     // y += a * P_XF_hat' * G * Q_R_x1
00883     if(P_XF_hat().nz())
00884       Vp_StPtMtV( y, a, P_XF_hat(), trans, G(), no_trans, Q_R_x1() );
00885     // y += a * P_plus_hat' * A_bar' * Q_R_x1
00886     if(P_plus_hat().nz())
00887       Vp_StPtMtV( y, a, P_plus_hat(), trans, A_bar(), trans, Q_R_x1() );
00888     // y += a * P_XF_hat' * A * x2
00889     if( m && P_XF_hat().nz() )
00890       Vp_StPtMtV( y, a, P_XF_hat(), trans, *A(), no_trans, x2 );
00891   }
00892   else {
00893     TEST_FOR_EXCEPT(true);  // Invalid value for M_trans
00894   }
00895 }
00896 
00897 void QPSchur::U_hat_t::Vp_StMtV(
00898   DVectorSlice* y, value_type a, BLAS_Cpp::Transp M_trans
00899   ,const SpVectorSlice& x, value_type b
00900   ) const
00901 {
00902 //  // Uncomment to use the default version
00903 //  MatrixOp::Vp_StMtV(y,a,M_trans,x,b); return;
00904 
00905   using BLAS_Cpp::no_trans;
00906   using BLAS_Cpp::trans;
00907   using DenseLinAlgPack::Vt_S;
00908   using LinAlgOpPack::V_MtV;
00909   using AbstractLinAlgPack::V_MtV;
00910   using AbstractLinAlgPack::Vp_StMtV;
00911   using LinAlgOpPack::V_MtV;
00912   using LinAlgOpPack::Vp_StMtV;
00913   using LinAlgOpPack::Vp_StPtMtV;
00914   using Teuchos::Workspace;
00915   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
00916 
00917   LinAlgOpPack::Vp_MtV_assert_sizes(y->dim(),rows(),cols(),M_trans,x.dim());
00918 
00919   //
00920   // U_hat = [  Q_R' * G * P_XF_hat + Q_R' * A_bar * P_plus_hat ]
00921   //         [                  A' * P_XF_hat                   ]
00922   //         
00923 
00924   const size_type
00925     n_R = Q_R_->cols(),
00926     m   = A() ? A()->cols()  : 0;
00927 
00928   if( M_trans == BLAS_Cpp::no_trans ) {
00929     //
00930     // y =  b*y + a * U_hat * x
00931     // 
00932     //   = b*y + a * [  Q_R' * G * P_XF_hat + Q_R' * A_bar * P_plus_hat ] * x
00933     //               [                  A' * P_XF_hat                   ]
00934     // 
00935     //  =>
00936     // 
00937     // y1 = b * y1 + a * Q_R' * G * P_XF_hat * x + a * Q_R' * A_bar * P_plus_hat * x
00938     // y2 = b * y2 + a * A' * P_XF_hat * x
00939     // 
00940     DVectorSlice
00941       y1 = (*y)(1,n_R),
00942       y2 = m ? (*y)(n_R+1,n_R+m) : DVectorSlice();
00943     SpVector
00944       P_XF_hat_x,
00945       P_plus_hat_x;
00946     // P_XF_hat_x = P_XF_hat * x
00947     if( P_XF_hat().nz() )
00948       V_MtV( &P_XF_hat_x, P_XF_hat(), no_trans, x );
00949     // P_plus_hat_x = P_plus_hat * x
00950     if(P_plus_hat().nz())
00951       V_MtV( &P_plus_hat_x, P_plus_hat(), no_trans, x );
00952     // y1 = b * y1
00953     if(b==0.0)      y1=0.0;
00954     else if(b!=1.0) Vt_S(&y1,b);
00955     // y1 += a * Q_R' * G * P_XF_hat_x
00956     if(P_XF_hat().nz())
00957       Vp_StPtMtV( &y1, a, Q_R(), trans, G(), no_trans, P_XF_hat_x() );
00958     // y1 += a * Q_R' * A_bar * P_plus_hat_x
00959     if(P_plus_hat().nz())
00960       Vp_StPtMtV( &y1, a, Q_R(), trans, A_bar(), no_trans, P_plus_hat_x() );
00961     if(m) {
00962       // y2 = b * y2
00963       if(b==0.0)      y2=0.0;
00964       else if(b!=1.0) Vt_S(&y2,b);
00965       // y2 += a * A' * P_XF_hat_x
00966       if(P_XF_hat().nz())
00967         Vp_StMtV( &y2, a, *A(), trans, P_XF_hat_x() );
00968     }
00969   }
00970   else if( M_trans == BLAS_Cpp::trans ) {
00971     //
00972     // y =  b*y + a * U_hat' * x
00973     // 
00974     //   = b*y + a * [  P_XF_hat' * G * Q_R + P_plus_hat' * A_bar' * Q_R, P_XF_hat' * A ] * [ x1 ]
00975     //                                                                                      [ x2 ]
00976     //  =>
00977     // 
00978     // y = b * y + a * P_XF_hat' * G * Q_R * x1 + a * P_plus_hat' * A_bar' * Q_R * x1
00979     //     + a * P_XF_hat' * A * x2
00980     // 
00981     const SpVectorSlice
00982       x1 = x(1,n_R),
00983       x2 = m ? x(n_R+1,n_R+m) : SpVectorSlice(NULL,0,0,0);
00984     SpVector
00985       Q_R_x1;
00986     // Q_R_x1 = Q_R * x1
00987     V_MtV( &Q_R_x1, Q_R(), no_trans, x1 );
00988     // y = b*y
00989     if(b ==0.0)     *y = 0.0;
00990     else if(b!=1.0) Vt_S( y, b );
00991     // y += a * P_XF_hat' * G * Q_R_x1
00992     if(P_XF_hat().nz())
00993       Vp_StPtMtV( y, a, P_XF_hat(), trans, G(), no_trans, Q_R_x1() );
00994     // y += a * P_plus_hat' * A_bar' * Q_R_x1
00995     if(P_plus_hat().nz())
00996       Vp_StPtMtV( y, a, P_plus_hat(), trans, A_bar(), trans, Q_R_x1() );
00997     // y += a * P_XF_hat' * A * x2
00998     if( m && P_XF_hat().nz() )
00999       Vp_StPtMtV( y, a, P_XF_hat(), trans, *A(), no_trans, x2 );
01000   }
01001   else {
01002     TEST_FOR_EXCEPT(true);  // Invalid value for M_trans
01003   }
01004 }
01005 
01006 // public member functions for QPSchur::ActiveSet
01007 
01008 QPSchur::ActiveSet::ActiveSet(
01009   const schur_comp_ptr_t& schur_comp
01010   ,MSADU::PivotTolerances   pivot_tols
01011   )
01012   :schur_comp_(schur_comp)
01013   ,pivot_tols_(pivot_tols)
01014   ,initialized_(false)
01015   ,test_(false)
01016   ,qp_(NULL)
01017   ,x_init_(NULL)
01018   ,n_(0)
01019   ,n_R_(0)
01020   ,m_(0)
01021   ,q_plus_hat_(0)
01022   ,q_F_hat_(0)
01023   ,q_C_hat_(0)
01024 {}
01025 
01026 void QPSchur::ActiveSet::initialize(
01027   QP& qp, size_type num_act_change, const int ij_act_change[]
01028   ,const EBounds bnds[], bool test,  bool salvage_init_schur_comp
01029   ,std::ostream *out, EOutputLevel output_level )
01030 {
01031   using LinAlgOpPack::V_mV;
01032   using LinAlgOpPack::V_MtV;
01033   using LinAlgOpPack::V_InvMtV;
01034   using LinAlgOpPack::Vp_StPtMtV;
01035   using AbstractLinAlgPack::V_MtV;
01036   using AbstractLinAlgPack::Mp_StPtMtP;
01037   using AbstractLinAlgPack::M_StMtInvMtM;
01038   using DenseLinAlgPack::sym;
01039   typedef MatrixSymAddDelUpdateable MSADU;
01040   namespace GPMSTP = AbstractLinAlgPack::GenPermMatrixSliceIteratorPack;
01041   using Teuchos::Workspace;
01042   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
01043   
01044   const size_type
01045     n   = qp.n(),
01046     n_R   = qp.n_R(),
01047     n_X   = n - n_R,
01048     m   = qp.m();
01049   const QP::x_init_t
01050     &x_init = qp.x_init();
01051   const QP::l_x_X_map_t
01052     &l_x_X_map = qp.l_x_X_map();
01053   const QP::i_x_X_map_t
01054     &i_x_X_map = qp.i_x_X_map();
01055   const DVectorSlice
01056     b_X = qp.b_X();
01057   const DVectorSlice
01058     g = qp.g();
01059   const MatrixSymOp
01060     &G = qp.G();
01061   const QP::Constraints
01062     &constraints = qp.constraints();
01063   const size_type
01064     m_breve = constraints.m_breve();
01065 
01066   try {
01067 
01068   // Count the number of each type of change
01069   size_type 
01070     q_plus_hat    = 0,
01071     q_F_hat     = 0,
01072     q_C_hat     = 0;
01073   if( num_act_change ) {
01074     for( size_type k = 1; k <= num_act_change; ++k ) {
01075       const int ij = ij_act_change[k-1];
01076       const EBounds bnd = bnds[k-1];
01077       if( ij < 0 ) {
01078         // Initially fixed variable being freed.
01079         if( x_init(-ij) == FREE ) {
01080           std::ostringstream omsg;
01081           omsg
01082             << "QPSchur::ActiveSet::initialize(...) : Error, "
01083             << "The variable x(" << -ij << ") is not initially fixed and can not "
01084             << "be freed by ij_act_change[" << k-1 << "]\n";
01085           throw std::invalid_argument( omsg.str() );
01086         }
01087         if( x_init(-ij) == EQUALITY ) {
01088           std::ostringstream omsg;
01089           omsg
01090             << "QPSchur::ActiveSet::initialize(...) : Error, "
01091             << "The variable x(" << -ij << ") is equality fixed and therefore can not "
01092             << "be freed by ij_act_change[" << k-1 << "]\n";
01093           throw std::invalid_argument( omsg.str() );
01094         }
01095         ++q_F_hat;
01096       }
01097       else {
01098         // Constraint being added to the active-set
01099         if( ij <= n ) {
01100           // Fixing a variable to a bound
01101           EBounds x_init_bnd = x_init(ij);
01102           if( x_init_bnd == FREE ) {
01103             // initially free variable being fixed
01104             ++q_plus_hat;
01105           }
01106           else if ( x_init_bnd == EQUALITY ) {
01107             // ToDo: Throw exception
01108             TEST_FOR_EXCEPT(true);
01109           }
01110           else if( x_init_bnd == bnd ) {
01111             // ToDo: Throw exception
01112             TEST_FOR_EXCEPT(true);
01113           }
01114           else {
01115             // Initially fixed variable being fixed to another bound
01116             ++q_F_hat;  // We must free the variable first
01117             ++q_C_hat;  // Now we fix it to a different bound.
01118           }
01119         }
01120         else {
01121           // Adding a general inequality (or equality) constraint
01122           if( ij > n + m_breve ) {
01123             // ToDo: Throw exception
01124             TEST_FOR_EXCEPT(true);
01125           }   
01126           ++q_plus_hat;
01127         }
01128       }
01129     } 
01130   }
01131 
01132   const size_type
01133     q_D_hat = (n - n_R) - q_F_hat,
01134     q_D_hat_max = n_X;
01135 
01136   // Now let's set stuff up: ij_map, constr_norm, bnds and part of d_hat
01137   const size_type
01138     q_hat = q_plus_hat + q_F_hat + q_C_hat,
01139     q_hat_max = n_X + n,  // If all the initially fixed variables where freed
01140                 // Then all the degrees of freedom where used up with other constraints.
01141     q_F_hat_max = n_X,
01142     q_plus_hat_max = n;
01143     
01144   ij_map_.resize(q_hat_max);
01145   constr_norm_.resize(q_hat_max);
01146   bnds_.resize(q_hat_max);
01147   d_hat_.resize(q_hat_max); // set the terms involving the bounds first.
01148 
01149   if( num_act_change ) {
01150     size_type s = 0;
01151     for( size_type k = 1; k <= num_act_change; ++k ) {
01152       const int ij = ij_act_change[k-1];
01153       const EBounds bnd = bnds[k-1];
01154       if( ij < 0 ) {
01155         // Initially fixed variable being freed.
01156         ij_map_[s]    = ij;
01157         constr_norm_[s] = 1.0;
01158         bnds_[s]    = FREE;
01159         d_hat_[s]   = - g(-ij);   // - g^X_{l^{(-)}}
01160         ++s;
01161       }
01162       else {
01163         // Constraint being added to the active-set
01164         if( ij <= n ) {
01165           // Fixing a variable to a bound
01166           EBounds x_init_bnd = x_init(ij);
01167           if( x_init_bnd == FREE ) {
01168             // initially free variable being fixed
01169             ij_map_[s]    = ij;
01170             constr_norm_[s] = 1.0;
01171             bnds_[s]    = bnd;
01172             d_hat_[s]   = constraints.get_bnd(ij,bnd);
01173             ++s;
01174           }
01175           else {
01176             // Initially fixed variable being fixed to another bound
01177             // Free the variable first
01178             ij_map_[s]    = ij;
01179             constr_norm_[s] = 1.0;
01180             bnds_[s]    = FREE;
01181             d_hat_[s]   = - g(ij);    // - g^X_{l^{(-)}}
01182             ++s;
01183             // Now fix to a different bound
01184             ij_map_[s]    = ij;
01185             constr_norm_[s] = 1.0;
01186             bnds_[s]    = bnd;
01187             d_hat_[s]   = constraints.get_bnd(ij,bnd) - b_X(l_x_X_map(ij));
01188             ++s;
01189           }
01190         }
01191         else {
01192           // Adding a general inequality (or equality) constraint
01193           ij_map_[s]    = ij;
01194           constr_norm_[s] = 1.0;  // ToDo: We need to compute this in an efficient way!
01195           bnds_[s]    = bnd;
01196           d_hat_[s]   = constraints.get_bnd(ij,bnd);  // \bar{b}_{j^{(+)}}
01197           ++s;
01198         }
01199       }
01200     }
01201     TEST_FOR_EXCEPT( !( s == q_hat ) );
01202   }
01203 
01204   // Setup P_XF_hat_ and P_plus_hat_
01205   P_XF_hat_row_.resize(q_F_hat_max);
01206   P_XF_hat_col_.resize(q_F_hat_max);
01207   P_FC_hat_row_.resize(q_F_hat_max);
01208   P_FC_hat_col_.resize(q_F_hat_max);
01209   P_plus_hat_row_.resize(q_plus_hat_max);
01210   P_plus_hat_col_.resize(q_plus_hat_max);
01211   if(q_hat) {
01212     // See ConstrainedOptPack_QPSchur.hpp for description of P_XF_hat and P_plus_hat
01213     size_type
01214       k_XF_hat = 0, // zero based
01215       k_plus_hat = 0; // zero based
01216     ij_map_t::const_iterator
01217       ij_itr    = ij_map_.begin(),
01218       ij_itr_end  = ij_itr + q_hat;
01219     for( size_type s = 1; ij_itr != ij_itr_end; ++ij_itr, ++s ) {
01220       const int ij = *ij_itr;
01221       EBounds x_init_ij;
01222       if( ij < 0 ) {
01223         const size_type i = -ij;
01224         TEST_FOR_EXCEPT( !(  i <= n  ) );
01225         // [P_XF_hat](:,s) = e(i)
01226         P_XF_hat_row_[k_XF_hat] = i;
01227         P_XF_hat_col_[k_XF_hat] = s;
01228         ++k_XF_hat;
01229       }
01230       else if( !(ij <= n && (x_init_ij = x_init(ij)) != FREE ) ) {
01231         const size_type j = ij;
01232         TEST_FOR_EXCEPT( !(  0 < j && j <= n + m_breve  ) );
01233         // [P_plus_hat](:,s) = e(j)
01234         P_plus_hat_row_[k_plus_hat] = j;
01235         P_plus_hat_col_[k_plus_hat] = s;
01236         ++k_plus_hat;
01237       }
01238     }
01239     TEST_FOR_EXCEPT( !(  k_XF_hat == q_F_hat  ) );
01240     TEST_FOR_EXCEPT( !(  k_plus_hat == q_plus_hat  ) );
01241   }
01242   P_XF_hat_.initialize_and_sort(
01243     n,q_hat,q_F_hat,0,0,GPMSTP::BY_ROW
01244     ,q_F_hat ? &P_XF_hat_row_[0] : NULL
01245     ,q_F_hat ? &P_XF_hat_col_[0] : NULL
01246     ,test
01247     );
01248   P_plus_hat_.initialize_and_sort(
01249     n+m_breve,q_hat,q_plus_hat,0,0,GPMSTP::BY_ROW
01250     ,q_plus_hat ? &P_plus_hat_row_[0] : NULL
01251     ,q_plus_hat ? &P_plus_hat_col_[0] : NULL
01252     ,test
01253     );
01254   
01255   // Setup P_FC_hat_
01256   if( q_C_hat ) {
01257     throw std::logic_error(
01258       error_msg(__FILE__,__LINE__,"QPSchur::ActiveSet::initialize(...) : "
01259             "Error, q_C_hat != 0, now supported yet!"));  
01260     // ToDo: We should implement this but it is unlikely to be needed
01261   }
01262   P_FC_hat_.initialize_and_sort(
01263     q_hat,q_hat,q_C_hat,0,0,GPMSTP::BY_ROW
01264     ,q_C_hat ? &P_FC_hat_row_[0] : NULL
01265     ,q_C_hat ? &P_FC_hat_col_[0] : NULL
01266     ,test
01267     );
01268 
01269   // Setup Q_XD_hat_
01270   Q_XD_hat_row_.resize(q_D_hat_max);
01271   Q_XD_hat_col_.resize(q_D_hat_max);
01272   if(q_D_hat) {
01273     // See ConstrainedOptPack_QPSchur.hpp for description of Q_XD_hat
01274     size_type
01275       k_XD_hat = 0; // zero based
01276     GenPermMatrixSlice::const_iterator
01277       Q_X_itr = qp.Q_X().begin(); // This is sorted by row already!
01278     P_row_t::const_iterator
01279       XF_search     = P_XF_hat_row_.begin(),  // These are already sorted by row!
01280       XF_search_end   = XF_search + q_F_hat;
01281     for( size_type l = 1; l <= n_X; ++l, ++Q_X_itr ) {
01282       const size_type i = Q_X_itr->row_i(); // Already sorted by row
01283       // Look for i in XF
01284       for( ; XF_search != XF_search_end && *XF_search < i; ++XF_search ) ;
01285       if( XF_search == XF_search_end || (XF_search < XF_search_end && *XF_search > i) ) {
01286         // We went right past i and did not find it so
01287         // this variable has not been freed so lets add it!
01288         Q_XD_hat_row_[k_XD_hat] = i;
01289         Q_XD_hat_col_[k_XD_hat] = k_XD_hat + 1;
01290         ++k_XD_hat;
01291       }
01292     }
01293     TEST_FOR_EXCEPT( !(  k_XD_hat == q_D_hat  ) );
01294   }
01295   Q_XD_hat_.initialize(
01296       n,q_D_hat,q_D_hat,0,0,GPMSTP::BY_ROW  // Should already be sorted by row!
01297     , q_D_hat ? &Q_XD_hat_row_[0] : NULL
01298     , q_D_hat ? &Q_XD_hat_col_[0] : NULL
01299     ,test
01300     );
01301 
01302   // Setup l_fxfx
01303   l_fxfx_.resize(q_D_hat_max);
01304   if(q_D_hat) {
01305     for( size_type k = 0; k < q_D_hat; ++k ) {
01306       l_fxfx_[k] = l_x_X_map(Q_XD_hat_row_[k]);
01307       TEST_FOR_EXCEPT( !(  l_fxfx_[k] != 0  ) );
01308     }
01309   }
01310 
01311   // Set the rest of the terms in d_hat involving matrices
01312   //
01313   // d_hat += - P_XF_hat' * G * b_XX - P_plus_hat' * A_bar' * b_XX
01314   // 
01315   // where: b_XX = Q_X * b_X
01316   // 
01317   if( q_hat ) {
01318     SpVector b_XX;
01319     V_MtV( &b_XX, qp.Q_X(), BLAS_Cpp::no_trans, b_X );
01320     Vp_StPtMtV( &d_hat_(1,q_hat), -1.0, P_XF_hat_, BLAS_Cpp::trans
01321       , G, BLAS_Cpp::no_trans, b_XX() );
01322     Vp_StPtMtV( &d_hat_(1,q_hat), -1.0, P_plus_hat_, BLAS_Cpp::trans
01323       , constraints.A_bar(), BLAS_Cpp::trans, b_XX() );
01324   }
01325 
01326   // Setup U_hat
01327   U_hat_.initialize( &G, m ? &qp.A() : NULL, &constraints.A_bar()
01328     , &qp.Q_R(), &P_XF_hat_, &P_plus_hat_ );
01329 
01330   // Set the rest of the members
01331   test_   = test;
01332   qp_     = &qp;
01333   x_init_   = &x_init;
01334   n_      = n;
01335   n_R_    = n_R;
01336   m_      = m;
01337   m_breve_  = m_breve;
01338   q_plus_hat_ = q_plus_hat;
01339   q_F_hat_  = q_F_hat;
01340   q_C_hat_  = q_C_hat;
01341 
01342   // Resize storage for z_hat, p_z_hat, mu_D_hat, and p_mu_D_hat and set to zero by default
01343   z_hat_.resize(q_hat_max);
01344   p_z_hat_.resize(q_hat_max);
01345   mu_D_hat_.resize(n_X);
01346   p_mu_D_hat_.resize(n_X);
01347 
01348   initialized_ = true;  // set to true tenatively so that we can
01349               // print this stuff.
01350 
01351   if( out && (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
01352     *out
01353       << "\nPrint definition of Active-Set before the Schur complement is formed...\n";
01354     dump_act_set_quantities( *this, *out, false );
01355   }
01356 
01357   // Initialize and factorize the schur complement
01358   if( q_hat ) {
01359     // Temporary storage for S (dense)
01360     DMatrix S_store(q_hat+1,q_hat+1);
01361     DMatrixSliceSym S_sym( S_store(2,q_hat+1,1,q_hat), BLAS_Cpp::lower );
01362     MatrixSymPosDefCholFactor S(&S_store());
01363     // S = -1.0 * U_hat' * inv(Ko) * U_hat
01364     M_StMtInvMtM( &S, -1.0, U_hat_, BLAS_Cpp::trans, qp.Ko()
01365       , MatrixSymNonsing::DUMMY_ARG );
01366     // Now add parts of V_hat
01367     if( q_F_hat ) {
01368       // S += P_XF_hat' * G * P_XF_hat
01369       Mp_StPtMtP( &S, 1.0, MatrixSymOp::DUMMY_ARG, qp_->G(), P_XF_hat_, BLAS_Cpp::no_trans );
01370     }
01371     if( q_F_hat && q_plus_hat ) {
01372       // S += P_XF_hat' * A_bar * P_plus_hat + P_plus_hat' * A_bar' * P_XF_hat
01373       AbstractLinAlgPack::syr2k(
01374         qp_->constraints().A_bar()
01375         ,BLAS_Cpp::no_trans, 1.0
01376         ,P_XF_hat_, BLAS_Cpp::no_trans
01377         ,P_plus_hat_, BLAS_Cpp::no_trans
01378         ,1.0, &S );
01379     }
01380     if( q_F_hat && q_C_hat ) {
01381       // S += P_FC_hat + P_FC_hat'
01382       throw std::logic_error(
01383         error_msg(__FILE__,__LINE__,"QPSchur::ActiveSet::initialize(...) : "
01384               "Error, q_C_hat != 0, now supported yet!"));  
01385       // ToDo: We should implement this but it is unlikely to be needed
01386     }
01387 
01388     if( out && (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
01389       *out
01390         << "\nIninitial Schur Complement before it is nonsingular:\n"
01391         << "\nS_hat =\nLower triangular part (ignore nonzeros above diagonal)\n"
01392         << S_store();
01393     }
01394     // Initialize and factorize the schur complement!
01395     try {
01396       schur_comp().update_interface().initialize(
01397         S_sym,q_hat_max,true,MSADU::Inertia( q_plus_hat + q_C_hat, 0, q_F_hat )
01398         ,pivot_tols() );
01399     }
01400     catch(const MSADU::WarnNearSingularUpdateException& excpt) {
01401       if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01402         *out
01403           << "\nActiveSet::initialize(...) : " << excpt.what()
01404           << std::endl;
01405       }
01406     }
01407     catch(const MSADU::SingularUpdateException& excpt) {
01408       if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01409         *out
01410         << "\nActiveSet::initialize(...) : " << excpt.what()
01411         << std::endl;
01412       }
01413       if(salvage_init_schur_comp) {
01414         if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01415           *out
01416             << "\nsalvage_init_schur_comp == true\n"
01417             << "We will attempt to add as many rows/cols of the "
01418             << "initial Schur complement as possible ...\n";
01419         }
01420         // ToDo: We will build the schur complement one row/col at a time
01421         // skipping those updates that cause it to become singular.  For each
01422         // update that causes the schur compplement to become singular we
01423         // will remove the corresponding change.
01424 
01425         throw; // For now just rethrow the exception!
01426       }
01427       else {
01428         if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01429           *out
01430             << "\nsalvage_init_schur_comp == false\n"
01431             << "We will just throw this singularity exception out of here ...\n";
01432         }
01433         throw;
01434       }
01435     }
01436     if( out && (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
01437       *out
01438         << "\nSchur Complement after factorization:\n"
01439         << "\nS_hat =\n"
01440          << schur_comp().op_interface();
01441     }
01442   }
01443   else {
01444     schur_comp().update_interface().set_uninitialized();
01445   }
01446 
01447   // Success, we are initialized!
01448   initialized_ = true;
01449   return;
01450 
01451   } // end try
01452   catch(...) {
01453     initialized_ = false;
01454     throw;
01455   }
01456 }
01457 
01458 void QPSchur::ActiveSet::refactorize_schur_comp()
01459 {
01460   // ToDo: Finish Me
01461   TEST_FOR_EXCEPT(true);
01462 }
01463 
01464 bool QPSchur::ActiveSet::add_constraint(
01465   size_type ja, EBounds bnd_ja, bool update_steps
01466   ,std::ostream *out, EOutputLevel output_level
01467   ,bool force_refactorization
01468   ,bool allow_any_cond
01469   )
01470 {
01471   using BLAS_Cpp::no_trans;
01472   using BLAS_Cpp::trans;
01473   using DenseLinAlgPack::dot;
01474   using AbstractLinAlgPack::dot;
01475   using LinAlgOpPack::V_StMtV;
01476   using LinAlgOpPack::Vp_StPtMtV;
01477   using LinAlgOpPack::V_InvMtV;
01478   using Teuchos::Workspace;
01479   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
01480   
01481   typedef AbstractLinAlgPack::EtaVector eta_t;
01482 
01483   assert_initialized();
01484 
01485   bool wrote_output = false;
01486 
01487   const QPSchurPack::QP::Constraints
01488     &constraints = qp_->constraints();
01489 
01490   if( is_init_fixed(ja) && (*x_init_)(ja) == bnd_ja ) {
01491     //
01492     // This is a variable that was initially fixed, then freed and now
01493     // is being fixed back to its original bound.
01494     //
01495     // Here we will shrink the augmented KKT system.
01496     //
01497     const size_type q_hat = this->q_hat();
01498     const size_type sd = s_map(-int(ja));
01499     const size_type la = qp_->l_x_X_map()(ja);
01500     TEST_FOR_EXCEPT( !( sd ) );
01501     TEST_FOR_EXCEPT( !( la ) );
01502     wrote_output = remove_augmented_element(
01503       sd,force_refactorization
01504       ,MatrixSymAddDelUpdateable::EIGEN_VAL_POS
01505       ,out,output_level,allow_any_cond);
01506     // We must remove (ja,sd) from P_XF_hat
01507     P_row_t::iterator
01508       itr = std::lower_bound( P_XF_hat_row_.begin(), P_XF_hat_row_.begin()+q_F_hat_, ja );
01509     TEST_FOR_EXCEPT( !(  itr != P_XF_hat_row_.end()  ) );
01510     const size_type p = itr - P_XF_hat_row_.begin();
01511     std::copy( P_XF_hat_row_.begin() + p + 1, P_XF_hat_row_.begin()+q_F_hat_,
01512       P_XF_hat_row_.begin() + p );
01513     std::copy( P_XF_hat_col_.begin() + p + 1, P_XF_hat_col_.begin()+q_F_hat_,
01514       P_XF_hat_col_.begin() + p );
01515     // Deincrement all counters in permutation matrices for removed element
01516     if( q_F_hat_ > 1 )
01517       deincrement_indices( sd, &P_XF_hat_col_, q_F_hat_-1 );
01518     if( q_C_hat_ > 0 )
01519       deincrement_indices( sd, &P_FC_hat_col_, q_C_hat_ );
01520     if( q_plus_hat_ > 0 )
01521       deincrement_indices( sd, &P_plus_hat_col_, q_plus_hat_ );
01522     //
01523     // Add the multiplier for mu_D_hat(...)
01524     //
01525     const size_type q_D_hat = this->q_D_hat();
01526     // add l_fxfx(q_D_hat+1) = la
01527     l_fxfx_[q_D_hat] = la;
01528     // add mu_D_hat(q_D_hat+1) = 0.0
01529     mu_D_hat_[q_D_hat] = 0.0;
01530     // add p_mu_D_hat(q_D_hat+1) = 1.0
01531     if(update_steps)
01532       p_mu_D_hat_[q_D_hat] = 1.0;
01533     // Insert the pair (ja,q_D_hat+1) into Q_XD_hat(...) (sorted by row)
01534     insert_pair_sorted(ja,q_D_hat+1,q_D_hat+1,&Q_XD_hat_row_,&Q_XD_hat_col_);
01535     //
01536     // Update the counts
01537     //
01538     --q_F_hat_;
01539   }
01540   else {
01541     //
01542     // Expand the size of the schur complement to add the new constraint
01543     //
01544      
01545     // Compute the terms for the update
01546     
01547     value_type      d_p = 0.0;
01548     const size_type   q_hat = this->q_hat();
01549     Workspace<value_type> t_hat_ws(wss,q_hat);
01550     DVectorSlice t_hat(t_hat_ws.size()?&t_hat_ws[0]:NULL,t_hat_ws.size());
01551     value_type      alpha_hat = 0.0;
01552     bool        changed_bounds = false;
01553     size_type           sd = 0; // Only used if changed_bounds == true
01554         
01555     if( ja <= n_ && !is_init_fixed(ja) ) {
01556       //
01557       // Fix an initially free variable is being fixed
01558       // 
01559       // u_p = [ Q_R' * e(ja) ] <: R^(n_R+m)
01560       //       [      0       ]
01561       //
01562       const size_type
01563         la = qp_->Q_R().lookup_col_j(ja);
01564       TEST_FOR_EXCEPT( !(  la  ) );
01565       const eta_t u_p = eta_t(la,n_R_+m_);
01566       // r = inv(Ko)*u_p
01567       DVector r;  // ToDo: Make this sparse!
01568       V_InvMtV( &r, qp_->Ko(), no_trans, u_p() );
01569       // t_hat = - U_hat' * r
01570       if(q_hat)
01571         V_StMtV( &t_hat(), -1.0, U_hat_, trans, r() );
01572       // alpha_hat = - u_p ' * r
01573       alpha_hat = - dot( u_p(), r() );
01574       // d_p = \bar{b}_{j^{(+)}}
01575       d_p = constraints.get_bnd( ja, bnd_ja );
01576 
01577       changed_bounds = false;
01578     }
01579     else if ( is_init_fixed(ja) ) {
01580       //
01581       // An intially fixed variable was freed and
01582       // is now being fixed to the other bound.
01583       //
01584       // Here we must expand the augmented KKT system for this
01585       // simple change.
01586       //
01587       // u_p = 0
01588       //
01589       // v_p = e(sd) <: R^(q_hat), where sd = s_map(-ja)
01590       //
01591       sd = s_map(-int(ja));
01592       TEST_FOR_EXCEPT( !( sd ) );
01593       const size_type la = qp_->l_x_X_map()(ja);
01594       TEST_FOR_EXCEPT( !( la ) );
01595       // t_hat = e(sd)
01596       t_hat = 0.0;
01597       t_hat(sd) = 1.0;
01598       // alpha_hat = 0.0
01599       alpha_hat = 0.0;
01600       // d_p = \bar{b}_{j^{(+)}} - b_X(la)
01601       d_p = constraints.get_bnd( ja, bnd_ja ) - qp_->b_X()(la);
01602 
01603       changed_bounds = true;
01604     }
01605     else {  // ja > n
01606       //
01607       // Add an extra equality or inequality constraint.
01608       //
01609       // u_p = [ Q_R' * A_bar * e(ja) ] n_R
01610       //       [        0             ] m
01611       const eta_t e_ja = eta_t(ja,n_+m_breve_);
01612       const MatrixOp &A_bar = constraints.A_bar();
01613       DVector u_p( n_R_ + m_ ); // ToDo: make this sparse
01614       Vp_StPtMtV( &u_p(1,n_R_), 1.0, qp_->Q_R(), trans, A_bar, no_trans, e_ja(), 0.0 );
01615       if( m_ )
01616         u_p(n_R_+1,n_R_+m_) = 0.0;
01617       // r = inv(Ko) * u_p
01618       DVector r;  // ToDo: Make this sparse!
01619       V_InvMtV( &r, qp_->Ko(), no_trans, u_p() );
01620       if(q_hat) {
01621         // t_hat = v_p - U_hat' * r
01622         //    where: v_p = P_XF_hat' * A_bar * e(ja)
01623         V_StMtV( &t_hat(), -1.0, U_hat_, trans, r() );
01624         Vp_StPtMtV( &t_hat(), 1.0, P_XF_hat_, trans, A_bar, no_trans, e_ja() );
01625       }
01626       // alpha_hat = - u_p ' * r
01627       alpha_hat = - dot( u_p(), r() );
01628       // d_p = \bar{b}_{j^{(+)}} - b_X' * Q_X' * A_bar * e(ja)
01629       // 
01630       // d_p = \bar{b}_{j^{(+)}}
01631       d_p = constraints.get_bnd( ja, bnd_ja );
01632       if(n_ > n_R_) {
01633         // d_p += - b_X' * Q_X' * A_bar * e(ja)
01634         r.resize( n_ - n_R_ );  // reuse storage
01635         Vp_StPtMtV( &r(), 1.0, qp_->Q_X(), trans, A_bar, no_trans, e_ja(), 0.0 );     
01636         d_p += - dot( qp_->b_X(), r() );
01637       }
01638       
01639       changed_bounds = false;
01640     }
01641     
01642     // Update the schur complement if nonsingular.  These
01643     // with throw exceptions if the matrix is singular
01644     // or has the wrong inertia
01645     if(q_hat) {
01646       try {
01647         schur_comp().update_interface().augment_update(
01648           &t_hat(), alpha_hat, force_refactorization
01649           ,MatrixSymAddDelUpdateable::EIGEN_VAL_NEG
01650           ,MSADU::PivotTolerances(
01651             pivot_tols().warning_tol
01652             ,allow_any_cond ? 0.0 : pivot_tols().singular_tol
01653             ,allow_any_cond ? 0.0 : pivot_tols().wrong_inertia_tol ) );
01654       }
01655       catch(const MSADU::WarnNearSingularUpdateException& excpt) {
01656         if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01657           *out
01658             << "\nActiveSet::add_constraint(...) : " << excpt.what()
01659             << std::endl;
01660           wrote_output = true;
01661         }
01662       }
01663     }
01664     else {
01665       schur_comp().update_interface().initialize(
01666         alpha_hat, (n_-n_R_) + n_-m_ );
01667     }
01668     // Update the rest of the augmented KKT system
01669     if( changed_bounds )
01670       ++q_C_hat_;
01671     else
01672       ++q_plus_hat_;
01673     const size_type q_hat_new = q_F_hat_ + q_C_hat_ + q_plus_hat_;
01674     // Add ij_map(q_hat) = ja to ij_map(...)
01675     ij_map_[q_hat_new - 1]  = ja;
01676     // Add constr_norm(q_hat) to constr_norm(...)
01677     constr_norm_[q_hat_new - 1] = 1.0;  // ToDo: Compute this for real!
01678     // Add bnds(q_hat)_ = bnd_ja to bnds(...)
01679     bnds_[q_hat_new - 1]  = bnd_ja;
01680     // Augment d_hat = [ d_hat; d_p ]
01681     d_hat_(q_hat_new) = d_p;
01682     // Augment z_hat with new (zeroed) multiplier value, z_hat = [ z_hat; 0 ]
01683     z_hat_(q_hat_new) = 0.0;
01684     if( update_steps ) {
01685       // Set the step for this multiplier to 1.0, p_z_hat = [ p_z_hat; 1 ]
01686       p_z_hat_(q_hat_new) = 1.0;
01687     }
01688     if( !changed_bounds ) {
01689       // Insert (ja, q_hat_new) into P_plus_hat, sorted by row
01690       insert_pair_sorted(ja,q_hat_new,q_plus_hat_,&P_plus_hat_row_,&P_plus_hat_col_);
01691     }
01692     else {
01693       TEST_FOR_EXCEPT( !( sd ) );
01694       // Insert (sd,q_hat_new) into P_FC_hat, sorted by row)
01695       insert_pair_sorted(sd,q_hat_new,q_C_hat_,&P_FC_hat_row_,&P_FC_hat_col_);
01696     }
01697   }
01698   // Update the permutation matrices and U_hat
01699   reinitialize_matrices(test_);
01700   return wrote_output;
01701 }
01702 
01703 bool QPSchur::ActiveSet::drop_constraint(
01704   int jd, std::ostream *out, EOutputLevel output_level
01705   ,bool force_refactorization, bool allow_any_cond
01706   )
01707 {
01708   using BLAS_Cpp::no_trans;
01709   using BLAS_Cpp::trans;
01710   using DenseLinAlgPack::dot;
01711   using AbstractLinAlgPack::dot;
01712   using LinAlgOpPack::V_StMtV;
01713   using LinAlgOpPack::V_MtV;
01714   using LinAlgOpPack::Vp_MtV;
01715   using LinAlgOpPack::Vp_StPtMtV;
01716   using LinAlgOpPack::V_InvMtV;
01717   using AbstractLinAlgPack::transVtMtV;
01718   using Teuchos::Workspace;
01719   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
01720   
01721   typedef AbstractLinAlgPack::EtaVector eta_t;
01722 
01723   assert_initialized();
01724 
01725   bool wrote_output = false;
01726 
01727   if( jd < 0 ) {
01728     //
01729     // A variable initially fixed is being freed.
01730     // Increase the dimension of the augmented the KKT system!
01731     //
01732     size_type
01733       q_hat      = this->q_hat(),
01734       q_F_hat    = this->q_F_hat(),
01735       q_plus_hat = this->q_plus_hat(),
01736       q_D_hat    = this->q_D_hat();
01737     // Get indexes
01738     const size_type id = -jd;
01739     TEST_FOR_EXCEPT( !(  1 <= id && id <= n_  ) );
01740     const size_type ld = qp_->l_x_X_map()(-jd);
01741     TEST_FOR_EXCEPT( !(  1 <= ld && ld <= n_ - n_R_  ) );
01742     size_type kd; // Find kd (this is unsorted)
01743     {for( kd = 1; kd <= q_D_hat; ++kd ) {
01744       if( l_fxfx_[kd-1] == ld ) break;
01745     }}
01746     TEST_FOR_EXCEPT( !(  kd <= q_D_hat  ) );
01747     // Get references
01748     const MatrixSymOp
01749       &G           = qp_->G();
01750     const DVectorSlice
01751       g            = qp_->g();
01752     const MatrixOp
01753       &A_bar       = qp_->constraints().A_bar();
01754     const MatrixSymOpNonsing
01755       &Ko          = qp_->Ko();
01756     const MatrixOp
01757       &U_hat       = this->U_hat();
01758     const GenPermMatrixSlice
01759       &Q_R         = qp_->Q_R(),
01760       &Q_X         = qp_->Q_X(),
01761       &P_XF_hat    = this->P_XF_hat(),
01762       &P_plus_hat  = this->P_plus_hat();
01763     const DVectorSlice
01764       b_X          = qp_->b_X();
01765     //
01766     // Compute the update quantities to augmented KKT system
01767     //
01768     // e_id
01769     eta_t e_id(id,n_);
01770     // u_p = [ Q_R'*G*e_id ; A'*e_id ] <: R^(n_R+m)
01771     DVector u_p(n_R_+m_);
01772     Vp_StPtMtV( &u_p(1,n_R_), 1.0, Q_R, trans, G, no_trans, e_id(), 0.0 );
01773     if( m_ )
01774       V_MtV( &u_p(n_R_+1,n_R_+m_), qp_->A(), trans, e_id() );
01775     const value_type
01776       nrm_u_p = DenseLinAlgPack::norm_inf( u_p() );
01777     // sigma = e_id'*G*e_id <: R
01778     const value_type
01779       sigma = transVtMtV( e_id(), G, no_trans, e_id() );
01780     // d_p = - g(id) - b_X'*(Q_X'*G*e_id) <: R
01781     DVector Q_X_G_e_id(Q_X.cols());
01782     Vp_StPtMtV( &Q_X_G_e_id(), 1.0, Q_X, trans, G, no_trans, e_id(), 0.0 );
01783     const value_type
01784       d_p = -g(id) - dot( b_X, Q_X_G_e_id() );
01785     // r = inv(Ko)*u_p <: R^(n_R+m)
01786     DVector r;
01787     if( nrm_u_p > 0.0 )
01788       V_InvMtV( &r, Ko, no_trans, u_p() );
01789     // t_hat = v_p - U_hat'*r
01790     // where: v_p = P_XF_hat'*G*e_id + P_plus_hat'*A_bar'*e_id <: R^(q_hat)
01791     Workspace<value_type>
01792       t_hat_ws(wss,q_hat);
01793     DVectorSlice
01794       t_hat(&t_hat_ws[0],q_hat);
01795     if(q_hat) {
01796       t_hat = 0.0;
01797       // t_hat += v_p
01798       if(q_F_hat_)
01799         Vp_StPtMtV( &t_hat(), 1.0, P_XF_hat, trans, G, no_trans, e_id() );
01800       if(q_plus_hat_)
01801         Vp_StPtMtV( &t_hat(), 1.0, P_plus_hat, trans, A_bar, trans, e_id() );
01802       // t_hat += U_hat'*r
01803       if( nrm_u_p > 0.0 )
01804         Vp_MtV( &t_hat(), U_hat, trans, r() );
01805     }
01806     // alpha_hat = sigma - u_p'*r
01807     const value_type
01808       alpha_hat = sigma - ( nrm_u_p > 0.0 ? dot(u_p(),r()) : 0.0 );
01809     //
01810     // Update the schur complement if nonsingular.  These
01811     // with throw exceptions if the matrix is singular
01812     // or has the wrong inertia
01813     //
01814     if(q_hat) {
01815       try {
01816         schur_comp().update_interface().augment_update(
01817           &t_hat(), alpha_hat, force_refactorization
01818           ,MatrixSymAddDelUpdateable::EIGEN_VAL_POS );
01819       }
01820       catch(const MSADU::WarnNearSingularUpdateException& excpt) {
01821         if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
01822           *out
01823             << "\nActiveSet::drop_constraint(...) : " << excpt.what()
01824             << std::endl;
01825           wrote_output = true;
01826         }
01827       }
01828     }
01829     else {
01830       schur_comp().update_interface().initialize(
01831         alpha_hat, (n_-n_R_) + n_-m_ );
01832     }
01833     //
01834     // Remove multiplier from mu_D_hat(...)
01835     //
01836     // remove l_fxfx(kd) == ld from l_fxfx(...)
01837     std::copy( l_fxfx_.begin() + kd, l_fxfx_.begin() + q_D_hat
01838       , l_fxfx_.begin() + (kd-1) );
01839     // remove mu_D_hat(kd) from mu_D_hat(...)
01840     std::copy( mu_D_hat_.begin() + kd, mu_D_hat_.begin() + q_D_hat
01841       , mu_D_hat_.begin() + (kd-1) );
01842     // remove Q_XD_hat(id,ld) from Q_XD_hat(...)
01843     P_row_t::iterator
01844       itr = std::lower_bound( Q_XD_hat_row_.begin(), Q_XD_hat_row_.begin()+q_D_hat, id );
01845     TEST_FOR_EXCEPT( !(  itr != Q_XD_hat_row_.end()  ) );
01846     const size_type p = itr - Q_XD_hat_row_.begin();
01847     std::copy( Q_XD_hat_row_.begin() + p + 1, Q_XD_hat_row_.begin()+q_D_hat,
01848       Q_XD_hat_row_.begin() + p );
01849     std::copy( Q_XD_hat_col_.begin() + p + 1, Q_XD_hat_col_.begin()+q_D_hat,
01850       Q_XD_hat_col_.begin() + p );
01851     if( q_D_hat > 1 )
01852       deincrement_indices( kd, &Q_XD_hat_col_, q_D_hat-1 );
01853     //
01854     // Update the counts
01855     //
01856     ++q_F_hat_;
01857     q_hat = this->q_hat();
01858     //
01859     // Add the elements for newly freed variable
01860     //
01861     // add ij_map(q_hat) == -id to ij_map(...)
01862     ij_map_[q_hat-1] = -id;
01863     // add s_map(-id) == q_hat to s_map(...)
01864     // ToDo: implement s_map(...)
01865     // add bnds(q_hat) == FREE to bnds(...)
01866     bnds_[q_hat-1] = FREE;
01867     // add d_hat(q_hat) == d_p to d_hat(...)
01868     d_hat_[q_hat-1] = d_p;
01869     // add p_X(ld) == 0 to the end of z_hat(...)
01870     z_hat_[q_hat-1] = 0.0; // This is needed so that (z_hat + beta*t_D*p_z_hat)(q_hat) == 0
01871     // Insert (id,q_hat) into P_XF_hat sorted by row
01872     insert_pair_sorted(id,q_hat,q_F_hat_,&P_XF_hat_row_,&P_XF_hat_col_);
01873   }
01874   else {
01875     //
01876     // Shrink the dimension of the augmented KKT system to remove the constraint!
01877     //
01878     const size_type q_hat = this->q_hat();
01879     const size_type sd = s_map(jd);
01880     TEST_FOR_EXCEPT( !( sd ) );
01881     wrote_output = remove_augmented_element(
01882       sd,force_refactorization
01883       ,MatrixSymAddDelUpdateable::EIGEN_VAL_NEG
01884       ,out,output_level,allow_any_cond
01885       );
01886     if( is_init_fixed(jd) ) {
01887       // This must be an intially fixed variable, currently fixed at a different bound.
01888       // We must remove this element from P_FC_hat(...)
01889       const size_type sd1 = s_map(-jd); // This is the position in the schur complement where first freed
01890       TEST_FOR_EXCEPT( !( sd1 ) );
01891       // Remove P_FC_hat(sd1,sd) from P_FC_hat(...)
01892       P_row_t::iterator
01893         itr = std::lower_bound( P_FC_hat_row_.begin(), P_FC_hat_row_.begin()+q_C_hat_, sd1 );
01894       TEST_FOR_EXCEPT( !(  itr != P_FC_hat_row_.end()  ) );
01895       const size_type p = itr - P_FC_hat_row_.begin();
01896       std::copy( P_FC_hat_row_.begin() + p + 1, P_FC_hat_row_.begin()+q_C_hat_,
01897         P_FC_hat_row_.begin() + p );
01898       std::copy( P_FC_hat_col_.begin() + p + 1, P_FC_hat_col_.begin()+q_C_hat_,
01899         P_FC_hat_col_.begin() + p );
01900       --q_C_hat_;
01901     }
01902     else {
01903       // We must remove P_plus_hat(jd,sd) from P_plus_hat(...)
01904       P_row_t::iterator
01905         itr = std::lower_bound( P_plus_hat_row_.begin(), P_plus_hat_row_.begin()+q_plus_hat_, jd );
01906       TEST_FOR_EXCEPT( !(  itr != P_plus_hat_row_.end()  ) );
01907       const size_type p = itr - P_plus_hat_row_.begin();
01908       std::copy( P_plus_hat_row_.begin() + p + 1, P_plus_hat_row_.begin()+q_plus_hat_,
01909         P_plus_hat_row_.begin() + p );
01910       std::copy( P_plus_hat_col_.begin() + p + 1, P_plus_hat_col_.begin()+q_plus_hat_,
01911         P_plus_hat_col_.begin() + p );
01912       --q_plus_hat_;
01913     }
01914     // Deincrement all counters in permutation matrices for removed element
01915     if( q_F_hat_ > 0 )
01916       deincrement_indices( sd, &P_XF_hat_col_, q_F_hat_ );
01917     if( q_C_hat_ > 0 )
01918       deincrement_indices( sd, &P_FC_hat_col_, q_C_hat_ );
01919     if( q_plus_hat_ > 0 )
01920       deincrement_indices( sd, &P_plus_hat_col_, q_plus_hat_ );
01921   }
01922   // Update the permutation matrices and U_hat
01923   reinitialize_matrices(test_);
01924   return wrote_output;
01925 }
01926 
01927 bool QPSchur::ActiveSet::drop_add_constraints(
01928   int jd, size_type ja, EBounds bnd_ja, bool update_steps
01929   ,std::ostream *out, EOutputLevel output_level
01930   )
01931 {
01932   bool wrote_output = false;
01933   if( drop_constraint( jd, out, output_level, false, true ) )
01934     wrote_output = true;
01935   if( add_constraint( ja, bnd_ja, update_steps, out, output_level, true, true ) )
01936     wrote_output = true;
01937   return wrote_output;
01938 }
01939 
01940 QPSchur::ActiveSet::QP&
01941 QPSchur::ActiveSet::qp()
01942 {
01943   assert_initialized();
01944   return *qp_;
01945 }
01946 
01947 const QPSchur::ActiveSet::QP&
01948 QPSchur::ActiveSet::qp() const
01949 {
01950   assert_initialized();
01951   return *qp_;
01952 }
01953 
01954 size_type QPSchur::ActiveSet::q_hat() const
01955 {
01956   assert_initialized();
01957   return q_plus_hat_ + q_F_hat_ + q_C_hat_;
01958 }
01959 
01960 size_type QPSchur::ActiveSet::q_plus_hat() const
01961 {
01962   assert_initialized();
01963   return q_plus_hat_;
01964 }
01965 
01966 size_type QPSchur::ActiveSet::q_F_hat() const
01967 {
01968   assert_initialized();
01969   return q_F_hat_;
01970 }
01971 
01972 size_type QPSchur::ActiveSet::q_C_hat() const
01973 {
01974   assert_initialized();
01975   return q_C_hat_;
01976 }
01977 
01978 size_type QPSchur::ActiveSet::q_D_hat() const
01979 {
01980   assert_initialized();
01981   return (n_ - n_R_) - q_F_hat_;  // n^{X} - \hat{q}^{F}
01982 }
01983 
01984 int QPSchur::ActiveSet::ij_map( size_type s ) const
01985 {
01986   TEST_FOR_EXCEPT( !(  1 <= s && s <= this->q_hat()  ) );
01987   return ij_map_[s-1];
01988 }
01989 
01990 size_type QPSchur::ActiveSet::s_map( int ij ) const
01991 {
01992   ij_map_t::const_iterator
01993     begin = ij_map_.begin(),
01994     end   = begin + q_hat(),
01995     itr = std::find( begin, end, ij );
01996   return ( itr != end ? (itr - begin) + 1 : 0 );
01997 }
01998 
01999 value_type QPSchur::ActiveSet::constr_norm( size_type s ) const
02000 {
02001   TEST_FOR_EXCEPT( !(  1 <= s && s <= this->q_hat()  ) );
02002   return constr_norm_(s);
02003 }
02004 
02005 EBounds QPSchur::ActiveSet::bnd( size_type s ) const
02006 {
02007   TEST_FOR_EXCEPT( !(  1 <= s && s <= this->q_hat()  ) );
02008   return bnds_[s-1];
02009 }
02010 
02011 size_type QPSchur::ActiveSet::l_fxfx( size_type k ) const
02012 {
02013   TEST_FOR_EXCEPT( !(  1 <= k && k <= this->q_D_hat()  ) );
02014   return l_fxfx_[k-1];
02015 }
02016 
02017 const QPSchur::U_hat_t& QPSchur::ActiveSet::U_hat() const
02018 {
02019   assert_initialized();
02020   return U_hat_;
02021 }
02022 
02023 const MatrixSymOpNonsing& QPSchur::ActiveSet::S_hat() const
02024 {
02025   assert_initialized();
02026   return schur_comp().op_interface();
02027 }
02028 
02029 const GenPermMatrixSlice& QPSchur::ActiveSet::P_XF_hat() const
02030 {
02031   assert_initialized();
02032   return P_XF_hat_;
02033 }
02034 
02035 const GenPermMatrixSlice& QPSchur::ActiveSet::P_FC_hat() const
02036 {
02037   assert_initialized();
02038   return P_FC_hat_;
02039 }
02040 
02041 const GenPermMatrixSlice& QPSchur::ActiveSet::P_plus_hat() const
02042 {
02043   assert_initialized();
02044   return P_plus_hat_;
02045 }
02046 
02047 const GenPermMatrixSlice& QPSchur::ActiveSet::Q_XD_hat() const
02048 {
02049   assert_initialized();
02050   return Q_XD_hat_;
02051 }
02052 
02053 const DVectorSlice QPSchur::ActiveSet::d_hat() const
02054 {
02055   assert_initialized();
02056   return d_hat_(1,q_hat());
02057 }
02058 
02059 DVectorSlice QPSchur::ActiveSet::z_hat()
02060 {
02061   assert_initialized();
02062   return z_hat_(1,q_hat());
02063 }
02064 
02065 const DVectorSlice QPSchur::ActiveSet::z_hat() const
02066 {
02067   assert_initialized();
02068   return z_hat_(1,q_hat());
02069 }
02070 
02071 DVectorSlice QPSchur::ActiveSet::p_z_hat()
02072 {
02073   assert_initialized();
02074   return p_z_hat_(1,q_hat());
02075 }
02076 
02077 const DVectorSlice QPSchur::ActiveSet::p_z_hat() const
02078 {
02079   assert_initialized();
02080   return p_z_hat_(1,q_hat());
02081 }
02082 
02083 DVectorSlice QPSchur::ActiveSet::mu_D_hat()
02084 {
02085   assert_initialized();
02086   return mu_D_hat_(1,q_D_hat());
02087 }
02088 
02089 const DVectorSlice QPSchur::ActiveSet::mu_D_hat() const
02090 {
02091   assert_initialized();
02092   return mu_D_hat_(1,q_D_hat());
02093 }
02094 
02095 DVectorSlice QPSchur::ActiveSet::p_mu_D_hat()
02096 {
02097   assert_initialized();
02098   return p_mu_D_hat_(1,q_D_hat());
02099 }
02100 
02101 const DVectorSlice QPSchur::ActiveSet::p_mu_D_hat() const
02102 {
02103   assert_initialized();
02104   return p_mu_D_hat_(1,q_D_hat());
02105 }
02106 
02107 bool QPSchur::ActiveSet::is_init_fixed( size_type j ) const
02108 {
02109   assert_initialized();
02110   return j <= n_ && (*x_init_)(j) != FREE;
02111 }
02112 
02113 bool QPSchur::ActiveSet::all_dof_used_up() const
02114 {
02115   return n_ - m_ == (n_ - n_R_) - q_F_hat_ + q_C_hat_ + q_plus_hat_;
02116 }
02117 
02118 // private member functions for QPSchur::ActiveSet
02119 
02120 void QPSchur::ActiveSet::assert_initialized() const
02121 {
02122   if( !initialized_ )
02123     throw std::logic_error(
02124       error_msg(__FILE__,__LINE__,"QPSchur::ActiveSet::assert_initialized() : Error, "
02125       "The active set has not been initialized yet!") );
02126 }
02127 
02128 void QPSchur::ActiveSet::assert_s( size_type s) const
02129 {
02130   TEST_FOR_EXCEPT( !(  s <= q_hat()  ) ); // ToDo: Throw an exception
02131 }
02132 
02133 void QPSchur::ActiveSet::reinitialize_matrices(bool test)
02134 {
02135   namespace GPMSTP = AbstractLinAlgPack::GenPermMatrixSliceIteratorPack;
02136 
02137   const size_type q_hat = this->q_hat();
02138   const size_type q_D_hat = this->q_D_hat();
02139 
02140   P_XF_hat_.initialize(
02141     n_,q_hat,q_F_hat_,0,0,GPMSTP::BY_ROW
02142     ,q_F_hat_ ? &P_XF_hat_row_[0] : NULL
02143     ,q_F_hat_ ? &P_XF_hat_col_[0] : NULL
02144     ,test
02145     );
02146   P_FC_hat_.initialize(
02147     q_hat,q_hat,q_C_hat_,0,0,GPMSTP::BY_ROW
02148     ,q_C_hat_ ? &P_FC_hat_row_[0] : NULL
02149     ,q_C_hat_ ? &P_FC_hat_col_[0] : NULL
02150     ,test
02151     );
02152   P_plus_hat_.initialize(
02153     n_+m_breve_,q_hat,q_plus_hat_,0,0,GPMSTP::BY_ROW
02154     ,q_plus_hat_ ? &P_plus_hat_row_[0] : NULL
02155     ,q_plus_hat_ ? &P_plus_hat_col_[0] : NULL
02156     ,test
02157     );
02158   Q_XD_hat_.initialize(
02159     n_,q_D_hat,q_D_hat,0,0,GPMSTP::BY_ROW
02160     ,q_D_hat ? &Q_XD_hat_row_[0] : NULL
02161     ,q_D_hat ? &Q_XD_hat_col_[0] : NULL
02162     ,test
02163     );
02164   U_hat_.initialize( 
02165     &qp_->G(), m_ ? &qp_->A() : NULL, &qp_->constraints().A_bar()
02166     ,&qp_->Q_R(), &P_XF_hat_, &P_plus_hat_);
02167 }
02168 
02169 bool QPSchur::ActiveSet::remove_augmented_element(
02170   size_type sd, bool force_refactorization
02171   ,MatrixSymAddDelUpdateable::EEigenValType eigen_val_drop
02172   ,std::ostream *out, EOutputLevel output_level
02173   ,bool allow_any_cond
02174   )
02175 {
02176   bool wrote_output = false;
02177   const size_type q_hat = this->q_hat();
02178   // Delete the sd row and column for S_hat
02179   try {
02180     schur_comp().update_interface().delete_update(
02181       sd,force_refactorization,eigen_val_drop
02182       ,MSADU::PivotTolerances(
02183         pivot_tols().warning_tol
02184         ,allow_any_cond ? 0.0 : pivot_tols().singular_tol
02185         ,allow_any_cond ? 0.0 : pivot_tols().wrong_inertia_tol ));
02186   }
02187   catch(const MSADU::WarnNearSingularUpdateException& excpt) {
02188     if( out && (int)output_level >= QPSchur::OUTPUT_BASIC_INFO ) {
02189       *out
02190         << "\nActiveSet::drop_constraint(...) : " << excpt.what()
02191         << std::endl;
02192       wrote_output = true;
02193     }
02194   }
02195   // Remove the ij_map(s) = jd element from ij_map(...)
02196   std::copy( ij_map_.begin() + sd, ij_map_.begin() + q_hat
02197          , ij_map_.begin() + (sd-1) );
02198   // Remove the constr_norm(s) elment from constr_norm(...)
02199   std::copy( constr_norm_.begin() + sd, constr_norm_.begin() + q_hat
02200          , constr_norm_.begin() + (sd-1) );
02201   // Remove the bnds(s) element from bnds(...)
02202   std::copy( bnds_.begin() + sd, bnds_.begin() + q_hat
02203          , bnds_.begin() + (sd-1) );
02204   // Remove the d_hat(s) element from d_hat(...)
02205   std::copy( d_hat_.begin() + sd, d_hat_.begin() + q_hat
02206          , d_hat_.begin() + (sd-1) );
02207   // Remove the z_hat(s) element from z_hat(...)
02208   std::copy( z_hat_.begin() + sd, z_hat_.begin() + q_hat
02209          , z_hat_.begin() + (sd-1) );
02210   // Remove the p_z_hat(s) element from p_z_hat(...)
02211   std::copy( p_z_hat_.begin() + sd, p_z_hat_.begin() + q_hat
02212          , p_z_hat_.begin() + (sd-1) );
02213   return wrote_output;
02214 }
02215 
02216 // public member functions for QPSchur
02217 
02218 value_type QPSchur::DEGENERATE_MULT = std::numeric_limits<value_type>::min();
02219 
02220 void QPSchur::pivot_tols( MSADU::PivotTolerances pivot_tols )
02221 {
02222   act_set_.pivot_tols(pivot_tols);
02223 }
02224 
02225 QPSchur::MSADU::PivotTolerances QPSchur::pivot_tols() const
02226 {
02227   return act_set_.pivot_tols();
02228 }
02229 
02230 QPSchur::QPSchur(
02231   const schur_comp_ptr_t&   schur_comp
02232   ,size_type                max_iter
02233   ,value_type               max_real_runtime
02234   ,value_type               feas_tol
02235   ,value_type               loose_feas_tol
02236   ,value_type               dual_infeas_tol
02237   ,value_type               huge_primal_step
02238   ,value_type               huge_dual_step
02239   ,value_type               warning_tol
02240   ,value_type               error_tol
02241   ,size_type                iter_refine_min_iter
02242   ,size_type                iter_refine_max_iter
02243   ,value_type               iter_refine_opt_tol
02244   ,value_type               iter_refine_feas_tol
02245   ,bool                     iter_refine_at_solution
02246   ,bool                     salvage_init_schur_comp
02247   ,MSADU::PivotTolerances   pivot_tols
02248   )
02249   :schur_comp_(schur_comp)
02250   ,max_iter_(max_iter)
02251   ,max_real_runtime_(max_real_runtime)
02252   ,feas_tol_(feas_tol)
02253   ,loose_feas_tol_(loose_feas_tol)
02254   ,dual_infeas_tol_(dual_infeas_tol)
02255   ,huge_primal_step_(huge_primal_step)
02256   ,huge_dual_step_(huge_dual_step)
02257   ,warning_tol_(warning_tol)
02258   ,error_tol_(error_tol)
02259   ,iter_refine_min_iter_(iter_refine_min_iter)
02260   ,iter_refine_max_iter_(iter_refine_max_iter)
02261   ,iter_refine_opt_tol_(iter_refine_opt_tol)
02262   ,iter_refine_feas_tol_(iter_refine_feas_tol)
02263   ,iter_refine_at_solution_(iter_refine_at_solution)
02264   ,salvage_init_schur_comp_(salvage_init_schur_comp)
02265   ,act_set_(schur_comp,pivot_tols)
02266 {}
02267 
02268 QPSchur::ESolveReturn QPSchur::solve_qp(
02269   QP& qp
02270   ,size_type num_act_change, const int ij_act_change[], const EBounds bnds[]
02271   ,std::ostream *out, EOutputLevel output_level, ERunTests test_what
02272   ,DVectorSlice* x, SpVector* mu, DVectorSlice* lambda, SpVector* lambda_breve
02273   ,size_type* iter, size_type* num_adds, size_type* num_drops
02274   )
02275 {
02276   using std::setw;
02277   using std::endl;
02278   using std::right;
02279   using DenseLinAlgPack::norm_inf;
02280   using AbstractLinAlgPack::norm_inf;
02281   using LinAlgOpPack::V_InvMtV;
02282   using Teuchos::Workspace;
02283   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
02284   using StopWatchPack::stopwatch;
02285 
02286   const value_type inf = std::numeric_limits<value_type>::max();
02287 
02288   if( !out )
02289     output_level = NO_OUTPUT;
02290 
02291   const int dbl_min_w = 20;
02292   const int dbl_w = (out ? my_max(dbl_min_w,int(out->precision()+8)) : 20 );
02293 
02294   // Set the schur complement
02295   act_set_.set_schur_comp( schur_comp_ );
02296 
02297   ESolveReturn
02298     solve_return = SUBOPTIMAL_POINT;
02299 
02300   // Print QPSchur output header
02301   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02302     *out
02303       << "\n*** Entering QPSchur::solve_qp(...)\n";
02304   }
02305 
02306   // Start the timer!
02307   stopwatch timer;
02308   timer.reset();
02309   timer.start();
02310 
02311   // Print the definition of the QP to be solved.
02312   if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02313     *out
02314       << "\n*** Dump the definition of the QP to be solved ***\n";
02315     qp.dump_qp(*out);
02316   }
02317   
02318   // Print warm start info.
02319   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02320     *out
02321       << "\n*** Warm start info\n"
02322       << "\nNumber of variables                                          = " << qp.n()
02323       << "\nNumber of initially fixed variables (not in Ko)              = " << (qp.n() - qp.n_R())
02324       << "\nNumber of changes to the initial KKT system (num_act_change) = " << num_act_change << endl;
02325     const size_type
02326       n = qp.n();
02327     size_type
02328       num_var_fixed = 0, num_var_freed = 0, num_gen_equ = 0, num_gen_inequ = 0;
02329     for( size_type s = 1; s <= num_act_change; ++s ) {
02330       const int ij = ij_act_change[s-1];
02331       const EBounds bnd = bnds[s-1];
02332       if( ij < 0 )
02333         ++num_var_freed;
02334       else if( ij < n )
02335         ++num_var_fixed;
02336       else if( bnd == EQUALITY )
02337         ++num_gen_equ;
02338       else
02339         ++num_gen_inequ;
02340     }
02341     *out
02342       << "\n    Number of initially fixed variables freed from a bound   = " << num_var_freed
02343       << "\n    Number of initially free variables fixed to a bound      = " << num_var_fixed
02344       << "\n    Number of general equality constraints added             = " << num_gen_equ
02345       << "\n    Number of general inequality constraints added           = " << num_gen_inequ << endl;
02346   }
02347   
02348   if( num_act_change > 0 && (int)output_level >= (int)OUTPUT_ACT_SET ) {
02349     *out
02350       << endl
02351       << right << setw(5) << "s"
02352       << right << setw(20) << "ij_act_change"
02353       << right << setw(10) << "bnds" << endl
02354       << right << setw(5) << "---"
02355       << right << setw(20) << "-------------"
02356       << right << setw(10) << "--------" << endl;
02357     for( size_type s = 1; s <= num_act_change; ++s )
02358       *out
02359         << right << setw(5) << s
02360         << right << setw(20) << ij_act_change[s-1]
02361         << right << setw(10) << bnd_str(bnds[s-1]) << endl;
02362   }
02363 
02364   // Initialize the active set.
02365   try {
02366     act_set_.initialize( qp, num_act_change, ij_act_change, bnds
02367       , test_what == RUN_TESTS, salvage_init_schur_comp(), out, output_level );
02368     // If this throws a WrongInteriaUpdateExecption it will be
02369     // thrown clean out of here!
02370   }
02371   catch( const MatrixSymAddDelUpdateable::SingularUpdateException& excpt ) {
02372     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02373       *out
02374         << "\n*** Error in initializing schur complement\n"
02375         << excpt.what() << std::endl
02376         << "\nSetting num_act_change = 0 and proceeding with a cold start...\n";
02377     }
02378     act_set_.initialize( qp, num_act_change = 0, ij_act_change, bnds
02379       , test_what == RUN_TESTS, salvage_init_schur_comp(), out, output_level );
02380   }
02381 
02382   // Compute vo =  inv(Ko) * fo
02383   Workspace<value_type> vo_ws(wss,qp.n_R()+qp.m());
02384   DVectorSlice vo(&vo_ws[0],vo_ws.size());
02385   V_InvMtV( &vo, qp.Ko(), BLAS_Cpp::no_trans, qp.fo() );
02386 
02387   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02388     *out
02389       << "\nSolution to the initial KKT system, vo = inv(Ko)*fo:\n\n||vo||inf = " << norm_inf(vo) << std::endl;
02390   }
02391   if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02392     *out
02393       << "\nvo =\n" << vo;
02394   }
02395 
02396   // ////////////////////////////////////////////////
02397   // Remove constraints until we are dual feasible.
02398   // 
02399   // Here we are essentially performing a primal algorithm where we are only
02400   // removing constraints.  If the dual variables are not dual feasible then
02401   // we will remove the one with the largest scaled dual feasibility
02402   // violation then compute the dual variables again.  Eventually we
02403   // will come to a point where we have a dual feasible point.  If
02404   // we have to, we will remove all of the inequality constraints and then
02405   // this will by default be a dual feasible point (i.e. we picked all the
02406   // wrong inequality constraints).
02407   // 
02408   // The difficulty here is in dealing with near degenerate constraints.
02409   // If a constraint is near degenerate then we would like to not drop
02410   // it since we may have to add it again later.
02411   // 
02412   *iter = 0;
02413   *num_adds = 0;
02414   *num_drops = 0;
02415   // Print header for removing constraints
02416   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02417     *out
02418       << "\n***"
02419       << "\n*** Removing constriants until we are dual feasible"
02420       << "\n***\n"
02421       << "\n*** Start by removing constraints within the Schur complement first\n";
02422   }
02423   // Print summary header for max_viol and jd.
02424   if( (int)OUTPUT_ITER_SUMMARY <= (int)output_level 
02425     && (int)output_level < (int)OUTPUT_ITER_QUANTITIES
02426     && num_act_change > 0  )
02427   {
02428     *out     
02429       << "\nIf max_viol > 0 and jd != 0 then constraint jd will be dropped from the active set\n\n"
02430       << right << setw(dbl_w) << "max_viol"
02431       << right << setw(5) << "sd"
02432       << right << setw(5) << "jd"   << endl
02433       << right << setw(dbl_w) << "--------------"
02434       << right << setw(5) << "----"
02435       << right << setw(5) << "----" << endl;
02436   }
02437   for( int k = num_act_change; k > 0; --k, ++(*iter) ) {
02438     // Check runtime
02439     if( timeout_return(&timer,out,output_level) )
02440       return MAX_RUNTIME_EXEEDED_FAIL;
02441     // Compute z_hat (z_hat = inv(S_hat)*(d_hat - U_hat'*vo))
02442     DVectorSlice z_hat = act_set_.z_hat();
02443     calc_z( act_set_.S_hat(), act_set_.d_hat(), act_set_.U_hat(), &vo
02444       , &z_hat );
02445     // Determine if we are dual feasible.
02446     value_type  max_viol = 0.0; // max scaled violation of dual feasability.
02447     size_type jd = 0;     // indice of constraint with max scaled violation.
02448     DVectorSlice::iterator
02449       z_itr = z_hat.begin();
02450     const size_type q_hat = act_set_.q_hat(); // Size of schur complement system.
02451     // Print header for s, z_hat(s), bnd(s), viol, max_viol and jd
02452     if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02453       *out
02454         << "\nLooking for a constraint with the maximum dual infeasiblity to drop...\n\n"
02455         << right << setw(5)   << "s"
02456         << right << setw(dbl_w) << "z_hat"
02457         << right << setw(20)  << "bnd"
02458         << right << setw(dbl_w) << "viol"
02459         << right << setw(dbl_w) << "max_viol"
02460         << right << setw(5)   << "jd" << endl
02461         << right << setw(5)   << "----"
02462         << right << setw(dbl_w) << "--------------"
02463         << right << setw(20)  << "--------------"
02464         << right << setw(dbl_w) << "--------------"
02465         << right << setw(dbl_w) << "--------------"
02466         << right << setw(5)   << "----" << endl;
02467     }
02468     for( int s = 1; s <= q_hat; ++s, ++z_itr ) {
02469       int j = act_set_.ij_map(s);
02470       if( j > 0 ) {
02471         // This is for an active constraint not initially in Ko so z_hat(s) = mu(j)
02472         EBounds bnd = act_set_.bnd(s);
02473         value_type viol;  // Get the amount of violation and fix near degeneracies
02474         const int dual_feas_status
02475           = correct_dual_infeas(
02476             j,bnd,0.0,act_set_.constr_norm(s),dual_infeas_tol(),DEGENERATE_MULT
02477             ,out,output_level,false
02478             ,"z_hat(s)", &(*z_itr), &viol );
02479         if( dual_feas_status < 0  && viol < max_viol ) {
02480           max_viol = viol;
02481           jd = j;
02482         }
02483         // Print row for s, z_hat(s), bnd(s), viol, max_viol and jd
02484         if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02485           *out
02486             << right << setw(5)   << s
02487             << right << setw(dbl_w) << *z_itr
02488             << right << setw(20)  << bnd_str(bnd)
02489             << right << setw(dbl_w) << viol
02490             << right << setw(dbl_w) << max_viol
02491             << right << setw(5)   << jd << endl;
02492         }
02493       }
02494     }
02495     // Print row of max_viol and jd
02496     if( (int)OUTPUT_ITER_SUMMARY <= (int)output_level 
02497       && (int)output_level < (int)OUTPUT_ITER_QUANTITIES )
02498     {
02499       *out
02500         << right << setw(dbl_w) << max_viol
02501         << right << setw(5)     << act_set_.s_map(jd)
02502         << right << setw(5)     << jd         << endl;
02503     }
02504     if( jd == 0 ) break;  // We have a dual feasible point w.r.t. these constraints
02505     // Remove the active constraint with the largest scaled violation.
02506     act_set_.drop_constraint( jd, out, output_level, true, true );
02507     ++(*iter);
02508     ++(*num_drops);
02509     // Print U_hat, S_hat and d_hat.
02510     if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02511       *out
02512         << "\nPrinting active set after dropping constraint jd = " << jd << " ...\n";
02513       dump_act_set_quantities( act_set_, *out );
02514     }
02515   }
02516 
02517   // Print how many constraints where removed from the schur complement
02518   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02519     *out
02520       << "\nThere where " << (*num_drops)
02521       << " constraints dropped from the schur complement from the initial guess of the active set.\n";
02522   }
02523 
02524   // Compute v
02525   Workspace<value_type> v_ws(wss,qp.n_R()+qp.m());
02526   DVectorSlice v(&v_ws[0],v_ws.size());
02527   if( act_set_.q_hat() > 0 ) {
02528     calc_v( qp.Ko(), &qp.fo(), act_set_.U_hat(), act_set_.z_hat(), &v );
02529     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02530       *out
02531         << "\nSolution to the system; v = inv(Ko)*(fo - U_hat*z_hat):\n"
02532         << "\n||v||inf = " << norm_inf(v) << std::endl;
02533     }
02534     if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02535       *out
02536         << "\nv =\n" << v;
02537     }
02538   }
02539   else {
02540     v = vo;
02541   }
02542 
02543   // Set x
02544   set_x( act_set_, v, x );
02545   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02546     *out
02547       << "\nCurrent guess for unknowns x:\n\n||x||inf = " << norm_inf(*x) << std::endl;
02548   }
02549   if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02550     *out
02551       << "\nx =\n" << *x;
02552   }
02553 
02554   //
02555   // Determine if any initially fixed variables need to be freed by checking mu_D_hat.
02556   //
02557   if( act_set_.q_D_hat() ) {
02558     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02559       *out << "\n*** Second, free initially fixed variables not in Ko\n";
02560     }
02561     const QPSchurPack::QP::i_x_X_map_t&  i_x_X_map = act_set_.qp().i_x_X_map();
02562     const QPSchurPack::QP::x_init_t&     x_init    = act_set_.qp().x_init();
02563     // Print summary header for max_viol and jd.
02564     if( (int)OUTPUT_ITER_SUMMARY <= (int)output_level 
02565       && (int)output_level < (int)OUTPUT_ITER_QUANTITIES )
02566     {
02567       *out     
02568         << "\nIf max_viol > 0 and id != 0 then the variable x(id) will be freed from its initial bound\n\n"
02569         << right << setw(dbl_w) << "max_viol"
02570         << right << setw(5) << "kd"
02571         << right << setw(5) << "id"   << endl
02572         << right << setw(dbl_w) << "--------------"
02573         << right << setw(5) << "----"
02574         << right << setw(5) << "----" << endl;
02575     }
02576     size_type q_D_hat = act_set_.q_D_hat(); // This will be deincremented
02577     while( q_D_hat > 0 ) {
02578       // Check runtime
02579       if( timeout_return(&timer,out,output_level) )
02580         return MAX_RUNTIME_EXEEDED_FAIL;
02581       // mu_D_hat = ???
02582       DVectorSlice mu_D_hat = act_set_.mu_D_hat();
02583       calc_mu_D( act_set_, *x, v, &mu_D_hat );
02584       // Determine if we are dual feasible.
02585       value_type  max_viol = 0.0; // max scaled violation of dual feasability.
02586       int     id = 0;     // indice of variable with max scaled violation.
02587       size_type   kd = 0;
02588       DVectorSlice::iterator
02589         mu_D_itr = mu_D_hat.begin();
02590       // Print header for k, mu_D_hat(k), bnd, viol, max_viol and id
02591       if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02592         *out
02593           << "\nLooking for a variable bound with the max dual infeasibility to drop...\n\n"
02594           << right << setw(5)   << "k"
02595           << right << setw(dbl_w) << "mu_D_hat"
02596           << right << setw(20)  << "bnd"
02597           << right << setw(dbl_w) << "viol"
02598           << right << setw(dbl_w) << "max_viol"
02599           << right << setw(5)   << "id" << endl
02600           << right << setw(5)   << "----"
02601           << right << setw(dbl_w) << "--------------"
02602           << right << setw(20)  << "--------------"
02603           << right << setw(dbl_w) << "--------------"
02604           << right << setw(dbl_w) << "--------------"
02605           << right << setw(5)   << "----" << endl;
02606       }
02607       for( int k = 1; k <= q_D_hat; ++k, ++mu_D_itr ) {
02608         int
02609           i = i_x_X_map(act_set_.l_fxfx(k));
02610         EBounds
02611           bnd = x_init(i);
02612         value_type viol;  // Get the amount of violation and fix near degeneracies
02613         const int dual_feas_status
02614           = correct_dual_infeas(
02615             i,bnd,0.0,1.0,dual_infeas_tol(),DEGENERATE_MULT
02616             ,out,output_level,false
02617             ,"mu_D_hat(k)", &(*mu_D_itr), &viol );
02618         if( dual_feas_status < 0  && viol < max_viol ) {
02619           max_viol = viol;
02620           kd = k;
02621           id = i;
02622         }
02623         // Print row for k, mu_D_hat(k), bnd, viol, max_viol and jd
02624         if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02625           *out
02626             << right << setw(5)   << k
02627             << right << setw(dbl_w) << *mu_D_itr
02628             << right << setw(20)  << bnd_str(bnd)
02629             << right << setw(dbl_w) << viol
02630             << right << setw(dbl_w) << max_viol
02631             << right << setw(5)   << id << endl;
02632         }
02633       }
02634       // Print row of max_viol and id
02635       if( (int)OUTPUT_ITER_SUMMARY <= (int)output_level 
02636         && (int)output_level < (int)OUTPUT_ITER_QUANTITIES )
02637       {
02638         *out
02639           << right << setw(dbl_w) << max_viol
02640           << right << setw(5) << kd
02641           << right << setw(5) << id         << endl;
02642       }
02643       if( id == 0 ) break;  // We have a dual feasible point w.r.t. these variable bounds
02644       // Remove the active constraint with the largest scaled violation.
02645       act_set_.drop_constraint( -id, out, output_level, true, true );
02646       ++(*iter);
02647       ++(*num_adds);
02648       --q_D_hat;
02649       // Print U_hat, S_hat and d_hat.
02650       if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02651         *out
02652           << "\nPrinting active set after freeing initially fixed variable id = " << id << " ...\n";
02653         dump_act_set_quantities( act_set_, *out );
02654       }
02655       if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02656         *out
02657           << "\nSolution to the new KKT system; z_hat = inv(S_hat)*(d_hat - U_hat'*vo), v = inv(Ko)*(fo - U_hat*z_hat):\n";
02658       }
02659       // Compute z_hat (z_hat = inv(S_hat)*(d_hat - U_hat'*vo))
02660       calc_z( act_set_.S_hat(), act_set_.d_hat(), act_set_.U_hat(), &vo
02661           , &act_set_.z_hat() );
02662       if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02663         *out
02664           << "\n||z_hat||inf = " << norm_inf(act_set_.z_hat()) << std::endl;
02665       }
02666       if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02667         *out
02668           << "\nz_hat =\n" << act_set_.z_hat();
02669       }
02670       // Compute v
02671       calc_v( qp.Ko(), &qp.fo(), act_set_.U_hat(), act_set_.z_hat(), &v );
02672       if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02673         *out
02674           << "\n||v||inf = " << norm_inf(v) << std::endl;
02675       }
02676       if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02677         *out
02678           << "\nv =\n" << v;
02679       }
02680       // Set x
02681       set_x( act_set_, v, x );
02682       if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02683         *out
02684           << "\nCurrent guess for unknowns x:\n\n||x||inf = " << norm_inf(*x) << std::endl;
02685       }
02686       if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02687         *out
02688           << "\nx =\n" << *x;
02689       }
02690     }
02691   }
02692 
02693   // Print how many initially fixed variables where freed
02694   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02695     *out
02696       << "\nThere where " << (*num_adds)
02697       << " initially fixed variables not in Ko that were freed and added to the schur complement.\n";
02698   }
02699 
02700   // Run the primal dual algorithm
02701   size_type  iter_refine_num_resid = 0, iter_refine_num_solves = 0;
02702   solve_return = qp_algo(
02703     PICK_VIOLATED_CONSTRAINT
02704     ,out, output_level, test_what
02705     ,vo, &act_set_, &v
02706     ,x, iter, num_adds, num_drops
02707     ,&iter_refine_num_resid, &iter_refine_num_solves
02708     ,&timer
02709     );
02710 
02711   if( solve_return != OPTIMAL_SOLUTION )
02712     set_x( act_set_, v, x );
02713 
02714   // Correct the sign of near degenerate multipliers in case it has not been done yet!
02715   if( solve_return != SUBOPTIMAL_POINT && act_set_.q_hat() ) {
02716     const size_type q_hat = act_set_.q_hat();
02717     DVectorSlice z_hat = act_set_.z_hat();
02718     for( size_type s = 1; s <= q_hat; ++s ) {
02719       const int       j    = act_set_.ij_map(s);
02720       value_type      viol = 0.0;
02721       const EBounds   bnd  = act_set_.bnd(s);
02722       if(bnd == FREE)
02723         continue;
02724       const int dual_feas_status
02725         = correct_dual_infeas(
02726           j,bnd,0.0,1.0,dual_infeas_tol(),DEGENERATE_MULT
02727           ,out,output_level,true,"z_hat(s)",&z_hat(s),&viol );
02728       if( dual_feas_status < 0 ) {
02729         solve_return = SUBOPTIMAL_POINT;
02730         break;
02731       }
02732     }
02733   }
02734   if( solve_return != SUBOPTIMAL_POINT && act_set_.q_D_hat() ) {
02735     const GenPermMatrixSlice&          Q_XD_hat = act_set_.Q_XD_hat();
02736     DVectorSlice                        mu_D_hat = act_set_.mu_D_hat();
02737     const QPSchurPack::QP::x_init_t&   x_init   = qp.x_init();
02738     for( GenPermMatrixSlice::const_iterator itr = Q_XD_hat.begin(); itr != Q_XD_hat.end(); ++itr ) {
02739       const int       i    = itr->row_i();
02740       value_type      viol = 0.0;
02741       const EBounds   bnd  = x_init(i);
02742       TEST_FOR_EXCEPT( !(  bnd != FREE  ) );
02743       const int dual_feas_status
02744         = correct_dual_infeas(
02745           i,bnd,0.0,1.0,dual_infeas_tol(),DEGENERATE_MULT
02746           ,out,output_level,true,"mu_D_hat(k)",&mu_D_hat(itr->col_j()),&viol );
02747       if( dual_feas_status < 0 ) {
02748         solve_return = SUBOPTIMAL_POINT;
02749         break;
02750       }
02751     }
02752   }
02753 
02754   set_multipliers( act_set_, v, mu, lambda, lambda_breve );
02755 
02756   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02757     switch(solve_return) {
02758       case OPTIMAL_SOLUTION:
02759         *out
02760           << "\n*** Solution found!\n";
02761         break;
02762       case MAX_ITER_EXCEEDED:
02763         *out
02764           << "\n*** Maximum iterations exceeded!\n";
02765         break;
02766       case MAX_RUNTIME_EXEEDED_DUAL_FEAS:
02767         *out
02768           << "\n*** Maximum runtime exceeded!\n";
02769         break;
02770       case MAX_ALLOWED_STORAGE_EXCEEDED:
02771         *out
02772           << "\n*** The maxinum size of the schur complement has been exceeded!\n";
02773         break;
02774       case INFEASIBLE_CONSTRAINTS:
02775         *out
02776           << "\n*** The constraints are infeasible!\n";
02777         break;
02778       case NONCONVEX_QP:
02779         *out
02780           << "\n*** The QP appears to be nonconvex but will return current point anyway!\n";
02781         break;
02782       case DUAL_INFEASIBILITY:
02783         *out
02784           << "\n*** The dual variables are infeasible (numerical roundoff?)!\n";
02785         break;
02786       case SUBOPTIMAL_POINT:
02787         *out
02788           << "\n*** The current point is suboptimal but we will return it anyway!\n";
02789         break;
02790       default:
02791         TEST_FOR_EXCEPT(true);
02792     }
02793     *out  << "\nNumber of QP iteratons                                = " << *iter;
02794     *out  << "\nNumber of iterative refinement residual calculations  = " << iter_refine_num_resid;
02795     *out  << "\nNumber of iterative refinement solves                 = " << iter_refine_num_solves << endl;
02796     *out  << "\n||x||inf                = " << norm_inf(*x);
02797     *out  << "\nmu.nz()                 = " << mu->nz();
02798     *out  << "\nmax(|mu(i)|)            = "   << norm_inf((*mu)());
02799     *out  << "\nmin(|mu(i)|)            = "   << min_abs((*mu)());
02800     if(lambda)
02801       *out
02802         << "\nmax(|lambda(i)|)        = " << norm_inf(*lambda)
02803         << "\nmin(|lambda(i)|)        = " << min_abs(*lambda);
02804     if(lambda_breve)
02805       *out
02806         << "\nlambda_breve.nz()       = " << lambda_breve->nz()
02807         << "\nmax(|lambda_breve(i)|)  = " << norm_inf((*lambda_breve)())
02808         << "\nmin(|lambda_breve(i)|)  = " << min_abs((*lambda_breve)());
02809     *out << std::endl;
02810   }
02811   // Print Solution x, lambda and mu
02812   if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02813     *out  << "\nx =\n"        << (*x)();
02814     *out  << "\nmu =\n"         << (*mu)();
02815     if(lambda)
02816       *out << "\nlambda =\n"      << (*lambda)();
02817     if(lambda_breve)
02818       *out << "\nlambda_breve =\n"  << (*lambda_breve)();
02819   }
02820   // Print 'goodby' header.
02821   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02822     *out
02823       << "\n*** Leaving QPSchur::solve_qp(...)\n";
02824   }
02825 
02826   return solve_return;
02827 }
02828 
02829 const QPSchur::ActiveSet& QPSchur::act_set() const
02830 {
02831   return act_set_;
02832 }
02833 
02834 // protected member functions for QPSchur
02835 
02836 QPSchur::ESolveReturn QPSchur::qp_algo(
02837   EPDSteps next_step
02838   , std::ostream *out, EOutputLevel output_level, ERunTests test_what
02839   , const DVectorSlice& vo, ActiveSet* act_set, DVectorSlice* v
02840   , DVectorSlice* x, size_type* iter, size_type* num_adds, size_type* num_drops
02841   , size_type* iter_refine_num_resid, size_type* iter_refine_num_solves
02842   , StopWatchPack::stopwatch* timer
02843   )
02844 {
02845   using std::setw;
02846   using std::endl;
02847   using std::right;
02848   using BLAS_Cpp::no_trans;
02849   using BLAS_Cpp::trans;
02850   using DenseLinAlgPack::dot;
02851   using DenseLinAlgPack::norm_inf;
02852   using DenseLinAlgPack::Vt_S;
02853   using DenseLinAlgPack::V_mV;
02854   using DenseLinAlgPack::Vp_StV;
02855   using DenseLinAlgPack::V_VmV;
02856   using LinAlgOpPack::Vp_V;
02857   using LinAlgOpPack::V_StV;
02858   using LinAlgOpPack::V_StMtV;
02859   using AbstractLinAlgPack::dot;
02860   using AbstractLinAlgPack::norm_inf;
02861   using AbstractLinAlgPack::V_MtV;
02862   using AbstractLinAlgPack::EtaVector;
02863   using LinAlgOpPack::V_MtV;
02864   using LinAlgOpPack::V_InvMtV;
02865   using LinAlgOpPack::Vp_StMtV;
02866   using LinAlgOpPack::Vp_StPtMtV;
02867   using Teuchos::Workspace;
02868   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
02869 
02870   // Print header for "Starting Primal-Dual Iterations"
02871   if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
02872     *out
02873       << "\n*** Starting Primal-Dual Iterations ***\n";
02874   }
02875 
02876   const int dbl_min_w = 20;
02877   const int dbl_w = (out ? my_max(dbl_min_w,int(out->precision()+8)): 20 );
02878 
02879   try {
02880 
02881   QPSchurPack::QP
02882     &qp = act_set->qp();
02883   const size_type
02884     n   = qp.n(),
02885     n_R   = qp.n_R(),
02886     m   = qp.m(),
02887     m_breve = qp.constraints().m_breve();
02888 
02889   Workspace<value_type>
02890     v_plus_ws(wss,v->dim()),
02891     z_hat_plus_ws(wss,(n-m)+(n-n_R)),
02892     p_v_ws(wss,v->dim());
02893   DVectorSlice
02894     v_plus(&v_plus_ws[0],v_plus_ws.size()),
02895     z_hat_plus,
02896     p_v(&p_v_ws[0],p_v_ws.size());
02897 
02898   const value_type
02899     inf = std::numeric_limits<value_type>::max();
02900   size_type itr;  // move to somewhere else?
02901 
02902   // Put these here because they need to be remembered between iterations if a linearly
02903   // dependent constriant is dropped.
02904   size_type       ja = 0;   // + indice of violated constraint to add to active set
02905   size_type       last_ja = 0; // + last_ja to be added
02906   value_type        con_ja_val; // value of violated constraint.
02907   value_type        b_a; // value of the violated bound
02908   value_type        norm_2_constr;  // norm of violated constraint
02909   EBounds         bnd_ja; // bound of constraint ja which is violated.
02910   bool          can_ignore_ja;  // true if we can ignore a constraint if it is LD.
02911   bool          assume_lin_dep_ja;
02912   value_type        gamma_plus; // used to store the new multipler value for the added
02913                     // constraint.
02914   const int       summary_lines_counter_max = 15;
02915   int           summary_lines_counter = 0;
02916   long int        jd = 0; // + indice of constraint to delete from active set.
02917                   // - indice of intially fixed variable to be freed
02918   long int        last_jd = 0; // Last jd change to the active set
02919   value_type        t_P;  // Primal step length (constraint ja made active)
02920   value_type        t_D;  // Dual step length ( longest step without violating dual
02921                   // feasibility of currently active constraints ).
02922   value_type        beta; // +1 if multiplier of constraint being added is positive
02923                   // -1 if multiplier of constraint being added is negative.
02924   bool          warned_degeneracy = false; // Will warn the user if degeneracy
02925                   // is detected.
02926   value_type        dual_infeas_scale = 1.0;  // Scaling for determining if a
02927                   // Lagrange multiplier is near degenerate.
02928   bool          return_to_init_fixed = false; // True if the constraint being added
02929                   // to the active set is a varible returning to its orginally
02930                                   // fixed variable bound.
02931   bool                    using_iter_refinement = false; // Will be set to true if instability detected
02932 
02933   for( itr = 0; itr <= max_iter_; ++itr, ++(*iter) ) {
02934     if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02935       *out
02936         << "\n************************************************"
02937         << "\n*** qp_iter = " << itr
02938         << "\n*** q_hat   = " << act_set->q_hat() << std::endl;
02939     }
02940     bool schur_comp_update_failed = false;
02941     switch( next_step ) { // no break; statements in this switch statement.
02942       case PICK_VIOLATED_CONSTRAINT: {
02943         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02944           *out
02945             << "\n*** PICK_VIOLATED_CONSTRAINT\n";
02946         }
02947         // Check runtime
02948         if( timeout_return(timer,out,output_level) )
02949           return MAX_RUNTIME_EXEEDED_DUAL_FEAS;
02950         // Save the indice of the last constriant to be added!
02951         last_ja = ja;
02952         // Set parts of x that are not currently fixed and may have changed.
02953         // Also, we want set specifially set those variables that where
02954         // initially free and then latter fixed to their bounds so that
02955         // they will not be seen as violated.
02956         set_x( *act_set, *v, x );
02957         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02958           *out
02959             << "\n||x||inf = " << norm_inf(*x) << std::endl;
02960         }
02961         if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
02962           *out
02963             << "\nx =\n" << *x;
02964         }
02965         if( test_what == RUN_TESTS ) {
02966           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02967             *out
02968               << "\nChecking current iteration quantities ...\n";
02969           }
02970           const char
02971             sep_line[] = "\n--------------------------------------------------------------------------------\n";
02972 //          AbstractLinAlgPack::TestingPack::CompareDenseVectors comp_v;
02973 
02974           //
02975           // Check the optimality conditions of the augmented KKT system!
02976           //
02977 
02978           // ToDo: Implement
02979           
02980           //
02981           // Check mu_D_hat
02982           //
02983           if( act_set->q_D_hat() ) {
02984             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02985               *out
02986                 << sep_line
02987                 << "\nComputing: mu_D_hat_calc = -Q_XD_hat'*g - Q_XD_hat'*G*x\n"
02988                 << "    - Q_XD_hat'*A*v(n_R+1:n_R+m) - Q_XD_hat'*A_bar*P_plus_hat*z_hat ...\n";
02989             }
02990             Workspace<value_type> mu_D_hat_calc_ws( wss, act_set->q_D_hat() );
02991             DVectorSlice mu_D_hat_calc( &mu_D_hat_calc_ws[0], mu_D_hat_calc_ws.size() );
02992             calc_mu_D( *act_set, *x, *v, &mu_D_hat_calc );
02993             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
02994               *out
02995                 << "\n||mu_D_hat_calc||inf = " << norm_inf(mu_D_hat_calc) << std::endl;
02996             }
02997             if( (int)output_level >= (int)OUTPUT_ACT_SET ) {
02998               *out
02999                 << "\nmu_D_hat_calc =\n" << mu_D_hat_calc;
03000             }
03001             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03002               *out
03003                 << "\nChecking mu_D_hat_calc == mu_D_hat\n";
03004             }
03005             DVector mu_D_hat_diff(mu_D_hat_calc.dim());
03006             LinAlgOpPack::V_VmV( &mu_D_hat_diff(), mu_D_hat_calc(), act_set->mu_D_hat() );
03007             const value_type
03008               mu_D_hat_err = norm_inf(mu_D_hat_diff()) / (1.0 + norm_inf(mu_D_hat_calc()));
03009             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03010               *out
03011                 << "\n||mu_D_hat_calc-mu_D_hat||inf/(1.0+||mu_D_hat_calc||inf) = "
03012                 << mu_D_hat_err << std::endl;
03013             }
03014             TEST_FOR_EXCEPTION(
03015               mu_D_hat_err >= error_tol(), TestFailed
03016               ,"QPSchur::qp_algo(...) : Error, "
03017               "||mu_D_hat_calc-mu_D_hat||inf/(1.0+||mu_D_hat_calc||inf) = "
03018               << mu_D_hat_err << " >= error_tol = " << error_tol()
03019               );
03020             if( mu_D_hat_err >= warning_tol() && (int)output_level >= (int)OUTPUT_ACT_SET ) {
03021               *out
03022                 << "\nWarning! ||mu_D_hat_calc-mu_D_hat||inf/(1.0+||mu_D_hat_calc||inf) = "
03023                 << mu_D_hat_err << " >= warning_tol = " << warning_tol() << std::endl;
03024             }
03025           }
03026           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03027             *out
03028               << sep_line;
03029           }
03030         }
03031         act_set->qp().constraints().pick_violated( *x, &ja, &con_ja_val
03032           , &b_a, &norm_2_constr, &bnd_ja, &can_ignore_ja );
03033         assume_lin_dep_ja = false;  // Assume this initially.
03034         if( ja > 0 && act_set->is_init_fixed(ja) && qp.x_init()(ja) == bnd_ja )
03035           return_to_init_fixed = true;
03036         else
03037           return_to_init_fixed = false;
03038         // Print ja, bnd_ja, can_ignore_ja
03039         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03040           *out
03041             << "\nja = " << ja  << endl;
03042           if(ja) {
03043             *out
03044               << "\ncon_ja_val           = "    << con_ja_val
03045               << "\nb_a                  = "    << b_a
03046               << "\nnorm_2_constr        = "    << norm_2_constr
03047               << "\nbnd_ja               = "    << bnd_str(bnd_ja)
03048               << "\ncan_ignore_ja        = "    << bool_str(can_ignore_ja)
03049               << "\nreturn_to_init_fixed = "    << bool_str(return_to_init_fixed)
03050               << endl
03051               ;
03052           }
03053         }
03054         // Print header for itr, nact, change (ADD, DROP), indice (ja, jb)
03055         //, bound (LOWER, UPPER, EQUALITY), violation (primal or dual), rank (LD,LI)
03056         if( (int)output_level == (int)OUTPUT_ITER_SUMMARY ) {
03057           if( summary_lines_counter <= 0 ) {
03058             summary_lines_counter = summary_lines_counter_max;
03059             *out
03060               << endl
03061               << right << setw(6) << "itr"
03062               << right << setw(6) << "qhat"
03063               << right << setw(6) << "q(+)"
03064               << right << setw(6) << "q_F"
03065               << right << setw(6) << "q_C"
03066               << right << setw(6) << "q_D"
03067               << right << setw(8) << "change"
03068               << right << setw(9) << "type"
03069               << right << setw(6) << "j"
03070               << right << setw(10)  << "bnd"
03071               << right << setw(dbl_w) << "viol, p_z(jd)"
03072               << right << setw(6) << "rank" << endl
03073               << right << setw(6) << "----"
03074               << right << setw(6) << "----"
03075               << right << setw(6) << "----"
03076               << right << setw(6) << "----"
03077               << right << setw(6) << "----"
03078               << right << setw(6) << "----"
03079               << right << setw(8) << "------"
03080               << right << setw(9) << "-------"
03081               << right << setw(6) << "----"
03082               << right << setw(10)  << "--------"
03083               << right << setw(dbl_w) << "--------------"
03084               << right << setw(6) << "----" << endl;
03085           }
03086         }
03087         // Print first part of row for itr, q_hat, q(+), q_D, q_C, q_F, change, type, index, bound, violation
03088         if( (int)output_level == (int)OUTPUT_ITER_SUMMARY ) {
03089           *out
03090             << right << setw(6) << itr                                   // itr
03091             << right << setw(6) << act_set->q_hat()                      // q_hat
03092             << right << setw(6) << act_set->q_plus_hat()                 // q(+)
03093             << right << setw(6) << act_set->q_F_hat()                    // q_F
03094             << right << setw(6) << act_set->q_C_hat()                    // q_C
03095             << right << setw(6) << act_set->q_D_hat()                    // q_D
03096             << right << setw(8)  << ( ja ? "ADD" : "-" )                 // change
03097             << right << setw(9);                                         // type
03098           if( ja == 0 ) {
03099             *out << "-";
03100           }
03101           else if( act_set->is_init_fixed(ja) ) {
03102             if( bnd_ja == qp.x_init()(ja) )
03103               *out << "X_F_X";
03104             else
03105               *out << "X_F_C";
03106           }
03107           else if( ja <= n ) {
03108             *out << "R_X";
03109           }
03110           else {
03111             *out << "GEN";
03112           }
03113           *out
03114             << right << setw(6) << ja                                    // index
03115             << right << setw(10) << ( ja ? bnd_str(bnd_ja) : "-" )       // bound
03116             << right << setw(dbl_w);                                     // violation
03117           if(ja)
03118             *out << (con_ja_val - b_a); 
03119           else
03120             *out << "-";
03121           if(!ja)
03122             *out << right << setw(6) << "-" << endl;                      // rank for last iteration
03123         }
03124         bool found_solution = false;
03125         const size_type sa = act_set->s_map(ja);
03126         value_type scaled_viol = 0.0;
03127         if( ja == 0 ) {
03128           found_solution = true;
03129         }
03130         else if( sa != 0 || ( act_set->is_init_fixed(ja) && act_set->s_map(-ja) == 0 ) ) {
03131           const bool is_most_violated
03132             = (qp.constraints().pick_violated_policy() == QPSchurPack::Constraints::MOST_VIOLATED);
03133           if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03134             *out
03135               << "\n\nWarning, we have picked the constriant a(" << ja << ") with violation\n"
03136               << "(a(ja)'*x - b_a) = (" << con_ja_val << " - " << b_a << ") = " << (con_ja_val - b_a)
03137               << "\nto add to the active set but it is already part of the active set.\n";
03138           }
03139           const EBounds act_bnd = ( sa != 0 ? act_set->bnd(sa) : qp.x_init()(ja) );
03140           if( act_bnd != bnd_ja ) {
03141             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03142               *out
03143                 << "However, this is not the same bound as the active bound!\n";
03144             }
03145             const value_type
03146               act_b_a = qp.constraints().get_bnd(ja,act_bnd);
03147             if( act_bnd == LOWER && act_b_a > b_a || act_bnd == UPPER && act_b_a < b_a ) {
03148               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03149                 *out
03150                   << "\nError, c_L_bar(" << ja <<") = " << (act_b_a > b_a ? act_b_a : b_a)
03151                   << " > c_U_bar(" << ja << ") = " << (act_b_a < b_a ? act_b_a : b_a) << ".\n";
03152               }
03153             }
03154             else {
03155               TEST_FOR_EXCEPT(true); // Should not happen!
03156             }
03157             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03158               *out
03159                 << "The constraints are infeasible!  Terminating the QP algorithm!\n";
03160             }
03161             return INFEASIBLE_CONSTRAINTS;
03162           }
03163           else {
03164             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03165               *out
03166                 << "This is the active bound so this is an indication of instability\n"
03167                 << "in the calculations.\n";
03168             }
03169           }
03170           summary_lines_counter = 0;
03171           if( !is_most_violated ) {
03172             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03173               *out
03174                 << "\nThis is not the most violated constraint so set\n"
03175                 << "pick_violated_policy = MOST_VIOLATED ...\n";
03176             }
03177             summary_lines_counter = 0;
03178             qp.constraints().pick_violated_policy(QP::Constraints::MOST_VIOLATED);
03179             next_step = PICK_VIOLATED_CONSTRAINT;
03180             continue; // Go back and pick the most violated constraint
03181           }
03182           else if( !using_iter_refinement ) {
03183             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03184               *out
03185                 << "\nThis is the most violated constraint but we are currently not using\n"
03186                 << "iterative refinement so let's switch it on ...\n";
03187             }
03188             summary_lines_counter = 0;
03189             EIterRefineReturn status = iter_refine(
03190               *act_set, out, output_level, -1.0, &qp.fo(), -1.0, act_set->q_hat() ? &act_set->d_hat() : NULL
03191               ,v, act_set->q_hat() ? &act_set->z_hat() : NULL
03192               ,iter_refine_num_resid, iter_refine_num_solves
03193               );
03194             if( status == ITER_REFINE_IMPROVED || status == ITER_REFINE_CONVERGED ) {
03195               using_iter_refinement = true; // Use iterative refinement from now on!
03196               next_step = PICK_VIOLATED_CONSTRAINT;
03197               continue; // Now go back
03198             }
03199             else {
03200               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03201                 *out
03202                   << "\nError, iterative refinement was not allowed or failed to improve the residuals.\n"
03203                   << "Terminating the QP algorithm ...\n";
03204               }
03205               return SUBOPTIMAL_POINT;
03206             }
03207           }
03208           else {
03209             scaled_viol = std::fabs(con_ja_val - b_a)/(1.0 + std::fabs(con_ja_val));
03210             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03211               *out
03212                 << "\n\nThis is the most violated constraint, we are using iterative refinement and\n"
03213                 << "|a("<<ja<<")'*x - b_a| / (1 + |a("<<ja<<")'*x|) = |"<<con_ja_val<<" - "<<b_a
03214                 << "| / (1 + |"<<con_ja_val<<"|) = "<<scaled_viol<< (scaled_viol<loose_feas_tol()?" < ":" > ")
03215                 << "loose_feas_tol = "<<loose_feas_tol();
03216             }
03217              if( scaled_viol < loose_feas_tol() ) {
03218               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03219                 *out
03220                   << "\nTerminating the algorithm with a near optimal solution!\n";
03221               }
03222               found_solution = true;
03223             }
03224             else { 
03225               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03226                 *out
03227                   << "\nError!  The QP algorithm is terminated!\n";
03228               }
03229               return SUBOPTIMAL_POINT;
03230             }
03231           }
03232         }
03233         else if( act_set->all_dof_used_up()
03234              && (scaled_viol = std::fabs(con_ja_val - b_a)/(1.0 + std::fabs(con_ja_val))) < feas_tol() )
03235         {
03236           if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03237             *out
03238               << "\nWith all the dof used up, the violated inequality constriant "
03239               << "a("<< ja << ")'*x statisfies:\n"
03240               << "|a("<<ja<<")'*x - b_a| / (1 + |a("<<ja<<")'*x|) = |" << con_ja_val << " - " << b_a
03241               << "| / (1 + |"<<con_ja_val<<"|) = "<<scaled_viol<<" < feas_tol = "<<feas_tol();
03242           }
03243           if( act_set->qp().constraints().pick_violated_policy() == QP::Constraints::MOST_VIOLATED ) {
03244             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03245               *out
03246                 << "\nThis is the most violated constraint so we will consider this\n"
03247                 << "a near degenerate constraint so we are done!\n";
03248             }
03249             found_solution = true;
03250           }
03251           else {
03252             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03253               *out
03254                 << "\nThis is not the most violated constraint so set pick_violated_policy = "
03255                 << "MOST_VIOLATED and pick another violated constraint ....\n";
03256             }
03257             act_set->qp().constraints().pick_violated_policy(QP::Constraints::MOST_VIOLATED);
03258             next_step = PICK_VIOLATED_CONSTRAINT;
03259             continue; // Go back and pick the most violated constraint
03260           }
03261         }
03262         if(found_solution) {
03263           //
03264           // Solution found!  All of the inequality constraints are satisfied!
03265           //
03266           // Use iterative refinement if we need to
03267           if( iter_refine_at_solution() && !using_iter_refinement ) {
03268             // The user has requested iterative refinement at the solution
03269             // and we have not been using iterative refinement up to this point.
03270             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03271               *out
03272                 << "\nWe think we have found the solution and are not currently using iterative refinement\n"
03273                 << "and iter_refine_at_solution==true so perform iterative refinement ...\n";
03274             }
03275             using_iter_refinement = true;
03276             EIterRefineReturn status = iter_refine(
03277               *act_set, out, output_level, -1.0, &qp.fo(), -1.0, act_set->q_hat() ? &act_set->d_hat() : NULL
03278               ,v, act_set->q_hat() ? &act_set->z_hat() : NULL
03279               ,iter_refine_num_resid, iter_refine_num_solves
03280               );
03281             switch(status) {
03282             case ITER_REFINE_ONE_STEP:
03283             case ITER_REFINE_IMPROVED:
03284             case ITER_REFINE_CONVERGED:
03285               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03286                 *out
03287                   << "\nIterative refinement may have altered the unknowns so go back and look for another violated constraint ...\n";
03288               }
03289               summary_lines_counter = 0;
03290               next_step = PICK_VIOLATED_CONSTRAINT;
03291               continue;
03292             case ITER_REFINE_NOT_PERFORMED:
03293             case ITER_REFINE_NOT_NEEDED:
03294             case ITER_REFINE_NOT_IMPROVED:
03295               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03296                 *out
03297                   << "\nIterative refinement did not alter the unknowns so exit with this solution...\n";
03298               }
03299               set_x( *act_set, *v, x );
03300               break;
03301             default:
03302               TEST_FOR_EXCEPT(true); // Local programming error only!
03303             }
03304           }
03305           if( iter_refine_at_solution() || using_iter_refinement ) {
03306             // The user has requested iterative refinement at the solution
03307             // or we have been using iterative refinement so we should
03308             // compute the lagrange multipliers for the initially fixed
03309             // variables that are still fixed.
03310             if( act_set->q_D_hat() ) {
03311               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03312                 *out
03313                   << "\nRecomputing final mu_D_hat at the solution ...\n";
03314               }
03315               calc_mu_D( *act_set, *x, *v, &act_set->mu_D_hat() );
03316               if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03317                 *out
03318                   << "\n||mu_D_hat||inf = " << norm_inf(act_set->mu_D_hat()) << std::endl;
03319               }
03320               if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03321                 *out
03322                   << "\nmu_D_hat =\n" << act_set->mu_D_hat();
03323               }
03324             }
03325           }
03326           return OPTIMAL_SOLUTION;  // current point is optimal.
03327         }
03328       }
03329       case UPDATE_ACTIVE_SET: {
03330         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03331           *out
03332             << "\n*** UPDATE_ACTIVE_SET\n";
03333         }
03334         ++(*num_adds);
03335         if( act_set->all_dof_used_up() || act_set->is_init_fixed(ja) ) {
03336           // All of the degrees of freedom are currently used up so we must
03337           // assume that we must remove one of these currently active
03338           // constraints and replace it with the violated constraint.
03339           // In this case we know that this will make the schur
03340           // complement singular so let's just skip the update
03341           // and set this now.  We also may be here if we are fixing
03342           // an initially fixed variable to some bound.
03343           assume_lin_dep_ja = true;
03344           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03345             if(act_set->all_dof_used_up()) {
03346               *out
03347                 << "\nAll of the degrees of freedom are used up so "
03348                 << "the constraint ja must be linearly dependent.\n"
03349                 << "The augmented KKT system is definitely singular!\n";
03350             }
03351             else {
03352               *out
03353                 << "\nThis is an initially fixed variable that was freed and "
03354                 << "now is being fixed again.\n"
03355                 << "The augmented KKT system could be singular or nonsingular, "
03356                 << "we don't know at this point.\n";
03357             }
03358             *out
03359               << "\nThe schur complement for the new KKT system will not be "
03360               << "updated yet in case it is singular!\n";
03361           }
03362         }
03363         else {
03364           assume_lin_dep_ja = false;
03365           try {
03366             if(act_set->add_constraint( ja, bnd_ja, false, out, output_level, true ))
03367               summary_lines_counter = 0;
03368             else {
03369               // Print end of row for rank if the right print level
03370               if( (int)output_level == (int)OUTPUT_ITER_SUMMARY ) {
03371                 *out << right << setw(6) << "LI" << endl;
03372                 out->flush();
03373                 --summary_lines_counter;
03374               }
03375             }
03376             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03377               *out << "\nNew KKT system is nonsingular! (linearly independent (LI) constraints)\n";
03378             }
03379             if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03380               *out
03381                 << "\nPrinting active set after adding constraint ja = " << ja
03382                 << " ...\n";
03383               dump_act_set_quantities( *act_set, *out );
03384             }
03385           }
03386           catch( const MatrixSymAddDelUpdateable::SingularUpdateException& excpt ) {
03387             // Constraint really is linearly dependent.
03388             if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
03389               *out
03390                 << "\n\nSchur complement update failed:\n"
03391                 << "(" << excpt.what() << ")\n"
03392                 << "\nConstraint ja = " << ja << " appears to be linearly dependent!\n\n";
03393             }
03394             summary_lines_counter = 0;
03395             if( !(act_set->q_D_hat() + act_set->q_plus_hat()) ) {
03396               // Print omsg.
03397               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03398                 *out
03399                   << "\nQPSchur::qp_algo(...) : "
03400                   << "Error, constraint j = "<< ja << " is linearly dependent "
03401                   << "and there are no other constraints to drop.\n"
03402                   << "The QP must be infeasible\n";
03403               }
03404               return INFEASIBLE_CONSTRAINTS;
03405             }
03406             assume_lin_dep_ja = true;
03407             schur_comp_update_failed = true;
03408           }
03409           catch( const MatrixSymAddDelUpdateable::WrongInertiaUpdateException& excpt ) {
03410             // Reduced Hessian has the wrong inertia
03411             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03412               *out
03413                 << "\n\nSchur complement appears to have the wrong inertia:\n"
03414                 << "(" << excpt.what() << ")\n"
03415                 << "\nThe QP appears to be nonconvex.\n"
03416                 << "\nWe have no choice but to terminate the primal-dual QP algorithm!\n";
03417             }
03418             return NONCONVEX_QP;
03419           }
03420         }
03421         if( assume_lin_dep_ja && can_ignore_ja ) {
03422           act_set->qp().constraints().ignore( ja );
03423           next_step = PICK_VIOLATED_CONSTRAINT;
03424           continue;
03425         }
03426         // Infer the sign of the multiplier for the new constraint being added
03427         beta = ( con_ja_val > b_a ? +1.0 : -1.0 );
03428         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03429           *out
03430             << "\nbeta = " << beta << endl;
03431         }
03432       }
03433       case COMPUTE_SEARCH_DIRECTION: {
03434         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03435           *out
03436             << "\n*** COMPUTE_SEARCH_DIRECTION\n";
03437         }
03438         const EtaVector e_ja( ja, n + m_breve );
03439         if( assume_lin_dep_ja ) {
03440           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03441             *out
03442               << "\nThe KKT system for the trial active set is assumed or known to be singular!\n"
03443               << "\nComputing the steps from:"
03444               << "\np_z_hat = inv(S_hat)*(-v_a+U_hat'*inv(Ko)*u_a), p_v = inv(Ko)*(-u_a-U_hat*p_z_hat) ...\n";
03445           }
03446           //
03447           // The schur complement is not updated so we must compute
03448           // p_z_hat and p_v explicitly.
03449           //
03450           // If all the degrees of freedom are used up then we know that the step for
03451           // the primal variables will be zero.  However, if m > 0 then v and p_v also
03452           // contain terms for the Lagrange multipliers for the equality constriants
03453           // but we don't need to compute these during the algorithm.
03454           // Therefore we can just set p_v = 0 and save a solve with Ko.
03455           // If the QP is feasible then a constraint will be dropped, the
03456           // KKT system will be updated and then v_plus will be computed
03457           // at the next iteration and be used to compute v so all is good.
03458           //
03459           const bool all_dof_used_up = act_set->all_dof_used_up();
03460           if( act_set->is_init_fixed(ja) ) {
03461             //
03462             // Fix a varaible that was fixed and then freed.
03463             //
03464             // [   Ko     U_hat ] [   p_v   ] = [   0  ]
03465             // [ U_hat'   V_hat ] [ p_z_hat ]   [ -v_a ]
03466             //
03467             // v_a = e(sa) <: R^q_hat            : where sa = s_map(-ja)
03468             //
03469             //        / 0                        : if x_init(ja) == bnd_ja
03470             // d_a =  |
03471             //        \ b_a - b_X(l_x_X_map(ja)) : otherwise
03472             //
03473             // p_z_hat = -inv(S_hat)*v_a
03474             //
03475             // p_v = inv(Ko)*(-U_hat*p_z_hat)
03476             //
03477             // gamma_plus = (d_a - v_a' * z_hat ) / ( v_a' * p_z_hat )
03478             //
03479             const size_type
03480               sa = act_set->s_map(-int(ja)),
03481               la = act_set->qp().l_x_X_map()(ja);
03482             TEST_FOR_EXCEPT( !( sa ) );
03483             TEST_FOR_EXCEPT( !( la ) );
03484             // v_a = e(sa) <: R^q_hat
03485             Workspace<value_type> v_a_ws(wss,act_set->q_hat());
03486             DVectorSlice v_a(&v_a_ws[0],v_a_ws.size());
03487             v_a = 0.0;
03488             v_a(sa) = 1.0;
03489             // d_a
03490             const value_type
03491               d_a = ( bnd_ja == qp.x_init()(ja) 
03492                   ? 0.0
03493                   : b_a - qp.b_X()(la) );
03494             // p_z_hat = -inv(S_hat) * v_a
03495             V_InvMtV( &act_set->p_z_hat(), act_set->S_hat(), no_trans, v_a() );
03496             Vt_S( &act_set->p_z_hat(), -1.0 );
03497             // p_v = inv(Ko)*(-U_hat*p_z_hat)
03498             if(!all_dof_used_up) {
03499               calc_v( qp.Ko(), NULL, act_set->U_hat(), act_set->p_z_hat(), &p_v() );
03500             }
03501             else {
03502               p_v = 0.0;
03503             }
03504             // Iterative refinement?
03505             if( using_iter_refinement ) {
03506               if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
03507                 *out
03508                   << "\n\nPerforming iterative refinement on p_v, p_z_hat system ...\n";
03509               }
03510               summary_lines_counter = 0;
03511               // [   Ko     U_hat ] [   p_v   ] = [   0  ]
03512               // [ U_hat'   V_hat ] [ p_z_hat ]   [ -v_a ]
03513               EIterRefineReturn status = iter_refine(
03514                 *act_set, out, output_level, 0.0, NULL, +1.0, &v_a
03515                 ,&p_v, &act_set->p_z_hat()
03516                 ,iter_refine_num_resid, iter_refine_num_solves
03517                 );
03518             }
03519             // gamma_plus = ( d_a - v_a'*z_hat ) / ( v_a'*p_z_hat )
03520             if(!all_dof_used_up)
03521               gamma_plus = ( ( d_a - dot(v_a(),act_set->z_hat()) )
03522                        / ( dot(v_a(),act_set->p_z_hat()) ) );
03523             else
03524               gamma_plus = beta * inf;
03525           }
03526           else {
03527             //
03528             // Add a constraint that is not an initially fixed
03529             // variable bound.
03530             // 
03531             // [   Ko     U_hat ] [   p_v   ] = [ -u_a ]
03532             // [ U_hat'   V_hat ] [ p_z_hat ]   [ -v_a ]
03533             //
03534             // p_z_hat = inv(S_hat) * ( - v_a + U_hat' * inv(Ko) * u_a )
03535             // 
03536             // p_v = inv(Ko) * ( -u_a - U_hat * p_z_hat )
03537             // 
03538             // gamma_plus = ( d_a - u_a'*v - v_a'*z_hat ) / ( u_a'*p_v + v_a'*p_z_hat )
03539             // 
03540             // ToDo: (9/25/00): Make u_a and v_a both sparse and combine the following code.
03541             // 
03542             if( ja <= n ) {
03543               // Fix an initially free variable
03544               //
03545               // u_a = [ Q_R' * e(ja) ] <: R^(n_R+m)
03546               //       [      0       ]
03547               //
03548               // v_a = 0     <: R^(q_hat)
03549               //
03550               // d_a = b_a   <: R
03551               // 
03552               const size_type
03553                 la = act_set->qp().Q_R().lookup_col_j(ja);
03554               TEST_FOR_EXCEPT( !(  la  ) );
03555               const EtaVector u_a = EtaVector(la,n_R+m);
03556               const value_type d_a = b_a;
03557               DVector t1;
03558               // t1 = inv(Ko) * u_a
03559               V_InvMtV( &t1, qp.Ko(), no_trans, u_a() );
03560               if( act_set->q_hat() ) {
03561                 // t2 = U_hat'*t1
03562                 DVector t2;
03563                 V_MtV( &t2, act_set->U_hat(), trans, t1() );
03564                 // p_z_hat = inv(S_hat) * t2
03565                 V_InvMtV( &act_set->p_z_hat(), act_set->S_hat(), no_trans, t2() );
03566                 // t1 = - u_a
03567                 V_StV( &t1, -1.0, u_a() );
03568                 // t1 += - U_hat * p_z_hat
03569                 Vp_StMtV( &t1(), -1.0, act_set->U_hat(), no_trans, act_set->p_z_hat() );
03570                 // p_v = inv(Ko) * t1
03571                 if(!all_dof_used_up)
03572                   V_InvMtV( &p_v, qp.Ko(), no_trans, t1() );
03573                 else
03574                   p_v = 0.0;
03575               }
03576               else {
03577                 // p_v = -t1
03578                 V_mV( &p_v, t1() );
03579               }
03580               // Iterative refinement?
03581               if( using_iter_refinement ) {
03582                 if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
03583                   *out
03584                     << "\n\nPerforming iterative refinement on p_v, p_z_hat system ...\n";
03585                 }
03586                 summary_lines_counter = 0;
03587                 // [   Ko     U_hat ] [   p_v   ] = [ -u_a ]
03588                 // [ U_hat'   V_hat ] [ p_z_hat ]   [   0  ]
03589                 Workspace<value_type> dense_u_a_ws(wss,u_a().dim());
03590                 DVectorSlice dense_u_a(&dense_u_a_ws[0],dense_u_a_ws.size());
03591                 dense_u_a = 0.0; // Make a dense copy of u_a!
03592                 dense_u_a(u_a().begin()->index()+u_a().offset()) = 1.0;
03593                 EIterRefineReturn status = iter_refine(
03594                   *act_set, out, output_level, +1.0, &dense_u_a, 0.0, NULL
03595                   ,&p_v, &act_set->p_z_hat()
03596                   ,iter_refine_num_resid, iter_refine_num_solves
03597                   );
03598                 summary_lines_counter = 0;
03599               }
03600               // gamma_plus = ( d_a - u_a'*v) / ( u_a'*p_v )
03601               if(!all_dof_used_up)
03602                 gamma_plus = ( d_a - dot(u_a(),*v) ) / dot(u_a(),p_v());
03603               else
03604                 gamma_plus = beta * inf;
03605             }
03606             else {
03607               // Add a general inequality (or equality) constraint
03608               //
03609               // u_a = [ Q_R' * A_bar * e(ja) ] <: R^(n_R + m)
03610               //       [          0           ]
03611               // 
03612               // v_a = P_XF_hat' * A_bar * e_ja <: R^(q_hat)
03613               //
03614               // d_a = b_a - b_X' * (Q_X' * A_bar * e_ja) <: R
03615               //
03616               Workspace<value_type> u_a_ws( wss, n_R + m );
03617               DVectorSlice u_a( &u_a_ws[0], u_a_ws.size() );
03618               Workspace<value_type> v_a_ws( wss, act_set->q_hat() );
03619               DVectorSlice v_a( &v_a_ws[0], v_a_ws.size() );
03620               // u_a(1:n_R) =  Q_R' * A_bar * e(ja)
03621               Vp_StPtMtV( &u_a(1,n_R), 1.0, qp.Q_R(), trans
03622                     , qp.constraints().A_bar(), no_trans, e_ja(), 0.0 );
03623               // u_a(n_R+1:n_R+m) = 0.0
03624               if(m)
03625                 u_a(n_R+1,n_R+m) = 0.0;
03626               // t0 = Q_X' * A_bar * e_ja
03627               Workspace<value_type> t0_ws( wss, n-n_R );
03628               DVectorSlice t0( &t0_ws[0], t0_ws.size() );
03629               if( n > n_R )
03630                 Vp_StPtMtV( &t0(), 1.0, qp.Q_X(), trans
03631                       , qp.constraints().A_bar(), no_trans, e_ja(), 0.0 );
03632               // d_a = b_a - b_X'*t0
03633               const value_type
03634                 d_a = b_a - ( n > n_R ? dot( qp.b_X(), t0() ) : 0.0 );
03635               // t1 = inv(Ko) * u_a
03636               Workspace<value_type> t1_ws( wss, n_R + m );
03637               DVectorSlice t1( &t1_ws[0], t1_ws.size() );
03638               V_InvMtV( &t1, qp.Ko(), no_trans, u_a );
03639               if( act_set->q_hat() ) {
03640                 // t2 = U_hat'*t1
03641                 Workspace<value_type> t2_ws( wss, act_set->q_hat() );
03642                 DVectorSlice t2( &t2_ws[0], t2_ws.size() );
03643                 V_MtV( &t2, act_set->U_hat(), trans, t1() );
03644                 // v_a = P_XF_hat' * A_bar * e_ja
03645                 Vp_StPtMtV( &v_a(), 1.0, act_set->P_XF_hat(), trans
03646                       , qp.constraints().A_bar(), no_trans, e_ja(), 0.0 );
03647                 // t2 += -v_a
03648                 Vp_StV( &t2(), -1.0, v_a() );
03649                 // p_z_hat = inv(S_hat) * t2
03650                 V_InvMtV( &act_set->p_z_hat(), act_set->S_hat(), no_trans, t2() );
03651                 if(!all_dof_used_up) {
03652                   // t1 = - u_a
03653                   V_StV( &t1, -1.0, u_a() );
03654                     // t1 += - U_hat * p_z_hat
03655                   Vp_StMtV( &t1(), -1.0, act_set->U_hat(), no_trans, act_set->p_z_hat() );
03656                     // p_v = inv(Ko) * t1
03657                   V_InvMtV( &p_v, qp.Ko(), no_trans, t1() );
03658                 }
03659                 else {
03660                   p_v = 0.0;
03661                 }
03662                 // Iterative refinement?
03663                 if( using_iter_refinement ) {
03664                   if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
03665                     *out
03666                       << "\n\nPerforming iterative refinement on p_v, p_z_hat system ...\n";
03667                   }
03668                   summary_lines_counter = 0;
03669                   // [   Ko     U_hat ] [   p_v   ] = [ -u_a ]
03670                   // [ U_hat'   V_hat ] [ p_z_hat ]   [ -v_a ]
03671                   EIterRefineReturn status = iter_refine(
03672                     *act_set, out, output_level, +1.0, &u_a, +1.0, act_set->q_hat() ? &v_a : NULL
03673                     ,&p_v, act_set->q_hat() ? &act_set->p_z_hat() : NULL
03674                     ,iter_refine_num_resid, iter_refine_num_solves
03675                     );
03676                   summary_lines_counter = 0;
03677                 }
03678                 // gamma_plus = ( d_a - u_a'*v - v_a'*z_hat ) / ( u_a'*p_v + v_a'*p_z_hat )
03679                 if(!all_dof_used_up)
03680                   gamma_plus = ( ( d_a - dot(u_a,*v) - dot(v_a(),act_set->z_hat()) )
03681                            / ( dot(u_a,p_v()) + dot(v_a(),act_set->p_z_hat()) ) );
03682                 else
03683                   gamma_plus = beta * inf;
03684               }
03685               else {
03686                 // p_v = -t1
03687                 if(!all_dof_used_up)
03688                   V_mV( &p_v, t1() );
03689                 else
03690                   p_v = 0.0;
03691                 // gamma_plus = ( d_a - u_a'*v) / ( u_a'*p_v )
03692                 if(!all_dof_used_up)
03693                   gamma_plus = ( d_a - dot(u_a,*v) ) / dot(u_a,p_v());
03694                 else
03695                   gamma_plus = beta * inf;
03696               }
03697             }
03698           }
03699           if( schur_comp_update_failed && gamma_plus * beta < 0 ) {
03700             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03701               *out
03702                 << "\nThe schur complement update failed and gamma_plus = " << gamma_plus << " is the wrong sign"
03703                 << "\nso we will assume the sign error for (...)/+-0 was due to arbitrary roundoff"
03704                 << "\nand therefore we will set gamma_plus = -gamma_plus\n";
03705             }
03706             gamma_plus = -gamma_plus;
03707           }
03708           // Print the steps p_v and p_z_hat
03709           if(act_set->q_hat()) {
03710             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03711               *out
03712                 << "\n||p_z_hat||inf = " << norm_inf(act_set->p_z_hat()) << endl;
03713             }
03714             if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03715               *out << "\np_z_hat =\n" << act_set->p_z_hat();
03716             }
03717           }
03718           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03719             *out
03720               << "\n||p_v||inf = " << norm_inf(p_v()) << endl;
03721           }
03722           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03723             *out << "\np_v =\n" << p_v();
03724           }
03725           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03726             *out
03727               << "\ngamma_plus = " << gamma_plus << endl;
03728           }
03729           // Compute step for mu_D_hat
03730           if( act_set->q_D_hat() ) {
03731             // Compute for steps of all the constraints in the current active set
03732             calc_p_mu_D( *act_set, p_v(), act_set->p_z_hat(), &ja, &act_set->p_mu_D_hat() );
03733             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03734               *out
03735                 << "\n||p_mu_D_hat||inf = " << norm_inf(act_set->p_mu_D_hat()) << std::endl;
03736             }
03737             if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03738               *out
03739                 << "\np_mu_D_hat =\n" << act_set->p_mu_D_hat();
03740             }
03741           }
03742         }
03743         else {
03744           // The new schur complement is already updated so compute
03745           // the solution outright.
03746           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03747             *out
03748               << "\nThe KKT system for the new active set is or known to be nonsingular!\n"
03749               << "\nComputing the steps from:"
03750               << "\nz_hat_plus = inv(S_hat)*(d_hat-U_hat'vo), v_plus = inv(Ko)*(fo-U_hat*z_hat_plus) ...\n";
03751           }
03752         
03753           // Compute z_hat_plus, v_plus
03754 
03755           // z_hat_plus = inv(S_hat) * ( d_hat - U_hat' * vo  )
03756           const size_type q_hat = act_set->q_hat();
03757           z_hat_plus.bind(DVectorSlice(&z_hat_plus_ws[0],q_hat));
03758           calc_z( act_set->S_hat(), act_set->d_hat(), act_set->U_hat(), &vo
03759             , &z_hat_plus );
03760           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03761             *out
03762               << "\n||z_hat_plus||inf = " << norm_inf(z_hat_plus()) << std::endl;
03763           }
03764           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03765             *out
03766               << "\nz_hat_plus =\n" << z_hat_plus();
03767           }
03768           // v_plus = inv(Ko) * (fo - U_hat * z_hat_plus)
03769           calc_v( qp.Ko(), &qp.fo(), act_set->U_hat(), z_hat_plus
03770             , &v_plus );
03771           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03772             *out
03773               << "\n||v_plus||inf = " << norm_inf(v_plus()) << std::endl;
03774           }
03775           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03776             *out
03777               << "\nv_plus =\n" << v_plus();
03778           }
03779           if( using_iter_refinement ) {
03780             if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
03781               *out
03782                 << "\n\nPerforming iterative refinement on v_plus, z_hat_plus system ...\n";
03783             }
03784             summary_lines_counter = 0;
03785             // [   Ko     U_hat ] [   v_plus   ] = [   fo  ]
03786             // [ U_hat'   V_hat ] [ z_hat_plus ]   [ d_hat ]
03787             EIterRefineReturn status = iter_refine(
03788               *act_set, out, output_level, -1.0, &qp.fo(), -1.0, &act_set->d_hat()
03789               ,&v_plus, &z_hat_plus
03790               ,iter_refine_num_resid, iter_refine_num_solves
03791               );
03792           }
03793           // Compute p_z_hat (change in z_hat w.r.t newly added constriant multiplier)
03794           DVectorSlice p_z_hat = act_set->p_z_hat();
03795           // p_z_hat = z_hat_plus - z_hat
03796           V_VmV( &p_z_hat(), z_hat_plus(), act_set->z_hat() );
03797           // p_v = v_plus - v
03798           V_VmV( &p_v(), v_plus(), *v );
03799           // p_mu_D_hat
03800           if( act_set->q_D_hat() )
03801             calc_p_mu_D( *act_set, p_v(), p_z_hat(), NULL, &act_set->p_mu_D_hat() );
03802           // gamma_plus
03803           const size_type sa = act_set->s_map(ja);
03804           if(sa) {
03805             // This is not an initially fixed variable that returned to its
03806             // initial.  The multiplier for this constriant may not be the
03807             // last element if an ADD/DROP was performed on the last iteration
03808             // in order to get here where the DROP was an initially fixed variable
03809             // that was freed and therefore the KKT system was augmented so this
03810             // multiplier is not the last element of z_hat(...).
03811             gamma_plus = z_hat_plus(sa);
03812           }
03813           else {
03814             // This must be an initially fixed variable that returned to its
03815             // initial bound.  This will be the last element even if an ADD/DROP
03816             // was just performed since a drop would only remove elements from
03817             // p_mu_D_hat, not add them.
03818             gamma_plus = act_set->p_mu_D_hat()(act_set->q_D_hat());
03819           }
03820           // p_z_hat = p_z_hat / gamma_plus
03821           Vt_S( &p_z_hat(), 1.0 / gamma_plus );
03822           // p_v = p_v / gamma_plus
03823           Vt_S( &p_v(), 1.0 / gamma_plus );
03824           // Print gama_plus, p_z_hat, p_v and p_mu_D_hat
03825           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03826             *out
03827               << "\ngamma_plus = " << gamma_plus << std::endl;
03828           }
03829           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03830             *out
03831               << "\n||p_z_hat||inf = " << norm_inf(p_z_hat()) << std::endl;
03832           }
03833           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03834             *out
03835               << "\np_z_hat =\n" << p_z_hat();
03836           }
03837           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03838             *out
03839               << "\n||p_v||inf = " << norm_inf(p_v()) << std::endl;
03840           }
03841           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03842             *out
03843               << "\np_v =\n" << p_v();
03844           }
03845           if( act_set->q_D_hat() ) {
03846             // p_mu_D_hat = p_mu_D_hat / gamma_plus
03847             Vt_S( &act_set->p_mu_D_hat(), 1.0 / gamma_plus ); 
03848             if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03849               *out
03850                 << "\n||p_mu_D_hat||inf =\n" << norm_inf(act_set->p_mu_D_hat()) << std::endl;
03851             }
03852             if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES ) {
03853               *out
03854                 << "\np_mu_D_hat =\n" << act_set->p_mu_D_hat();
03855             }
03856           }
03857         }
03858       }
03859       case COMPUTE_STEP_LENGTHS: {
03860 
03861         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03862           *out
03863             << "\n*** COMPUTE_STEP_LENGTHS\n";
03864         }
03865         // Compute the dual infeasibility scaling
03866         const size_type q_hat = act_set->q_hat();
03867         dual_infeas_scale = 1.0;
03868 //        if( q_hat )
03869 //          dual_infeas_scale = my_max( dual_infeas_scale, norm_inf( act_set->z_hat() ) );
03870 //        if( m )
03871 //          dual_infeas_scale = my_max( dual_infeas_scale, norm_inf( (*v)(n_R+1,n_R+m) ) );
03872 //        if( act_set->q_D_hat() )
03873 //          dual_infeas_scale = my_max( dual_infeas_scale, norm_inf( act_set->mu_D_hat() ) );
03874         
03875         // Primal step length, t_P = beta * gamma_plus, z_plus = [ z_hat_plus; gama_plus ].
03876         // Or constraint ja is linearly dependent in which case p_x is zero so
03877         // t_P is infinite.
03878         t_P = beta * gamma_plus;  // Could be < 0
03879         if( t_P < 0.0 ) {
03880           if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03881             *out
03882               << "\nWarning, A near degenerate inequality constraint ja = " << ja
03883               << " is being added that has the wrong sign with:\n"
03884               << "    t_P                     = " << t_P          << std::endl
03885               << "    dual_infeas_scale       = " << dual_infeas_scale  << std::endl
03886               << "    norm_2_constr           = " << norm_2_constr      << std::endl
03887               << "    |t_P/(norm_2_constr*dual_infeas_scale)| = "
03888               << std::fabs(t_P/(norm_2_constr*dual_infeas_scale))
03889               << " <= dual_infeas_tol = " << dual_infeas_tol()      << std::endl;
03890           }
03891           summary_lines_counter = 0;
03892           if( !using_iter_refinement ) {
03893             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03894               *out << "We are not using iterative refinement yet so turn it on"
03895                  << "\nthen recompute the steps ...\n";
03896             }
03897             using_iter_refinement = true;
03898             next_step = COMPUTE_SEARCH_DIRECTION;
03899             continue;
03900           }
03901           else {
03902             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
03903               *out
03904                 << "We are already using iterative refinement so the QP algorithm is terminated!\n";
03905             }
03906             return DUAL_INFEASIBILITY;
03907           }
03908         }
03909         t_P = beta * gamma_plus;  // Now guaranteed to be > 0
03910         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
03911           *out
03912             << "\nt_P = " << t_P << endl;
03913         }
03914 
03916         // Dual step length.  Largest step t that does not cause violation in
03917         // dual feasibility (i.e. lagrange multipliers for inequalities are
03918         // dual feasible, or primal optimal ).
03919         // lambda_hat_new = lambda_hat + beta * t_D * p_lambda_hat must be dual feasible.
03920         t_D = inf;
03921         jd = 0;
03922         value_type max_feas_viol = 0.0; // Remember the amount of violation.
03923         int j_degen = 0;  // remember which (if any) constraint was near
03924                   // degenerate and had an incorrect sign.
03925         EBounds     bnd_jd; // The bound of the constraint to be dropped.
03926 
03927         // Search through Lagrange multipliers in z_hat
03928         if( act_set->q_hat() ) {
03929           DVectorSlice z_hat = act_set->z_hat();
03930           DVectorSlice p_z_hat = act_set->p_z_hat();
03931           DVectorSlice::iterator
03932             z_itr   = z_hat.begin(),
03933             p_z_itr   = p_z_hat.begin();
03934           const size_type
03935             qq = assume_lin_dep_ja || (!assume_lin_dep_ja && return_to_init_fixed)
03936               ? q_hat : q_hat - 1;
03937           // Print header for s, j, z_hat(s), p_z_hat(s), bnds(s), t, t_D, jd
03938           if( qq > 0 && (int)output_level >= (int)OUTPUT_ACT_SET ) {
03939               *out
03940               << "\nComputing the maximum step for multiplers for dual feasibility\n\n"
03941               << right << setw(5) << "s"
03942               << right << setw(5) << "j"
03943               << right << setw(dbl_w) << "z_hat"
03944               << right << setw(dbl_w) << "p_z_hat"
03945               << right << setw(20)  << "bnd"
03946               << right << setw(dbl_w) << "t"
03947               << right << setw(dbl_w) << "t_D"
03948               << right << setw(5) << "jd" << endl
03949               << right << setw(5) << "----"
03950               << right << setw(5) << "----"
03951               << right << setw(dbl_w) << "--------------"
03952               << right << setw(dbl_w) << "--------------"
03953               << right << setw(20)  << "--------------"
03954               << right << setw(dbl_w) << "--------------"
03955               << right << setw(dbl_w) << "--------------"
03956               << right << setw(5) << "----" << endl;
03957           }
03958           for( int s = 1; s <= qq; ++s, ++z_itr, ++p_z_itr) {
03959             int j = act_set->ij_map(s);
03960             if( j > 0 ) {
03961               namespace ns = QPSchurPack;
03962               EBounds bnd = act_set->bnd(s);
03963               // Print first part of row for s, j, z_hat(s), p_z_hat(s), bnds(s) ....
03964               if( (int)output_level >= (int)OUTPUT_ACT_SET ) {
03965                 *out
03966                   << right << setw(5) << s
03967                   << right << setw(5) << j
03968                   << right << setw(dbl_w) << *z_itr
03969                   << right << setw(dbl_w) << *p_z_itr
03970                   << right << setw(20)  << bnd_str(bnd);
03971               }
03972               value_type t = inf;
03973               // Lookout for degeneracy.
03974               bool j_is_degen = false;
03975               value_type viol;
03976               const int dual_feas_status
03977                 = correct_dual_infeas(
03978                   j,bnd,t_P,1.0,dual_infeas_tol(),DEGENERATE_MULT
03979                   ,out,output_level,true,"z_hat(s)",&(*z_itr),&viol
03980                   ,"p_z_hat(s)",&(*p_z_itr),"z_hat_plus(s)"
03981                   , (assume_lin_dep_ja ? NULL: &z_hat_plus(s) ) );
03982               if( dual_feas_status < 0 ) {
03983                 if( !using_iter_refinement ) {
03984                   if( (int)output_level >= (int)OUTPUT_BASIC_INFO )
03985                     *out << "We are not using iterative refinement yet so turn it on"
03986                        << "\nthen recompute the steps ...\n";
03987                   using_iter_refinement = true;
03988                   next_step = COMPUTE_SEARCH_DIRECTION;
03989                   continue;
03990                 }
03991                 else {
03992                   if( (int)output_level >= (int)OUTPUT_BASIC_INFO )
03993                     *out << "We are already using iterative refinement so the QP algorithm is terminated!\n";
03994                   return DUAL_INFEASIBILITY;
03995                 }
03996               }
03997               else if( dual_feas_status == 0 ) {
03998                 j_is_degen = true;
03999               }
04000               // If we get here either the dual variable was feasible or it
04001               // was near degenerate and was corrected!
04002               const value_type feas_viol = beta*(*p_z_itr);
04003               if( bnd == EQUALITY )
04004                 ; // We don't care
04005               else if( bnd == LOWER && feas_viol <= 0.0 )
04006                 ; // dual feasible for all t > 0
04007               else if( bnd == UPPER && feas_viol >= 0.0 )
04008                 ; // dual feasible for all t > 0
04009               else {
04010                 // finite t.
04011                 t = -beta*(*z_itr)/(*p_z_itr);
04012                 if( t < t_D ) { // remember minimum step length
04013                   t_D = t;
04014                   jd = j;
04015                   if(j_is_degen) j_degen = j;
04016                   max_feas_viol = feas_viol;
04017                   bnd_jd = bnd;
04018                 }
04019               }
04020               // Print rest of row for ... t, t_D, jd
04021               if( (int)output_level >= (int)OUTPUT_ACT_SET ) {
04022                 *out
04023                   << right << setw(dbl_w) << t
04024                   << right << setw(dbl_w) << t_D
04025                   << right << setw(5) << jd << endl;
04026               }
04027             }
04028           }
04029         }
04030           
04031         // Search through Lagrange multipliers in mu_D_hat
04032         if( act_set->q_D_hat() ) {
04033           const QPSchurPack::QP::x_init_t     &x_init    = qp.x_init();
04034           const QPSchurPack::QP::i_x_X_map_t  &i_x_X_map = qp.i_x_X_map();
04035           const size_type q_D_hat = act_set->q_D_hat();
04036           DVectorSlice mu_D_hat = act_set->mu_D_hat();
04037           DVectorSlice p_mu_D_hat = act_set->p_mu_D_hat();
04038           const size_type
04039             qD = assume_lin_dep_ja && return_to_init_fixed ? q_D_hat-1 : q_D_hat;
04040           // Print header for k, i, mu_D_hat(k), p_mu_D_hat(k), x_init(k), t, t_D, jd
04041           if( qD > 0 && (int)output_level >= (int)OUTPUT_ACT_SET ) {
04042             *out
04043               << "\nComputing the maximum step for multiplers for dual feasibility\n\n"
04044               << right << setw(5) << "k"
04045               << right << setw(5) << "i"
04046               << right << setw(dbl_w) << "mu_D_hat"
04047               << right << setw(dbl_w) << "p_mu_D_hat"
04048               << right << setw(20)  << "x_init"
04049               << right << setw(dbl_w) << "t"
04050               << right << setw(dbl_w) << "t_D"
04051               << right << setw(5) << "jd" << endl
04052               << right << setw(5) << "----"
04053               << right << setw(5) << "----"
04054               << right << setw(dbl_w) << "--------------"
04055               << right << setw(dbl_w) << "--------------"
04056               << right << setw(20)  << "--------------"
04057               << right << setw(dbl_w) << "--------------"
04058               << right << setw(dbl_w) << "--------------"
04059               << right << setw(5) << "----" << endl;
04060           }
04061           GenPermMatrixSlice::const_iterator
04062             Q_XD_itr = act_set->Q_XD_hat().begin(),
04063             Q_XD_end = Q_XD_itr + qD;
04064           for( ; Q_XD_itr != Q_XD_end; ++Q_XD_itr ) {
04065             const size_type k = Q_XD_itr->col_j();
04066             const size_type i = Q_XD_itr->row_i();
04067             DVectorSlice::iterator
04068               mu_D_itr    = mu_D_hat.begin() + (k-1),
04069               p_mu_D_itr    = p_mu_D_hat.begin() + (k-1);
04070             const size_type l = act_set->l_fxfx(k);
04071             EBounds bnd = qp.x_init()(i);
04072             // Print first part of row for s, j, z_hat(s), p_z_hat(s), bnds(s) ....
04073             if( (int)output_level >= (int)OUTPUT_ACT_SET ) {
04074               *out
04075                 << right << setw(5) << k
04076                 << right << setw(5) << i
04077                 << right << setw(dbl_w) << *mu_D_itr
04078                 << right << setw(dbl_w) << *p_mu_D_itr
04079                 << right << setw(20)  << bnd_str(bnd);
04080             }
04081             value_type t = inf;
04082             // Lookout for degeneracy.
04083             bool j_is_degen = false;
04084             value_type viol;
04085             const int dual_feas_status
04086               = correct_dual_infeas(
04087                 i,bnd,t_P,1.0,dual_infeas_tol(),DEGENERATE_MULT
04088                 ,out,output_level,true,"mu_D_hat(k)",&(*mu_D_itr),&viol
04089                 ,"p_mu_D_hat(k)",&(*p_mu_D_itr) );
04090             if( dual_feas_status < 0 ) {
04091               if( !using_iter_refinement ) {
04092                 if( (int)output_level >= (int)OUTPUT_BASIC_INFO )
04093                   *out << "We are not using iterative refinement yet so turn it on"
04094                      << "\nthen recompute the steps ...\n";
04095                 using_iter_refinement = true;
04096                 next_step = COMPUTE_SEARCH_DIRECTION;
04097                 continue;
04098               }
04099               else {
04100                 if( (int)output_level >= (int)OUTPUT_BASIC_INFO )
04101                   *out << "We are already using iterative refinement so the QP algorithm is terminated!\n";
04102                 return DUAL_INFEASIBILITY;
04103               }
04104             }
04105             else if( dual_feas_status == 0 ) {
04106               j_is_degen = true;
04107             }
04108             // If we get here either the dual variable was feasible or it
04109             // was near degenerate and was corrected!
04110             const value_type feas_viol = beta*(*p_mu_D_itr);
04111             if( bnd == EQUALITY )
04112               ; // We don't care
04113             else if( bnd == LOWER && feas_viol <= 0.0 )
04114               ; // dual feasible for all t > 0
04115             else if( bnd == UPPER && feas_viol >= 0.0 )
04116               ; // dual feasible for all t > 0
04117             else {
04118               // finite t.
04119               t = -beta*(*mu_D_itr)/(*p_mu_D_itr);
04120               if( t < t_D ) { // remember minimum step length
04121                 t_D = t;
04122                 jd = -i;
04123                 if(j_is_degen) j_degen = jd;
04124                 max_feas_viol = feas_viol;
04125                 bnd_jd = bnd;
04126                 }
04127             }
04128             // Print rest of row for ... t, t_D, jd
04129             if( (int)output_level >= (int)OUTPUT_ACT_SET ) {
04130               *out
04131                 << right << setw(dbl_w) << t
04132                 << right << setw(dbl_w) << t_D
04133                 << right << setw(5)     << jd   << endl;
04134             }
04135           }
04136         }
04137         // Print t_D, jd
04138         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
04139           *out
04140             << "\nt_D = " << t_D  << endl
04141             << "jd = "    << jd << endl;
04142         }
04143         if( jd == j_degen && jd != 0 && t_D < t_P ) {
04144           if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04145             *out
04146               << "\nWarning, the near degenerate constraint j = "
04147               << jd << " which had the incorrect sign\nand was adjusted "
04148               << "was selected to be dropped from the active set.\n";
04149           }
04150         }
04151         // Print end of row for rank if the right print level
04152         if( assume_lin_dep_ja && !schur_comp_update_failed && (int)output_level == (int)OUTPUT_ITER_SUMMARY ) {
04153           if( t_P < huge_primal_step() )
04154             *out << right << setw(6) << "LI" << endl;
04155           else
04156             *out << right << setw(6) << "LD" << endl;
04157           out->flush();
04158           --summary_lines_counter;
04159         }
04160         // Print start of row for itr, q_hat, q(+), q_D, q_C, q_F, change, type, index, bound, violation
04161         if( t_D < t_P && (int)output_level == (int)OUTPUT_ITER_SUMMARY ) {
04162           *out
04163             << right << setw(6) << itr                   // itr
04164             << right << setw(6) << act_set->q_hat()      // q_hat
04165             << right << setw(6) << act_set->q_plus_hat() // q(+)
04166             << right << setw(6) << act_set->q_F_hat()    // q_F
04167             << right << setw(6) << act_set->q_C_hat()    // q_C
04168             << right << setw(6) << act_set->q_D_hat()    // q_D
04169             << right << setw(8) << "DROP"                // change
04170             << right << setw(9);                         // type
04171           if( jd < 0 ) {
04172             *out << "X_F";
04173           }
04174           else if( jd <= n ) {
04175             if( bnd_jd == qp.x_init()(jd) )
04176               *out << "X_F_C_F";
04177             else
04178               *out << "R_X_R";
04179           }
04180           else {
04181             *out << "GEN";
04182           }
04183           *out
04184             << right << setw(6)   << jd                   // index
04185             << right << setw(10)  << bnd_str(bnd_jd)      // bound
04186             << right << setw(dbl_w) << max_feas_viol        // violation
04187             << right << setw(6)   << "LI" << endl;        // rank (this should be true!)
04188         }
04189       }
04190       case TAKE_STEP: {
04191         if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
04192           *out
04193             << "\n*** TAKE_STEP\n";
04194         }
04195         if( t_P >= huge_primal_step() && t_D >= huge_dual_step() ) {
04196           if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04197             *out
04198               << "Error, QP is infeasible, inconsistent constraint a("<<ja<<") detected\n";
04199           }
04200           if( using_iter_refinement ) {
04201             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04202               *out
04203                 << "We are already using iterative refinement so the QP algorithm is terminated!\n";
04204             }
04205             return INFEASIBLE_CONSTRAINTS;
04206           }
04207           else {
04208             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04209               *out << "We are not using iterative refinement yet so turn it on";
04210               if(assume_lin_dep_ja)
04211                 *out << "\nthen pick another violated constriant to add ... \n";
04212               else
04213                 *out << "\nthen recompute the steps ...\n";
04214             }
04215             summary_lines_counter = 0;
04216             last_jd = 0;  // erase this memory!
04217             last_ja = 0;  // ..
04218             using_iter_refinement = true;
04219             if(assume_lin_dep_ja) {
04220               EIterRefineReturn status = iter_refine(
04221                 *act_set, out, output_level, -1.0, &qp.fo(), -1.0, act_set->q_hat() ? &act_set->d_hat() : NULL
04222                 ,v, act_set->q_hat() ? &act_set->z_hat() : NULL
04223                 ,iter_refine_num_resid, iter_refine_num_solves
04224                 );
04225               next_step = PICK_VIOLATED_CONSTRAINT;
04226             }
04227             else {
04228               // Iterative refinement will be performed there
04229               next_step = COMPUTE_SEARCH_DIRECTION;
04230             }
04231             continue;
04232           }
04233         }
04234         else if( t_P > t_D ) {
04235           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
04236             if( t_P >= huge_primal_step() ) {
04237               *out
04238                 << "\n*** (b) Dual Step (t_P = " << t_P << " >= huge_primal_step = "
04239                   << huge_primal_step() << ")\n";
04240             }
04241             else {
04242               *out
04243                 << "\n*** (b) Partial Primal-Dual Step\n";
04244             }
04245           }
04246           // Check for cycling
04247           if( ja == last_jd && jd == last_ja ) {
04248             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04249               *out
04250                 << "\n\nQPSchur::qp_algo(...) : Error, the constraint "
04251                 << "a(" << ja << ") with violation\n"
04252                 << "(a(ja)'*x - b_a) = (" << con_ja_val
04253                 << " - " << b_a << ") = " << (con_ja_val - b_a) << "\n"
04254                 << "we are adding to the active set and the constraint constriant\n"
04255                 << "a(" << jd << ") we are dropping were just dropped and added respectively!\n"
04256                 << "The algorithm is cycling!\n";
04257             }
04258             if( using_iter_refinement ) {
04259               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04260                 *out
04261                   << "We are already using iterative refinement so the QP algorithm is terminated!\n";
04262               }
04263               return SUBOPTIMAL_POINT;
04264             }
04265             else {
04266               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04267                 *out << "We are not using iterative refinement yet so turn it on";
04268                 if(assume_lin_dep_ja)
04269                   *out << "\nthen pick another violated constriant to add ... \n";
04270                 else
04271                   *out << "\nthen recompute the steps ...\n";
04272               }
04273               summary_lines_counter = 0;
04274               last_jd = 0;  // erase this memory!
04275               last_ja = 0;  // ..
04276               using_iter_refinement = true;
04277               if(assume_lin_dep_ja) {
04278                 EIterRefineReturn status = iter_refine(
04279                   *act_set, out, output_level, -1.0, &qp.fo(), -1.0, act_set->q_hat() ? &act_set->d_hat() : NULL
04280                   ,v, act_set->q_hat() ? &act_set->z_hat() : NULL
04281                   ,iter_refine_num_resid, iter_refine_num_solves
04282                   );
04283                 next_step = PICK_VIOLATED_CONSTRAINT;
04284               }
04285               else {
04286                 // Iterative refinement will be performed there
04287                 next_step = COMPUTE_SEARCH_DIRECTION;
04288               }
04289               continue;
04290             }
04291           }
04292           // Update the augmented KKT system
04293           try {
04294             if( assume_lin_dep_ja ) {
04295               if(act_set->drop_add_constraints( jd, ja, bnd_ja, true, out, output_level ))
04296                 summary_lines_counter = 0;
04297             }
04298             else {
04299               if(act_set->drop_constraint( jd, out, output_level, true, true ))
04300                 summary_lines_counter = 0;
04301             }
04302           }
04303           catch( const MatrixSymAddDelUpdateable::SingularUpdateException& excpt ) {
04304             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04305               *out
04306                 << "\n\nSchur complement appears to be singular and should not be:\n"
04307                 << excpt.what()
04308                 << "\nThe QP appears to be nonconvex and we therefore terminate the primal-dual QP algorithm!\n";
04309             }
04310             return NONCONVEX_QP;
04311           }
04312           catch( const MatrixSymAddDelUpdateable::WrongInertiaUpdateException& excpt ) {
04313             if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04314               *out
04315                 << "\n\nSchur complement appears to have the wrong inertia:\n"
04316                 << excpt.what()
04317                 << "\nThe QP appears to be nonconvex and we therefore terminate the primal-dual QP algorithm!\n";
04318             }
04319             return NONCONVEX_QP;
04320           }
04321           // z_hat = z_hat + beta * t_D * p_z_hat
04322           if(act_set->q_hat())
04323             Vp_StV( &act_set->z_hat(), beta * t_D, act_set->p_z_hat() );
04324           // v = v + beta * t_D * p_v
04325           Vp_StV( v, beta * t_D, p_v() );
04326           // mu_D_hat = mu_D_hat + beta * t_D * p_mu_D_hat
04327           if(act_set->q_D_hat())
04328             Vp_StV( &act_set->mu_D_hat(), beta * t_D, act_set->p_mu_D_hat() );
04329 
04330           ++(*num_drops);
04331 
04332           if( (int)output_level >= (int)OUTPUT_ITER_STEPS )
04333           {
04334             *out
04335               << "\nUpdated primal and dual variables:\n"
04336               << "\n||v||inf           = " << norm_inf(*v) << endl;
04337             if(act_set->q_hat()) {
04338               *out
04339                 << "||z_hat||inf       = " << norm_inf(act_set->z_hat()) << endl;
04340             }
04341             if(act_set->q_D_hat()) {
04342               *out
04343                 << "max(|mu_D_hat(i)|) = " << norm_inf(act_set->mu_D_hat()) << endl
04344                 << "min(|mu_D_hat(i)|) = " << min_abs(act_set->mu_D_hat()) << endl;
04345             }
04346           }
04347           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES )
04348           {
04349             *out << "\nv = \n" << *v << endl;
04350             if(assume_lin_dep_ja) {
04351               *out
04352                 << "\nPrinting active set after dropping constraint jd = " << jd
04353                 << " and adding constraint ja = " << ja << " ...\n";
04354             }
04355             else {
04356               *out
04357                 << "\nPrinting active set after dropping constraint jd = " << jd
04358                 << " ...\n";
04359             }
04360             dump_act_set_quantities( *act_set, *out );
04361           }
04362           last_jd = jd;
04363           assume_lin_dep_ja = false;  // If we get here then we know these are true!
04364           next_step = COMPUTE_SEARCH_DIRECTION;
04365           continue;
04366         }
04367         else {  // t_P < t_D
04368           if( (int)output_level >= (int)OUTPUT_ITER_STEPS ) {
04369             *out
04370               << "\n*** (c) Full Primal-Dual Step\n";
04371           }
04372           ++(*num_adds);
04373           if( !assume_lin_dep_ja ) {
04374             act_set->z_hat()  = z_hat_plus;
04375             *v          = v_plus;
04376           }
04377           else {
04378             bool threw_exception = false;
04379             try {
04380               if(act_set->add_constraint( ja, bnd_ja, true, out, output_level, true, true ))
04381                 summary_lines_counter = 0;
04382             }
04383             catch( const MatrixSymAddDelUpdateable::SingularUpdateException& excpt ) {
04384               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04385                 *out
04386                   << "\n\nSchur complement appears to be singular and should not be:\n"
04387                   << excpt.what() << std::endl;
04388               }
04389               threw_exception = true;
04390             }
04391             catch( const MatrixSymAddDelUpdateable::WrongInertiaUpdateException& excpt ) {
04392               if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04393                 *out
04394                   << "\n\nSchur complement appears to have the wrong inertia:\n"
04395                   << excpt.what() << std::endl;
04396               }
04397               threw_exception = true;
04398             }
04399             if( threw_exception ) {
04400               if( !using_iter_refinement ) {
04401                 if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04402                   *out << "We are not using iterative refinement yet so turn it on and\n"
04403                      << "go back and pick a new violated constraint to add to the active set ...\n";
04404                 }
04405                 using_iter_refinement = true;
04406                 if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04407                   *out
04408                     << "\n\nPerforming iterative refinement on v, z_hat system ...\n";
04409                 }
04410                 summary_lines_counter = 0;
04411                 // [   Ko     U_hat ] [   v   ] = [   fo  ]
04412                 // [ U_hat'   V_hat ] [ z_hat ]   [ d_hat ]
04413                 EIterRefineReturn status = iter_refine(
04414                   *act_set, out, output_level, -1.0, &qp.fo(), -1.0, &act_set->d_hat()
04415                   ,v, &act_set->z_hat()
04416                   ,iter_refine_num_resid, iter_refine_num_solves
04417                   );
04418                 next_step = PICK_VIOLATED_CONSTRAINT;
04419                 continue;
04420               }
04421               else {
04422                 if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04423                   *out << "Darn, we are already using iterative refinement!"
04424                      << "\nThe QP appears to be nonconvex and we therefore terminate the primal-dual QP algorithm!\n";
04425                 }
04426                 return NONCONVEX_QP;
04427               }
04428             }
04429             // z_hat = z_hat + beta * t_P * p_z_hat
04430             if(act_set->q_hat())
04431               Vp_StV( &act_set->z_hat(), beta * t_P, act_set->p_z_hat() );
04432             // v = v + beta * t_P * p_v
04433             Vp_StV( v, beta * t_P, p_v() );
04434           }
04435           // mu_D_hat = mu_D_hat + beta * t_P * p_mu_D_hat
04436           if(act_set->q_D_hat())
04437             Vp_StV( &act_set->mu_D_hat(), beta * t_P, act_set->p_mu_D_hat() );
04438 
04439 
04440           if( (int)output_level >= (int)OUTPUT_ITER_STEPS )
04441           {
04442             *out
04443               << "\nUpdated primal and dual variables:\n"
04444               << "\n||v||inf           = " << norm_inf(*v) << endl;
04445             if(act_set->q_hat()) {
04446               *out
04447                 << "||z_hat||inf       = " << norm_inf(act_set->z_hat()) << endl;
04448             }
04449             if(act_set->q_D_hat()) {
04450               *out
04451                 << "max(|mu_D_hat(i)|) = " << norm_inf(act_set->mu_D_hat()) << endl
04452                 << "min(|mu_D_hat(i)|) = " << min_abs(act_set->mu_D_hat()) << endl;
04453             }
04454           }
04455           if( (int)output_level >= (int)OUTPUT_ITER_QUANTITIES )
04456           {
04457             *out << "\nv = \n" << *v << endl;
04458             if( assume_lin_dep_ja ) {
04459               *out
04460                 << "\nPrinting active set after adding constraint ja = " << ja
04461                 << " ...\n";
04462               dump_act_set_quantities( *act_set, *out );
04463             }
04464             else {
04465               if(act_set->q_hat())
04466                 *out << "\nz_hat =\n" << act_set->z_hat();
04467               if(act_set->q_D_hat())
04468                 *out << "\nmu_D_hat =\n" << act_set->mu_D_hat();
04469             }
04470           }
04471           assume_lin_dep_ja = false;  // If we get here then we know these are true!
04472           next_step = PICK_VIOLATED_CONSTRAINT;
04473           continue;
04474         }
04475       }
04476       default:
04477         TEST_FOR_EXCEPT(true);  // only a local programming error
04478     }
04479   }
04480 
04481   } // end try
04482   catch( std::exception& excpt ) {
04483     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04484       *out
04485         << "\n\n*** Caught a standard exception :\n"
04486         << excpt.what()
04487         << "\n*** Rethrowing the exception ...\n";
04488     }
04489     throw;
04490   }
04491   catch(...) {
04492     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04493       *out
04494         << "\n\n*** Caught an unknown exception.  Rethrowing the exception ...\n";
04495     }
04496     throw;
04497   }
04498   // If you get here then the maximum number of QP iterations has been exceeded
04499   return MAX_ITER_EXCEEDED;
04500 }
04501 
04502 void QPSchur::set_x( const ActiveSet& act_set, const DVectorSlice& v, DVectorSlice* x )
04503 {
04504   using BLAS_Cpp::no_trans;
04505   using LinAlgOpPack::V_MtV;
04506   using LinAlgOpPack::Vp_MtV;
04507   
04508   // x = Q_R * v(1:n_R) + Q_X * b_X + P_XF_hat * z_hat
04509   V_MtV( x, act_set.qp().Q_R(), no_trans, v(1,act_set.qp().n_R()) );
04510   if( act_set.qp().n() > act_set.qp().n_R() )
04511     Vp_MtV( x, act_set.qp().Q_X(), no_trans, act_set.qp().b_X() );
04512   if( act_set.q_F_hat() )
04513     Vp_MtV( x, act_set.P_XF_hat(), no_trans, act_set.z_hat() );
04514 }
04515 
04516 void QPSchur::set_multipliers(
04517   const ActiveSet& act_set, const DVectorSlice& v
04518   ,SpVector* mu, DVectorSlice* lambda, SpVector* lambda_breve
04519   )
04520 {
04521   using BLAS_Cpp::no_trans;
04522   using LinAlgOpPack::V_MtV;
04523   using LinAlgOpPack::Vp_MtV;
04524   using LinAlgOpPack::Vp_StMtV;
04525   using LinAlgOpPack::V_MtV;
04526   using LinAlgOpPack::Vp_MtV;
04527   using AbstractLinAlgPack::V_MtV;
04528   using AbstractLinAlgPack::Vp_MtV;
04529   namespace GPMSTP = AbstractLinAlgPack::GenPermMatrixSliceIteratorPack;
04530   using Teuchos::Workspace;
04531   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
04532 
04533   const size_type
04534     n       = act_set.qp().n(),
04535     n_R     = act_set.qp().n_R(),
04536     m       = act_set.qp().m(),
04537     m_breve = act_set.qp().constraints().m_breve(),
04538     q_hat   = act_set.q_hat();
04539   const QPSchurPack::QP::x_init_t
04540     &x_init = act_set.qp().x_init();
04541 
04542   //
04543   // mu = P_plus_hat(1:n,:) * z_hat + Q_XD_hat * mu_D + (steps for initially fixed
04544   //    variables fixed to the other bounds)
04545   //
04546   // lambda_breve = P_plus_hat(n+1:n+m_breve,:) * z_hat
04547   //
04548   typedef SpVector::element_type ele_t;
04549   mu->resize( n, n-m );                 // Resize for the maxinum number
04550   lambda_breve->resize( m_breve, n-m );   // of active constraints possible.
04551   // mu += Q_XD_hat * mu_D_hat
04552   if( act_set.q_D_hat() )
04553     Vp_MtV( mu, act_set.Q_XD_hat(), no_trans, act_set.mu_D_hat() );
04554   // Set all the multipliers in z_hat
04555   if(q_hat){
04556     const DVectorSlice
04557       z_hat = act_set.z_hat();
04558     for( size_type s = 1; s <= q_hat; ++s ) {
04559       const int ij = act_set.ij_map(s);
04560       if(ij > 0) {
04561         const size_type j = ij;
04562         if( j <= n )
04563           mu->add_element(ele_t(j,z_hat(s)));
04564         else
04565           lambda_breve->add_element(ele_t(j-n,z_hat(s)));
04566       }
04567     }
04568   }
04569   mu->sort();
04570   lambda_breve->sort();
04571   // lambda = v(n_R+1,n_R+m)
04572   if( m ) {
04573     *lambda = v(n_R+1,n_R+m);
04574   }
04575 }
04576 
04577 bool QPSchur::timeout_return( StopWatchPack::stopwatch* timer, std::ostream *out, EOutputLevel output_level ) const
04578 {
04579   const value_type minutes = timer->read() / 60;
04580   if( minutes >= max_real_runtime() ) {
04581     if( (int)output_level >= (int)OUTPUT_BASIC_INFO ) {
04582       *out
04583         << "\n*** Runtime = " << minutes << " min >= max_real_runtime = " << max_real_runtime() << " min!\n"
04584         << "Must terminite the algorithm!\n";
04585     }
04586     return true;
04587   }
04588   return false;
04589 }
04590 
04591 QPSchur::EIterRefineReturn
04592 QPSchur::iter_refine(
04593   const ActiveSet      &act_set
04594   ,std::ostream        *out
04595   ,EOutputLevel        output_level
04596   ,const value_type    ao
04597   ,const DVectorSlice  *bo
04598   ,const value_type    aa
04599   ,const DVectorSlice  *ba
04600   ,DVectorSlice        *v
04601   ,DVectorSlice        *z
04602   ,size_type           *iter_refine_num_resid
04603   ,size_type           *iter_refine_num_solves
04604   )
04605 {
04606   using std::endl;
04607   using std::setw;
04608   using std::left;
04609   using std::right;
04610   using BLAS_Cpp::no_trans;
04611   using BLAS_Cpp::trans;
04612   using DenseLinAlgPack::norm_inf;
04613   using DenseLinAlgPack::Vp_StV;
04614   using LinAlgOpPack::Vp_V;
04615   using LinAlgOpPack::Vp_StMtV;
04616   using LinAlgOpPack::V_InvMtV;
04617   using Teuchos::Workspace;
04618   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
04619 
04620   typedef DenseLinAlgPack::value_type           extra_value_type;
04621 
04622   const value_type small_num = std::numeric_limits<value_type>::min();
04623 
04624   const int int_w = 8;
04625   const char int_ul[] = "------";
04626   const int dbl_min_w = 20;
04627   const int dbl_w = ( out ? my_max(dbl_min_w,int(out->precision()+8)): 20 );
04628   const char dbl_ul[] = "------------------";
04629 
04630   const QPSchurPack::QP
04631     &qp    = act_set.qp();
04632   const MatrixSymOpNonsing
04633     &Ko    = qp.Ko(),
04634     &S_hat = act_set.S_hat();
04635   const MatrixOp
04636     &U_hat = act_set.U_hat();
04637   const DenseLinAlgPack::size_type
04638     n          = qp.n(),
04639     n_R        = qp.n_R(),
04640     m          = qp.m(),
04641     q_hat      = act_set.q_hat();
04642   const DVectorSlice
04643     fo    = qp.fo(),
04644     d_hat = (q_hat ? act_set.d_hat() : DVectorSlice());
04645 
04646   Workspace<extra_value_type>
04647     ext_ro_ws(wss,n_R+m),
04648     ext_ra_ws(wss,q_hat);
04649   DenseLinAlgPack::VectorSliceTmpl<extra_value_type>
04650     ext_ro(&ext_ro_ws[0],ext_ro_ws.size()),
04651     ext_ra(ext_ra_ws.size()?&ext_ra_ws[0]:NULL,ext_ra_ws.size());
04652   Workspace<value_type>
04653     ro_ws(wss,n_R+m),
04654     ra_ws(wss,q_hat),
04655     t1_ws(wss,n_R+m),
04656     del_v_ws(wss,n_R+m),
04657     del_z_ws(wss,q_hat),
04658     v_itr_ws(wss,n_R+m),
04659     z_itr_ws(wss,q_hat);
04660   DVectorSlice
04661     ro(&ro_ws[0],ro_ws.size()),
04662     ra(ra_ws.size()?&ra_ws[0]:NULL,ra_ws.size()),
04663     t1(&t1_ws[0],t1_ws.size()),
04664     del_v(&del_v_ws[0],del_v_ws.size()),
04665     del_z(del_z_ws.size()?&del_z_ws[0]:NULL,del_z_ws.size()),
04666     v_itr(&v_itr_ws[0],v_itr_ws.size()),
04667     z_itr(z_itr_ws.size()?&z_itr_ws[0]:NULL,z_itr_ws.size());
04668   
04669   // Accumulate into temporary variables
04670   v_itr = *v;
04671   if(q_hat)
04672     z_itr = *z;
04673 
04674   // Print summary header
04675   if( out && (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04676     *out
04677       << "\nBeginning iterative refinement ..."
04678       << "\niter_refine_opt_tol = " << iter_refine_opt_tol()
04679       << ", iter_refine_feas_tol = " << iter_refine_feas_tol()
04680       << "\niter_refine_min_iter = " << iter_refine_min_iter()
04681       << ", iter_refine_max_iter = " << iter_refine_max_iter() << "\n\n";
04682     //
04683     *out
04684       << right << setw(int_w) << "ir_itr"
04685       << right << setw(dbl_w) << "roR_scaling"
04686       << right << setw(dbl_w) << "||roR||s"
04687       << left  << setw(1)     << " ";
04688     if(m) {
04689       *out
04690         << right << setw(dbl_w) << "rom_scaling"
04691         << right << setw(dbl_w) << "||rom||s"
04692         << left  << setw(1)     << " ";
04693     }
04694     if(q_hat) {
04695       *out
04696         << right << setw(dbl_w) << "ra_scaling"
04697         << right << setw(dbl_w) << "||ra||s"
04698         << left  << setw(1)     << " ";
04699     }
04700     *out
04701       << right << setw(dbl_w) << "||del_v||/||v||inf"
04702       << right << setw(dbl_w) << "||del_z||/||z||inf"
04703       << endl;
04704     //
04705     *out
04706       << right << setw(int_w) << int_ul
04707       << right << setw(dbl_w) << dbl_ul
04708       << right << setw(dbl_w) << dbl_ul
04709       << left  << setw(1)     << " ";
04710     if(m) {
04711       *out
04712       << right << setw(dbl_w) << dbl_ul
04713       << right << setw(dbl_w) << dbl_ul
04714       << left  << setw(1)     << " ";
04715     }
04716     if(q_hat) {
04717       *out
04718       << right << setw(dbl_w) << dbl_ul
04719       << right << setw(dbl_w) << dbl_ul
04720       << left  << setw(1)     << " ";
04721     }
04722     *out
04723       << right << setw(dbl_w) << dbl_ul
04724       << right << setw(dbl_w) << dbl_ul
04725       << endl;
04726   }
04727   //
04728   // Perform iterative refinement iterations
04729   //
04730   EIterRefineReturn return_status = ITER_REFINE_NOT_PERFORMED;
04731   value_type
04732     roR_nrm_o,   rom_nrm_o,   ra_nrm_o,
04733     roR_nrm,     rom_nrm,     ra_nrm;
04734   for( size_type iter_refine_k = 0;
04735      iter_refine_k < iter_refine_max_iter() && return_status != ITER_REFINE_CONVERGED;
04736      ++iter_refine_k)
04737   {
04738     //
04739     // Compute the residual (in extended precision?)
04740     //
04741     // [ ro ] = [   Ko     U_hat ] [ v ] + [ ao*bo ]
04742     // [ ra ]   [ U_hat'   V_hat ] [ z ]   [ aa*ba ]
04743     //
04744     value_type
04745       roR_scaling = 0.0,
04746       rom_scaling = 0.0,
04747       ra_scaling  = 0.0;
04748     ++(*iter_refine_num_resid);
04749     calc_resid(
04750       act_set
04751       ,v_itr, z_itr
04752       ,ao
04753       ,bo
04754       ,&ext_ro
04755       ,&roR_scaling
04756       ,m ? &rom_scaling : NULL
04757       ,aa
04758       ,ba
04759       ,q_hat ? &ext_ra : NULL
04760       ,q_hat ? &ra_scaling : NULL
04761       );
04762     std::copy(ext_ro.begin(),ext_ro.end(),ro.begin());  // Convert back to standard precision
04763     if(q_hat) std::copy(ext_ra.begin(),ext_ra.end(),ra.begin());
04764     //
04765     // Calcuate convergence criteria
04766     //
04767     roR_nrm  = norm_inf(ro(1,n_R));
04768     rom_nrm  = (m ? norm_inf(ro(n_R+1,n_R+m)) : 0.0);
04769     ra_nrm   = (q_hat ? norm_inf(ra) : 0.0);
04770     if( iter_refine_k == 0 ) {
04771       roR_nrm_o = roR_nrm;
04772       rom_nrm_o = rom_nrm;
04773       ra_nrm_o  = rom_nrm;
04774     }
04775     const bool
04776       is_roR_conv = roR_nrm / (1.0 + roR_scaling) < iter_refine_opt_tol(),
04777       is_rom_conv = (m ? rom_nrm / (1.0 + rom_scaling) < iter_refine_feas_tol() : true ),
04778       is_ra_conv  = (q_hat ?  ra_nrm / (1.0 + ra_scaling) < iter_refine_feas_tol() : true ),
04779       is_conv     = is_roR_conv && is_rom_conv && is_ra_conv;
04780     //
04781     // Print beginning of summary line for residuals
04782     //
04783     if( out && (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04784       *out
04785         << right << setw(int_w) << iter_refine_k
04786         << right << setw(dbl_w) << roR_scaling
04787         << right << setw(dbl_w) << (roR_nrm / (1.0 + roR_scaling))
04788         << left  << setw(1)     << (is_roR_conv ? "*" : " ");
04789       if(m) {
04790         *out
04791           << right << setw(dbl_w) << rom_scaling
04792           << right << setw(dbl_w) << (rom_nrm /(1.0 + rom_scaling))
04793           << left  << setw(1)     << (is_rom_conv ? "*" : " ");
04794       }
04795       if(q_hat) {
04796         *out
04797           << right << setw(dbl_w) << ra_scaling
04798           << right << setw(dbl_w) << (ra_nrm /(1.0 + ra_scaling))
04799           << left  << setw(1)     << (is_ra_conv ? "*" : " ");
04800       }
04801     }
04802     //
04803     // Check for convergence
04804     //
04805     if( iter_refine_k + 1 < iter_refine_min_iter() ) {
04806       // Keep on going even if we have converged to desired tolerances!
04807     }
04808     else if( is_conv ) {
04809       if( out && (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04810         *out
04811           << right << setw(dbl_w) << "-"
04812           << right << setw(dbl_w) << "-"
04813           << endl;
04814       }
04815       if( iter_refine_k == 0 )
04816         return_status = ITER_REFINE_NOT_NEEDED;
04817       else
04818         return_status = ITER_REFINE_CONVERGED;
04819       break;
04820     }
04821     // Make sure we have made progress
04822     if( roR_nrm_o < roR_nrm && rom_nrm_o < rom_nrm && ra_nrm_o < rom_nrm ) {
04823       return_status = ITER_REFINE_NOT_IMPROVED;
04824       break; // No progress was make in converging the equations!
04825     }
04826     //
04827     // Solve for the steps
04828     //
04829     // [   Ko     U_hat ] [ del_v ] = [ ro ]
04830     // [ U_hat'   V_hat ] [ del_z ]   [ ra ]
04831     //
04832     ++(*iter_refine_num_solves);
04833     if( q_hat ) {
04834       // del_z = inv(S_hat)*(ra - U_hat'*inv(Ko)*ro)
04835       V_InvMtV( &t1, Ko, no_trans, ro );
04836       calc_z( act_set.S_hat(), ra, act_set.U_hat(), &t1, &del_z );
04837     }
04838     calc_v( Ko, &ro, U_hat, del_z, &del_v );
04839     //
04840     // Compute steps:
04841     //
04842     // v += -del_v
04843     // z += -del_z
04844     //
04845     Vp_StV( &v_itr, -1.0, del_v );
04846     if( q_hat )
04847       Vp_StV( &z_itr, -1.0, del_z );
04848     //
04849     // Print rest of summary line for steps
04850     //
04851     if( out && (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04852       *out
04853         << right << setw(dbl_w) << norm_inf(del_v) / (norm_inf(v_itr) + small_num)
04854         << right << setw(dbl_w) << norm_inf(del_z) / (norm_inf(z_itr) + small_num)
04855         << endl;
04856     }
04857   }
04858   if( iter_refine_max_iter() == 0 ) {
04859     if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04860       *out
04861         << "\nWarning, iter_refine_max_iter == 0.  Iterative refinement was not performed."
04862         << "\nLeaving the original solution intact ...\n";
04863     }
04864     return_status = ITER_REFINE_NOT_PERFORMED;
04865   }
04866   else {
04867     if( iter_refine_max_iter() == 1 ) {
04868       if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04869         *out
04870           << "\nWarning, iter_refine_max_iter == 1.  Only one step of iterative refinement"
04871           << "was performed and the step is taken which out checking the residual ...\n";
04872       }
04873       *v = v_itr;
04874       if(q_hat)
04875         *z = z_itr;
04876       return_status = ITER_REFINE_ONE_STEP;
04877     }
04878     else if( roR_nrm_o < roR_nrm && rom_nrm_o < rom_nrm && ra_nrm_o < rom_nrm ) {
04879       if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04880         *out
04881           << "\nNo progress was made in reducing the residuals."
04882           << "\nLeaving the original solution intact ...\n";
04883       }
04884       return_status = ITER_REFINE_NOT_IMPROVED;
04885     }
04886     else {
04887       // The residuals were at least not increased so let's take the new solution
04888       *v = v_itr;
04889       if(q_hat)
04890         *z = z_itr;
04891       if( return_status != ITER_REFINE_CONVERGED && return_status != ITER_REFINE_NOT_NEEDED ) {
04892         if( (int)output_level >= (int)OUTPUT_ITER_SUMMARY ) {
04893           *out
04894             << "\nThe residuals were not converged but they were not increased either."
04895             << "\nTake the new point anyway ...\n";
04896         }
04897         return_status = ITER_REFINE_IMPROVED;
04898       }
04899     }
04900   }
04901   return return_status;
04902 }
04903 
04904 // private static member functions for QPSchur
04905 
04906 void QPSchur::dump_act_set_quantities(
04907   const ActiveSet& act_set, std::ostream& out
04908   ,bool print_S_hat
04909   )
04910 {
04911   using std::endl;
04912   using std::setw;
04913   using std::left;
04914   using std::right;
04915 
04916   const QPSchurPack::QP
04917     &qp = act_set.qp();
04918   const QPSchurPack::Constraints
04919     &constraints = qp.constraints();
04920 
04921   const int  int_w = 10;
04922   const char int_ul[] = "--------";
04923   const int  dbl_min_w = 20;
04924   const int  dbl_w = my_max(dbl_min_w,int(out.precision()+8));
04925   const char dbl_ul[] = "------------------";
04926 
04927     out << "\n*** Dumping the current active set ***\n"
04928     << "\nDimensions of the current active set:\n"
04929     << "\nn           = " << right << setw(int_w) << qp.n()         << " (Number of unknowns)"
04930     << "\nn_R         = " << right << setw(int_w) << qp.n_R()       << " (Number of initially free variables in Ko)"
04931     << "\nm           = " << right << setw(int_w) << qp.m()         << " (Number of initially fixed variables not in Ko)"
04932     << "\nm_breve     = " << right << setw(int_w) << constraints.m_breve()  << " (Number of extra general equality/inequality constriants)"
04933     << "\nq_hat       = " << right << setw(int_w) << act_set.q_hat()    << " (Number of augmentations to the initial KKT system Ko)"
04934     << "\nq_plus_hat  = " << right << setw(int_w) << act_set.q_plus_hat() << " (Number of added variable bounds and general constraints)"
04935     << "\nq_F_hat     = " << right << setw(int_w) << act_set.q_F_hat()    << " (Number of initially fixed variables not at their initial bound)"
04936     << "\nq_C_hat     = " << right << setw(int_w) << act_set.q_C_hat()    << " (Number of initially fixed variables at the other bound)"
04937     << "\nq_D_hat     = " << right << setw(int_w) << act_set.q_D_hat()    << " (Number of initially fixed variables still fixed at initial bound)"
04938     << endl;
04939 
04940   // Print table of quantities in augmented KKT system
04941   out << "\nQuantities for augmentations to the initial KKT system:\n";
04942   const size_type q_hat = act_set.q_hat();
04943   out << endl
04944     << right << setw(int_w) << "s"
04945     << right << setw(int_w) << "ij_map(s)"
04946     << right << setw(int_w) << "bnd(s)"
04947     << right << setw(dbl_w) << "constr_norm(s)"
04948     << right << setw(dbl_w) << "d_hat(s)"
04949     << right << setw(dbl_w) << "z_hat(s)"
04950     << right << setw(dbl_w) << "p_z_hat(s)"
04951     << endl;
04952   out << right << setw(int_w) << int_ul
04953     << right << setw(int_w) << int_ul
04954     << right << setw(int_w) << int_ul
04955     << right << setw(dbl_w) << dbl_ul
04956     << right << setw(dbl_w) << dbl_ul
04957     << right << setw(dbl_w) << dbl_ul
04958     << right << setw(dbl_w) << dbl_ul
04959     << endl;
04960   {for( size_type s = 1; s <= q_hat; ++s ) {
04961     out << right << setw(int_w) << s
04962       << right << setw(int_w) << act_set.ij_map(s)
04963       << right << setw(int_w) << bnd_str(act_set.bnd(s))
04964       << right << setw(dbl_w) << act_set.constr_norm(s)
04965       << right << setw(dbl_w) << act_set.d_hat()(s)
04966       << right << setw(dbl_w) << act_set.z_hat()(s)
04967       << right << setw(dbl_w) << act_set.p_z_hat()(s)
04968       << endl;
04969   }}
04970   
04971   // Print P_XF_hat, P_FC_hat, P_plus_hat, U_hat and S_hat
04972   out << "\nP_XF_hat =\n"   << act_set.P_XF_hat();
04973   out << "\nP_FC_hat =\n"   << act_set.P_FC_hat();
04974   out << "\nP_plus_hat =\n"   << act_set.P_plus_hat();
04975   out << "\nU_hat =\n"    << act_set.U_hat();
04976   if(print_S_hat)
04977     out << "\nS_hat =\n"    << act_set.S_hat();
04978   
04979   // Print table of multipliers for q_D_hat
04980   out << "\nQuantities for initially fixed variables which are still fixed at their initial bound:\n";
04981   const size_type q_D_hat = act_set.q_D_hat();
04982   out << endl
04983     << right << setw(int_w) << "k"
04984     << right << setw(int_w) << "l_fxfx(k)"
04985     << right << setw(dbl_w) << "mu_D_hat(k)"
04986     << right << setw(dbl_w) << "p_mu_D_hat(s)"
04987     << endl;
04988   out << right << setw(int_w) << int_ul
04989     << right << setw(int_w) << int_ul
04990     << right << setw(dbl_w) << dbl_ul
04991     << right << setw(dbl_w) << dbl_ul
04992     << endl;
04993   {for( size_type k = 1; k <= q_D_hat; ++k ) {
04994     out << right << setw(int_w) << k
04995       << right << setw(int_w) << act_set.l_fxfx(k)
04996       << right << setw(dbl_w) << act_set.mu_D_hat()(k)
04997       << right << setw(dbl_w) << act_set.p_mu_D_hat()(k)
04998       << endl;
04999   }}
05000   
05001   // Print Q_XD_hat
05002   out << "\nQ_XD_hat =\n" << act_set.Q_XD_hat();
05003 
05004   out << "\n*** End dump of current active set ***\n";
05005 }
05006 
05007 // QPSchurPack::QP
05008 
05009 void QPSchurPack::QP::dump_qp( std::ostream& out )
05010 {
05011   using std::endl;
05012   using std::setw;
05013   using std::left;
05014   using std::right;
05015   using Teuchos::Workspace;
05016   Teuchos::WorkspaceStore* wss = Teuchos::get_default_workspace_store().get();
05017 
05018   const Constraints
05019     &constraints = this->constraints();
05020 
05021   const size_type
05022     n = this->n(),
05023     n_R = this->n_R(),
05024     m = this->m(),
05025     m_breve = constraints.m_breve();
05026 
05027   out << "\n*** Original QP ***\n"
05028     << "\nn       = " << n
05029     << "\nm       = " << m
05030     << "\nm_breve = " << m_breve
05031     << endl;
05032   out << "\ng =\n" << g();
05033   out << "\nG =\n" << G();
05034   if(m) {
05035     out << "\nA =\n" << A();
05036     // Le'ts recover c from fo(n_R+1:n_R+m) = c - A' * Q_X * b_x
05037     throw std::logic_error(
05038       error_msg(__FILE__,__LINE__,"QPSchurPack::QP::dump_qp(...) : Error, "
05039             "m != not supported yet!"));  
05040     // ToDo: Implement this when needed!
05041   }
05042   out << "\nA_bar =\n" << constraints.A_bar();
05043   // Get c_L_bar and c_U_bar
05044   DVector c_L_bar(n+m_breve), c_U_bar(n+m_breve);
05045   {for( size_type j = 1; j <= n+m_breve; ++j ){
05046     c_L_bar(j) = constraints.get_bnd(j,LOWER);
05047     c_U_bar(j) = constraints.get_bnd(j,UPPER);
05048   }}
05049   out << "\nc_L_bar =\n" << c_L_bar();
05050   out << "\nc_U_bar =\n" << c_U_bar();
05051   
05052   out << "\n*** Initial KKT system (fixed and free variables) ***\n"
05053     << "\nn_R = " << n_R
05054     << endl;
05055   out << "\nb_X =\n" << b_X();
05056   out << "\nQ_R =\n" << Q_R();
05057   out << "\nQ_X =\n" << Q_X();
05058   out << "\nKo =\n" << Ko();
05059   out << "\nfo =\n" << fo();
05060 }
05061 
05062 } // end namespace ConstrainedOptPack

Generated on Tue Oct 20 12:51:45 2009 for MOOCHO (Single Doxygen Collection) by doxygen 1.4.7