qp_basis_inverse_impl.h

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

// Copyright (c) 1997-2007  ETH Zurich (Switzerland).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL: svn+ssh://scm.gforge.inria.fr/svn/cgal/branches/CGAL-3.3-branch/QP_solver/include/CGAL/QP_solver/QP_basis_inverse_impl.h $
// $Id: QP_basis_inverse_impl.h 38453 2007-04-27 00:34:44Z gaertner $
// 
//
// Author(s)     : Sven Schoenherr 
//                 Bernd Gaertner <[e-mail]>
//                 Franz Wessendorp 
//                 Kaspar Fischer 

CGAL_BEGIN_NAMESPACE

// =============================
// class implementation (cont'd)
// =============================

// creation and initialization
// ---------------------------
// set-up
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
set( int n, int m, int nr_equalities)
{
    CGAL_qpe_assertion( n >= 0);
    CGAL_qpe_assertion( m >= 0);
    b = s = 0;
    // l is the maximum size of the basis in phase I
    l = (std::min)( n+nr_equalities+1, m);
    if ( ! M.empty()) M.clear();
    set( Is_LP());
    
    if ( ! x_l.empty()) x_l.clear();
    if ( ! x_x.empty()) x_x.clear();
       
    x_l.insert( x_l.end(), l, et0);
    x_x.insert( x_x.end(), l, et0); // has to grow later QP-case
    
    if ( ! tmp_l.empty()) tmp_l.clear();
    if ( ! tmp_x.empty()) tmp_x.clear();

    tmp_l.insert( tmp_l.end(), l, et0);
    tmp_x.insert( tmp_x.end(), l, et0); // has to grow later QP-case

}

