qp_solver_impl.h

很多二维 三维几何计算算法 C++ 类库

// Copyright (c) 1997-2007  ETH Zurich (Switzerland).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL: svn+ssh://scm.gforge.inria.fr/svn/cgal/branches/CGAL-3.3-branch/QP_solver/include/CGAL/QP_solver/QP_solver_impl.h $
// $Id: QP_solver_impl.h 38453 2007-04-27 00:34:44Z gaertner $
// 
//
// Author(s)     : Sven Schoenherr
//                 Bernd Gaertner <[e-mail]>
//                 Franz Wessendorp
//                 Kaspar Fischer

#include <CGAL/QP_solver/Initialization.h>

CGAL_BEGIN_NAMESPACE

// =============================
// class implementation (cont'd)
// =============================

// transition (to phase II)
// ------------------------
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
transition( )
{
    CGAL_qpe_debug {
	if ( vout.verbose()) {
	    vout2 << std::endl
		  << "----------------------" << std::endl
		  << 'T';
	    vout1 << "[ t"; vout  << "ransition to phase II"; vout1 << " ]";
	    vout  << std::endl;
	    vout2 << "----------------------";
	}
    }

    // update status
    m_phase    = 2;
    is_phaseI  = false;
    is_phaseII = true;

    // remove artificial variables
    in_B.erase( in_B.begin()+qp_n+slack_A.size(), in_B.end());
    //ensure_size(tmp_x_2, tmp_x.size());
    // update basis inverse
    CGAL_qpe_debug {
	vout4 << std::endl << "basis-inverse:" << std::endl;
    }
    transition( Is_linear());
    CGAL_qpe_debug {
        check_basis_inverse();
    }

    // initialize exact version of `-qp_c' (implicit conversion to ET)
    C_by_index_accessor  c_accessor( qp_c);
    std::transform( C_by_index_iterator( B_O.begin(), c_accessor),
                    C_by_index_iterator( B_O.end  (), c_accessor),
                    minus_c_B.begin(), std::negate<ET>());
    
    // compute initial solution of phase II
    compute_solution(Is_nonnegative());

    // diagnostic output
    CGAL_qpe_debug {
	if ( vout.verbose()) print_solution();
    }

    // notify pricing strategy
    strategyP->transition();
}

// access
// ------
// numerator of current solution; the denominator is 2*d*d, so we should
// compute here d*d*(x^T2Dx + x^T2c + 2c0)
template < typename Q, typename ET, typename Tags >
ET QP_solver<Q, ET, Tags>::
solution_numerator( ) const
{
    ET   s, z = et0;
    int  i, j;

    if (check_tag(Is_nonnegative()) || is_phaseI) {
      // standard form or phase I; it suffices to go
      // through the basic variables; all D- and c-entries
      // are obtained through the appropriate iterators 
      Index_const_iterator  i_it;
      Value_const_iterator  x_i_it, c_it;

      // foreach i
      x_i_it =       x_B_O.begin();
      c_it = minus_c_B  .begin();
      for ( i_it = B_O.begin(); i_it != B_O.end(); ++i_it, ++x_i_it, ++c_it){
        i = *i_it;

        // compute quadratic part: 2D_i x
        s = et0;
        if ( is_QP && is_phaseII) {
	  // half the off-diagonal contribution
	  s += std::inner_product(x_B_O.begin(), x_i_it,
				  D_pairwise_iterator(
					 B_O.begin(),
					 D_pairwise_accessor( qp_D, i)),
				  et0);
	  // the other half
	  s *= et2;
	  // diagonal contribution
	  s += ET( (*(qp_D + i))[ i]) * *x_i_it;
        }
        // add linear part: 2c_i
        s -= d * et2 * ET( *c_it);

        // add x_i(2D_i x + 2c_i)
        z += s * *x_i_it; // endowed with a factor of d*d now
      }
    } else {
      // nonstandard form and phase II, 
      // take all original variables into account; all D- and c-entries
      // are obtained from the input data directly
      // order in i_it and j_it matches original variable order
      if (is_QP) {
	// quadratic part
	i=0;
	for (Variable_numerator_iterator 
	       i_it = this->original_variables_numerator_begin(); 
	     i_it < this->original_variables_numerator_end(); ++i_it, ++i) {
	  // do something only if *i_it != 0
	  if (*i_it == et0) continue;
	  s = et0; // contribution of i-th row
	  Variable_numerator_iterator j_it = 
	    this->original_variables_numerator_begin();
	  // half the off-diagonal contribution
	  j=0;
	  for (; j<i; ++j_it, ++j)
	    s += ET((*(qp_D+i))[j]) * *j_it;
	  // the other half
	  s *= et2;
	  // the diagonal entry
	  s += ET((*(qp_D+i))[j]) * *j_it;
	  // accumulate
	  z += s * *i_it;
	}
      }
      // linear part
      j=0; s = et0;
      for (Variable_numerator_iterator 
	     j_it = this->original_variables_numerator_begin();
	   j_it < this->original_variables_numerator_end(); ++j_it, ++j)
	s +=  et2 * ET(qp_c[j]) * *j_it;
      z += d * s;
    }
    // finally, add the constant term (phase II only)
    if (is_phaseII) z += et2 * ET(qp_c0) * d * d;
    return z;
}

