qp_solver.h

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

ratio_test_1__q_x_O( Tag_true)
{
  inv_M_B.multiply_x( A_Cj.begin(), q_x_O.begin());
}

template < typename Q, typename ET, typename Tags >  inline                                 // QP case
void  QP_solver<Q, ET, Tags>::
ratio_test_1__q_x_O( Tag_false)
{
  if ( is_phaseI) {                                   // phase I
    inv_M_B.multiply_x(     A_Cj.begin(),    q_x_O.begin());
  } else {                                            // phase II
    inv_M_B.multiply  (     A_Cj.begin(), two_D_Bj.begin(),
			    q_lambda.begin(),    q_x_O.begin());
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // no ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_1__q_x_S( Tag_true)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline                                 // has ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_1__q_x_S( Tag_false)
{
  // A_S_BxB_O * q_x_O
  multiply__A_S_BxB_O( q_x_O.begin(), q_x_S.begin());

  // ( A_S_BxB_O * q_x_O) - A_S_Bxj
  if ( j < qp_n) {
    std::transform( q_x_S.begin(),
		    q_x_S.begin()+S_B.size(),
		    A_by_index_iterator( S_B.begin(),
					 A_by_index_accessor( *(qp_A + j))),
		    q_x_S.begin(),
		    compose2_2( std::minus<ET>(),
				Identity<ET>(),
				std::bind1st( std::multiplies<ET>(), d)));
  }

  // q_x_S = -+ ( A_S_BxB_O * q_x_O - A_S_Bxj)
  Value_iterator  q_it = q_x_S.begin();
  Index_iterator  i_it;
  for ( i_it = B_S.begin(); i_it != B_S.end(); ++i_it, ++q_it) {
    if ( ! slack_A[ *i_it - qp_n].second) *q_it = -(*q_it);
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // no check
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_i( Index_iterator, Index_iterator,
		   Value_iterator, Value_iterator, Tag_true)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline                                 // check
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_i( Index_iterator i_it, Index_iterator end_it,
		   Value_iterator x_it, Value_iterator   q_it, Tag_false)
{
  // check `t_i's
  for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it) {
    if ( ( *q_it > et0) && ( ( *x_it * q_i) < ( x_i * *q_it))) {
      i = *i_it; x_i = *x_it; q_i = *q_it;
    }
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // LP case
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_j( Tag_true)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline                                 // QP case
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_j( Tag_false)
{
  if ( is_phaseII) {

    // compute `nu' and `mu_j' 
    mu = mu_j(j);
    nu = inv_M_B.inner_product(     A_Cj.begin(), two_D_Bj.begin(),
				    q_lambda.begin(),    q_x_O.begin());
    if ( j < qp_n) {                                // original variable
      nu -= d*ET( (*(qp_D + j))[ j]);
    }
    CGAL_qpe_assertion_msg(nu <= et0,
			   "nu <= et0 violated -- is your D matrix positive semidefinite?");

    // check `t_j'
    CGAL_qpe_assertion(mu != et0);
    // bg: formula below compares abs values, assuming mu < 0
    if ( ( nu < et0) && ( ( (mu < et0 ? mu : -mu) * q_i) > ( x_i * nu))) {
      i = -1; q_i = et1;
    }
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // LP case
void  QP_solver<Q, ET, Tags>::
ratio_test_2( Tag_true)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline                                 // no ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_2__p( Tag_true)
{
  // get column index of entering variable in basis
  int  col = in_B[ j];
 
  CGAL_qpe_assertion( col >= 0);
  col += l;

  // get (last) column of `M_B^{-1}' (Note: `p_...' is stored in `q_...')
  Value_iterator  it;
  int             row;
  unsigned int    k;
  for (   k = 0,            row = 0,   it = q_lambda.begin();
	  k < C.size();
	  ++k,              ++row,     ++it                   ) {
    *it = inv_M_B.entry( row, col);
  }
  for (   k = 0,            row = l,   it = q_x_O.begin();
	  k < B_O.size();
	  ++k,              ++row,     ++it                   ) {
    *it = inv_M_B.entry( row, col);
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // has ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_2__p( Tag_false)
{
  Value_iterator  v_it;
  Index_iterator  i_it;

  // compute 'p_lambda' and 'p_x_O' (Note: `p_...' is stored in `q_...')
  // -------------------------------------------------------------------

  // type of entering variable
  if ( j < qp_n) {                                        // original

    // use 'no_ineq' variant
    ratio_test_2__p( Tag_true());

  } else {                                                // slack

    j -= qp_n;

    // get column A_{S_j,B_O}^T (i.e. row of A_{S_B,B_O})
    int             row  = slack_A[ j].first;
    bool            sign = slack_A[ j].second;

    for (   i_it =  B_O.begin(),   v_it = tmp_x.begin();
	    i_it != B_O.end();
	    ++i_it,                ++v_it                ) {
      *v_it = ( sign ? 
		(*(qp_A+ *i_it))[ row] : - (*(qp_A + *i_it))[ row]);
    }

    // compute  ( p_l | p_x_O )^T = M_B^{-1} * ( 0 | A_{S_j,B_O} )^T
    std::fill_n( tmp_l.begin(), C.size(), et0);
    inv_M_B.multiply( tmp_l     .begin(), tmp_x  .begin(),
		      q_lambda.begin(),   q_x_O.begin());

    j += qp_n;
  }

  // compute 'p_x_S'
  // ---------------
  // A_S_BxB_O * p_x_O
  multiply__A_S_BxB_O( q_x_O.begin(), q_x_S.begin());

  // p_x_S = +- ( A_S_BxB_O * p_x_O)
  for (   i_it =  B_S.begin(),   v_it = q_x_S.begin();
	  i_it != B_S.end();
	  ++i_it,                ++v_it                ) {
    if ( ! slack_A[ *i_it - qp_n].second) *v_it = -(*v_it);
  }
}

// update
// ------
template < typename Q, typename ET, typename Tags >  inline                                 // LP case
void  QP_solver<Q, ET, Tags>::
update_1( Tag_true)
{
  // replace leaving with entering variable
  if ((i == j) && (i >= 0)) {
    enter_and_leave_variable();
  } else {
    replace_variable();
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // QP case
void  QP_solver<Q, ET, Tags>::
update_1( Tag_false)
{
  if ( is_phaseI) {                                   // phase I

    // replace leaving with entering variable
    if ((i == j) && (i >= 0)) {
      enter_and_leave_variable();
    } else {
      replace_variable();
    }

  } else {                                            // phase II
        
    if ((i == j) && (i >= 0)) {
      enter_and_leave_variable();
    } else {

      if ( ( i >= 0) && basis_matrix_stays_regular()) {

	// leave variable from basis, if
	// - some leaving variable was found  and
	// - basis matrix stays regular
	leave_variable();

      } else {

	// enter variable into basis, if
	// - no leaving variable was found  or
	// - basis matrix would become singular when variable i leaves

	if ( i < 0 ) {
	  enter_variable();
	} else {
	  z_replace_variable();
	}
      }
    }
  }
}

template < typename Q, typename ET, typename Tags >  inline                                 // LP case
void  QP_solver<Q, ET, Tags>::
update_2( Tag_true)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline                                 // no ineq.
void  QP_solver<Q, ET, Tags>::
replace_variable( Tag_true)
{
  replace_variable_original_original();
  strategyP->leaving_basis( i);
}

template < typename Q, typename ET, typename Tags >  inline                                 // has ineq.
void  QP_solver<Q, ET, Tags>::
replace_variable( Tag_false)
{
  // determine type of variables
  bool  enter_original = ( (j < qp_n) || (j >= (int)( qp_n+slack_A.size())));
  bool  leave_original = ( (i < qp_n) || (i >= (int)( qp_n+slack_A.size())));

  // update basis & basis inverse
  if ( leave_original) {
    if ( enter_original) {                              // orig  <--> orig
      replace_variable_original_original();
    } else {                                            // slack <--> orig
      replace_variable_slack_original();
    }

    // special artificial variable removed?
    if ( is_phaseI && ( i == art_s_i)) {
      // remove the fake column - it corresponds
      // to the special artificial variable which is
      // (like all artificial variables) not needed
      // anymore once it leaves the basis. Note:
      // regular artificial variables are only removed
      // from the problem after phase I
      // art_s_i == -1 -> there is no special artificial variable
      // art_s_i == -2 -> there was a special artificial variable, 
      // but has been removed  
      art_s_i = -2;
      art_A.pop_back();
      CGAL_qpe_assertion(in_B[in_B.size()-1] == -1); // really removed?
      in_B.pop_back();
      // BG: shouldn't the pricing strategy be notfied also here?
    } else {
      strategyP->leaving_basis( i);
    }
  } else {
    if ( enter_original) {                              // orig  <--> slack
      replace_variable_original_slack();
    } else {                                            // slack <--> slack
      replace_variable_slack_slack();
    }
    strategyP->leaving_basis( i);
  }
}

template < typename Q, typename ET, typename Tags >  inline
bool  QP_solver<Q, ET, Tags>::
basis_matrix_stays_regular()
{
  CGAL_qpe_assertion( is_phaseII);
  int new_row, k;
    
  if ( has_ineq && (i >= qp_n)) {	// slack variable
    new_row = slack_A[ i-qp_n].first;
    A_row_by_index_accessor  a_accessor( A_accessor( qp_A, 0, qp_n), new_row);
    std::copy( A_row_by_index_iterator( B_O.begin(), a_accessor),
	       A_row_by_index_iterator( B_O.end  (), a_accessor),
	       tmp_x.begin());	   
    inv_M_B.multiply( tmp_x.begin(),                        // dummy (not used)
		      tmp_x.begin(), tmp_l_2.begin(), tmp_x_2.begin(),
		      Tag_false(),                                 // QP
		      Tag_false());                             // ignore 1st argument
    return ( -inv_M_B.inner_product_x( tmp_x_2.begin(), tmp_x.begin()) != et0);

	
  } else {						// check original variable
    k = l+in_B[ i];
    return ( inv_M_B.entry( k, k) != et0);
  }

  /* ToDo: check, if really not needed in 'update_1':
     - basis has already minimal size  or
     || ( B_O.size()==C.size()) 
  */
}

// current solution
// ----------------
template < typename Q, typename ET, typename Tags >  inline             // no inequalities, upper bounded
void  QP_solver<Q, ET, Tags>::
compute__x_B_S( Tag_true  /*has_equalities_only_and_full_rank*/,
                Tag_false /*is_nonnegative*/)
{
  // nop
}

template < typename Q, typename ET, typename Tags >  inline             // no inequalities, standard form
void  QP_solver<Q, ET, Tags>::
compute__x_B_S( Tag_true /*has_equalities_only_and_full_rank*/,
                Tag_true /*is_nonnegative*/)
{
  // nop
}


template < typename Q, typename ET, typename Tags >  inline             // has inequalities, upper bounded
void  QP_solver<Q, ET, Tags>::
compute__x_B_S( Tag_false /*has_equalities_only_and_full_rank*/,
                Tag_false /*is_nonnegative*/)
{
  // A_S_BxB_O * x_B_O
  multiply__A_S_BxB_O( x_B_O.begin(), x_B_S.begin());

  // b_S_B - ( A_S_BxB_O * x_B_O)
  B_by_index_accessor  b_accessor( qp_b);
  std::transform( B_by_index_iterator( S_B.begin(), b_accessor),
		  B_by_index_iterator( S_B.end  (), b_accessor),
		  x_B_S.begin(),
		  x_B_S.begin(),
		  compose2_2( std::minus<ET>(),
			      std::bind1st( std::multiplies<ET>(), d),
			      Identity<ET>()));
				
  // b_S_B - ( A_S_BxB_O * x_B_O) - r_S_B
  std::transform(x_B_S.begin(), x_B_S.begin()+S_B.size(),
		 r_S_B.begin(), x_B_S.begin(),
		 compose2_2(std::minus<ET>(),
			    Identity<ET>(),
			    std::bind1st( std::multiplies<ET>(), d)));
                        

  // x_B_S = +- ( b_S_B - A_S_BxB_O * x_B_O)
  Value_iterator