qp_basis_inverse_impl.h

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

}


// replacing of slack by slack variable with precondition in QP-case
// for phaseII                               (update type UZ_4)
template < class ET_, class Is_LP_ >
template < class ForwardIterator >
void  QP_basis_inverse<ET_,Is_LP_>::
z_replace_slack_by_slack(ForwardIterator u_x_it, unsigned int k_j)
{

    // assert QP case and phaseII
    CGAL_qpe_assertion(is_QP && is_phaseII);

    // prepare \hat{v} -vector in x_l, x_x
    multiply(u_x_it, u_x_it, x_l.begin(), x_x.begin(),Tag_false(),
             Tag_false());
    x_l[k_j] -= d;
    
    // prepare \hat{\varrho} -vector in tmp_l, tmp_x
    copy_row_in_C(tmp_l.begin(), tmp_x.begin(), k_j);
    
    // prepare \hat{k}_{1} -scalar
    ET  hat_k_1 = inner_product_x(tmp_x.begin(), u_x_it);
    
    // prepare \hat{k}_{3} -scalar
    ET  hat_k_3 = -M[k_j][k_j];
    
    CGAL_qpe_assertion( d != et0);    
    
    // update matrix in place
    z_update_inplace(x_l.begin(), x_x.begin(), tmp_l.begin(), tmp_x.begin(),
                     hat_k_1 * hat_k_1, -hat_k_3, -hat_k_1, d * d);
		     
    // store new denominator
    d = CGAL::integral_division(hat_k_1 * hat_k_1, d);

    CGAL_qpe_assertion( d > et0);

    CGAL_qpe_debug {
        if ( vout.verbose()) print();
    }
      
}


// copying of reduced basis inverse row in (upper) C-half
template < class ET_, class Is_LP_ >
template < class OutIt >
void  QP_basis_inverse<ET_,Is_LP_>::
copy_row_in_C(OutIt y_l_it, OutIt y_x_it, unsigned int r)
{
    typename Matrix::const_iterator  matrix_it;
    typename Row   ::const_iterator     row_it;
    unsigned int  count;
    
    // P-block: left of diagonal (including element on diagonal)
    matrix_it = M.begin()+r;
    for (  row_it = matrix_it->begin();
           row_it != matrix_it->end(); 
	 ++row_it, ++y_l_it            ) {
        *y_l_it = *row_it;    
    }
    
    // P-block: right of diagonal (excluding element on diagonal)
    for (  matrix_it = M.begin()+r+1, count = r+1;
           count < s; 
	 ++matrix_it,  ++count,  ++y_l_it         ) {
        *y_l_it = (*matrix_it)[r];
    }
    
    // Q-block
    for (  matrix_it = M.begin()+l, count = 0;
           count < b;
	 ++matrix_it,  ++count,  ++y_x_it     ) {
	*y_x_it = (*matrix_it)[r]; 
    } 
}


// copying of reduced basis inverse row in (lower) B_O-half
template < class ET_, class Is_LP_ >
template < class OutIt >
void  QP_basis_inverse<ET_,Is_LP_>::
copy_row_in_B_O(OutIt y_l_it, OutIt y_x_it, unsigned int r)
{
    typename Matrix::const_iterator  matrix_it;
    typename Row   ::const_iterator     row_it;
    unsigned int  count;
    
    // Q-block
    matrix_it = M.begin()+l+r;
    for (  row_it = matrix_it->begin(), count = 0;
           count < s;
	 ++row_it,  ++count,  ++y_l_it           ) {
        *y_l_it = *row_it;
    }
    
    // R-block: left of diagonal (including element on diagonal)
    for (  row_it = matrix_it->begin()+l; 
           row_it != matrix_it->end();
	 ++row_it,  ++y_x_it            ) {
        *y_x_it = *row_it;
    }
    
    // R-block: right of diagonal (excluding element on diagonal)
    for (  matrix_it = M.begin()+l+r+1, count = r+1;
           count < b;
	 ++matrix_it,  ++count,  ++y_x_it           ) {
        *y_x_it = (*matrix_it)[l+r];
    }

}

