qp_solver_impl.h

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

    if (no_ineq || (j < qp_n)) {              // original variable
    
        // updates for the upper bounded case:
        enter_variable_original_upd_w_r(Is_nonnegative());

	// enter original variable [ in: j ]:
	if (minus_c_B.size() <= B_O.size()) { // Note: minus_c_B and the
					      // containers resized in this
					      // if-block are only enlarged
					      // and never made smaller
					      // (while B_O always has the
					      // correct size). We check here
					      // whether we need to enlarge
					      // them.
	  CGAL_qpe_assertion(minus_c_B.size() == B_O.size());
	    minus_c_B.push_back(et0);
	        q_x_O.push_back(et0);
	      tmp_x  .push_back(et0);
	      tmp_x_2.push_back(et0);
	     two_D_Bj.push_back(et0);
	        x_B_O.push_back(et0);
	}
	minus_c_B[B_O.size()] = -ET(qp_c[ j]); // Note: B_O has always the
					       // correct size.
	
	in_B[j] = B_O.size();
	B_O.push_back(j);

	// diagnostic output
	CGAL_qpe_debug {
	    if (vout2.verbose())
	      print_basis();
	}
	    
	// update basis inverse
	// note: (-1)\hat{\nu} is stored instead of \hat{\nu}
	inv_M_B.enter_original(q_lambda.begin(), q_x_O.begin(), -nu);
	
    } else {                                  // slack variable

        // updates for the upper bounded case:
        enter_variable_slack_upd_w_r(Is_nonnegative());

	// enter slack variable [ in: j ]:
	in_B  [ j] = B_S.size();
	   B_S.push_back( j);
	   S_B.push_back( slack_A[ j-qp_n].first);

	// leave inequality constraint [ out: j ]:
	int old_row = slack_A[ j-qp_n].first;
	int k = in_C[old_row];
	
	// reflect change of active constraints heading C in b_C:
	b_C[ k] = b_C[C.size()-1];
		
	   C[ k] = C.back();
	in_C[ C.back()      ] = k;
	in_C[ old_row       ] = -1;
	   C.pop_back();
	
	// diagnostic output:
	CGAL_qpe_debug {
	    if (vout2.verbose())
	      print_basis();
	}

	// update basis inverse:
	inv_M_B.swap_constraint(k);  // swap to back
	inv_M_B.enter_slack();       // drop drop
    }

    // variable entered:
    j -= in_B.size();
}

// update of the vectors w and r for U_1 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Standard form      
void  QP_solver<Q, ET, Tags>::
enter_variable_original_upd_w_r(Tag_true )
{
}

// update of the vectors w and r for U_1 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Upper bounded     
void  QP_solver<Q, ET, Tags>::
enter_variable_original_upd_w_r(Tag_false )
{

    ET      x_j = nonbasic_original_variable_value(j);

    // Note: w needs to be updated before r_C, r_S_B
    update_w_r_B_O__j(x_j);
    update_r_C_r_S_B__j(x_j);
    
    // append w_j to r_B_O
    if (!check_tag(Is_linear())) // (kf.)
      r_B_O.push_back(w[j]);
    
    // update x_O_v_i
    x_O_v_i[j] = BASIC;
}

// update of the vectors w and r for U_3 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Standard form      
void  QP_solver<Q, ET, Tags>::
enter_variable_slack_upd_w_r(Tag_true )
{
}

// update of the vectors w and r for U_3 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Upper bounded     
void  QP_solver<Q, ET, Tags>::
enter_variable_slack_upd_w_r(Tag_false )
{
    
    int     sigma_j = slack_A[ j-qp_n].first;       
    
    // append r_gamma_C(sigma_j) to r_S_B
    r_S_B.push_back(r_C[in_C[sigma_j]]);
    
    // remove r_gamma_C(sigma_j) from r_C   
    r_C[in_C[sigma_j]] = r_C.back();
    r_C.pop_back();
}

