qp_solver_impl.h

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

	A_Cj_it[ k] = ( art_A[ k].second ? -et1 : et1);
    }
}

template < typename Q, typename ET, typename Tags >                                        // has ineq.
void  QP_solver<Q, ET, Tags>::
ratio_test_init__A_Cj( Value_iterator A_Cj_it, int j_, Tag_false)
{
    // store exact version of `A_Cj' (implicit conversion)
    if ( j_ < qp_n) {                                   // original variable
	A_by_index_accessor  a_accessor( *(qp_A + j_));
	std::copy( A_by_index_iterator( C.begin(), a_accessor),
		   A_by_index_iterator( C.end  (), a_accessor),
		   A_Cj_it);

    } else {
	unsigned int  k = j_;
	k -= qp_n;
	std::fill_n( A_Cj_it, C.size(), et0);

	if ( k < slack_A.size()) {                      // slack variable

	    A_Cj_it[ in_C[ slack_A[ k].first]] = ( slack_A[ k].second ? -et1
						                      :  et1);

	} else {                                        // artificial variable
	    k -= slack_A.size();

	    if ( j_ != art_s_i) {                           // normal art.

		A_Cj_it[ in_C[ art_A[ k].first]] = ( art_A[ k].second ? -et1
						                      :  et1);

	    } else {                                        // special art.
		S_by_index_accessor  s_accessor( art_s.begin());
		std::copy( S_by_index_iterator( C.begin(), s_accessor),
			   S_by_index_iterator( C.end  (), s_accessor),
			   A_Cj_it);
	    }	
	}
    }
}

// ratio test (step 1)
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
ratio_test_1( )
{

    // diagnostic output
    CGAL_qpe_debug {
	if ( vout2.verbose()) {
	    vout2.out() << std::endl;
	    if ( is_LP || is_phaseI) {
		vout2.out() << "----------" << std::endl
			    << "Ratio Test" << std::endl
			    << "----------" << std::endl;
	    } else {
		vout2.out() << "Ratio Test (Step 1)" << std::endl
			    << "-------------------" << std::endl;
	    }
	    if ( vout3.verbose()) {
		vout3.out() << "    A_Cj: ";
		std::copy( A_Cj.begin(), A_Cj.begin()+C.size(),
			   std::ostream_iterator<ET>( vout3.out()," "));
		vout3.out() << std::endl;
		if ( is_QP && is_phaseII) {
		    vout3.out() << "  2 D_Bj: ";
		    std::copy( two_D_Bj.begin(), two_D_Bj.begin()+B_O.size(),
			       std::ostream_iterator<ET>( vout3.out()," "));
		    vout3.out() << std::endl;
		}
		vout3.out() << std::endl;
	    }
	}
    }
    
    // compute `q_lambda' and `q_x'
    ratio_test_1__q_x_O( Is_linear());
    ratio_test_1__q_x_S( no_ineq);

    // diagnostic output
    CGAL_qpe_debug {
	if ( vout3.verbose()) {
	    if ( is_QP && is_phaseII) {
		vout3.out() << "q_lambda: ";
		std::copy( q_lambda.begin(), q_lambda.begin()+C.size(),
			   std::ostream_iterator<ET>( vout3.out()," "));
		vout3.out() << std::endl;
	    }
	    vout3.out() << "   q_x_O: ";
	    std::copy( q_x_O.begin(), q_x_O.begin()+B_O.size(),
		       std::ostream_iterator<ET>( vout3.out()," "));
	    vout3.out() << std::endl;

	    if ( has_ineq) {
		vout3.out() << "   q_x_S: ";
		std::copy( q_x_S.begin(), q_x_S.begin()+B_S.size(),
			   std::ostream_iterator<ET>( vout3.out()," "));
		vout3.out() << std::endl;
	    }
	    vout3.out() << std::endl;
	}
    }

    // check `t_i's
    x_i = et1;                                          // trick: initialize
    q_i = et0;                                          // minimum with +oo

    // computation of t_{min}^{j}
    ratio_test_1__t_min_j(Is_nonnegative());
    CGAL_qpe_debug { // todo kf: at first sight, this debug message should
                     // only be output for problems in nonstandard form...
        if (vout2.verbose()) {
            vout2.out() << "t_min_j: " << x_i << '/' << q_i << std::endl;
            vout2.out() << std::endl;
        }
    }    

    // what happens, if all original variables are nonbasic?
/*
    ratio_test_1__t_i(   B_O.begin(),   B_O.end(),
		       x_B_O.begin(), q_x_O.begin(), Tag_false());
    ratio_test_1__t_i(   B_S.begin(),   B_S.end(),
		       x_B_S.begin(), q_x_S.begin(), no_ineq);
*/		       
    ratio_test_1__t_min_B(no_ineq);    

    // check `t_j'
    ratio_test_1__t_j( Is_linear());

    // diagnostic output
    CGAL_qpe_debug {
        if ( vout2.verbose()) {
            for ( unsigned int k = 0; k < B_O.size(); ++k) {
                print_ratio_1_original(k, x_B_O[k], q_x_O[k]);
            }     
            if ( has_ineq) {
                for ( unsigned int k = 0; k < B_S.size(); ++k) {
                    /*
                    vout2.out() << "t_S_" << k << ": "
				    << x_B_S[ k] << '/' << q_x_S[ k]
				    << ( ( q_i > et0) && ( i == B_S[ k]) ? " *":"")
				    << std::endl;
				    */
				    print_ratio_1_slack(k, x_B_S[k], q_x_S[k]);
                }
            }
	    if ( is_QP && is_phaseII) {
		vout2.out() << std::endl
			    << "  t_j: " << mu << '/' << nu
			    << ( ( q_i > et0) && ( i < 0) ? " *" : "")
			    << std::endl;
	    }
	    vout2.out() << std::endl;
	}
    }
    if ( q_i > et0) {
      if ( i < 0) {
	vout2 << "leaving variable: none" << std::endl;
      } else {
	vout << ", ";
	vout  << "leaving: ";
	vout  << i;
	CGAL_qpe_debug {
	  if ( vout2.verbose()) {
	    if ( ( i < qp_n) || ( i >= (int)( qp_n+slack_A.size()))) {
	      vout2.out() << " (= B_O[ " << in_B[ i] << "]: "
			  << variable_type( i) << ')';
	    } else {
	      vout2.out() << " (= B_S[ " << in_B[ i] << "]: slack)";
	    }
	  }
	  vout2 << std::endl;
	}
	
      }
    }
    
}