template < class ET_, class Is_LP_ >
template < class ForIt >
void  QP_basis_inverse<ET_,Is_LP_>::
z_update_inplace( ForIt psi1_l_it, ForIt psi1_x_it,
                  ForIt psi2_l_it, ForIt psi2_x_it,
	             const ET& omega0, const ET& omega1,
		         const ET& omega2, const ET& omega3)
{
    typename Matrix::      iterator  matrix_it;
    typename Row   ::      iterator     row_it;
    typename Row   ::const_iterator      y_it1_r, y_it1_c, y_it2_r, y_it2_c;
	
    unsigned int  row, col, k = l+b;
    ET           u_elem;

    // rows: 0..s-1  ( P )
    for (  row = 0, matrix_it = M.begin(),
           y_it1_r = psi1_l_it,  y_it2_r = psi2_l_it;
	   row < s;
         ++row, ++matrix_it, ++y_it1_r, ++y_it2_r  ) {
	      
        // columns: 0..row  ( P )
        for (   row_it =  matrix_it->begin(),
	        y_it1_c = psi1_l_it,  y_it2_c = psi2_l_it;
                row_it != matrix_it->end();
              ++row_it,  ++y_it1_c,  ++y_it2_c            ) {
                
            u_elem = (*y_it1_r * *y_it2_c) + (*y_it2_r * *y_it1_c);
	    u_elem *= omega2;
	    u_elem += omega1 * *y_it1_r * *y_it1_c;
            update_entry( *row_it, omega0, u_elem, omega3);
        } 
    }
	
    // rows: l..k-1  ( Q R )
    for (  row = l, matrix_it = M.begin()+l,
	   y_it1_r = psi1_x_it,  y_it2_r = psi2_x_it;
	   row != k;
	 ++row,  ++matrix_it,  ++y_it1_r,  ++y_it2_r ) {
	    
        // columns: 0..s-1  ( Q )
        for (   col = 0,   row_it =  matrix_it->begin(),
	        y_it1_c = psi1_l_it,  y_it2_c = psi2_l_it;
                col < s;
              ++col, ++row_it,  ++y_it1_c,  ++y_it2_c     ){
    
            u_elem = (*y_it1_r * *y_it2_c) + (*y_it2_r * *y_it1_c);
	       u_elem *= omega2;
	       u_elem += omega1 * *y_it1_r * *y_it1_c; 
	       update_entry( *row_it, omega0, u_elem, omega3);
        }
    
        // columns: l..k-1  ( R )
        for (  row_it = matrix_it->begin()+l,
	       y_it1_c = psi1_x_it,  y_it2_c = psi2_x_it;
               row_it != matrix_it->end();
             ++row_it,  ++y_it1_c,  ++y_it2_c            ){
		 
            u_elem = (*y_it1_r * *y_it2_c) + (*y_it2_r * *y_it1_c);
            u_elem *= omega2;
	        u_elem += omega1 * *y_it1_r * *y_it1_c;     
            update_entry( *row_it, omega0, u_elem, omega3);
        }
	    
    } 
	
} 


// swap functions
// --------------
// swap variable ``to the end'' of R
template < class ET_, class Is_LP_ >                            // LP case
void  QP_basis_inverse<ET_,Is_LP_>::
swap_variable( unsigned int j, Tag_true)
{
    unsigned int  k = b-1;
    if ( j == k) return;

    // swap rows
    // ---------
    typename Row::iterator   row_j_it = M[ j].begin();
    typename Row::iterator   row_k_it = M[ k].begin();
    unsigned int  count;

    // swap entries 0..b-1 (row <-> row) [in Q]
    for (   count = 0;
            count < b;
          ++count,     ++row_j_it, ++row_k_it) {
        std::iter_swap( row_j_it, row_k_it);
    }
}

