qp_basis_inverse.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.h $
// $Id: QP_basis_inverse.h 38453 2007-04-27 00:34:44Z gaertner $
// 
//
// Author(s)     : Sven Schoenherr
//                 Bernd Gaertner <[e-mail]>
//                 Franz Wessendorp 
//                 Kaspar Fischer 
                                                                               
#ifndef CGAL_QP_SOLVER_QP_BASIS_INVERSE_H
#define CGAL_QP_SOLVER_QP_BASIS_INVERSE_H

#include <CGAL/QP_solver/basic.h>
#include <CGAL/IO/Verbose_ostream.h>
#include <vector>

CGAL_BEGIN_NAMESPACE
                    
// =================
// class declaration
// =================
template < class ET_, class Is_LP_ >
class QP_basis_inverse;

// ===============
// class interface
// ===============
template < class ET_, class Is_LP_ >
class QP_basis_inverse {
  public:
    // self
    typedef  ET_                        ET;
    typedef  Is_LP_                     Is_LP;
    typedef  QP_basis_inverse<ET,Is_LP>
                                        Self;

  private:
    
    // private types
    typedef std::vector<ET>             Row;
    typedef std::vector<Row>            Matrix;

  public:

    /*
     * Note: Some member functions below are suffixed with '_'.
     * They are member templates and their declaration is "hidden",
     * because they are also implemented in the class interface.
     * This is a workaround for M$-VC++, which otherwise fails to
     * instantiate them correctly.
     */

    // creation and initialization
    // ---------------------------
    QP_basis_inverse( CGAL::Verbose_ostream&  verbose_ostream);

    // set-up
    void  set( int n, int m, int nr_equalities);
    
    // init
    template < class InputIterator >                            // phase I
    void  init_( unsigned int art_size, InputIterator art_first);

    /*
    template < class InputIterator >                            // phase II
    void  init_( ...);
    */

    // transition to phase II
    template < class InputIterator >                            // QP case
    void  transition_( InputIterator twice_D_it);

    void  transition( );                                        // LP case

    // access
    // ------
    const ET&  denominator( ) const { return d; }

    const ET&  entry( unsigned int row, unsigned int column) const
        { return entry( row, column, Is_LP()); }

    // multiplication functions
    // ------------------------
    // matrix-vector multiplication ( y = M v )
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply( ForwardIterator v_l_it, ForwardIterator v_x_it,
                     OutputIterator y_l_it,  OutputIterator y_x_it) const
        { multiply( v_l_it, v_x_it, y_l_it, y_x_it, Is_LP(), Tag_true()); }
    
    // special matrix-vector multiplication functions for LPs
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply_l( ForwardIterator v_x_it, OutputIterator y_l_it) const
        { CGAL_qpe_assertion( is_LP || is_phaseI);
          multiply__l( v_x_it, y_l_it); }
    
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply_x( ForwardIterator v_l_it, OutputIterator y_x_it) const
        { CGAL_qpe_assertion( is_LP || is_phaseI);
	  multiply__x( v_l_it, y_x_it); }
    
    // vector-matrix multiplication ( x^T = u^T M )
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply_transposed( ForwardIterator u_l_it, ForwardIterator u_x_it,
                                OutputIterator x_l_it,  OutputIterator x_x_it)
                                                                          const
        { multiply( u_l_it, u_x_it, x_l_it, x_x_it); } // M_B^{-1} is symmetric
    
    // special vector-matrix multiplication functions for LPs
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply_transposed_l( ForwardIterator u_x_it,
                                  OutputIterator x_l_it) const
        { multiply_l( u_x_it, x_l_it); }        // Note: M_B^{-1} is symmetric
    
    template < class ForwardIterator, class OutputIterator >  inline
    void  multiply_transposed_x( ForwardIterator u_l_it,
                                  OutputIterator x_x_it) const
        { multiply_x( u_l_it, x_x_it); }        // Note: M_B^{-1} is symmetric
    