// leave variable from basis
template < typename Q, typename ET, typename Tags >
void
QP_solver<Q, ET, Tags>::
leave_variable( )
{
    CGAL_qpe_debug {
	vout2 << "<-- basic (" << variable_type( i) << ") variable "
	      << i << " leaves basis" << std::endl << std::endl;
    }

    // update basis & basis inverse
    int  k = in_B[ i];
    if ( no_ineq || ( i < qp_n)) {                      // original variable
        
        // updates for the upper bounded case
        leave_variable_original_upd_w_r(Is_nonnegative());

	// leave original variable [ out: i ]
	in_B  [ B_O.back()] = k;
	in_B  [ i         ] = -1; 
	//in_B  [ B_O.back()] = k;
	   B_O[ k] = B_O.back(); B_O.pop_back();

	minus_c_B [ k] = minus_c_B [ B_O.size()];
	  two_D_Bj[ k] =   two_D_Bj[ B_O.size()];
	  

	// diagnostic output
	CGAL_qpe_debug {
	    if ( vout2.verbose()) print_basis();
	}

	// update basis inverse
	inv_M_B.swap_variable( k);
	inv_M_B.leave_original();

    } else {                                            // slack variable
        
        // updates for the upper bounded case
        leave_variable_slack_upd_w_r(Is_nonnegative());

	// leave slack variable [ out: i ]
	in_B  [ B_S.back()] = k;      // former last var moves to position k
	in_B  [ i         ] = -1;     // i gets deleted
	   B_S[ k] = B_S.back(); B_S.pop_back();
	   S_B[ k] = S_B.back(); S_B.pop_back();

	// enter inequality constraint [ in: i ]
	int new_row = slack_A[ i-qp_n].first;

	A_Cj[ C.size()] = ( j < qp_n ? ET( (*(qp_A + j))[ new_row]) : et0);

	 b_C[ C.size()] = ET( qp_b[ new_row]);
	in_C[ new_row ] = C.size();
	   C.push_back( new_row);

	// diagnostic output
	CGAL_qpe_debug {
	    if ( vout2.verbose()) print_basis();
	}

	// update basis inverse
	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.leave_slack( tmp_x.begin());
    }

    // notify pricing strategy
    strategyP->leaving_basis( i);

    // variable left
    i = -1;
}

// update of the vectors w and r for U_2 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Standard form      
void  QP_solver<Q, ET, Tags>::
leave_variable_original_upd_w_r(Tag_true )
{
}

// update of the vectors w and r for U_2 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Upper bounded      
void  QP_solver<Q, ET, Tags>::
leave_variable_original_upd_w_r(Tag_false )
{

    ET      x_i = (ratio_test_bound_index == LOWER) ? qp_l[i] : qp_u[i];
    
    // Note: w needs to be updated before r_C, r_S_B
    update_w_r_B_O__i(x_i);
    update_r_C_r_S_B__i(x_i);    
    
    // remove r_beta_O(i) from r_B_O
    if (!check_tag(Is_linear())) { // (kf.)
      r_B_O[in_B[i]] = r_B_O.back();
      r_B_O.pop_back();
    }
    
    // update x_O_v_i
    x_O_v_i[i] = ratio_test_bound_index;
}

// update of the vectors w and r for U_4 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Standard form      
void  QP_solver<Q, ET, Tags>::
leave_variable_slack_upd_w_r(Tag_true )
{
}

// update of the vectors w and r for U_4 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Upper bounded      
void  QP_solver<Q, ET, Tags>::
leave_variable_slack_upd_w_r(Tag_false )
{
    
    // append r_gamma_S_B(sigma_i) to r_C
    r_C.push_back(r_S_B[in_B[i]]);
    
    // remove r_gamma_S_B(sigma_i) from r_S_B
    r_S_B[in_B[i]] = r_S_B.back();
    r_S_B.pop_back();
}


// replace variable in basis QP-case, transition to Ratio Test Step 2
template < typename Q, typename ET, typename Tags >
void QP_solver<Q, ET, Tags>::
z_replace_variable( )
{
    CGAL_qpe_debug {
	vout2 <<   "<--> nonbasic (" << variable_type( j) << ") variable " << j
	      << " z_replaces basic (" << variable_type( i) << ") variable " << i
	      << std::endl << std::endl;
    }

    // replace variable
    z_replace_variable( no_ineq);

    // pivot step not yet completely done
    i = -1;
    j -= in_B.size();
    is_RTS_transition = true;
}


