qp_basis_inverse.h

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

	b = art_size;
    }
    
    // append row in Q if no allocated row available
    void ensure_physical_row (unsigned int row) {
    	unsigned int rows = M.size();
	CGAL_qpe_assertion(rows >= row);
	if (rows == row) {
            M.push_back(Row(row+1, et0));
	    
	    // do we have to grow x_x?
	    CGAL_qpe_assertion(x_x.size() >= row-l);
	    if (x_x.size() == row-l)
	       x_x.push_back(et0);
	    
	    // do we have to grow tmp_x?
	    CGAL_qpe_assertion(tmp_x.size() >= row-l);
	    if (tmp_x.size() == row-l)
	       tmp_x.push_back(et0);
	    
            CGAL_qpe_assertion(M[row].size()==row+1);
	    CGAL_qpe_debug {
	      if ( vout.verbose()) {
                    vout << "physical row " << (row) << " appended in Q\n";
	      }
            }
	}
    }
    
    // matrix-vector multiplication (QP case)
    template < class ForIt, class OutIt, class Use1stArg >
    void
    multiply( ForIt v_l_it, ForIt v_x_it,
              OutIt y_l_it, OutIt y_x_it, Tag_false,
	      Use1stArg use_1st_arg) const
    {
	// use 'LP' functions in phase I
	if ( is_phaseI) {
	    multiply__l( v_x_it, y_l_it);
	    multiply__x( v_l_it, y_x_it);
	    return;
	}

	// phase II
        typename Matrix::const_iterator  matrix_it;
        typename Row   ::const_iterator     row_it;     // left  of diagonal
        typename Matrix::const_iterator  column_it;     // right of diagonal
        ForIt                                 v_it;
    
        unsigned int  row, count, k = l+b;
        ET            sum;
    
        // compute  P v_l + Q^T v_x	
        for (   row = 0,   matrix_it = M.begin();
                row < s;
              ++row,                                                ++y_l_it) {
            sum = et0;

	    // P v_l
	    if ( check_tag( use_1st_arg)) {

		// P: left of diagonal (including)
		for (   row_it =  matrix_it->begin(),            v_it = v_l_it;
			row_it != matrix_it->end();
		      ++row_it,                                ++v_it) {
		    sum += *row_it * *v_it;
		}

		// P: right of diagonal (excluding)
		for (   count = row+1,   column_it  = ++matrix_it;
			count < s;
		      ++count,         ++column_it,                ++v_it) {
		    sum += (*column_it)[ row] * *v_it;
		}
	    }
    
            // Q^T:
            for (   count = 0,       column_it  = M.begin()+l,   v_it = v_x_it;
                    count < b;
                  ++count,         ++column_it,                ++v_it) {
                sum += (*column_it)[ row] * *v_it;
            }
    
            // store result
            *y_l_it = sum;
        }
    
        // compute  Q v_l + R v_x
        for (   row = l,   matrix_it = M.begin()+l;
                row < k;
              ++row,                                                ++y_x_it) {
            sum = et0;

	    // Q v_l
	    if ( check_tag( use_1st_arg)) {

		// Q:
		for (   count = 0,  row_it = matrix_it->begin(), v_it = v_l_it;
			count < s;
		      ++count,    ++row_it,                    ++v_it) {
		    sum += *row_it * *v_it;
		}
	    }
    
	    // R: left of diagonal (including)
            for (                row_it =  matrix_it->begin()+l, v_it = v_x_it;
                                 row_it != matrix_it->end();
                               ++row_it,                       ++v_it) {
                sum += *row_it * *v_it;
            }
    
            // R: right of diagonal (excluding)
            for (   count = row+1,   column_it = ++matrix_it;
                    count < k;
                  ++count,         ++column_it,                ++v_it) {
                sum += (*column_it)[ row] * *v_it;
            }
    
            // store result
            *y_x_it = sum;
        }
    }
    
    // matrix-vector multiplication (LP case)
    template < class ForIt, class OutIt, class Dummy > inline
    void
    multiply( ForIt v_l_it, ForIt v_x_it,
              OutIt y_l_it, OutIt y_x_it, Tag_true, Dummy) const
    {
        multiply__l( v_x_it, y_l_it);
        multiply__x( v_l_it, y_x_it);
    }
    
    // special matrix-vector multiplication functions for LPs
    template < class ForIt, class OutIt > inline
    void
    multiply__l( ForIt v_x_it, OutIt y_l_it) const
    {
        typename Matrix::const_iterator  matrix_it = M.begin();
        typename Matrix::const_iterator  column_it;
        ForIt                                 v_it;
    
        unsigned int  row, count;
        ET            sum;
    
	// QP?
	if ( is_QP) matrix_it += l;

        // compute  Q^T v_x
        for ( row = 0; row < s; ++row,                              ++y_l_it) {
            sum = et0;
    
            for (   count = 0,   column_it = matrix_it,   v_it = v_x_it;
                    count < b;
                  ++count,     ++column_it,             ++v_it) {
                sum += (*column_it)[ row] * *v_it;
            }
    
            *y_l_it = sum;
        }
    }
    
    template < class ForIt, class OutIt > inline
    void
    multiply__x( ForIt v_l_it, OutIt y_x_it) const
    {
        typename Matrix::const_iterator  matrix_it = M.begin();
        unsigned int  row;

	// QP?
	if ( is_QP) matrix_it += l;

        // compute  Q v_l
        for (   row = 0;
                row < b;
              ++row,     ++matrix_it, ++y_x_it) {

	    *y_x_it = inner_product( matrix_it->begin(), v_l_it, s);
	}
    }
    
    // vector-vector multiplication  
    template < class InIt1, class InIt2 > inline
    typename std::iterator_traits<InIt1>::value_type  
    inner_product( InIt1 u_it, InIt2 v_it, unsigned int n) const
    {
	typedef  typename std::iterator_traits<InIt1>::value_type  NT;
    
        // compute u^T v
	NT sum = NT( 0);
        for ( unsigned int count = 0; count < n; ++count, ++u_it, ++v_it) {
            sum += NT(*u_it) * NT(*v_it);
        }
    
        return sum;
    }
    
    // in-place update
    template < class ForIt >                                    // QP case
    void  update_inplace_QP( ForIt y_l_it, ForIt y_x_it,
			     const ET& d_new, const ET& d_old)
    {
        typename Matrix::      iterator  matrix_it;
        typename Row   ::      iterator     row_it;
        typename Row   ::const_iterator      y_it1, y_it2;
    
        unsigned int  row, col, k = l+b;
    
        // rows: 0..s-1  ( P )
        for (   row = 0,   y_it1 = y_l_it,   matrix_it = M.begin();
                row < s;
              ++row,     ++y_it1,          ++matrix_it            ) {
    
            // columns: 0..row  ( P )
            for (   row_it =  matrix_it->begin(),   y_it2 = y_l_it;
                    row_it != matrix_it->end();
                  ++row_it,                       ++y_it2         ) {
    
                update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
            }
        }
    
        // rows: l..k-1  ( Q R )
        for (   row = l,   y_it1 = y_x_it,   matrix_it += l-s;
                row < k;
              ++row,     ++y_it1,          ++matrix_it       ) {
    
            // columns: 0..s-1  ( Q )
            for (   col = 0,   row_it =  matrix_it->begin(),   y_it2 = y_l_it;
                    col < s;
                  ++col,     ++row_it,                       ++y_it2         ){
    
                update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
            }
    
            // columns: l..k-1  ( R )
            for (              row_it += l-s,                  y_it2 = y_x_it;
                               row_it != matrix_it->end();
                             ++row_it,                       ++y_it2         ){
    
                update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
            }
        }
    }
    
    template < class ForIt1, class ForIt2 >                     // LP case
    void  update_inplace_LP( ForIt1 x_x_it, ForIt2 y_x_it,
			     const ET& d_new, const ET& d_old)
    {
        typename Matrix::      iterator  matrix_it;
        typename Row   ::      iterator     row_it;
	ForIt1                                x_it;
    
        unsigned int  row, col;
	ET            y;

	// QP (in phase I)?
	matrix_it = M.begin();
	if ( is_QP) matrix_it += l;

        // rows: 0..s-1  ( Q )
        for (   row = 0;
                row < s;
              ++row,     ++y_x_it, ++matrix_it) {
    
            // columns: 0..b-1  ( Q )
	    y = *y_x_it;
            for (   col = 0,   row_it =  matrix_it->begin(),   x_it = x_x_it;
		    col < b;
		  ++col,     ++row_it,                       ++x_it         ){
    
                update_entry( *row_it, d_new, y * *x_it, d_old);
            }
        }
    }
    
    
    template < class RandomAccessIterator >
    typename std::iterator_traits<RandomAccessIterator>::value_type 
    inv_M_B_row_dot_col( int row, RandomAccessIterator u_l_it) const
    {
	typename Row::const_iterator row_it;
	if ( is_QP) {
	    row_it = M[l + row].begin();
	} else {
	    row_it = M[row].begin();
	}
	return inner_product(row_it, u_l_it, b);	
    }

};