    // vector-vector multiplication ( y = u^T v )
    template < class InputIterator1, class InputIterator2 >  inline
    typename std::iterator_traits<InputIterator1>::value_type
    inner_product( InputIterator1 u_l_it, InputIterator1 u_x_it,
		   InputIterator2 v_l_it, InputIterator2 v_x_it) const
        { return inner_product_l( u_l_it, v_l_it)
	       + inner_product_x( u_x_it, v_x_it); }
    
    template < class InputIterator1, class InputIterator2 >  inline
    typename std::iterator_traits<InputIterator1>::value_type
    inner_product_l( InputIterator1 u_l_it, InputIterator2 v_l_it) const
        { return inner_product( u_l_it, v_l_it, s); }
    
    template < class InputIterator1, class InputIterator2 >  inline
    typename std::iterator_traits<InputIterator1>::value_type
    inner_product_x( InputIterator1 u_x_it, InputIterator2 v_x_it) const
        { return inner_product( u_x_it, v_x_it, b); }
	
    
    // 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);
    
    // leaving of original variable (update type U2)
    void  leave_original( );
    
    // entering of slack variable (update type U3)
    void  enter_slack( );

    // leaving of slack variable (update type U4)
    template < class ForwardIterator >
    void  leave_slack_( ForwardIterator u_x_it);
    
    // replacing of original by original variable (update type U5)
    template < class ForwardIterator >
    void  enter_original_leave_original_( ForwardIterator y, unsigned int k);
    
    // replacing of slack by slack variable (update type U6)
    template < class ForwardIterator >
    void  enter_slack_leave_slack_( ForwardIterator u, unsigned int k);
    
    // replacing of slack by original variable (update type U7)
    template < class ForwardIterator1, class ForwardIterator2 >
    void  enter_original_leave_slack_( ForwardIterator1 y, ForwardIterator2 u);
    
    // replacing of original by slack variable (update type U8)
    void  enter_slack_leave_original( );
    
    
    // replacing of original by original variable with precondition in QP-case
    // for phaseII                               (update type UZ_1)
    template < class ForwardIterator >
    void  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);


    // replacing of original by slack variable with precondition in QP-case
    // for phaseII                               (update type UZ_2)
    void  z_replace_original_by_slack( );


    // replacing of slack by original variable with precondition in QP-case
    // for phaseII                               (update type UZ_3)
    template < class ForwardIterator >
    void  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);


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

    // copying of reduced basis inverse row in (upper) C-half
    template < class OutIt >
    void  copy_row_in_C(OutIt y_l_it, OutIt y_x_it, unsigned int k);
    
    // copying of reduced basis inverse row in (lower) B_O-half
    template < class OutIt >
    void  copy_row_in_B_O(OutIt y_l_it, OutIt y_x_it, unsigned int k);


    // swap functions
    void  swap_variable( unsigned int j)                // ``to the end'' of R
        { CGAL_qpe_assertion( j < b);
	  swap_variable( j, Is_LP()); }
    void  swap_constraint( unsigned int i)              // ``to the end'' of P
        { CGAL_qpe_assertion( i < s);
	  swap_constraint( i, Is_LP()); }

  private:
    
    // constants
    const ET                 et0, et1, et2;
                                        
    // data members
    Matrix                   M;         // basis inverse, stored row-wise
    ET                       d;         // denominator

    unsigned int             l;         // minimum of `n' and `m'
    unsigned int             s;         // size of `E \cup S_N'
    unsigned int             b;         // size of `B_O'

    bool                     is_phaseI; // flag indicating phase I
    bool                     is_phaseII;// flag indicating phase II
    const bool               is_LP;     // flag indicating a linear    program
    const bool               is_QP;     // flag indicating a quadratic program

    CGAL::Verbose_ostream&   vout;      // used for diagnostic output

    Row                      x_l, tmp_l;  // used in the 
    Row                      x_x, tmp_x;  // update functions

    // private member functions
    // ------------------------
    // set-up
    void  set( Tag_false);        // QP case
    void  set( Tag_true );        // LP case

    // init
    template < class InIt >                                     // QP case
    void  init_( unsigned int art_size, InIt art_first, Tag_false);
    template < class InIt >                                     // LP case
    void  init_( unsigned int art_size, InIt art_first, Tag_true );

    // access
    const ET&  entry( unsigned int row,
		      unsigned int column, Tag_false) const;    // QP case
    const ET&  entry( unsigned int row,
		      unsigned int column, Tag_true ) const;    // LP case

    // matrix-vector multiplication
    template < class ForIt, class OutIt, class Use1stArg >      // QP case
    void  multiply_( ForIt v_l_it, ForIt v_x_it,
		     OutIt y_l_it, OutIt y_x_it, Tag_false, Use1stArg) const;
    template < class ForIt, class OutIt, class  DummyArg >      // LP case
    void  multiply_( ForIt v_l_it, ForIt v_x_it,
		     OutIt y_l_it, OutIt y_x_it, Tag_true,   DummyArg) const;

    // special matrix-vector multiplication functions for LPs
    template < class ForIt, class OutIt >
    void  multiply__l_( ForIt v_x_it, OutIt y_l_it) const;
    template < class ForIt, class OutIt >
    void  multiply__x_( ForIt v_l_it, OutIt y_x_it) const;

    // 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);
    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);
			      
    template < class ForIt >                                  // QP case only
    void  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); 

    void  update_entry( ET& entry,   const ET& d_new,
			const ET& y, const ET& d_old) const;

    // swap functions
    void  swap_variable  ( unsigned int, Tag_true );            // LP case
    void  swap_variable  ( unsigned int, Tag_false);            // QP case
    void  swap_constraint( unsigned int, Tag_true );            // LP case
    void  swap_constraint( unsigned int, Tag_false);            // QP case	
    
    // diagnostic output
    void  print( );

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