template < class ET_, class Is_LP_ >                            // QP case
void  QP_basis_inverse<ET_,Is_LP_>::
swap_variable( unsigned int j, Tag_false)
{
    unsigned int  i = l+j, k = l+b-1;
    if ( i == k) return;

    // swap rows and columns
    // ---------------------
    typename    Row::iterator   row_i_it = M[ i].begin();
    typename    Row::iterator   row_k_it = M[ k].begin();
    typename Matrix::iterator  matrix_it = M.begin()+(i+1);
    unsigned int  count;

    // swap entries 0..s-1 (row <-> row) [in Q]
    for (   count = 0;
            count < s;
          ++count,     ++row_i_it, ++row_k_it) {
        std::iter_swap( row_i_it, row_k_it);
    }

    if ( is_phaseII) {

	// swap entries l..i-1 (row <-> row) [in R]
	for (   count = l,   row_i_it += l-s,   row_k_it += l-s;
		count < i;
	      ++count,     ++row_i_it,        ++row_k_it       ) {
	    std::iter_swap( row_i_it, row_k_it);
	}

	// swap entries i+1..k-1 (column <-> row) [in R]
	for ( ++count,                        ++row_k_it;
		count < k;
	      ++count,     ++matrix_it,       ++row_k_it) {
	    std::swap( ( *matrix_it)[ i], *row_k_it);
	}

	// swap entries i,i with k,k (entry <-> entry) [in R]
	std::iter_swap( row_i_it, row_k_it);
    }
}

// swap constraint ``to the end'' of P
template < class ET_, class Is_LP_ >                            // LP case
void  QP_basis_inverse<ET_,Is_LP_>::
swap_constraint( unsigned int i, Tag_true)
{
    unsigned int  k = s-1;
    if ( i == k) return;

    // swap columns
    // ------------
    typename Matrix::iterator  matrix_it = M.begin();
    unsigned int  count;

    // swap entries 0..s-1 (column <-> column) [in Q]
    for (   count = 0;
            count < s;
          ++count,     ++matrix_it) {
        std::swap( ( *matrix_it)[ i], ( *matrix_it)[ k]);
    }
}

template < class ET_, class Is_LP_ >                            // QP case
void  QP_basis_inverse<ET_,Is_LP_>::
swap_constraint( unsigned int i, Tag_false)
{
 
    if ( i == s-1) return;

    // swap rows and columns
    // ---------------------
    typename    Row::iterator   row_i_it = M[ i].begin();
    typename    Row::iterator   row_k_it = M[ s-1].begin();
    typename Matrix::iterator  matrix_it = M.begin()+i;
    unsigned int  count;

    if ( is_phaseI) {

	// skip empty P
	matrix_it =M.begin() + l;

    } else {

	// swap entries 0..i-1 (row <-> row) [in P]
	for (   count =  0;
		count < i;
	      ++count,      ++row_i_it, ++row_k_it) {
	    std::iter_swap( row_i_it, row_k_it);
	}

	// swap entries i+1..s-2 (column <-> row) [in P]
	for ( count = i + 1, ++matrix_it, ++row_k_it;
		count < s-1;
	      ++count,     ++matrix_it, ++row_k_it) {
	    std::swap( ( *matrix_it)[ i], *row_k_it);
	}
	// the remaining two entries to be swapped on the main diagonal
	std::swap(M[i][i], M[s-1][s-1]);

	// advance to Q
	matrix_it = M.begin() + l;
    }

    // swap entries l..l+b (column <-> column) [in Q]
    for (   count = 0;
            count < b;
          ++count,     ++matrix_it) {
        std::swap( ( *matrix_it)[ i], ( *matrix_it)[ s-1]);
    }
}

// diagnostic output
// -----------------
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
print( )
{
    // P
    if ( is_LP || is_phaseII) {
	for ( unsigned int row = 0; row < s; ++row) {
	    std::copy( M[ row].begin(),
		       M[ row].begin() + ( is_LP ? s : row+1),
		       std::ostream_iterator<ET>( vout.out(), " "));
	    vout.out() << std::endl;
	}
	if ( is_QP) vout.out() << "= = = = = = = = = =" << std::endl;
    }

    // Q & R
    if ( is_QP) {
	for ( unsigned int row = l; row < l+b; ++row) {
	    std::copy( M[ row].begin(), M[ row].begin()+s,
		       std::ostream_iterator<ET>( vout.out(), " "));
	    if ( is_phaseII) {
		vout.out() << "|| ";
		std::copy( M[ row].begin()+l, M[ row].end(),
			   std::ostream_iterator<ET>( vout.out(), " "));
	    }
	    vout.out() << std::endl;
	}
    }
    vout.out() << "denominator = " << d << std::endl;
}

CGAL_END_NAMESPACE

// ===== EOF ==================================================================