// pivot step
// ----------
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
pivot_step( )
{
    ++m_pivots;

    // diagnostic output
    CGAL_qpe_debug {
        vout2 << std::endl
              << "==========" << std::endl
              << "Pivot Step" << std::endl
              << "==========" << std::endl;
    }
    vout  << "[ phase " << ( is_phaseI ? "I" : "II")
	  << ", iteration " << m_pivots << " ]" << std::endl;
    
	    
    // pricing
    // -------
    pricing();

	    
    // check for optimality
    if ( j < 0) {

        if ( is_phaseI) {                               // phase I
	    // since we no longer assume full row rank and subsys assumption
	    // we have to strengthen the precondition for infeasibility
            if (this->solution_numerator() > et0) {    
	      // problem is infeasible
	        m_phase  = 3;
	        m_status = QP_INFEASIBLE;
	        
		vout << "  ";
		vout << "problem is INFEASIBLE" << std::endl;
	       
	    } else {  // Drive/remove artificials out of basis
	        expel_artificial_variables_from_basis();
	        transition();
	    }
        } else {                                        // phase II

	    // optimal solution found
	    m_phase  = 3;
            m_status = QP_OPTIMAL;
  
	    vout << "  ";
	    vout  << "solution is OPTIMAL" << std::endl;
            
        }
        return;
    }

	    
    // ratio test & update (step 1)
    // ----------------------------
    // initialize ratio test
    ratio_test_init();
    

    // diagnostic output
    CGAL_qpe_debug {
	if ( vout2.verbose() && is_QP && is_phaseII) {
	    vout2.out() << std::endl
			<< "----------------------------" << std::endl
			<< "Ratio Test & Update (Step 1)" << std::endl
			<< "----------------------------" << std::endl;
	}
    }

    // loop (step 1)
    do {

        // ratio test
        ratio_test_1();

        // check for unboundedness
        if ( q_i == et0) {
            m_phase  = 3;
            m_status = QP_UNBOUNDED;
            
	    vout << "  ";
	    vout << "problem is UNBOUNDED" << std::endl;
	    
	    CGAL_qpe_debug {
		//nu should be zero in this case
		// note: (-1)/hat{\nu} is stored instead of \hat{\nu}
		// todo kf: as this is just used for an assertion check,
		// all the following lines should only be executed if
		// assertions are enabled...
		nu = inv_M_B.inner_product(     A_Cj.begin(), two_D_Bj.begin(),
		    q_lambda.begin(),    q_x_O.begin());
	        if (is_QP) {
		    if (j < qp_n) {
		        nu -= d*ET((*(qp_D+j))[j]);
		    }
		}
		CGAL_qpe_assertion(nu == et0);
            }
            return;
        }
	
        // update
        update_1();

    } while ( j >= 0);

    // ratio test & update (step 2)
    // ----------------------------
/*    
    if ( i >= 0) {

	// diagnostic output
	CGAL_qpe_debug {
	    vout2 << std::endl
		  << "----------------------------" << std::endl
		  << "Ratio Test & Update (Step 2)" << std::endl
		  << "----------------------------" << std::endl;
	}

	// compute index of entering variable
	j += in_B.size();

	// loop (step 2)
	while ( ( i >= 0) && basis_matrix_stays_regular()) {

	    // update
	    update_2( Is_linear());

	    // ratio test
	    ratio_test_2( Is_linear());
	}
    }
*/
    // instead of the above piece of code we now have
    // diagnostic output
    if (is_RTS_transition) {
        is_RTS_transition = false;
     
        CGAL_qpe_debug {
            vout2 << std::endl
		  << "----------------------------" << std::endl
		  << "Ratio Test & Update (Step 2)" << std::endl
		  << "----------------------------" << std::endl;
        }

        // compute index of entering variable
        j += in_B.size();

        ratio_test_2( Is_linear());
    
        while ((i >= 0) && basis_matrix_stays_regular()) {
        
	    update_2(Is_linear());
	
	    ratio_test_2(Is_linear());
	
        }
    } 



    // ratio test & update (step 3)
    // ----------------------------
    CGAL_qpe_assertion_msg( i < 0, "Step 3 should never be reached!");

    // diagnostic output
    
    if ( vout.verbose()) print_basis();
    if ( vout.verbose()) print_solution();

    // transition to phase II (if possible)
    // ------------------------------------
    if ( is_phaseI && ( art_basic == 0)) {
	CGAL_qpe_debug {
	    if ( vout2.verbose()) {
		vout2.out() << std::endl
			    << "all artificial variables are nonbasic"
			    << std::endl;
	    }
	}
	transition();
    }
}