template < typename Q, typename ET, typename Tags >  inline                           // no inequalities
void QP_solver<Q, ET, Tags>::
z_replace_variable( Tag_true)
{
    z_replace_variable_original_by_original();
    strategyP->leaving_basis(i);

}


template < typename Q, typename ET, typename Tags >  inline                          // has inequalities
void QP_solver<Q, ET, Tags>::
z_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 and basis inverse
    if ( leave_original) {
        if ( enter_original) {               
	    z_replace_variable_original_by_original();
	} else {                             
	    z_replace_variable_original_by_slack();
	}
    } else {
        if ( enter_original) {
	    z_replace_variable_slack_by_original();
	} else {
	    z_replace_variable_slack_by_slack();
	}
    }
    strategyP->leaving_basis( i);
}


// replacement with precond det(M_{B \setminus \{i\}})=0
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
z_replace_variable_original_by_original( )
{
    // updates for the upper bounded case
    z_replace_variable_original_by_original_upd_w_r(Is_nonnegative());
    
    int  k = in_B[ i];

    // replace original variable [ in: j | out: i ]
    in_B  [ i] = -1;
    in_B  [ j] = k;
       B_O[ k] = j;

    minus_c_B[ k] = -ET( qp_c[ j]);

    // diagnostic output
    CGAL_qpe_debug {
	if ( vout2.verbose()) print_basis();
    }
	
    // compute s_delta
    D_pairwise_accessor  d_accessor( qp_D, j);
    ET                   s_delta =d_accessor(j)-d_accessor(i); 
    	    
    // update basis inverse
    // note: (-1)\hat{\nu} is stored instead of \hat{\nu}
    inv_M_B.z_replace_original_by_original( q_lambda.begin(), q_x_O.begin(), 
        s_delta, -nu, k);

}

// update of the vectors w and r for U_Z_1 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                            // Standard form      
void  QP_solver<Q, ET, Tags>::
z_replace_variable_original_by_original_upd_w_r(Tag_true )
{
}

// update of the vectors w and r for U_Z_1 with upper bounding, note that we 
// need the headings C, S_{B}, B_{O} before they are updated
template < typename Q, typename ET, typename Tags >                           
// Upper bounded      
void  QP_solver<Q, ET, Tags>::
z_replace_variable_original_by_original_upd_w_r(Tag_false )
{

    ET      x_j = nonbasic_original_variable_value(j);
    ET      x_i = (ratio_test_bound_index == LOWER) ? qp_l[i] : qp_u[i];
    
    // Note: w needs to be updated before r_C, r_S_B
    update_w_r_B_O__j_i(x_j, x_i);
    update_r_C_r_S_B__j_i(x_j, x_i);
    
    // replace r_beta_O(i) with w_j    
    r_B_O[in_B[i]] = w[j];
    
    // update x_O_v_i
    x_O_v_i[j] = BASIC;
    x_O_v_i[i] = ratio_test_bound_index;    
}


// replacement with precond det(M_{B \setminus \{i\}})=0
template < typename Q, typename ET, typename Tags >
void  QP_solver<Q, ET, Tags>::
z_replace_variable_original_by_slack( )
{
    // updates for the upper bounded case
    z_replace_variable_original_by_slack_upd_w_r(Is_nonnegative());
    
    int  k = in_B[ i];

    // leave original variable [ out: i ]
    in_B  [ B_O.back()] = k;
       B_O[ k] = B_O.back();
       in_B  [ i         ] = -1;
       B_O.pop_back();

    minus_c_B[ k] = minus_c_B[ B_O.size()];

    // enter slack variable [ in: j ]
    int  old_row = slack_A[ j-qp_n].first;
    in_B  [ j] = B_S.size();
       B_S.push_back( j);
       S_B.push_back( old_row);

    // leave inequality constraint [ out: j ]
    int  l = in_C[ old_row];
     b_C[ l       ] = b_C[ C.size()-1];
       C[ l       ] = C.back();
    in_C[ C.back()] = l;
    in_C[ old_row ] = -1;
       C.pop_back();
    
    // diagnostic output
    CGAL_qpe_debug {
	if ( vout2.verbose()) print_basi