// ----------------------------------------------------------------------------

// =============================
// class implementation (inline)
// =============================

// creation
template < class ET_, class Is_LP_ >  inline
QP_basis_inverse<ET_,Is_LP_>::
QP_basis_inverse( CGAL::Verbose_ostream&  verbose_ostream)
    : et0( 0), et1( 1), et2( 2),
      is_LP( check_tag( Is_LP())), is_QP( ! is_LP),
      vout( verbose_ostream)
{ }

// transition (LP case)
template < class ET_, class Is_LP_ >  inline
void  QP_basis_inverse<ET_,Is_LP_>::
transition( )
{
    is_phaseI  = false;
    is_phaseII = true;

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

// set-up (QP case)
template < class ET_, class Is_LP_ >  inline
void  QP_basis_inverse<ET_,Is_LP_>::
set( Tag_false)
{
    M.reserve( l);
    // only allocate empty rows
    for ( unsigned int i = 0; i < l; ++i )
       M.push_back(Row(0, et0)); 
}
    
// set-up (LP case)
template < class ET_, class Is_LP_ >  inline
void  QP_basis_inverse<ET_,Is_LP_>::
set( Tag_true)
{
    M.reserve( l);
    for ( unsigned int i = 0; i < l; ++i) M.push_back( Row( l, et0));
}

// access (QP case)
template < class ET_, class Is_LP_ >  inline
const ET_&  QP_basis_inverse<ET_,Is_LP_>::
entry( unsigned int r, unsigned int c, Tag_false) const
{
    CGAL_qpe_assertion( ( r < s) || ( ( r >= l) && ( r < l+b)));
    CGAL_qpe_assertion( ( c < s) || ( ( c >= l) && ( c < l+b)));
    return ( c < r ? M[ r][ c] : M[ c][ r]);
}

// access (LP case)
template < class ET_, class Is_LP_ >  inline
const ET_&  QP_basis_inverse<ET_,Is_LP_>::
entry( unsigned int r, unsigned int c, Tag_true) const
{
    CGAL_qpe_assertion( r < s);
    CGAL_qpe_assertion( c < b);
    return M[ r][ c];
}

// in-place update
template < class ET_, class Is_LP_ >  inline
void  QP_basis_inverse<ET_,Is_LP_>::
update_entry( ET& entry, const ET& d_new, const ET& y, const ET& d_old) const
{
    entry *= d_new;
    entry += y;
    entry = CGAL::integral_division(entry, d_old);
}

CGAL_END_NAMESPACE

#include <CGAL/QP_solver/QP_basis_inverse_impl.h>

#endif // CGAL_QP_SOLVER_QP_BASIS_INVERSE_H

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