// pricing
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
pricing( )
{
    // diagnostic output
    CGAL_qpe_debug {
	if ( vout2.verbose()) {
	    vout2 << std::endl
		  << "-------" << std::endl
		  << "Pricing" << std::endl
		  << "-------" << std::endl;
	}
    }

    // call pricing strategy
    j = strategyP->pricing(direction);

    // diagnostic output

    if ( vout.verbose()) {
      if ( j < 0) {
	CGAL_qpe_debug {
	  vout2 << "entering variable: none" << std::endl;
	}
      } else {
	vout  << "  ";
	vout  << "entering: ";
	vout  << j;
	CGAL_qpe_debug {
	  vout2 << " (" << variable_type( j) << ')' << std::endl;
	  vout2 << "direction: "
		<< ((direction == 1) ? "positive" : "negative") << std::endl;
	}
      }
    }
}

// initialization of ratio-test
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
ratio_test_init( )
{
    // store exact version of `A_Cj' (implicit conversion)
    ratio_test_init__A_Cj( A_Cj.begin(), j, no_ineq);

    // store exact version of `2 D_{B_O,j}'
    ratio_test_init__2_D_Bj( two_D_Bj.begin(), j, Is_linear());
}

template < typename Q, typename ET, typename Tags >                                         // no ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_init__A_Cj( Value_iterator A_Cj_it, int j_, Tag_true)
{
    // store exact version of `A_Cj' (implicit conversion)
    if ( j_ < qp_n) {                                   // original variable

	CGAL::copy_n( *(qp_A + j_), qp_m, A_Cj_it);

    } else {                                            // artificial variable

	unsigned int  k = j_;
	k -= qp_n;
	std::fill_n( A_Cj_it, qp_m, et0);