template < typename Q, typename ET, typename Tags >                         // Standard form
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_min_j(Tag_true /*is_nonnegative*/)
{
}

// By the pricing step we have the following precondition
// direction == +1 => x_O_v_i[j] == (LOWER v ZERO)
// direction == -1 => x_O_v_i[j] == (UPPER v ZERO) 
template < typename Q, typename ET, typename Tags >                         // Upper bounded
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_min_j(Tag_false /*is_nonnegative*/)
{
    if (j < qp_n) {                                 // original variable
        if (direction == 1) {
            if (x_O_v_i[j] == LOWER) {              // has lower bound value
                if (*(qp_fu+j)) {                   // has finite upper bound
                    x_i = (qp_u[j] - qp_l[j]);
                    q_i = et1;
                    i = j;
                    ratio_test_bound_index = UPPER;
                } else {                            // has infinite upper bound
                    x_i = et1;
                    q_i = et0;
                }
            } else {                                // has value zero
                if (*(qp_fu+j)) {                   // has finite upper bound
                    x_i = qp_u[j];
                    q_i = et1;
                    i = j;
                    ratio_test_bound_index = UPPER;
                } else {                            // has infinite upper bound
                    x_i = et1;
                    q_i = et0;                    
                }
            }
        } else {                                    // direction == -1
            if (x_O_v_i[j] == UPPER) {              // has upper bound value
                if (*(qp_fl+j)) {                   // has finite lower bound
                    x_i = (qp_u[j] - qp_l[j]);
                    q_i = et1;
                    i = j;
                    ratio_test_bound_index = LOWER;
                } else {                            // has infinite lower bound
                    x_i = et1;
                    q_i = et0;
                }
            } else {                                // has value zero
                if (*(qp_fl+j)) {                   // has finite lower bound
                    x_i = -qp_l[j];
                    q_i = et1;
                    i = j;
                    ratio_test_bound_index = LOWER;
                } else {                            // has infinite lower bound
                    x_i = et1;
                    q_i = et0;
                }
            }
        }
    } else {                                        // slack or artificial var
        x_i = et1;
        q_i = et0;
    }
}

template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_min_B(Tag_true  /*has_equalities_only_and_full_rank*/)
{
    ratio_test_1_B_O__t_i(B_O.begin(), B_O.end(), x_B_O.begin(),
                        q_x_O.begin(), Is_nonnegative());
}

template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
ratio_test_1__t_min_B(Tag_false /*has_equalities_only_and_full_rank*/)
{
    ratio_test_1_B_O__t_i(B_O.begin(), B_O.end(), x_B_O.begin(),
                        q_x_O.begin(), Is_nonnegative());
    ratio_test_1_B_S__t_i(B_S.begin(), B_S.end(), x_B_S.begin(),
                        q_x_S.begin(), Is_nonnegative());
}    

// ratio test for the basic original variables
template < typename Q, typename ET, typename Tags >                         // Standard form
void  QP_solver<Q, ET, Tags>::
ratio_test_1_B_O__t_i(Index_iterator i_it, Index_iterator end_it,
                    Value_iterator x_it, Value_iterator q_it,
                    Tag_true  /*is_nonnegative*/)
{    
    for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
        test_implicit_bounds_dir_pos(*i_it, *x_it, *q_it, i, x_i, q_i);
    }
}