// update functions
// ----------------
// leaving of original variable (update type U2)
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
leave_original( )
{
    // assert QP case
    Assert_compile_time_tag( Tag_false(), Is_LP());

    // determine new denominator (`z')
    --b;
    ET    z     = M[ l+b][ l+b];
    bool  z_neg = ( z < et0);
    CGAL_qpe_assertion( z != et0);

    // update matrix in place
    update_inplace_QP( M[ l+b].begin(), M[ l+b].begin()+l,
		       -z, ( z_neg ? d : -d));
                                                                 
    // store new denominator
    d = ( z_neg ? -z : z);
    CGAL_qpe_assertion( d > et0);

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

// entering of slack variable (update type U3)
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
enter_slack( )
{
    // assert QP case
    Assert_compile_time_tag( Tag_false(), Is_LP());

    // determine new denominator (`z')
    --s;
    ET    z     = M[ s][ s];
    bool  z_neg = ( z < et0);
    CGAL_qpe_assertion( z != et0);

    // update matrix in place
    typename Matrix::iterator  col_it;
    typename Row   ::iterator    x_it;
    unsigned int               col;
    for (   col = 0,   col_it = M.begin()+l,   x_it = x_x.begin();
            col < b;
          ++col,     ++col_it,               ++x_it              ) {
        *x_it = (*col_it)[ s];
    }
    update_inplace_QP( M[ s].begin(), x_x.begin(), -z, ( z_neg ? d : -d));

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

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

// replacing of original by slack variable (update type U8)
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
enter_slack_leave_original( )
{
    // assert LP case or phase I
    CGAL_qpe_assertion( is_LP || is_phaseI);

    // update matrix in-place
    // ----------------------
    typename Matrix::iterator  matrix_it;
    typename Row   ::iterator       x_it;
    unsigned int                     row;

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

    // get last column of basis inverse (store it in 'x_x')
    --s; --b;
    for (   row = 0,   x_it = x_x.begin();
	    row < s;
	  ++row,     ++x_it,               ++matrix_it) {
	*x_it = (*matrix_it)[ b];
    }
    ET    z     = (*matrix_it)[ b];
    bool  z_neg = ( z < et0);
    CGAL_qpe_assertion( z != et0);

    // update matrix
    update_inplace_LP( matrix_it->begin(), x_x.begin(), -z, ( z_neg ? d : -d));

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

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


// replacing of original by original variable with precondition in QP-case
// for phaseII                               (update type UZ_1)
template < class ET_, class Is_LP_ >
template < class ForwardIterator >
void  QP_basis_inverse<ET_,Is_LP_>::
z_replace_original_by_original(ForwardIterator y_l_it,
                               ForwardIterator y_x_it, const ET& s_delta,
                               const ET& s_nu, unsigned int k_i)
{

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


    // prepare \hat{k}_{1} -scalar
    ET  hat_k_1 = *(y_x_it + k_i);

    // prepare \hat{\rho} -vector in x_l, x_x
    copy_row_in_B_O(x_l.begin(), x_x.begin(), k_i);
    
    // prepare \hat{v} -vector in tmp_l, tmp_x
    
    // tmp_l -part
    std::transform(y_l_it, (y_l_it+s), x_l.begin(), tmp_l.begin(),
        compose2_2(std::plus<ET>(), Identity<ET>(),
        std::bind1st(std::multiplies<ET>(), s_delta)));
    
    // tmp_x -part    
    std::transform(y_x_it, (y_x_it+b), x_x.begin(), tmp_x.begin(),
        compose2_2(std::plus<ET>(), Identity<ET>(),
        std::bind1st(std::multiplies<ET>(), s_delta)));
    tmp_x[k_i] -= d;
    
    // prepare \hat{k}_{2} -scalar
    ET  hat_k_2 = s_nu - (et2 * s_delta * hat_k_1);
    
    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_2, -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();
    }
    
}


// replacing of original by slack variable with precondition in QP-case
// for phaseII                               (update type UZ_2)
template < class ET_, class Is_LP_ >
void  QP_basis_inverse<ET_,Is_LP_>::
z_replace_original_by_slack( )
{

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

    // adapt s and b
    --s; --b;

    // prepare \hat{\rho} -vector in x_l, x_x
    copy_row_in_B_O(x_l.begin(), x_x.begin(), b);
    
    // prepare \hat{\varrho} -vector in tmp_l, tmp_x
    copy_row_in_C(tmp_l.begin(), tmp_x.begin(), s);
    
    // prepare \hat{\kappa} -scalar
    ET  hat_kappa = M[l+b][s];
    
    // prepare \hat{\xi} -scalar
    ET hat_xi = M[s][s];
        
    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_kappa * hat_kappa, hat_xi, -hat_kappa, d * d);
		     
    // store new denominator
    d = CGAL::integral_division(hat_kappa * hat_kappa, d);

    CGAL_qpe_assertion( d > et0);

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

}


// replacing of slack by original variable with precondition in QP-case
// for phaseII                               (update type UZ_3)
template < class ET_, class Is_LP_ >
template < class ForwardIterator >
void  QP_basis_inverse<ET_,Is_LP_>::
z_replace_slack_by_original(ForwardIterator y_l_it,
                            ForwardIterator y_x_it,
			                ForwardIterator u_x_it, const ET& hat_kappa,
		                    const ET& hat_nu)
{
    // assert QP case and phaseII
    CGAL_qpe_assertion(is_QP && is_phaseII);
    
    // get copies of y_l_it and y_x_it for later use
    ForwardIterator y_l_it_copy = y_l_it;
    ForwardIterator y_x_it_copy = y_x_it;

    CGAL_qpe_assertion( d != et0);
    
    // prepare \hat{\phi}
     
    // prepare \hat{\varphi} -vector in x_l, x_x
    multiply(u_x_it, u_x_it, x_l.begin(), x_x.begin(), Tag_false(),
             Tag_false());
	     
    // prepare \hat{\kappa} -scalar
    
    // prepare \hat{\nu} -scalar
   
    // update matrix in place
    z_update_inplace(x_l.begin(), x_x.begin(), y_l_it, y_x_it,
                     hat_kappa * hat_kappa, -hat_nu, hat_kappa, d * d);    
    
    // append new rows and columns
    // ---------------------------
    typename Row   ::iterator  row_it, x_l_it, x_x_it;
    typename Matrix::iterator  matrix_it;
    unsigned int               count;
    
    // insert new row and column at the end of block P
    CGAL_qpe_assertion(M.size()>=s+1);
    if (M[s].size()==0) {
	// row has to be filled first
        M[s].insert(M[s].end(), s+1, et0);
    }
     
    
    // P-block: left of diagonal (including element on diagonal)
    y_l_it = y_l_it_copy;
    for (  row_it = M[s].begin(), x_l_it = x_l.begin();
           row_it != M[s].end() - 1;
	 ++row_it,  ++x_l_it,  ++y_l_it                ) {
        *row_it = 
	  CGAL::integral_division((hat_nu * *x_l_it)-(hat_kappa * *y_l_it), d);  
    } 
    *row_it = -hat_nu;
     
    // Q-block
    y_x_it = y_x_it_copy;
    for (  matrix_it = M.begin()+l, count = 0, x_x_it = x_x.begin();
           count < b;
	 ++matrix_it,  ++count, ++x_x_it, ++y_x_it                  ) {
        (*matrix_it)[s] = 
	  CGAL::integral_division((hat_nu * *x_x_it) - (hat_kappa * *y_x_it), d);
    }
          
    // insert new row and column at the end of blocks Q and R
    ensure_physical_row(l+b);
    
    // Q-block
    for (  row_it = M[l+b].begin(), count = 0, x_l_it = x_l.begin();
           count < s;
	 ++row_it,  ++count,  ++x_l_it                              ) {
        *row_it = CGAL::integral_division(-hat_kappa * *x_l_it, d);
    }
    *row_it = hat_kappa;
    
    // R-block
    for (  row_it = M[l+b].begin()+l, count = 0, x_x_it = x_x.begin();
           count < b;
	 ++row_it,  ++count,  ++x_x_it                                ) {
        *row_it = CGAL::integral_division(-hat_kappa * *x_x_it, d);
    }
    *row_it = et0;
    
    //adapt s and b
    ++s; ++b; 

    // store new denominator
    d = CGAL::integral_division(hat_kappa * hat_kappa, d);

    CGAL_qpe_assertion( d > et0);

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