qp_basis_inverse.h

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

	    // store resulting column in 'P' and 'R'
	    x_it  = x_x.begin();
	    m_it2 = r_begin;
	    for ( row = l+col; row >= l; --row, --m_it2, ++x_it) {
		(*m_it2)[ row] = *x_it;
	    }
	    m_it1 = p_begin;
	    for ( row = col+1; row <  s; ++row, ++m_it1, ++x_it) {
		(*m_it1)[ col] = *x_it;
	    }
	}

	// compute '(Q^T * 2 D_B) * Q' ( Q = A_B^-1 )
	m_it1 = M.begin();
	m_it2 = r_begin;
	for ( row = 0; row < s; ++row, ++m_it1, --m_it2) {

	    // get row of '(Q^T * 2 D_B)' (stored in 'P' and 'R')
	    std::copy(m_it1->begin()  ,m_it1->begin()+row,    x_l.begin());
	    std::copy(m_it2->begin()+l,m_it2->begin()+(l+b-row),
	    	x_l.begin()+row);

	    // compute '(Q^T * 2 D_B)_i * Q'
	    multiply__l( x_l.begin(), x_x.begin());

	    // negate and store result in 'P'
	    std::transform( x_x.begin(), x_x.begin()+row+1,
			    m_it1->begin(), std::negate<ET>());

	    // clean up in 'R'
	    std::fill_n( m_it2->begin()+l, b-row, et0);
	}

	// scale A_B^-1
	m_it1 = M.begin()+l;
	for ( row = 0; row < s; ++row, ++m_it1) {
	    std::transform( m_it1->begin(), m_it1->begin()+s, m_it1->begin(),
			    std::bind2nd( std::multiplies<ET>(), d));
	}

	// new denominator: |det(A_B)|^2
	d *= d;

	// update status
	transition();
    }

    // update functions
    // ----------------
    // entering of original variable (update type U1)
    template < class ForwardIterator >
    void
    enter_original( ForwardIterator y_l_it,
                    ForwardIterator y_x_it, const ET& z)
    {
        // assert QP case
        Assert_compile_time_tag( Tag_false(), Is_LP());
    
        // update matrix in-place
        // ----------------------
        // handle sign of new denominator
        CGAL_qpe_assertion( z != et0);
        bool  z_neg = ( z < et0);

        // update matrix
        update_inplace_QP( y_l_it, y_x_it, z, ( z_neg ? -d : d));
                                                                      
        // append new row and column ("after R")
        // -------------------------------------
        typename Row::iterator  row_it;
	ForwardIterator           y_it;
        unsigned int            col, k = l+(++b);
    
//      // allocate new row, if necessary
//      // BG: replaced this by the ensure_physical_row construct below
//      if ( k <= M.size()) {
//           row_it = M[ k-1].begin();
//      } else {
//           row_it = M.insert( M.end(), Row( k, et0))->begin();
//           x_x.push_back( et0);
// 	     tmp_x.push_back( et0);
//      }
	ensure_physical_row(k-1);
	row_it = M[ k-1].begin();
    
        // store entries in new row
        for (   col = 0,                              y_it = y_l_it;
                col < s;
              ++col,     ++row_it,                  ++y_it         ) {
            *row_it = ( z_neg ? *y_it : -( *y_it));
        }
        for (   col = l,     row_it += l-s,           y_it = y_x_it;
                col < k-1;
              ++col,       ++row_it,                ++y_it         ) {
            *row_it = ( z_neg ? *y_it : -( *y_it));
        }
        *row_it = ( z_neg ? -d : d);
    
        // store new denominator
	// ---------------------
        d = ( z_neg ? -z : z);
        CGAL_qpe_assertion( d > et0);
    
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
        }
    }
    
    // leaving of slack variable (update type U4)
    template < class ForwardIterator >
    void
    leave_slack( ForwardIterator u_x_it)
    {
        // assert QP case
        Assert_compile_time_tag( Tag_false(), Is_LP());
    
        // update matrix in-place
        // ----------------------
        // compute new row/column of basis inverse
        multiply( u_x_it,                               // dummy (not used)
		  u_x_it, x_l.begin(), x_x.begin(),
		  Tag_false(),                          // QP
		  Tag_false());                         // ignore 1st argument
        ET    z     = -inner_product_x( x_x.begin(), u_x_it);
        bool  z_neg = ( z < et0);
        CGAL_qpe_assertion( z != et0);
    
        // update matrix
        update_inplace_QP( x_l.begin(), x_x.begin(), z, ( z_neg ? -d : d));
                                                                      
        // insert new row and column ("after P")
        // -------------------------------------
        typename Row   ::iterator  row_it, x_it;
        typename Matrix::iterator  col_it;
        unsigned int               col, k = l+b;
    
        // store entries in new row
	if (M[s].size()==0) {
	   // row has to be filled first
	   M[s].insert(M[s].end(), s+1, et0);
	}
        for (   col = 0,   row_it = M[ s].begin(),        x_it = x_l.begin();
                col < s;
              ++col,     ++row_it,                      ++x_it              ) {
            *row_it = ( z_neg ? *x_it : -( *x_it));
        }
        *row_it = ( z_neg ? -d : d);
    
        for (   col = l,   col_it = M.begin()+l,          x_it = x_x.begin();
                col < k;
              ++col,     ++col_it,                      ++x_it              ) {
            (*col_it)[ s] = ( z_neg ? *x_it : -( *x_it));
        }
        ++s;
    
        // store new denominator
	// ---------------------
        d = ( z_neg ? -z : z);
        CGAL_qpe_assertion( d > et0);
    
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
	}
    }

    // replacing of original variable (update type U5) [ replace column ]
    template < class RandomAccessIterator >
    void
    enter_original_leave_original( RandomAccessIterator y_x_it, unsigned int k)
    {
        // assert LP case or phase I
	CGAL_qpe_assertion( is_LP || is_phaseI);
	CGAL_qpe_assertion( k < b);

        // update matrix in place
        // ----------------------
        typename Matrix::iterator  matrix_it;
        typename Row   ::iterator     row_it, row_k_it, row_k;

        // handle sign of new denominator
        ET    z     = y_x_it[ k];
        bool  z_neg = ( z < et0);
        if ( z_neg) d = -d;

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

        // rows: 0..s-1 without k
        unsigned int  row, col;
	ET            minus_y;
        for (   row = 0;
                row < s;
              ++row,     ++matrix_it, ++y_x_it) {
	    if ( row != k) {

		// columns: 0..b-1
		minus_y = -( *y_x_it);
		for (   col = 0, row_it = matrix_it->begin(), row_k_it = row_k;
			col < b;
		      ++col,   ++row_it,                    ++row_k_it       ){
        
		    // update in place
		    update_entry( *row_it, z, minus_y * *row_k_it, d);
		}
	    }
        }

	// rows: k (flip signs, if necessary)
	if ( z_neg) {
	    for (   col = 0,   row_it = row_k;
		    col < b;
		  ++col,     ++row_it        ) {
        
		*row_it = -( *row_it);
	    }
	}

        // store new denominator
        // ---------------------
        d = ( z_neg ? -z : z);
        CGAL_qpe_assertion( d > et0);

	// diagnostic output
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
	}
    }
    
    // replacing of slack variable (update type U6) [ replace row ]
    template < class ForwardIterator >
    void
    enter_slack_leave_slack( ForwardIterator u_x_it, unsigned int k)
    {
        // assert LP case or phase I
	CGAL_qpe_assertion( is_LP || is_phaseI);
	CGAL_qpe_assertion( k < s);

        // compute new row of basis inverse
        multiply__l( u_x_it, x_x.begin());

        // update matrix in place
        // ----------------------
        typename Matrix::iterator  matrix_it;
        typename Row   ::iterator     row_it, x_it;

        // handle sign of new denominator
        ET    z     = x_x[ k];
        bool  z_neg = ( z < et0);
        if ( z_neg) d = -d;

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

        // rows: 0..s-1
        unsigned int          row, col;
	ET            minus_m_row;
        for (   row = 0;
                row < s;
              ++row,     ++matrix_it) {

	    // columns: 0..b-1
	    minus_m_row = -( *matrix_it)[ k];
	    for (   col = 0,   row_it = matrix_it->begin(), x_it = x_x.begin();
		    col < b;
		  ++col,     ++row_it,                    ++x_it             ){

		if ( col != k) {                // all columns but k

		    // update in place
		    update_entry( *row_it, z, minus_m_row * *x_it, d);

		} else {                        // column k

		    // flip sign, if necessary
		    if ( z_neg) *row_it = -( *row_it);

		}
	    }
	}

        // store new denominator
        // ---------------------
        d = ( z_neg ? -z : z);
        CGAL_qpe_assertion( d > et0);

	// diagnostic output
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
	}
    }
    
    // replacing of slack by original variable (update type U7) [ grow ]
    template < class ForwardIterator1, class ForwardIterator2 >
    void  enter_original_leave_slack( ForwardIterator1 y_x_it,
				      ForwardIterator2 u_x_it)
    {
        // assert LP case or phase I
	CGAL_qpe_assertion( is_LP || is_phaseI);

        // update matrix in-place
        // ----------------------
        // compute new row of basis inverse
        multiply__l( u_x_it, x_x.begin());
        ET    z     = d*u_x_it[ b] - inner_product_x( y_x_it, u_x_it);
        bool  z_neg = ( z < et0);
        CGAL_qpe_assertion( z != et0);
	
        // update matrix
	update_inplace_LP( x_x.begin(), y_x_it, z, ( z_neg ? -d : d));
                                                  
        // append new row and column
        // -------------------------
        typename Matrix::iterator  matrix_it;
        typename Row   ::iterator     row_it, x_it;
        unsigned int                  row, col;

	// QP (in phase I)?
	if ( is_QP) {
	    ensure_physical_row(l+b);
	    row_it = M[l+b].begin();
	    matrix_it = M.begin() + l;
	} else {
	    row_it = M[s].begin();
	    matrix_it = M.begin();
	} 	
	
	// store 'x_x' in new row 
	x_it = x_x.begin();
	for ( col = 0; col < b; ++col, ++row_it, ++x_it) {
		*row_it = ( z_neg ? *x_it : -( *x_it));
	}
        *row_it = ( z_neg ? -d : d);
	
	// store 'y_x' in new col
	for ( row = 0; row < s; ++row, ++matrix_it, ++y_x_it) {
		(*matrix_it)[b] = ( z_neg ? *y_x_it : -( *y_x_it));
	}
	++s; ++b;
    
        // store new denominator
	// ---------------------
        d = ( z_neg ? -z : z);
        CGAL_qpe_assertion( d > et0);
    
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
        }
    }
  // due to basis_matrix_stays_regular fix, needs reconsideration
  //private:

    // private member functions
    // ------------------------
    // init (QP case)
    template < class InIt >  inline
    void
    init( unsigned int art_size, InIt art_first, Tag_false)
    {
	// only Q is used to store A_B^-1 in phase I
        for ( s = l, b = 0; b < art_size; ++s, ++b, ++art_first) {
	    ensure_physical_row(s);
            M[ s][ b] = ( art_first->second ? -et1 : et1);
        }

        s = art_size;
    }

    // init (LP case)
    template < class InIt >  inline
    void
    init( unsigned int art_size, InIt art_first, Tag_true)
    {
        for ( s = 0; s < art_size; ++s, ++art_first) {
	    std::fill_n( M[ s].begin(), art_size, et0);
	    M[ s][ s] = ( art_first->second ? -et1 : et1);
	}