// ratio test for the basic original variables                    
template < typename Q, typename ET, typename Tags >                         // Upper bounded
void  QP_solver<Q, ET, Tags>::
ratio_test_1_B_O__t_i(Index_iterator i_it, Index_iterator end_it,
                    Value_iterator x_it, Value_iterator q_it,
                    Tag_false /*is_nonnegative*/)
{
    if (is_phaseI) {
        if (direction == 1) {
            for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
                test_mixed_bounds_dir_pos(*i_it, *x_it, *q_it, i, x_i, q_i);
            }
        } else {
            for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
                test_mixed_bounds_dir_neg(*i_it, *x_it, *q_it, i, x_i, q_i);
            }
        }
    } else {
        if (direction == 1) {
            for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
                test_explicit_bounds_dir_pos(*i_it, *x_it, *q_it, i, x_i, q_i);
            }
        } else {
            for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
                test_explicit_bounds_dir_neg(*i_it, *x_it, *q_it, i, x_i, q_i);
            }
        }
    }
}

// ratio test for the basic slack variables
template < typename Q, typename ET, typename Tags >                         // Standard form
void  QP_solver<Q, ET, Tags>::
ratio_test_1_B_S__t_i(Index_iterator i_it, Index_iterator end_it,
                Value_iterator x_it, Value_iterator q_it,
                Tag_true  /*is_nonnegative*/)
{
    for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
        test_implicit_bounds_dir_pos(*i_it, *x_it, *q_it, i, x_i, q_i);
    }
}

// ratio test for the basic slack variables
template < typename Q, typename ET, typename Tags >                         // Upper bounded
void  QP_solver<Q, ET, Tags>::
ratio_test_1_B_S__t_i(Index_iterator i_it, Index_iterator end_it,
                Value_iterator x_it, Value_iterator q_it,
                Tag_false /*is_nonnegative*/)
{
    if (direction == 1) {
        for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
            test_implicit_bounds_dir_pos(*i_it, *x_it, *q_it, i, x_i, q_i);
        }
    } else {
        for ( ; i_it != end_it; ++i_it, ++x_it, ++q_it ) {
            test_implicit_bounds_dir_neg(*i_it, *x_it, *q_it, i, x_i, q_i);
        }    
    }
}

// test for one basic variable with implicit bounds only,
// note that this function writes the member variables i, x_i, q_i
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
test_implicit_bounds_dir_pos(int k, const ET& x_k, const ET& q_k, 
                                int& i_min, ET& d_min, ET& q_min)
{
    if ((q_k > et0) && (x_k * q_min < d_min * q_k)) {
        i_min = k;
        d_min = x_k;
        q_min = q_k;
    }
}

// test for one basic variable with implicit bounds only,
// note that this function writes the member variables i, x_i, q_i
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
test_implicit_bounds_dir_neg(int k, const ET& x_k, const ET& q_k, 
                                int& i_min, ET& d_min, ET& q_min)
{
    if ((q_k < et0) && (x_k * q_min < -(d_min * q_k))) {
        i_min = k;
        d_min = x_k;
        q_min = -q_k;
    }
}

// test for one basic variable with explicit bounds only,
// note that this function writes the member variables i, x_i, q_i and
// ratio_test_bound_index, although the second and third variable name
// are in the context of upper bounding misnomers
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
test_explicit_bounds_dir_pos(int k, const ET& x_k, const ET& q_k, 
                                int& i_min, ET& d_min, ET& q_min)
{
    if (q_k > et0) {                                // check for lower bound
        if (*(qp_fl+k)) {
            ET  diff = x_k - (d * ET(qp_l[k])); 
            if (diff * q_min < d_min * q_k) {
                i_min = k;
                d_min = diff;
                q_min = q_k;
                ratio_test_bound_index = LOWER;
            }
        }
    } else {                                        // check for upper bound
        if ((q_k < et0) && (*(qp_fu+k))) {
            ET  diff = (d * ET(qp_u[k])) - x_k;
            if (diff * q_min < -(d_min * q_k)) {
                i_min = k;
                d_min = diff;
                q_min = -q_k;
                ratio_test_bound_index = UPPER;
            }    
        }
    }
}

// test for one basic variable with explicit bounds only,
// note that this function writes the member variables i, x_i, q_i and
// ratio_test_bound_index, although the second and third variable name
// are in the context of upper bounding misnomers
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
test_explicit_bounds_dir_neg(int k, const ET& x_k, const ET& q_k, 
                                int& i_min, ET& d_min, ET& q_min)
{
    if (q_k < et0) {                                // check for lower bound
        if (*(qp_fl+k)) {
            ET  diff = x_k - (d * ET(qp_l[k])); 
            if (diff * q_min < -(d_min * q_k)) {
                i_min = k;
                d_min = diff;
                q_min = -q_k;
                ratio_test_bound_index = LOWER;
            }
        }
    } else {                                        // check for upper bound
        if ((q_k > et0) && (*(qp_fu+k))) {
            ET  diff = (d * ET(qp_u[k])) - x_k;
            if (diff * q_min < d_min * q_k) {
                i_min = k;