1 // Copyright (c) 1997-2007 ETH Zurich (Switzerland).
2 // All rights reserved.
3 //
4 // This file is part of CGAL (www.cgal.org).
5 //
6 // $URL: https://github.com/CGAL/cgal/blob/v5.3/QP_solver/include/CGAL/QP_solver/QP_basis_inverse.h $
7 // $Id: QP_basis_inverse.h a925a64 2021-01-05T18:42:53+01:00 Sébastien Loriot
8 // SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
9 //
10 //
11 // Author(s) : Sven Schoenherr
12 // Bernd Gaertner <gaertner@inf.ethz.ch>
13 // Franz Wessendorp
14 // Kaspar Fischer
15
16 #ifndef CGAL_QP_SOLVER_QP_BASIS_INVERSE_H
17 #define CGAL_QP_SOLVER_QP_BASIS_INVERSE_H
18
19 #include <CGAL/license/QP_solver.h>
20
21
22 #include <CGAL/QP_solver/basic.h>
23 #include <CGAL/IO/Verbose_ostream.h>
24 #include <vector>
25
26 namespace CGAL {
27
28 // =================
29 // class declaration
30 // =================
31 template < class ET_, class Is_LP_ >
32 class QP_basis_inverse;
33
34 // ===============
35 // class interface
36 // ===============
37 template < class ET_, class Is_LP_ >
38 class QP_basis_inverse {
39 public:
40 // self
41 typedef ET_ ET;
42 typedef Is_LP_ Is_LP;
43 typedef QP_basis_inverse<ET,Is_LP>
44 Self;
45
46 private:
47
48 // private types
49 typedef std::vector<ET> Row;
50 typedef std::vector<Row> Matrix;
51
52 public:
53
54 /*
55 * Note: Some member functions below are suffixed with '_'.
56 * They are member templates and their declaration is "hidden",
57 * because they are also implemented in the class interface.
58 * This is a workaround for M$-VC++, which otherwise fails to
59 * instantiate them correctly.
60 */
61
62 // creation and initialization
63 // ---------------------------
64 QP_basis_inverse( CGAL::Verbose_ostream& verbose_ostream);
65
66 // set-up
67 void set( int n, int m, int nr_equalities);
68
69 // init
70 template < class InputIterator > // phase I
71 void init_( unsigned int art_size, InputIterator art_first);
72
73 /*
74 template < class InputIterator > // phase II
75 void init_( ...);
76 */
77
78 // transition to phase II
79 template < class InputIterator > // QP case
80 void transition_( InputIterator twice_D_it);
81
82 void transition( ); // LP case
83
84 // access
85 // ------
denominator()86 const ET& denominator( ) const { return d; }
87
entry(unsigned int row,unsigned int column)88 const ET& entry( unsigned int row, unsigned int column) const
89 { return entry( row, column, Is_LP()); }
90
91 // multiplication functions
92 // ------------------------
93 // matrix-vector multiplication ( y = M v )
94 template < class ForwardIterator, class OutputIterator > inline
multiply(ForwardIterator v_l_it,ForwardIterator v_x_it,OutputIterator y_l_it,OutputIterator y_x_it)95 void multiply( ForwardIterator v_l_it, ForwardIterator v_x_it,
96 OutputIterator y_l_it, OutputIterator y_x_it) const
97 { multiply( v_l_it, v_x_it, y_l_it, y_x_it, Is_LP(), Tag_true()); }
98
99 // special matrix-vector multiplication functions for LPs
100 template < class ForwardIterator, class OutputIterator > inline
multiply_l(ForwardIterator v_x_it,OutputIterator y_l_it)101 void multiply_l( ForwardIterator v_x_it, OutputIterator y_l_it) const
102 { CGAL_qpe_assertion( is_LP || is_phaseI);
103 multiply__l( v_x_it, y_l_it); }
104
105 template < class ForwardIterator, class OutputIterator > inline
multiply_x(ForwardIterator v_l_it,OutputIterator y_x_it)106 void multiply_x( ForwardIterator v_l_it, OutputIterator y_x_it) const
107 { CGAL_qpe_assertion( is_LP || is_phaseI);
108 multiply__x( v_l_it, y_x_it); }
109
110 // vector-matrix multiplication ( x^T = u^T M )
111 template < class ForwardIterator, class OutputIterator > inline
multiply_transposed(ForwardIterator u_l_it,ForwardIterator u_x_it,OutputIterator x_l_it,OutputIterator x_x_it)112 void multiply_transposed( ForwardIterator u_l_it, ForwardIterator u_x_it,
113 OutputIterator x_l_it, OutputIterator x_x_it)
114 const
115 { multiply( u_l_it, u_x_it, x_l_it, x_x_it); } // M_B^{-1} is symmetric
116
117 // special vector-matrix multiplication functions for LPs
118 template < class ForwardIterator, class OutputIterator > inline
multiply_transposed_l(ForwardIterator u_x_it,OutputIterator x_l_it)119 void multiply_transposed_l( ForwardIterator u_x_it,
120 OutputIterator x_l_it) const
121 { multiply_l( u_x_it, x_l_it); } // Note: M_B^{-1} is symmetric
122
123 template < class ForwardIterator, class OutputIterator > inline
multiply_transposed_x(ForwardIterator u_l_it,OutputIterator x_x_it)124 void multiply_transposed_x( ForwardIterator u_l_it,
125 OutputIterator x_x_it) const
126 { multiply_x( u_l_it, x_x_it); } // Note: M_B^{-1} is symmetric
127
128 // vector-vector multiplication ( y = u^T v )
129 template < class InputIterator1, class InputIterator2 > inline
130 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)131 inner_product( InputIterator1 u_l_it, InputIterator1 u_x_it,
132 InputIterator2 v_l_it, InputIterator2 v_x_it) const
133 { return inner_product_l( u_l_it, v_l_it)
134 + inner_product_x( u_x_it, v_x_it); }
135
136 template < class InputIterator1, class InputIterator2 > inline
137 typename std::iterator_traits<InputIterator1>::value_type
inner_product_l(InputIterator1 u_l_it,InputIterator2 v_l_it)138 inner_product_l( InputIterator1 u_l_it, InputIterator2 v_l_it) const
139 { return inner_product( u_l_it, v_l_it, s); }
140
141 template < class InputIterator1, class InputIterator2 > inline
142 typename std::iterator_traits<InputIterator1>::value_type
inner_product_x(InputIterator1 u_x_it,InputIterator2 v_x_it)143 inner_product_x( InputIterator1 u_x_it, InputIterator2 v_x_it) const
144 { return inner_product( u_x_it, v_x_it, b); }
145
146
147 // update functions
148 // ----------------
149 // entering of original variable (update type U1)
150 template < class ForwardIterator >
151 void enter_original_( ForwardIterator y_l_it,
152 ForwardIterator y_x_it, const ET& z);
153
154 // leaving of original variable (update type U2)
155 void leave_original( );
156
157 // entering of slack variable (update type U3)
158 void enter_slack( );
159
160 // leaving of slack variable (update type U4)
161 template < class ForwardIterator >
162 void leave_slack_( ForwardIterator u_x_it);
163
164 // replacing of original by original variable (update type U5)
165 template < class ForwardIterator >
166 void enter_original_leave_original_( ForwardIterator y, unsigned int k);
167
168 // replacing of slack by slack variable (update type U6)
169 template < class ForwardIterator >
170 void enter_slack_leave_slack_( ForwardIterator u, unsigned int k);
171
172 // replacing of slack by original variable (update type U7)
173 template < class ForwardIterator1, class ForwardIterator2 >
174 void enter_original_leave_slack_( ForwardIterator1 y, ForwardIterator2 u);
175
176 // replacing of original by slack variable (update type U8)
177 void enter_slack_leave_original( );
178
179
180 // replacing of original by original variable with precondition in QP-case
181 // for phaseII (update type UZ_1)
182 template < class ForwardIterator >
183 void z_replace_original_by_original(ForwardIterator y_l_it,
184 ForwardIterator y_x_it,
185 const ET& s_delta, const ET& s_nu,
186 unsigned int k_i);
187
188
189 // replacing of original by slack variable with precondition in QP-case
190 // for phaseII (update type UZ_2)
191 void z_replace_original_by_slack( );
192
193
194 // replacing of slack by original variable with precondition in QP-case
195 // for phaseII (update type UZ_3)
196 template < class ForwardIterator >
197 void z_replace_slack_by_original(ForwardIterator y_l_it,
198 ForwardIterator y_x_it,
199 ForwardIterator u_x_it, const ET& hat_kappa,
200 const ET& hat_nu);
201
202
203 // replacing of slack by slack variable with precondition in QP-case
204 // for phaseII (update type UZ_4)
205 template < class ForwardIterator >
206 void z_replace_slack_by_slack(ForwardIterator u_x_it, unsigned int k_j);
207
208
209 // copying of reduced basis inverse row in (upper) C-half
210 template < class OutIt >
211 void copy_row_in_C(OutIt y_l_it, OutIt y_x_it, unsigned int k);
212
213 // copying of reduced basis inverse row in (lower) B_O-half
214 template < class OutIt >
215 void copy_row_in_B_O(OutIt y_l_it, OutIt y_x_it, unsigned int k);
216
217
218 // swap functions
swap_variable(unsigned int j)219 void swap_variable( unsigned int j) // ``to the end'' of R
220 { CGAL_qpe_assertion( j < b);
221 swap_variable( j, Is_LP()); }
swap_constraint(unsigned int i)222 void swap_constraint( unsigned int i) // ``to the end'' of P
223 { CGAL_qpe_assertion( i < s);
224 swap_constraint( i, Is_LP()); }
225
226 private:
227
228 // constants
229 const ET et0, et1, et2;
230
231 // data members
232 Matrix M; // basis inverse, stored row-wise
233 ET d; // denominator
234
235 unsigned int l; // minimum of `n' and `m'
236 unsigned int s; // size of `E \cup S_N'
237 unsigned int b; // size of `B_O'
238
239 bool is_phaseI; // flag indicating phase I
240 bool is_phaseII;// flag indicating phase II
241 const bool is_LP; // flag indicating a linear program
242 const bool is_QP; // flag indicating a quadratic program
243
244 CGAL::Verbose_ostream& vout; // used for diagnostic output
245
246 Row x_l, tmp_l; // used in the
247 Row x_x, tmp_x; // update functions
248
249 // private member functions
250 // ------------------------
251 // set-up
252 void set( Tag_false); // QP case
253 void set( Tag_true ); // LP case
254
255 // init
256 template < class InIt > // QP case
257 void init_( unsigned int art_size, InIt art_first, Tag_false);
258 template < class InIt > // LP case
259 void init_( unsigned int art_size, InIt art_first, Tag_true );
260
261 // access
262 const ET& entry( unsigned int row,
263 unsigned int column, Tag_false) const; // QP case
264 const ET& entry( unsigned int row,
265 unsigned int column, Tag_true ) const; // LP case
266
267 // matrix-vector multiplication
268 template < class ForIt, class OutIt, class Use1stArg > // QP case
269 void multiply_( ForIt v_l_it, ForIt v_x_it,
270 OutIt y_l_it, OutIt y_x_it, Tag_false, Use1stArg) const;
271 template < class ForIt, class OutIt, class DummyArg > // LP case
272 void multiply_( ForIt v_l_it, ForIt v_x_it,
273 OutIt y_l_it, OutIt y_x_it, Tag_true, DummyArg) const;
274
275 // special matrix-vector multiplication functions for LPs
276 template < class ForIt, class OutIt >
277 void multiply__l_( ForIt v_x_it, OutIt y_l_it) const;
278 template < class ForIt, class OutIt >
279 void multiply__x_( ForIt v_l_it, OutIt y_x_it) const;
280
281 // in-place update
282 template < class ForIt > // QP case
283 void update_inplace_QP_( ForIt y_l_it, ForIt y_x_it,
284 const ET& d_new, const ET& d_old);
285 template < class ForIt1, class ForIt2 > // LP case
286 void update_inplace_LP_( ForIt1 x_x_it, ForIt2 y_x_it,
287 const ET& d_new, const ET& d_old);
288
289 template < class ForIt > // QP case only
290 void z_update_inplace( ForIt psi1_l_it, ForIt psi1_x_it,
291 ForIt psi2_l_it, ForIt psi2_x_it,
292 const ET& omega0, const ET& omega1,
293 const ET& omega2, const ET& omega3);
294
295 void update_entry( ET& entry, const ET& d_new,
296 const ET& y, const ET& d_old) const;
297
298 // swap functions
299 void swap_variable ( unsigned int, Tag_true ); // LP case
300 void swap_variable ( unsigned int, Tag_false); // QP case
301 void swap_constraint( unsigned int, Tag_true ); // LP case
302 void swap_constraint( unsigned int, Tag_false); // QP case
303
304 // diagnostic output
305 void print( );
306
307 // ----------------------------------------------------------------------------
308
309 // ===============================
310 // class implementation (template)
311 // ===============================
312
313 public:
314
315 // creation and initialization
316 // ---------------------------
317 // init
318 template < class InputIterator >
319 void
init(unsigned int art_size,InputIterator art_first)320 init( unsigned int art_size, InputIterator art_first)
321 {
322 CGAL_qpe_assertion_msg( art_size <= l, \
323 "There are more equality constraints than original variables!");
324
325 init( art_size, art_first, Is_LP());
326 d = et1;
327 CGAL_qpe_assertion( s == art_size);
328 CGAL_qpe_assertion( b == art_size);
329
330 is_phaseI = true;
331 is_phaseII = false;
332
333 if ( x_x.size() < art_size) {
334 x_x.insert( x_x.end(), art_size-x_x.size(), et0);
335 }
336
337 if ( tmp_x.size() < art_size) {
338 tmp_x.insert( tmp_x.end(), art_size-tmp_x.size(), et0);
339 }
340
341 CGAL_qpe_debug {
342 if ( vout.verbose()) print();
343 }
344 }
345
346 // transition to phase II
347 template < class InputIterator > // QP case
transition(InputIterator twice_D_it)348 void transition( InputIterator twice_D_it)
349 {
350 typename Matrix::iterator m_it1, m_it2, p_begin, r_begin;
351 typename Row ::iterator x_it;
352 unsigned int row, col;
353
354 // fill missing rows
355 for (row= 0; row< s; ++ row) {
356 CGAL_qpe_assertion(M[row].size()==0);
357 M[row].insert(M[row].end(), row+1, et0);
358 }
359
360 // compute new basis inverse [ upper-left part: -(Q^T * 2 D_B * Q) ]
361 // -----------------------------------------------------------------
362 // compute 'Q^T * 2 D_B' ( Q = A_B^-1 )
363 p_begin = M.begin();
364 r_begin = M.begin();
365 if (b > 0) r_begin += l+s-1; // initialize only if for loops
366 // are entered
367 for ( col = 0; col < b; ++col, ++twice_D_it) {
368 ++p_begin;
369
370 // get column of D (D symmetric)
371 std::copy( *twice_D_it, *twice_D_it+b, x_l.begin());
372
373 // compute 'Q^T * 2 D_Bj'
374 multiply__l( x_l.begin(), x_x.begin());
375
376 // store resulting column in 'P' and 'R'
377 x_it = x_x.begin();
378 m_it2 = r_begin;
379 for ( row = l+col; row >= l; --row, --m_it2, ++x_it) {
380 (*m_it2)[ row] = *x_it;
381 }
382 m_it1 = p_begin;
383 for ( row = col+1; row < s; ++row, ++m_it1, ++x_it) {
384 (*m_it1)[ col] = *x_it;
385 }
386 }
387
388 // compute '(Q^T * 2 D_B) * Q' ( Q = A_B^-1 )
389 m_it1 = M.begin();
390 m_it2 = r_begin;
391 for ( row = 0; row < s; ++row, ++m_it1, --m_it2) {
392
393 // get row of '(Q^T * 2 D_B)' (stored in 'P' and 'R')
394 std::copy(m_it1->begin() ,m_it1->begin()+row, x_l.begin());
395 std::copy(m_it2->begin()+l,m_it2->begin()+(l+b-row),
396 x_l.begin()+row);
397
398 // compute '(Q^T * 2 D_B)_i * Q'
399 multiply__l( x_l.begin(), x_x.begin());
400
401 // negate and store result in 'P'
402 std::transform( x_x.begin(), x_x.begin()+row+1,
403 m_it1->begin(), std::negate<ET>());
404
405 // clean up in 'R'
406 std::fill_n( m_it2->begin()+l, b-row, et0);
407 }
408
409 // scale A_B^-1
410 m_it1 = M.begin()+l;
411 for ( row = 0; row < s; ++row, ++m_it1) {
412 std::transform( m_it1->begin(), m_it1->begin()+s, m_it1->begin(),
413 [this](const ET& v){return v * this->d;});
414 }
415
416 // new denominator: |det(A_B)|^2
417 d *= d;
418
419 // update status
420 transition();
421 }
422
423 // update functions
424 // ----------------
425 // entering of original variable (update type U1)
426 template < class ForwardIterator >
427 void
enter_original(ForwardIterator y_l_it,ForwardIterator y_x_it,const ET & z)428 enter_original( ForwardIterator y_l_it,
429 ForwardIterator y_x_it, const ET& z)
430 {
431 // assert QP case
432 Assert_compile_time_tag( Tag_false(), Is_LP());
433
434 // update matrix in-place
435 // ----------------------
436 // handle sign of new denominator
437 CGAL_qpe_assertion( z != et0);
438 bool z_neg = ( z < et0);
439
440 // update matrix
441 update_inplace_QP( y_l_it, y_x_it, z, ( z_neg ? -d : d));
442
443 // append new row and column ("after R")
444 // -------------------------------------
445 typename Row::iterator row_it;
446 ForwardIterator y_it;
447 unsigned int col, k = l+(++b);
448
449 // // allocate new row, if necessary
450 // // BG: replaced this by the ensure_physical_row construct below
451 // if ( k <= M.size()) {
452 // row_it = M[ k-1].begin();
453 // } else {
454 // row_it = M.insert( M.end(), Row( k, et0))->begin();
455 // x_x.push_back( et0);
456 // tmp_x.push_back( et0);
457 // }
458 ensure_physical_row(k-1);
459 row_it = M[ k-1].begin();
460
461 // store entries in new row
462 for ( col = 0, y_it = y_l_it;
463 col < s;
464 ++col, ++row_it, ++y_it ) {
465 *row_it = ( z_neg ? *y_it : -( *y_it));
466 }
467 for ( col = l, row_it += l-s, y_it = y_x_it;
468 col < k-1;
469 ++col, ++row_it, ++y_it ) {
470 *row_it = ( z_neg ? *y_it : -( *y_it));
471 }
472 *row_it = ( z_neg ? -d : d);
473
474 // store new denominator
475 // ---------------------
476 d = ( z_neg ? -z : z);
477 CGAL_qpe_assertion( d > et0);
478
479 CGAL_qpe_debug {
480 if ( vout.verbose()) print();
481 }
482 }
483
484 // leaving of slack variable (update type U4)
485 template < class ForwardIterator >
486 void
leave_slack(ForwardIterator u_x_it)487 leave_slack( ForwardIterator u_x_it)
488 {
489 // assert QP case
490 Assert_compile_time_tag( Tag_false(), Is_LP());
491
492 // update matrix in-place
493 // ----------------------
494 // compute new row/column of basis inverse
495 multiply( u_x_it, // dummy (not used)
496 u_x_it, x_l.begin(), x_x.begin(),
497 Tag_false(), // QP
498 Tag_false()); // ignore 1st argument
499 ET z = -inner_product_x( x_x.begin(), u_x_it);
500 bool z_neg = ( z < et0);
501 CGAL_qpe_assertion( z != et0);
502
503 // update matrix
504 update_inplace_QP( x_l.begin(), x_x.begin(), z, ( z_neg ? -d : d));
505
506 // insert new row and column ("after P")
507 // -------------------------------------
508 typename Row ::iterator row_it, x_it;
509 typename Matrix::iterator col_it;
510 unsigned int col, k = l+b;
511
512 // store entries in new row
513 if (M[s].size()==0) {
514 // row has to be filled first
515 M[s].insert(M[s].end(), s+1, et0);
516 }
517 for ( col = 0, row_it = M[ s].begin(), x_it = x_l.begin();
518 col < s;
519 ++col, ++row_it, ++x_it ) {
520 *row_it = ( z_neg ? *x_it : -( *x_it));
521 }
522 *row_it = ( z_neg ? -d : d);
523
524 for ( col = l, col_it = M.begin()+l, x_it = x_x.begin();
525 col < k;
526 ++col, ++col_it, ++x_it ) {
527 (*col_it)[ s] = ( z_neg ? *x_it : -( *x_it));
528 }
529 ++s;
530
531 // store new denominator
532 // ---------------------
533 d = ( z_neg ? -z : z);
534 CGAL_qpe_assertion( d > et0);
535
536 CGAL_qpe_debug {
537 if ( vout.verbose()) print();
538 }
539 }
540
541 // replacing of original variable (update type U5) [ replace column ]
542 template < class RandomAccessIterator >
543 void
enter_original_leave_original(RandomAccessIterator y_x_it,unsigned int k)544 enter_original_leave_original( RandomAccessIterator y_x_it, unsigned int k)
545 {
546 // assert LP case or phase I
547 CGAL_qpe_assertion( is_LP || is_phaseI);
548 CGAL_qpe_assertion( k < b);
549
550 // update matrix in place
551 // ----------------------
552 typename Matrix::iterator matrix_it;
553 typename Row ::iterator row_it, row_k_it, row_k;
554
555 // handle sign of new denominator
556 ET z = y_x_it[ k];
557 bool z_neg = ( z < et0);
558 if ( z_neg) d = -d;
559
560 // QP (in phase I)?
561 matrix_it = M.begin();
562 if ( is_QP) matrix_it += l;
563 row_k = ( matrix_it+k)->begin();
564
565 // rows: 0..s-1 without k
566 unsigned int row, col;
567 ET minus_y;
568 for ( row = 0;
569 row < s;
570 ++row, ++matrix_it, ++y_x_it) {
571 if ( row != k) {
572
573 // columns: 0..b-1
574 minus_y = -( *y_x_it);
575 for ( col = 0, row_it = matrix_it->begin(), row_k_it = row_k;
576 col < b;
577 ++col, ++row_it, ++row_k_it ){
578
579 // update in place
580 update_entry( *row_it, z, minus_y * *row_k_it, d);
581 }
582 }
583 }
584
585 // rows: k (flip signs, if necessary)
586 if ( z_neg) {
587 for ( col = 0, row_it = row_k;
588 col < b;
589 ++col, ++row_it ) {
590
591 *row_it = -( *row_it);
592 }
593 }
594
595 // store new denominator
596 // ---------------------
597 d = ( z_neg ? -z : z);
598 CGAL_qpe_assertion( d > et0);
599
600 // diagnostic output
601 CGAL_qpe_debug {
602 if ( vout.verbose()) print();
603 }
604 }
605
606 // replacing of slack variable (update type U6) [ replace row ]
607 template < class ForwardIterator >
608 void
enter_slack_leave_slack(ForwardIterator u_x_it,unsigned int k)609 enter_slack_leave_slack( ForwardIterator u_x_it, unsigned int k)
610 {
611 // assert LP case or phase I
612 CGAL_qpe_assertion( is_LP || is_phaseI);
613 CGAL_qpe_assertion( k < s);
614
615 // compute new row of basis inverse
616 multiply__l( u_x_it, x_x.begin());
617
618 // update matrix in place
619 // ----------------------
620 typename Matrix::iterator matrix_it;
621 typename Row ::iterator row_it, x_it;
622
623 // handle sign of new denominator
624 ET z = x_x[ k];
625 bool z_neg = ( z < et0);
626 if ( z_neg) d = -d;
627
628 // QP (in phase I)?
629 matrix_it = M.begin();
630 if ( is_QP) matrix_it += l;
631
632 // rows: 0..s-1
633 unsigned int row, col;
634 ET minus_m_row;
635 for ( row = 0;
636 row < s;
637 ++row, ++matrix_it) {
638
639 // columns: 0..b-1
640 minus_m_row = -( *matrix_it)[ k];
641 for ( col = 0, row_it = matrix_it->begin(), x_it = x_x.begin();
642 col < b;
643 ++col, ++row_it, ++x_it ){
644
645 if ( col != k) { // all columns but k
646
647 // update in place
648 update_entry( *row_it, z, minus_m_row * *x_it, d);
649
650 } else { // column k
651
652 // flip sign, if necessary
653 if ( z_neg) *row_it = -( *row_it);
654
655 }
656 }
657 }
658
659 // store new denominator
660 // ---------------------
661 d = ( z_neg ? -z : z);
662 CGAL_qpe_assertion( d > et0);
663
664 // diagnostic output
665 CGAL_qpe_debug {
666 if ( vout.verbose()) print();
667 }
668 }
669
670 // replacing of slack by original variable (update type U7) [ grow ]
671 template < class ForwardIterator1, class ForwardIterator2 >
enter_original_leave_slack(ForwardIterator1 y_x_it,ForwardIterator2 u_x_it)672 void enter_original_leave_slack( ForwardIterator1 y_x_it,
673 ForwardIterator2 u_x_it)
674 {
675 // assert LP case or phase I
676 CGAL_qpe_assertion( is_LP || is_phaseI);
677
678 // update matrix in-place
679 // ----------------------
680 // compute new row of basis inverse
681 multiply__l( u_x_it, x_x.begin());
682 ET z = d*u_x_it[ b] - inner_product_x( y_x_it, u_x_it);
683 bool z_neg = ( z < et0);
684 CGAL_qpe_assertion( z != et0);
685
686 // update matrix
687 update_inplace_LP( x_x.begin(), y_x_it, z, ( z_neg ? -d : d));
688
689 // append new row and column
690 // -------------------------
691 typename Matrix::iterator matrix_it;
692 typename Row ::iterator row_it, x_it;
693 unsigned int row, col;
694
695 // QP (in phase I)?
696 if ( is_QP) {
697 ensure_physical_row(l+b);
698 row_it = M[l+b].begin();
699 matrix_it = M.begin() + l;
700 } else {
701 row_it = M[s].begin();
702 matrix_it = M.begin();
703 }
704
705 // store 'x_x' in new row
706 x_it = x_x.begin();
707 for ( col = 0; col < b; ++col, ++row_it, ++x_it) {
708 *row_it = ( z_neg ? *x_it : -( *x_it));
709 }
710 *row_it = ( z_neg ? -d : d);
711
712 // store 'y_x' in new col
713 for ( row = 0; row < s; ++row, ++matrix_it, ++y_x_it) {
714 (*matrix_it)[b] = ( z_neg ? *y_x_it : -( *y_x_it));
715 }
716 ++s; ++b;
717
718 // store new denominator
719 // ---------------------
720 d = ( z_neg ? -z : z);
721 CGAL_qpe_assertion( d > et0);
722
723 CGAL_qpe_debug {
724 if ( vout.verbose()) print();
725 }
726 }
727 // due to basis_matrix_stays_regular fix, needs reconsideration
728 //private:
729
730 // private member functions
731 // ------------------------
732 // init (QP case)
733 template < class InIt > inline
734 void
init(unsigned int art_size,InIt art_first,Tag_false)735 init( unsigned int art_size, InIt art_first, Tag_false)
736 {
737 // only Q is used to store A_B^-1 in phase I
738 for ( s = l, b = 0; b < art_size; ++s, ++b, ++art_first) {
739 ensure_physical_row(s);
740 M[ s][ b] = ( art_first->second ? -et1 : et1);
741 }
742
743 s = art_size;
744 }
745
746 // init (LP case)
747 template < class InIt > inline
748 void
init(unsigned int art_size,InIt art_first,Tag_true)749 init( unsigned int art_size, InIt art_first, Tag_true)
750 {
751 for ( s = 0; s < art_size; ++s, ++art_first) {
752 std::fill_n( M[ s].begin(), art_size, et0);
753 M[ s][ s] = ( art_first->second ? -et1 : et1);
754 }
755 b = art_size;
756 }
757
758 // append row in Q if no allocated row available
ensure_physical_row(unsigned int row)759 void ensure_physical_row (unsigned int row) {
760 unsigned int rows = static_cast<unsigned int>(M.size());
761 CGAL_qpe_assertion(rows >= row);
762 if (rows == row) {
763 M.push_back(Row(row+1, et0));
764
765 // do we have to grow x_x?
766 CGAL_qpe_assertion(x_x.size() >= row-l);
767 if (x_x.size() == row-l)
768 x_x.push_back(et0);
769
770 // do we have to grow tmp_x?
771 CGAL_qpe_assertion(tmp_x.size() >= row-l);
772 if (tmp_x.size() == row-l)
773 tmp_x.push_back(et0);
774
775 CGAL_qpe_assertion(M[row].size()==row+1);
776 CGAL_qpe_debug {
777 if ( vout.verbose()) {
778 vout << "physical row " << (row) << " appended in Q\n";
779 }
780 }
781 }
782 }
783
784 // matrix-vector multiplication (QP case)
785 template < class ForIt, class OutIt, class Use1stArg >
786 void
multiply(ForIt v_l_it,ForIt v_x_it,OutIt y_l_it,OutIt y_x_it,Tag_false,Use1stArg use_1st_arg)787 multiply( ForIt v_l_it, ForIt v_x_it,
788 OutIt y_l_it, OutIt y_x_it, Tag_false,
789 Use1stArg use_1st_arg) const
790 {
791 // use 'LP' functions in phase I
792 if ( is_phaseI) {
793 multiply__l( v_x_it, y_l_it);
794 multiply__x( v_l_it, y_x_it);
795 return;
796 }
797
798 // phase II
799 typename Matrix::const_iterator matrix_it;
800 typename Row ::const_iterator row_it; // left of diagonal
801 typename Matrix::const_iterator column_it; // right of diagonal
802 ForIt v_it;
803
804 unsigned int row, count, k = l+b;
805 ET sum;
806
807 // compute P v_l + Q^T v_x
808 for ( row = 0, matrix_it = M.begin();
809 row < s;
810 ++row, ++y_l_it) {
811 sum = et0;
812
813 // P v_l
814 if ( check_tag( use_1st_arg)) {
815
816 // P: left of diagonal (including)
817 for ( row_it = matrix_it->begin(), v_it = v_l_it;
818 row_it != matrix_it->end();
819 ++row_it, ++v_it) {
820 sum += *row_it * *v_it;
821 }
822
823 // P: right of diagonal (excluding)
824 for ( count = row+1, column_it = ++matrix_it;
825 count < s;
826 ++count, ++column_it, ++v_it) {
827 sum += (*column_it)[ row] * *v_it;
828 }
829 }
830
831 // Q^T:
832 for ( count = 0, column_it = M.begin()+l, v_it = v_x_it;
833 count < b;
834 ++count, ++column_it, ++v_it) {
835 sum += (*column_it)[ row] * *v_it;
836 }
837
838 // store result
839 *y_l_it = sum;
840 }
841
842 // compute Q v_l + R v_x
843 for ( row = l, matrix_it = M.begin()+l;
844 row < k;
845 ++row, ++y_x_it) {
846 sum = et0;
847
848 // Q v_l
849 if ( check_tag( use_1st_arg)) {
850
851 // Q:
852 for ( count = 0, row_it = matrix_it->begin(), v_it = v_l_it;
853 count < s;
854 ++count, ++row_it, ++v_it) {
855 sum += *row_it * *v_it;
856 }
857 }
858
859 // R: left of diagonal (including)
860 for ( row_it = matrix_it->begin()+l, v_it = v_x_it;
861 row_it != matrix_it->end();
862 ++row_it, ++v_it) {
863 sum += *row_it * *v_it;
864 }
865
866 // R: right of diagonal (excluding)
867 for ( count = row+1, column_it = ++matrix_it;
868 count < k;
869 ++count, ++column_it, ++v_it) {
870 sum += (*column_it)[ row] * *v_it;
871 }
872
873 // store result
874 *y_x_it = sum;
875 }
876 }
877
878 // matrix-vector multiplication (LP case)
879 template < class ForIt, class OutIt, class Dummy > inline
880 void
multiply(ForIt v_l_it,ForIt v_x_it,OutIt y_l_it,OutIt y_x_it,Tag_true,Dummy)881 multiply( ForIt v_l_it, ForIt v_x_it,
882 OutIt y_l_it, OutIt y_x_it, Tag_true, Dummy) const
883 {
884 multiply__l( v_x_it, y_l_it);
885 multiply__x( v_l_it, y_x_it);
886 }
887
888 // special matrix-vector multiplication functions for LPs
889 template < class ForIt, class OutIt > inline
890 void
multiply__l(ForIt v_x_it,OutIt y_l_it)891 multiply__l( ForIt v_x_it, OutIt y_l_it) const
892 {
893 typename Matrix::const_iterator matrix_it = M.begin();
894 typename Matrix::const_iterator column_it;
895 ForIt v_it;
896
897 unsigned int row, count;
898 ET sum;
899
900 // QP?
901 if ( is_QP) matrix_it += l;
902
903 // compute Q^T v_x
904 for ( row = 0; row < s; ++row, ++y_l_it) {
905 sum = et0;
906
907 for ( count = 0, column_it = matrix_it, v_it = v_x_it;
908 count < b;
909 ++count, ++column_it, ++v_it) {
910 sum += (*column_it)[ row] * *v_it;
911 }
912
913 *y_l_it = sum;
914 }
915 }
916
917 template < class ForIt, class OutIt > inline
918 void
multiply__x(ForIt v_l_it,OutIt y_x_it)919 multiply__x( ForIt v_l_it, OutIt y_x_it) const
920 {
921 typename Matrix::const_iterator matrix_it = M.begin();
922 unsigned int row;
923
924 // QP?
925 if ( is_QP) matrix_it += l;
926
927 // compute Q v_l
928 for ( row = 0;
929 row < b;
930 ++row, ++matrix_it, ++y_x_it) {
931
932 *y_x_it = inner_product( matrix_it->begin(), v_l_it, s);
933 }
934 }
935
936 // vector-vector multiplication
937 template < class InIt1, class InIt2 > inline
938 typename std::iterator_traits<InIt1>::value_type
inner_product(InIt1 u_it,InIt2 v_it,unsigned int n)939 inner_product( InIt1 u_it, InIt2 v_it, unsigned int n) const
940 {
941 typedef typename std::iterator_traits<InIt1>::value_type NT;
942
943 // compute u^T v
944 NT sum = NT( 0);
945 for ( unsigned int count = 0; count < n; ++count, ++u_it, ++v_it) {
946 sum += NT(*u_it) * NT(*v_it);
947 }
948
949 return sum;
950 }
951
952 // in-place update
953 template < class ForIt > // QP case
update_inplace_QP(ForIt y_l_it,ForIt y_x_it,const ET & d_new,const ET & d_old)954 void update_inplace_QP( ForIt y_l_it, ForIt y_x_it,
955 const ET& d_new, const ET& d_old)
956 {
957 typename Matrix:: iterator matrix_it;
958 typename Row :: iterator row_it;
959 typename Row ::const_iterator y_it1, y_it2;
960
961 unsigned int row, col, k = l+b;
962
963 // rows: 0..s-1 ( P )
964 for ( row = 0, y_it1 = y_l_it, matrix_it = M.begin();
965 row < s;
966 ++row, ++y_it1, ++matrix_it ) {
967
968 // columns: 0..row ( P )
969 for ( row_it = matrix_it->begin(), y_it2 = y_l_it;
970 row_it != matrix_it->end();
971 ++row_it, ++y_it2 ) {
972
973 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
974 }
975 }
976
977 // rows: l..k-1 ( Q R )
978 for ( row = l, y_it1 = y_x_it, matrix_it += l-s;
979 row < k;
980 ++row, ++y_it1, ++matrix_it ) {
981
982 // columns: 0..s-1 ( Q )
983 for ( col = 0, row_it = matrix_it->begin(), y_it2 = y_l_it;
984 col < s;
985 ++col, ++row_it, ++y_it2 ){
986
987 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
988 }
989
990 // columns: l..k-1 ( R )
991 for ( row_it += l-s, y_it2 = y_x_it;
992 row_it != matrix_it->end();
993 ++row_it, ++y_it2 ){
994
995 update_entry( *row_it, d_new, *y_it1 * *y_it2, d_old);
996 }
997 }
998 }
999
1000 template < class ForIt1, class ForIt2 > // LP case
update_inplace_LP(ForIt1 x_x_it,ForIt2 y_x_it,const ET & d_new,const ET & d_old)1001 void update_inplace_LP( ForIt1 x_x_it, ForIt2 y_x_it,
1002 const ET& d_new, const ET& d_old)
1003 {
1004 typename Matrix:: iterator matrix_it;
1005 typename Row :: iterator row_it;
1006 ForIt1 x_it;
1007
1008 unsigned int row, col;
1009 ET y;
1010
1011 // QP (in phase I)?
1012 matrix_it = M.begin();
1013 if ( is_QP) matrix_it += l;
1014
1015 // rows: 0..s-1 ( Q )
1016 for ( row = 0;
1017 row < s;
1018 ++row, ++y_x_it, ++matrix_it) {
1019
1020 // columns: 0..b-1 ( Q )
1021 y = *y_x_it;
1022 for ( col = 0, row_it = matrix_it->begin(), x_it = x_x_it;
1023 col < b;
1024 ++col, ++row_it, ++x_it ){
1025
1026 update_entry( *row_it, d_new, y * *x_it, d_old);
1027 }
1028 }
1029 }
1030
1031
1032 template < class RandomAccessIterator >
1033 typename std::iterator_traits<RandomAccessIterator>::value_type
inv_M_B_row_dot_col(int row,RandomAccessIterator u_l_it)1034 inv_M_B_row_dot_col( int row, RandomAccessIterator u_l_it) const
1035 {
1036 typename Row::const_iterator row_it;
1037 if ( is_QP) {
1038 row_it = M[l + row].begin();
1039 } else {
1040 row_it = M[row].begin();
1041 }
1042 return inner_product(row_it, u_l_it, b);
1043 }
1044
1045 };
1046
1047 // ----------------------------------------------------------------------------
1048
1049 // =============================
1050 // class implementation (inline)
1051 // =============================
1052
1053 // creation
1054 template < class ET_, class Is_LP_ > inline
1055 QP_basis_inverse<ET_,Is_LP_>::
QP_basis_inverse(CGAL::Verbose_ostream & verbose_ostream)1056 QP_basis_inverse( CGAL::Verbose_ostream& verbose_ostream)
1057 : et0( 0), et1( 1), et2( 2),
1058 is_LP( check_tag( Is_LP())), is_QP( ! is_LP),
1059 vout( verbose_ostream)
1060 { }
1061
1062 // transition (LP case)
1063 template < class ET_, class Is_LP_ > inline
1064 void QP_basis_inverse<ET_,Is_LP_>::
transition()1065 transition( )
1066 {
1067 is_phaseI = false;
1068 is_phaseII = true;
1069
1070 CGAL_qpe_debug {
1071 if ( vout.verbose()) print();
1072 }
1073 }
1074
1075 // set-up (QP case)
1076 template < class ET_, class Is_LP_ > inline
1077 void QP_basis_inverse<ET_,Is_LP_>::
set(Tag_false)1078 set( Tag_false)
1079 {
1080 M.reserve( l);
1081 // only allocate empty rows
1082 for ( unsigned int i = 0; i < l; ++i )
1083 M.push_back(Row(0, et0));
1084 }
1085
1086 // set-up (LP case)
1087 template < class ET_, class Is_LP_ > inline
1088 void QP_basis_inverse<ET_,Is_LP_>::
set(Tag_true)1089 set( Tag_true)
1090 {
1091 M.reserve( l);
1092 for ( unsigned int i = 0; i < l; ++i) M.push_back( Row( l, et0));
1093 }
1094
1095 // access (QP case)
1096 template < class ET_, class Is_LP_ > inline
1097 const ET_& QP_basis_inverse<ET_,Is_LP_>::
entry(unsigned int r,unsigned int c,Tag_false)1098 entry( unsigned int r, unsigned int c, Tag_false) const
1099 {
1100 CGAL_qpe_assertion( ( r < s) || ( ( r >= l) && ( r < l+b)));
1101 CGAL_qpe_assertion( ( c < s) || ( ( c >= l) && ( c < l+b)));
1102 return ( c < r ? M[ r][ c] : M[ c][ r]);
1103 }
1104
1105 // access (LP case)
1106 template < class ET_, class Is_LP_ > inline
1107 const ET_& QP_basis_inverse<ET_,Is_LP_>::
entry(unsigned int r,unsigned int c,Tag_true)1108 entry( unsigned int r, unsigned int c, Tag_true) const
1109 {
1110 CGAL_qpe_assertion( r < s);
1111 CGAL_qpe_assertion( c < b);
1112 return M[ r][ c];
1113 }
1114
1115 // in-place update
1116 template < class ET_, class Is_LP_ > inline
1117 void QP_basis_inverse<ET_,Is_LP_>::
update_entry(ET & entry,const ET & d_new,const ET & y,const ET & d_old)1118 update_entry( ET& entry, const ET& d_new, const ET& y, const ET& d_old) const
1119 {
1120 entry *= d_new;
1121 entry += y;
1122 entry = CGAL::integral_division(entry, d_old);
1123 }
1124
1125 } //namespace CGAL
1126
1127 #include <CGAL/QP_solver/QP_basis_inverse_impl.h>
1128
1129 #endif // CGAL_QP_SOLVER_QP_BASIS_INVERSE_H
1130
1131 // ===== EOF ==================================================================
1132