// ===============================
// class implementation (template)
// ===============================

  public:

    // creation and initialization
    // ---------------------------
    // init
    template < class InputIterator >
    void
    init( unsigned int art_size, InputIterator art_first)
    {
	CGAL_qpe_assertion_msg( art_size <= l, \
	    "There are more equality constraints than original variables!");

        init( art_size, art_first, Is_LP());
	d = et1;
        CGAL_qpe_assertion( s == art_size);
        CGAL_qpe_assertion( b == art_size);

	is_phaseI  = true;
	is_phaseII = false;

        if ( x_x.size() < art_size) {
            x_x.insert( x_x.end(), art_size-x_x.size(), et0);
        }
	
        if ( tmp_x.size() < art_size) {
            tmp_x.insert( tmp_x.end(), art_size-tmp_x.size(), et0);
        }
        
        CGAL_qpe_debug {
            if ( vout.verbose()) print();
        }
    }

    // transition to phase II
    template < class InputIterator >                            // QP case
    void  transition( InputIterator twice_D_it)
    {
	typename Matrix::iterator  m_it1, m_it2, p_begin, r_begin;
	typename Row   ::iterator  x_it;
	unsigned int      row, col;

	// fill missing rows
	for (row= 0; row< s; ++ row) {
	    CGAL_qpe_assertion(M[row].size()==0);
	    M[row].insert(M[row].end(), row+1, et0);
	}

	// compute new basis inverse [ upper-left part: -(Q^T * 2 D_B * Q) ]
	// -----------------------------------------------------------------
	// compute 'Q^T * 2 D_B' ( Q = A_B^-1 )	
	p_begin = M.begin();
	r_begin = M.begin();
	if (b > 0) r_begin += l+s-1;   	// initialize only if for loops 
					// are entered
	for ( col = 0; col < b; ++col, ++twice_D_it) {
            ++p_begin;

	    // get column of D (D symmetric)
	    std::copy( *twice_D_it, *twice_D_it+b, x_l.begin());

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