|
BWAPI
|
00001 // Copyright (c) 1997-2007 ETH Zurich (Switzerland). 00002 // All rights reserved. 00003 // 00004 // This file is part of CGAL (www.cgal.org); you may redistribute it under 00005 // the terms of the Q Public License version 1.0. 00006 // See the file LICENSE.QPL distributed with CGAL. 00007 // 00008 // Licensees holding a valid commercial license may use this file in 00009 // accordance with the commercial license agreement provided with the software. 00010 // 00011 // This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 00012 // WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 00013 // 00014 // $URL: svn+ssh://scm.gforge.inria.fr/svn/cgal/branches/CGAL-3.5-branch/QP_solver/include/CGAL/QP_solver/QP_basis_inverse.h $ 00015 // $Id: QP_basis_inverse.h 38453 2007-04-27 00:34:44Z gaertner $ 00016 // 00017 // 00018 // Author(s) : Sven Schoenherr 00019 // Bernd Gaertner <gaertner@inf.ethz.ch> 00020 // Franz Wessendorp 00021 // Kaspar Fischer 00022 00023 #ifndef CGAL_QP_SOLVER_QP_BASIS_INVERSE_H 00024 #define CGAL_QP_SOLVER_QP_BASIS_INVERSE_H 00025 00026 #include <CGAL/QP_solver/basic.h> 00027 #include <CGAL/IO/Verbose_ostream.h> 00028 #include <vector> 00029 00030 CGAL_BEGIN_NAMESPACE 00031 00032 // ================= 00033 // class declaration 00034 // ================= 00035 template < class ET_, class Is_LP_ > 00036 class QP_basis_inverse; 00037 00038 // =============== 00039 // class interface 00040 // =============== 00041 template < class ET_, class Is_LP_ > 00042 class QP_basis_inverse { 00043 public: 00044 // self 00045 typedef ET_ ET; 00046 typedef Is_LP_ Is_LP; 00047 typedef QP_basis_inverse<ET,Is_LP> 00048 Self; 00049 00050 private: 00051 00052 // private types 00053 typedef std::vector<ET> Row; 00054 typedef std::vector<Row> Matrix; 00055 00056 public: 00057 00058 /* 00059 * Note: Some member functions below are suffixed with '_'. 00060 * They are member templates and their declaration is "hidden", 00061 * because they are also implemented in the class interface. 00062 * This is a workaround for M$-VC++, which otherwise fails to 00063 * instantiate them correctly. 00064 */ 00065 00066 // creation and initialization 00067 // --------------------------- 00068 QP_basis_inverse( CGAL::Verbose_ostream& verbose_ostream); 00069 00070 // set-up 00071 void set( int n, int m, int nr_equalities); 00072 00073 // init 00074 template < class InputIterator > // phase I 00075 void init_( unsigned int art_size, InputIterator art_first); 00076 00077 /* 00078 template < class InputIterator > // phase II 00079 void init_( ...); 00080 */ 00081 00082 // transition to phase II 00083 template < class InputIterator > // QP case 00084 void transition_( InputIterator twice_D_it); 00085 00086 void transition( ); // LP case 00087 00088 // access 00089 // ------ 00090 const ET& denominator( ) const { return d; } 00091 00092 const ET& entry( unsigned int row, unsigned int column) const 00093 { return entry( row, column, Is_LP()); } 00094 00095 // multiplication functions 00096 // ------------------------ 00097 // matrix-vector multiplication ( y = M v ) 00098 template < class ForwardIterator, class OutputIterator > inline 00099 void multiply( ForwardIterator v_l_it, ForwardIterator v_x_it, 00100 OutputIterator y_l_it, OutputIterator y_x_it) const 00101 { multiply( v_l_it, v_x_it, y_l_it, y_x_it, Is_LP(), Tag_true()); } 00102 00103 // special matrix-vector multiplication functions for LPs 00104 template < class ForwardIterator, class OutputIterator > inline 00105 void multiply_l( ForwardIterator v_x_it, OutputIterator y_l_it) const 00106 { CGAL_qpe_assertion( is_LP || is_phaseI); 00107 multiply__l( v_x_it, y_l_it); } 00108 00109 template < class ForwardIterator, class OutputIterator > inline 00110 void multiply_x( ForwardIterator v_l_it, OutputIterator y_x_it) const 00111 { CGAL_qpe_assertion( is_LP || is_phaseI); 00112 multiply__x( v_l_it, y_x_it); } 00113 00114 // vector-matrix multiplication ( x^T = u^T M ) 00115 template < class ForwardIterator, class OutputIterator > inline 00116 void multiply_transposed( ForwardIterator u_l_it, ForwardIterator u_x_it, 00117 OutputIterator x_l_it, OutputIterator x_x_it) 00118 const 00119 { multiply( u_l_it, u_x_it, x_l_it, x_x_it); } // M_B^{-1} is symmetric 00120 00121 // special vector-matrix multiplication functions for LPs 00122 template < class ForwardIterator, class OutputIterator > inline 00123 void multiply_transposed_l( ForwardIterator u_x_it, 00124 OutputIterator x_l_it) const 00125 { multiply_l( u_x_it, x_l_it); } // Note: M_B^{-1} is symmetric 00126 00127 template < class ForwardIterator, class OutputIterator > inline 00128 void multiply_transposed_x( ForwardIterator u_l_it, 00129 OutputIterator x_x_it) const 00130 { multiply_x( u_l_it, x_x_it); } // Note: M_B^{-1} is symmetric 00131 00132 // vector-vector multiplication ( y = u^T v ) 00133 template < class InputIterator1, class InputIterator2 > inline 00134 typename std::iterator_traits<InputIterator1>::value_type 00135 inner_product( InputIterator1 u_l_it, InputIterator1 u_x_it, 00136 InputIterator2 v_l_it, InputIterator2 v_x_it) const 00137 { return inner_product_l( u_l_it, v_l_it) 00138 + inner_product_x( u_x_it, v_x_it); } 00139 00140 template < class InputIterator1, class InputIterator2 > inline 00141 typename std::iterator_traits<InputIterator1>::value_type 00142 inner_product_l( InputIterator1 u_l_it, InputIterator2 v_l_it) const 00143 { return inner_product( u_l_it, v_l_it, s); } 00144 00145 template < class InputIterator1, class InputIterator2 > inline 00146 typename std::iterator_traits<InputIterator1>::value_type 00147 inner_product_x( InputIterator1 u_x_it, InputIterator2 v_x_it) const 00148 { return inner_product( u_x_it, v_x_it, b); } 00149 00150 00151 // update functions 00152 // ---------------- 00153 // entering of original variable (update type U1) 00154 template < class ForwardIterator > 00155 void enter_original_( ForwardIterator y_l_it, 00156 ForwardIterator y_x_it, const ET& z); 00157 00158 // leaving of original variable (update type U2) 00159 void leave_original( ); 00160 00161 // entering of slack variable (update type U3) 00162 void enter_slack( ); 00163 00164 // leaving of slack variable (update type U4) 00165 template < class ForwardIterator > 00166 void leave_slack_( ForwardIterator u_x_it); 00167 00168 // replacing of original by original variable (update type U5) 00169 template < class ForwardIterator > 00170 void enter_original_leave_original_( ForwardIterator y, unsigned int k); 00171 00172 // replacing of slack by slack variable (update type U6) 00173 template < class ForwardIterator > 00174 void enter_slack_leave_slack_( ForwardIterator u, unsigned int k); 00175 00176 // replacing of slack by original variable (update type U7) 00177 template < class ForwardIterator1, class ForwardIterator2 > 00178 void enter_original_leave_slack_( ForwardIterator1 y, ForwardIterator2 u); 00179 00180 // replacing of original by slack variable (update type U8) 00181 void enter_slack_leave_original( ); 00182 00183 00184 // replacing of original by original variable with precondition in QP-case 00185 // for phaseII (update type UZ_1) 00186 template < class ForwardIterator > 00187 void z_replace_original_by_original(ForwardIterator y_l_it, 00188 ForwardIterator y_x_it, 00189 const ET& s_delta, const ET& s_nu, 00190 unsigned int k_i); 00191 00192 00193 // replacing of original by slack variable with precondition in QP-case 00194 // for phaseII (update type UZ_2) 00195 void z_replace_original_by_slack( ); 00196 00197 00198 // replacing of slack by original variable with precondition in QP-case 00199 // for phaseII (update type UZ_3) 00200 template < class ForwardIterator > 00201 void z_replace_slack_by_original(ForwardIterator y_l_it, 00202 ForwardIterator y_x_it, 00203 ForwardIterator u_x_it, const ET& hat_kappa, 00204 const ET& hat_nu); 00205 00206 00207 // replacing of slack by slack variable with precondition in QP-case 00208 // for phaseII (update type UZ_4) 00209 template < class ForwardIterator > 00210 void z_replace_slack_by_slack(ForwardIterator u_x_it, unsigned int k_j); 00211 00212 00213 // copying of reduced basis inverse row in (upper) C-half 00214 template < class OutIt > 00215 void copy_row_in_C(OutIt y_l_it, OutIt y_x_it, unsigned int k); 00216 00217 // copying of reduced basis inverse row in (lower) B_O-half 00218 template < class OutIt > 00219 void copy_row_in_B_O(OutIt y_l_it, OutIt y_x_it, unsigned int k); 00220 00221 00222 // swap functions 00223 void swap_variable( unsigned int j) // ``to the end'' of R 00224 { CGAL_qpe_assertion( j < b); 00225 swap_variable( j, Is_LP()); } 00226 void swap_constraint( unsigned int i) // ``to the end'' of P 00227 { CGAL_qpe_assertion( i < s); 00228 swap_constraint( i, Is_LP()); } 00229 00230 private: 00231 00232 // constants 00233 const ET et0, et1, et2; 00234 00235 // data members 00236 Matrix M; // basis inverse, stored row-wise 00237 ET d; // denominator 00238 00239 unsigned int l; // minimum of `n' and `m' 00240 unsigned int s; // size of `E \cup S_N' 00241 unsigned int b; // size of `B_O' 00242 00243 bool is_phaseI; // flag indicating phase I 00244 bool is_phaseII;// flag indicating phase II 00245 const bool is_LP; // flag indicating a linear program 00246 const bool is_QP; // flag indicating a quadratic program 00247 00248 CGAL::Verbose_ostream& vout; // used for diagnostic output 00249 00250 Row x_l, tmp_l; // used in the 00251 Row x_x, tmp_x; // update functions 00252 00253 // private member functions 00254 // ------------------------ 00255 // set-up 00256 void set( Tag_false); // QP case 00257 void set( Tag_true ); // LP case 00258 00259 // init 00260 template < class InIt > // QP case 00261 void init_( unsigned int art_size, InIt art_first, Tag_false); 00262 template < class InIt > // LP case 00263 void init_( unsigned int art_size, InIt art_first, Tag_true ); 00264 00265 // access 00266 const ET& entry( unsigned int row, 00267 unsigned int column, Tag_false) const; // QP case 00268 const ET& entry( unsigned int row, 00269 unsigned int column, Tag_true ) const; // LP case 00270 00271 // matrix-vector multiplication 00272 template < class ForIt, class OutIt, class Use1stArg > // QP case 00273 void multiply_( ForIt v_l_it, ForIt v_x_it, 00274 OutIt y_l_it, OutIt y_x_it, Tag_false, Use1stArg) const; 00275 template < class ForIt, class OutIt, class DummyArg > // LP case 00276 void multiply_( ForIt v_l_it, ForIt v_x_it, 00277 OutIt y_l_it, OutIt y_x_it, Tag_true, DummyArg) const; 00278 00279 // special matrix-vector multiplication functions for LPs 00280 template < class ForIt, class OutIt > 00281 void multiply__l_( ForIt v_x_it, OutIt y_l_it) const; 00282 template < class ForIt, class OutIt > 00283 void multiply__x_( ForIt v_l_it, OutIt y_x_it) const; 00284 00285 // in-place update 00286 template < class ForIt > // QP case 00287 void update_inplace_QP_( ForIt y_l_it, ForIt y_x_it, 00288 const ET& d_new, const ET& d_old); 00289 template < class ForIt1, class ForIt2 > // LP case 00290 void update_inplace_LP_( ForIt1 x_x_it, ForIt2 y_x_it, 00291 const ET& d_new, const ET& d_old); 00292 00293 template < class ForIt > // QP case only 00294 void z_update_inplace( ForIt psi1_l_it, ForIt psi1_x_it, 00295 ForIt psi2_l_it, ForIt psi2_x_it, 00296 const ET& omega0, const ET& omega1, 00297 const ET& omega2, const ET& omega3); 00298 00299 void update_entry( ET& entry, const ET& d_new, 00300 const ET& y, const ET& d_old) const; 00301 00302 // swap functions 00303 void swap_variable ( unsigned int, Tag_true ); // LP case 00304 void swap_variable ( unsigned int, Tag_false); // QP case 00305 void swap_constraint( unsigned int, Tag_true ); // LP case 00306 void swap_constraint( unsigned int, Tag_false); // QP case 00307 00308 // diagnostic output 00309 void print( ); 00310 00311 // ---------------------------------------------------------------------------- 00312 00313 // =============================== 00314 // class implementation (template) 00315 // =============================== 00316 00317 public: 00318 00319 // creation and initialization 00320 // --------------------------- 00321 // init 00322 template < class InputIterator > 00323 void 00324 init( unsigned int art_size, InputIterator art_first) 00325 { 00326 CGAL_qpe_assertion_msg( art_size <= l, \ 00327 "There are more equality constraints than original variables!"); 00328 00329 init( art_size, art_first, Is_LP()); 00330 d = et1; 00331 CGAL_qpe_assertion( s == art_size); 00332 CGAL_qpe_assertion( b == art_size); 00333 00334 is_phaseI = true; 00335 is_phaseII = false; 00336 00337 if ( x_x.size() < art_size) { 00338 x_x.insert( x_x.end(), art_size-x_x.size(), et0); 00339 } 00340 00341 if ( tmp_x.size() < art_size) { 00342 tmp_x.insert( tmp_x.end(), art_size-tmp_x.size(), et0); 00343 } 00344 00345 CGAL_qpe_debug { 00346 if ( vout.verbose()) print(); 00347 } 00348 } 00349 00350 // transition to phase II 00351 template < class InputIterator > // QP case 00352 void transition( InputIterator twice_D_it) 00353 { 00354 typename Matrix::iterator m_it1, m_it2, p_begin, r_begin; 00355 typename Row ::iterator x_it; 00356 unsigned int row, col; 00357 00358 // fill missing rows 00359 for (row= 0; row< s; ++ row) { 00360 CGAL_qpe_assertion(M[row].size()==0); 00361 M[row].insert(M[row].end(), row+1, et0); 00362 } 00363 00364 // compute new basis inverse [ upper-left part: -(Q^T * 2 D_B * Q) ] 00365 // ----------------------------------------------------------------- 00366 // compute 'Q^T * 2 D_B' ( Q = A_B^-1 ) 00367 p_begin = M.begin(); 00368 r_begin = M.begin(); 00369 if (b > 0) r_begin += l+s-1; // initialize only if for loops 00370 // are entered 00371 for ( col = 0; col < b; ++col, ++twice_D_it) { 00372 ++p_begin; 00373 00374 // get column of D (D symmetric) 00375 std::copy( *twice_D_it, *twice_D_it+b, x_l.begin()); 00376 00377 // compute 'Q^T * 2 D_Bj' 00378 multiply__l( x_l.begin(), x_x.begin()); 00379 00380 // store resulting column in 'P' and 'R' 00381 x_it = x_x.begin(); 00382 m_it2 = r_begin; 00383 for ( row = l+col; row >= l; --row, --m_it2, ++x_it) { 00384 (*m_it2)[ row] = *x_it; 00385 } 00386 m_it1 = p_begin; 00387 for ( row = col+1; row < s; ++row, ++m_it1, ++x_it) { 00388 (*m_it1)[ col] = *x_it; 00389 } 00390 } 00391 00392 // compute '(Q^T * 2 D_B) * Q' ( Q = A_B^-1 ) 00393 m_it1 = M.begin(); 00394 m_it2 = r_begin; 00395 for ( row = 0; row < s; ++row, ++m_it1, --m_it2) { 00396 00397 // get row of '(Q^T * 2 D_B)' (stored in 'P' and 'R') 00398 std::copy(m_it1->begin() ,m_it1->begin()+row, x_l.begin()); 00399 std::copy(m_it2->begin()+l,m_it2->begin()+(l+b-row), 00400 x_l.begin()+row); 00401 00402 // compute '(Q^T * 2 D_B)_i * Q' 00403 multiply__l( x_l.begin(), x_x.begin()); 00404 00405 // negate and store result in 'P' 00406 std::transform( x_x.begin(), x_x.begin()+row+1, 00407 m_it1->begin(), std::negate<ET>()); 00408 00409 // clean up in 'R' 00410 std::fill_n( m_it2->begin()+l, b-row, et0); 00411 } 00412 00413 // scale A_B^-1 00414 m_it1 = M.begin()+l; 00415 for ( row = 0; row < s; ++row, ++m_it1) { 00416 std::transform( m_it1->begin(), m_it1->begin()+s, m_it1->begin(), 00417 std::bind2nd( std::multiplies<ET>(), d)); 00418 } 00419 00420 // new denominator: |det(A_B)|^2 00421 d *= d; 00422 00423 // update status 00424 transition(); 00425 } 00426 00427 // update functions 00428 // ---------------- 00429 // entering of original variable (update type U1) 00430 template < class ForwardIterator > 00431 void 00432 enter_original( ForwardIterator y_l_it, 00433 ForwardIterator y_x_it, const ET& z) 00434 { 00435 // assert QP case 00436 Assert_compile_time_tag( Tag_false(), Is_LP()); 00437 00438 // update matrix in-place 00439 // ---------------------- 00440 // handle sign of new denominator 00441 CGAL_qpe_assertion( z != et0); 00442 bool z_neg = ( z < et0); 00443 00444 // update matrix 00445 update_inplace_QP( y_l_it, y_x_it, z, ( z_neg ? -d : d)); 00446 00447 // append new row and column ("after R") 00448 // ------------------------------------- 00449 typename Row::iterator row_it; 00450 ForwardIterator y_it; 00451 unsigned int col, k = l+(++b); 00452 00453 // // allocate new row, if necessary 00454 // // BG: replaced this by the ensure_physical_row construct below 00455 // if ( k <= M.size()) { 00456 // row_it = M[ k-1].begin(); 00457 // } else { 00458 // row_it = M.insert( M.end(), Row( k, et0))->begin(); 00459 // x_x.push_back( et0); 00460 // tmp_x.push_back( et0); 00461 // } 00462 ensure_physical_row(k-1); 00463 row_it = M[ k-1].begin(); 00464 00465 // store entries in new row 00466 for ( col = 0, y_it = y_l_it; 00467 col < s; 00468 ++col, ++row_it, ++y_it ) { 00469 *row_it = ( z_neg ? *y_it : -( *y_it)); 00470 } 00471 for ( col = l, row_it += l-s, y_it = y_x_it; 00472 col < k-1; 00473 ++col, ++row_it, ++y_it ) { 00474 *row_it = ( z_neg ? *y_it : -( *y_it)); 00475 } 00476 *row_it = ( z_neg ? -d : d); 00477 00478 // store new denominator 00479 // --------------------- 00480 d = ( z_neg ? -z : z); 00481 CGAL_qpe_assertion( d > et0); 00482 00483 CGAL_qpe_debug { 00484 if ( vout.verbose()) print(); 00485 } 00486 } 00487 00488 // leaving of slack variable (update type U4) 00489 template < class ForwardIterator > 00490 void 00491 leave_slack( ForwardIterator u_x_it) 00492 { 00493 // assert QP case 00494 Assert_compile_time_tag( Tag_false(), Is_LP()); 00495 00496 // update matrix in-place 00497 // ---------------------- 00498 // compute new row/column of basis inverse 00499 multiply( u_x_it, // dummy (not used) 00500 u_x_it, x_l.begin(), x_x.begin(), 00501 Tag_false(), // QP 00502 Tag_false()); // ignore 1st argument 00503 ET z = -inner_product_x( x_x.begin(), u_x_it); 00504 bool z_neg = ( z < et0); 00505 CGAL_qpe_assertion( z != et0); 00506 00507 // update matrix 00508 update_inplace_QP( x_l.begin(), x_x.begin(), z, ( z_neg ? -d : d)); 00509 00510 // insert new row and column ("after P") 00511 // ------------------------------------- 00512 typename Row ::iterator row_it, x_it; 00513 typename Matrix::iterator col_it; 00514 unsigned int col, k = l+b; 00515 00516 // store entries in new row 00517 if (M[s].size()==0) { 00518 // row has to be filled first 00519 M[s].insert(M[s].end(), s+1, et0); 00520 } 00521 for ( col = 0, row_it = M[ s].begin(), x_it = x_l.begin(); 00522 col < s; 00523 ++col, ++row_it, ++x_it ) { 00524 *row_it = ( z_neg ? *x_it : -( *x_it)); 00525 } 00526 *row_it = ( z_neg ? -d : d); 00527 00528 for ( col = l, col_it = M.begin()+l, x_it = x_x.begin(); 00529 col < k; 00530 ++col, ++col_it, ++x_it ) { 00531 (*col_it)[ s] = ( z_neg ? *x_it : -( *x_it)); 00532 } 00533 ++s; 00534 00535 // store new denominator 00536 // --------------------- 00537 d = ( z_neg ? -z : z); 00538 CGAL_qpe_assertion( d > et0); 00539 00540 CGAL_qpe_debug { 00541 if ( vout.verbose()) print(); 00542 } 00543 } 00544 00545 // replacing of original variable (update type U5) [ replace column ] 00546 template < class RandomAccessIterator > 00547 void 00548 enter_original_leave_original( RandomAccessIterator y_x_it, unsigned int k) 00549 { 00550 // assert LP case or phase I 00551 CGAL_qpe_assertion( is_LP || is_phaseI); 00552 CGAL_qpe_assertion( k < b); 00553 00554 // update matrix in place 00555 // ---------------------- 00556 typename Matrix::iterator matrix_it; 00557 typename Row ::iterator row_it, row_k_it, row_k; 00558 00559 // handle sign of new denominator 00560 ET z = y_x_it[ k]; 00561 bool z_neg = ( z < et0); 00562 if ( z_neg) d = -d; 00563 00564 // QP (in phase I)? 00565 matrix_it = M.begin(); 00566 if ( is_QP) matrix_it += l; 00567 row_k = ( matrix_it+k)->begin(); 00568 00569 // rows: 0..s-1 without k 00570 unsigned int row, col; 00571 ET minus_y; 00572 for ( row = 0; 00573 row < s; 00574 ++row, ++matrix_it, ++y_x_it) { 00575 if ( row != k) { 00576 00577 // columns: 0..b-1 00578 minus_y = -( *y_x_it); 00579 for ( col = 0, row_it = matrix_it->begin(), row_k_it = row_k; 00580 col < b; 00581 ++col, ++row_it, ++row_k_it ){ 00582 00583 // update in place 00584 update_entry( *row_it, z, minus_y * *row_k_it, d); 00585 } 00586 } 00587 } 00588 00589 // rows: k (flip signs, if necessary) 00590 if ( z_neg) { 00591 for ( col = 0, row_it = row_k; 00592 col < b; 00593 ++col, ++row_it ) { 00594 00595 *row_it = -( *row_it); 00596 } 00597 } 00598 00599 // store new denominator 00600 // --------------------- 00601 d = ( z_neg ? -z : z); 00602 CGAL_qpe_assertion( d > et0); 00603 00604 // diagnostic output 00605 CGAL_qpe_debug { 00606 if ( vout.verbose()) print(); 00607 } 00608 } 00609 00610 // replacing of slack variable (update type U6) [ replace row ] 00611 template < class ForwardIterator > 00612 void 00613 enter_slack_leave_slack( ForwardIterator u_x_it, unsigned int k) 00614 { 00615 // assert LP case or phase I 00616 CGAL_qpe_assertion( is_LP || is_phaseI); 00617 CGAL_qpe_assertion( k < s); 00618 00619 // compute new row of basis inverse 00620 multiply__l( u_x_it, x_x.begin()); 00621 00622 // update matrix in place 00623 // ---------------------- 00624 typename Matrix::iterator matrix_it; 00625 typename Row ::iterator row_it, x_it; 00626 00627 // handle sign of new denominator 00628 ET z = x_x[ k]; 00629 bool z_neg = ( z < et0); 00630 if ( z_neg) d = -d; 00631 00632 // QP (in phase I)? 00633 matrix_it = M.begin(); 00634 if ( is_QP) matrix_it += l; 00635 00636 // rows: 0..s-1 00637 unsigned int row, col; 00638 ET minus_m_row; 00639 for ( row = 0; 00640 row < s; 00641 ++row, ++matrix_it) { 00642 00643 // columns: 0..b-1 00644 minus_m_row = -( *matrix_it)[ k]; 00645 for ( col = 0, row_it = matrix_it->begin(), x_it = x_x.begin(); 00646 col < b; 00647 ++col, ++row_it, ++x_it ){ 00648 00649 if ( col != k) { // all columns but k 00650 00651 // update in place 00652 update_entry( *row_it, z, minus_m_row * *x_it, d); 00653 00654 } else { // column k 00655 00656 // flip sign, if necessary 00657 if ( z_neg) *row_it = -( *row_it); 00658 00659 } 00660 } 00661 } 00662 00663 // store new denominator 00664 // --------------------- 00665 d = ( z_neg ? -z : z); 00666 CGAL_qpe_assertion( d > et0); 00667 00668 // diagnostic output 00669 CGAL_qpe_debug { 00670 if ( vout.verbose()) print(); 00671 } 00672 } 00673 00674 // replacing of slack by original variable (update type U7) [ grow ] 00675 template < class ForwardIterator1, class ForwardIterator2 > 00676 void enter_original_leave_slack( ForwardIterator1 y_x_it, 00677 ForwardIterator2 u_x_it) 00678 { 00679 // assert LP case or phase I 00680 CGAL_qpe_assertion( is_LP || is_phaseI); 00681 00682 // update matrix in-place 00683 // ---------------------- 00684 // compute new row of basis inverse 00685 multiply__l( u_x_it, x_x.begin()); 00686 ET z = d*u_x_it[ b] - inner_product_x( y_x_it, u_x_it); 00687 bool z_neg = ( z < et0); 00688 CGAL_qpe_assertion( z != et0); 00689 00690 // update matrix 00691 update_inplace_LP( x_x.begin(), y_x_it, z, ( z_neg ? -d : d)); 00692 00693 // append new row and column 00694 // ------------------------- 00695 typename Matrix::iterator matrix_it; 00696 typename Row ::iterator row_it, x_it; 00697 unsigned int row, col; 00698 00699 // QP (in phase I)? 00700 if ( is_QP) { 00701 ensure_physical_row(l+b); 00702 row_it = M[l+b].begin(); 00703 matrix_it = M.begin() + l; 00704 } else { 00705 row_it = M[s].begin(); 00706 matrix_it = M.begin(); 00707 } 00708 00709 // store 'x_x' in new row 00710 x_it = x_x.begin(); 00711 for ( col = 0; col < b; ++col, ++row_it, ++x_it) { 00712 *row_it = ( z_neg ? *x_it : -( *x_it)); 00713 } 00714 *row_it = ( z_neg ? -d : d); 00715 00716 // store 'y_x' in new col 00717 for ( row = 0; row < s; ++row, ++matrix_it, ++y_x_it) { 00718 (*matrix_it)[b] = ( z_neg ? *y_x_it : -( *y_x_it)); 00719 } 00720 ++s; ++b; 00721 00722 // store new denominator 00723 // --------------------- 00724 d = ( z_neg ? -z : z); 00725 CGAL_qpe_assertion( d > et0); 00726 00727 CGAL_qpe_debug { 00728 if ( vout.verbose()) print(); 00729 } 00730 } 00731 // due to basis_matrix_stays_regular fix, needs reconsideration 00732 //private: 00733 00734 // private member functions 00735 // ------------------------ 00736 // init (QP case) 00737 template < class InIt > inline 00738 void 00739 init( unsigned int art_size, InIt art_first, Tag_false) 00740 { 00741 // only Q is used to store A_B^-1 in phase I 00742 for ( s = l, b = 0; b < art_size; ++s, ++b, ++art_first) { 00743 ensure_physical_row(s); 00744 M[ s][ b] = ( art_first->second ? -et1 : et1); 00745 } 00746 00747 s = art_size; 00748 } 00749 00750 // init (LP case) 00751 template < class InIt > inline 00752 void 00753 init( unsigned int art_size, InIt art_first, Tag_true) 00754 { 00755 for ( s = 0; s < art_size; ++s, ++art_first) { 00756 std::fill_n( M[ s].begin(), art_size, et0); 00757 M[ s][ s] = ( art_first->second ? -et1 : et1); 00758 } 00759 b = art_size; 00760 } 00761 00762 // append row in Q if no allocated row available 00763 void ensure_physical_row (unsigned int row) { 00764 unsigned int rows = M.size(); 00765 CGAL_qpe_assertion(rows >= row); 00766 if (rows == row) { 00767 M.push_back(Row(row+1, et0)); 00768 00769 // do we have to grow x_x? 00770 CGAL_qpe_assertion(x_x.size() >= row-l); 00771 if (x_x.size() == row-l) 00772 x_x.push_back(et0); 00773 00774 // do we have to grow tmp_x? 00775 CGAL_qpe_assertion(tmp_x.size() >= row-l); 00776 if (tmp_x.size() == row-l) 00777 tmp_x.push_back(et0); 00778 00779 CGAL_qpe_assertion(M[row].size()==row+1); 00780 CGAL_qpe_debug { 00781 if ( vout.verbose()) { 00782 vout << "physical row " << (row) << " appended in Q\n"; 00783 } 00784 } 00785 } 00786 } 00787 00788 // matrix-vector multiplication (QP case) 00789 template < class ForIt, class OutIt, class Use1stArg > 00790 void 00791 multiply( ForIt v_l_it, ForIt v_x_it, 00792 OutIt y_l_it, OutIt y_x_it, Tag_false, 00793 Use1stArg use_1st_arg) const 00794 { 00795 // use 'LP' functions in phase I 00796 if ( is_phaseI) { 00797 multiply__l( v_x_it, y_l_it); 00798 multiply__x( v_l_it, y_x_it); 00799 return; 00800 } 00801 00802 // phase II 00803 typename Matrix::const_iterator matrix_it; 00804 typename Row ::const_iterator row_it; // left of diagonal 00805 typename Matrix::const_iterator column_it; // right of diagonal 00806 ForIt v_it; 00807 00808 unsigned int row, count, k = l+b; 00809 ET sum; 00810 00811 // compute P v_l + Q^T v_x 00812 for ( row = 0, matrix_it = M.begin(); 00813 row < s; 00814 ++row, ++y_l_it) { 00815 sum = et0; 00816 00817 // P v_l 00818 if ( check_tag( use_1st_arg)) { 00819 00820 // P: left of diagonal (including) 00821 for ( row_it = matrix_it->begin(), v_it = v_l_it; 00822 row_it != matrix_it->end(); 00823 ++row_it, ++v_it) { 00824 sum += *row_it * *v_it; 00825 } 00826 00827 // P: right of diagonal (excluding) 00828 for ( count = row+1, column_it = ++matrix_it; 00829 count < s; 00830 ++count, ++column_it, ++v_it) { 00831 sum += (*column_it)[ row] * *v_it; 00832 } 00833 } 00834 00835 // Q^T: 00836 for ( count = 0, column_it = M.begin()+l, v_it = v_x_it; 00837 count < b; 00838 ++count, ++column_it, ++v_it) { 00839 sum += (*column_it)[ row] * *v_it; 00840 } 00841 00842 // store result 00843 *y_l_it = sum; 00844 } 00845 00846 // compute Q v_l + R v_x 00847 for ( row = l, matrix_it = M.begin()+l; 00848 row < k; 00849 ++row, ++y_x_it) { 00850 sum = et0; 00851 00852 // Q v_l 00853 if ( check_tag( use_1st_arg)) { 00854 00855 // Q: 00856 for ( count = 0, row_it = matrix_it->begin(), v_it = v_l_it; 00857 count < s; 00858 ++count, ++row_it, ++v_it) { 00859 sum += *row_it * *v_it; 00860 } 00861 } 00862 00863 // R: left of diagonal (including) 00864 for ( row_it = matrix_it->begin()+l, v_it = v_x_it; 00865 row_it != matrix_it->end(); 00866 ++row_it, ++v_it) { 00867 sum += *row_it * *v_it; 00868 } 00869 00870 // R: right of diagonal (excluding) 00871 for ( count = row+1, column_it = ++matrix_it; 00872 count < k; 00873 ++count, ++column_it, ++v_it) { 00874 sum += (*column_it)[ row] * *v_it; 00875 } 00876 00877 // store result 00878 *y_x_it = sum; 00879 } 00880 } 00881 00882 // matrix-vector multiplication (LP case) 00883 template < class ForIt, class OutIt, class Dummy > inline 00884 void 00885 multiply( ForIt v_l_it, ForIt v_x_it, 00886 OutIt y_l_it, OutIt y_x_it, Tag_true, Dummy) const 00887 { 00888 multiply__l( v_x_it, y_l_it); 00889 multiply__x( v_l_it, y_x_it); 00890 } 00891 00892 // special matrix-vector multiplication functions for LPs 00893 template < class ForIt, class OutIt > inline 00894 void 00895 multiply__l( ForIt v_x_it, OutIt y_l_it) const 00896 { 00897 typename Matrix::const_iterator matrix_it = M.begin(); 00898 typename Matrix::const_iterator column_it; 00899 ForIt v_it; 00900 00901 unsigned int row, count; 00902 ET sum; 00903 00904 // QP? 00905 if ( is_QP) matrix_it += l; 00906 00907 // compute Q^T v_x 00908 for ( row = 0; row < s; ++row, ++y_l_it) { 00909 sum = et0; 00910 00911 for ( count = 0, column_it = matrix_it, v_it = v_x_it; 00912 count < b; 00913 ++count, ++column_it, ++v_it) { 00914 sum += (*column_it)[ row] * *v_it; 00915 } 00916 00917 *y_l_it = sum; 00918 } 00919 } 00920 00921 template < class ForIt, class OutIt > inline 00922 void 00923 multiply__x( ForIt v_l_it, OutIt y_x_it) const 00924 { 00925 typename Matrix::const_iterator matrix_it = M.begin(); 00926 unsigned int row; 00927 00928 // QP? 00929 if ( is_QP) matrix_it += l; 00930 00931 // compute Q v_l 00932 for ( row = 0; 00933 row < b; 00934 ++row, ++matrix_it, ++y_x_it) { 00935 00936 *y_x_it = inner_product( matrix_it->begin(), v_l_it, s); 00937 } 00938 } 00939 00940 // vector-vector multiplication 00941 template < class InIt1, class InIt2 > inline 00942 typename std::iterator_traits<InIt1>::value_type 00943 inner_product( InIt1 u_it, InIt2 v_it, unsigned int n) const 00944 { 00945 typedef typename std::iterator_traits<InIt1>::value_type NT; 00946 00947 // compute u^T v 00948 NT sum = NT( 0); 00949 for ( unsigned int count = 0; count < n; ++count, ++u_it, ++v_it) { 00950 sum += NT(*u_it) * NT(*v_it); 00951 } 00952 00953 return sum; 00954 } 00955 00956 // in-place update 00957 template < class ForIt > // QP case 00958 void update_inplace_QP( ForIt y_l_it, ForIt y_x_it, 00959 const ET& d_new, const ET& d_old) 00960 { 00961 typename Matrix:: iterator matrix_it; 00962 typename Row :: iterator row_it; 00963 typename Row ::const_iterator y_it1, y_it2; 00964 00965 unsigned int row, col, k = l+b; 00966 00967 // rows: 0..s-1 ( P ) 00968 for ( row = 0, y_it1 = y_l_it, matrix_it = M.begin(); 00969 row < s; 00970 ++row, ++y_it1, ++matrix_it ) { 00971 00972 // columns: 0..row ( P ) 00973 for ( row_it = matrix_it->begin(), y_it2 = y_l_it; 00974 row_it != matrix_it->end(); 00975 ++row_it, ++y_it2 ) { 00976 00977 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old); 00978 } 00979 } 00980 00981 // rows: l..k-1 ( Q R ) 00982 for ( row = l, y_it1 = y_x_it, matrix_it += l-s; 00983 row < k; 00984 ++row, ++y_it1, ++matrix_it ) { 00985 00986 // columns: 0..s-1 ( Q ) 00987 for ( col = 0, row_it = matrix_it->begin(), y_it2 = y_l_it; 00988 col < s; 00989 ++col, ++row_it, ++y_it2 ){ 00990 00991 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old); 00992 } 00993 00994 // columns: l..k-1 ( R ) 00995 for ( row_it += l-s, y_it2 = y_x_it; 00996 row_it != matrix_it->end(); 00997 ++row_it, ++y_it2 ){ 00998 00999 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old); 01000 } 01001 } 01002 } 01003 01004 template < class ForIt1, class ForIt2 > // LP case 01005 void update_inplace_LP( ForIt1 x_x_it, ForIt2 y_x_it, 01006 const ET& d_new, const ET& d_old) 01007 { 01008 typename Matrix:: iterator matrix_it; 01009 typename Row :: iterator row_it; 01010 ForIt1 x_it; 01011 01012 unsigned int row, col; 01013 ET y; 01014 01015 // QP (in phase I)? 01016 matrix_it = M.begin(); 01017 if ( is_QP) matrix_it += l; 01018 01019 // rows: 0..s-1 ( Q ) 01020 for ( row = 0; 01021 row < s; 01022 ++row, ++y_x_it, ++matrix_it) { 01023 01024 // columns: 0..b-1 ( Q ) 01025 y = *y_x_it; 01026 for ( col = 0, row_it = matrix_it->begin(), x_it = x_x_it; 01027 col < b; 01028 ++col, ++row_it, ++x_it ){ 01029 01030 update_entry( *row_it, d_new, y * *x_it, d_old); 01031 } 01032 } 01033 } 01034 01035 01036 template < class RandomAccessIterator > 01037 typename std::iterator_traits<RandomAccessIterator>::value_type 01038 inv_M_B_row_dot_col( int row, RandomAccessIterator u_l_it) const 01039 { 01040 typename Row::const_iterator row_it; 01041 if ( is_QP) { 01042 row_it = M[l + row].begin(); 01043 } else { 01044 row_it = M[row].begin(); 01045 } 01046 return inner_product(row_it, u_l_it, b); 01047 } 01048 01049 }; 01050 01051 // ---------------------------------------------------------------------------- 01052 01053 // ============================= 01054 // class implementation (inline) 01055 // ============================= 01056 01057 // creation 01058 template < class ET_, class Is_LP_ > inline 01059 QP_basis_inverse<ET_,Is_LP_>:: 01060 QP_basis_inverse( CGAL::Verbose_ostream& verbose_ostream) 01061 : et0( 0), et1( 1), et2( 2), 01062 is_LP( check_tag( Is_LP())), is_QP( ! is_LP), 01063 vout( verbose_ostream) 01064 { } 01065 01066 // transition (LP case) 01067 template < class ET_, class Is_LP_ > inline 01068 void QP_basis_inverse<ET_,Is_LP_>:: 01069 transition( ) 01070 { 01071 is_phaseI = false; 01072 is_phaseII = true; 01073 01074 CGAL_qpe_debug { 01075 if ( vout.verbose()) print(); 01076 } 01077 } 01078 01079 // set-up (QP case) 01080 template < class ET_, class Is_LP_ > inline 01081 void QP_basis_inverse<ET_,Is_LP_>:: 01082 set( Tag_false) 01083 { 01084 M.reserve( l); 01085 // only allocate empty rows 01086 for ( unsigned int i = 0; i < l; ++i ) 01087 M.push_back(Row(0, et0)); 01088 } 01089 01090 // set-up (LP case) 01091 template < class ET_, class Is_LP_ > inline 01092 void QP_basis_inverse<ET_,Is_LP_>:: 01093 set( Tag_true) 01094 { 01095 M.reserve( l); 01096 for ( unsigned int i = 0; i < l; ++i) M.push_back( Row( l, et0)); 01097 } 01098 01099 // access (QP case) 01100 template < class ET_, class Is_LP_ > inline 01101 const ET_& QP_basis_inverse<ET_,Is_LP_>:: 01102 entry( unsigned int r, unsigned int c, Tag_false) const 01103 { 01104 CGAL_qpe_assertion( ( r < s) || ( ( r >= l) && ( r < l+b))); 01105 CGAL_qpe_assertion( ( c < s) || ( ( c >= l) && ( c < l+b))); 01106 return ( c < r ? M[ r][ c] : M[ c][ r]); 01107 } 01108 01109 // access (LP case) 01110 template < class ET_, class Is_LP_ > inline 01111 const ET_& QP_basis_inverse<ET_,Is_LP_>:: 01112 entry( unsigned int r, unsigned int c, Tag_true) const 01113 { 01114 CGAL_qpe_assertion( r < s); 01115 CGAL_qpe_assertion( c < b); 01116 return M[ r][ c]; 01117 } 01118 01119 // in-place update 01120 template < class ET_, class Is_LP_ > inline 01121 void QP_basis_inverse<ET_,Is_LP_>:: 01122 update_entry( ET& entry, const ET& d_new, const ET& y, const ET& d_old) const 01123 { 01124 entry *= d_new; 01125 entry += y; 01126 entry = CGAL::integral_division(entry, d_old); 01127 } 01128 01129 CGAL_END_NAMESPACE 01130 01131 #include <CGAL/QP_solver/QP_basis_inverse_impl.h> 01132 01133 #endif // CGAL_QP_SOLVER_QP_BASIS_INVERSE_H 01134 01135 // ===== EOF ==================================================================
1.7.6.1
