ConstrainedSolverT.cc 46.4 KB
Newer Older
Henrik Zimmer's avatar
Henrik Zimmer committed
1
2
/*===========================================================================*\
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
3
 *                               CoMISo                                      *
Henrik Zimmer's avatar
Henrik Zimmer committed
4
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
Henrik Zimmer's avatar
Henrik Zimmer committed
5
 *                           www.rwth-graphics.de                            *
Henrik Zimmer's avatar
Henrik Zimmer committed
6
 *                                                                           *
7
 *---------------------------------------------------------------------------*
Henrik Zimmer's avatar
Henrik Zimmer committed
8
 *  This file is part of CoMISo.                                             *
Henrik Zimmer's avatar
Henrik Zimmer committed
9
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
10
11
12
13
 *  CoMISo is free software: you can redistribute it and/or modify           *
 *  it under the terms of the GNU General Public License as published by     *
 *  the Free Software Foundation, either version 3 of the License, or        *
 *  (at your option) any later version.                                      *
Henrik Zimmer's avatar
Henrik Zimmer committed
14
15
16
17
 *                                                                           *
 *  CoMISo is distributed in the hope that it will be useful,                *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
Henrik Zimmer's avatar
Henrik Zimmer committed
18
 *  GNU General Public License for more details.                             *
Henrik Zimmer's avatar
Henrik Zimmer committed
19
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
20
21
 *  You should have received a copy of the GNU General Public License        *
 *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
Henrik Zimmer's avatar
Henrik Zimmer committed
22
 *                                                                           *
23
\*===========================================================================*/
Henrik Zimmer's avatar
Henrik Zimmer committed
24
25


26
27
28
29
30
31
//=============================================================================
//
//  CLASS MISolver - IMPLEMENTATION
//
//=============================================================================

32
#define COMISO_CONSTRAINEDSOLVER_C
33
34
35
//== INCLUDES =================================================================

#include "ConstrainedSolver.hh"
36
#include <CoMISo/Utils/gmm.h>
David Bommes's avatar
David Bommes committed
37
#include "GMM_Tools.hh"
38
#include <float.h>
David Bommes's avatar
David Bommes committed
39
#include <CoMISo/Utils/MutablePriorityQueueT.hh>
40

41
42
43
#include <Base/Utils/StopWatch.hh>
#include <Base/Debug/DebOut.hh>

44
#include <type_traits>
45

46
47
//== NAMESPACES ===============================================================

48
namespace COMISO {
49
50
51

//== IMPLEMENTATION ==========================================================

52
53
54
55
56
// cf. issue #3 - gmm5.2 compat
template <typename T>
using linalg_traits = typename gmm::linalg_traits<typename std::remove_const<typename std::remove_reference<T>::type>::type>;


David Bommes's avatar
David Bommes committed
57
58
template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT >
void 
59
60
61
62
63
64
65
66
ConstrainedSolver::solve_const(const RMatrixT& _constraints,
  const CMatrixT& _A,
  VectorT&  _x,
  const VectorT&  _rhs,
  const VectorIT& _idx_to_round,
  double    _reg_factor,
  bool      _show_miso_settings,
  bool      _show_timings)
David Bommes's avatar
David Bommes committed
67
68
{
  // copy matrices
69
  RMatrixT constraints(gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
David Bommes's avatar
David Bommes committed
70
71
  gmm::copy(_constraints, constraints);

72
  CMatrixT A(gmm::mat_nrows(_A), gmm::mat_ncols(_A));
David Bommes's avatar
David Bommes committed
73
74
75
76
77
78
79
80
  gmm::copy(_A, A);

  // ... and vectors
  VectorT  rhs(_rhs);
  VectorIT idx_to_round(_idx_to_round);

  // call non-const function
  solve(constraints,
81
82
83
84
85
86
87
    A,
    _x,
    rhs,
    idx_to_round,
    _reg_factor,
    _show_miso_settings,
    _show_timings);
David Bommes's avatar
David Bommes committed
88
89
90
}


91
92
93
//-----------------------------------------------------------------------------


David Bommes's avatar
David Bommes committed
94
95
template<class RMatrixT, class VectorT, class VectorIT >
void 
96
97
98
99
100
101
102
ConstrainedSolver::solve_const(const RMatrixT& _constraints,
  const RMatrixT& _B,
  VectorT&  _x,
  const VectorIT& _idx_to_round,
  double    _reg_factor,
  bool      _show_miso_settings,
  bool      _show_timings)
David Bommes's avatar
David Bommes committed
103
104
{
  // copy matrices
105
  RMatrixT constraints(gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
David Bommes's avatar
David Bommes committed
106
107
  gmm::copy(_constraints, constraints);

108
  RMatrixT B(gmm::mat_nrows(_B), gmm::mat_ncols(_B));
David Bommes's avatar
David Bommes committed
109
110
111
112
113
114
115
  gmm::copy(_B, B);

  // ... and vectors
  VectorIT idx_to_round(_idx_to_round);

  // call non-const function
  solve(constraints,
116
117
118
119
120
121
    B,
    _x,
    idx_to_round,
    _reg_factor,
    _show_miso_settings,
    _show_timings);
David Bommes's avatar
David Bommes committed
122
123
124
125
126
127
}


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


128
template<class RMatrixT, class VectorT, class VectorIT >
129
130
131
132
133
134
135
136
137
void
ConstrainedSolver::solve(
  RMatrixT& _constraints,
  RMatrixT& _B,
  VectorT&  _x,
  VectorIT& _idx_to_round,
  double    _reg_factor,
  bool      _show_miso_settings,
  bool      _show_timings)
138
{
David Bommes's avatar
David Bommes committed
139
140
141
  // convert into quadratic system
  VectorT rhs;
  gmm::col_matrix< gmm::rsvector< double > > A;
David Bommes's avatar
David Bommes committed
142
  COMISO_GMM::factored_to_quadratic(_B, A, rhs);
David Bommes's avatar
David Bommes committed
143
144

  // solve
145
146
147
148
  solve(_constraints, A, _x, rhs,
    _idx_to_round, _reg_factor,
    _show_miso_settings,
    _show_timings);
David Bommes's avatar
David Bommes committed
149

150
151
152
  //   int nrows = gmm::mat_nrows(_B);
  //   int ncols = gmm::mat_ncols(_B);
  //   int ncons = gmm::mat_nrows(_constraints);
David Bommes's avatar
David Bommes committed
153

154
  //   if( _show_timings) std::cerr << __FUNCTION__ << "\n Initial dimension: " << nrows << " x " << ncols << ", number of constraints: " << ncons << std::endl;
155

156
157
  //   // StopWatch for Timings
  //   Base::StopWatch sw, sw2; sw.start(); sw2.start();
158

159
160
161
  //   // c_elim[i] = index of variable which is eliminated in condition i
  //   // or -1 if condition is invalid
  //   std::vector<int> c_elim( ncons);
162

163
164
165
  //   // apply sparse gauss elimination to make subsequent _constraints independent
  //   make_constraints_independent( _constraints, _idx_to_round, c_elim);
  //   double time_gauss = sw.stop()/1000.0; sw.start();
166

167
168
  //   // eliminate conditions and return column matrix Bcol
  //   gmm::col_matrix< gmm::rsvector< double > > Bcol( nrows, ncols);
169

170
171
  //   // reindexing vector
  //   std::vector<int>                          new_idx;
172

173
174
  //   eliminate_constraints( _constraints, _B, _idx_to_round, c_elim, new_idx, Bcol);
  //   double time_eliminate = sw.stop()/1000.0; 
175

176
  //   if( _show_timings) std::cerr << "Eliminated dimension: " << gmm::mat_nrows(Bcol) << " x " << gmm::mat_ncols(Bcol) << std::endl;
177

178
179
180
  //   // setup and solve system
  //   double time_setup = setup_and_solve_system( Bcol, _x, _idx_to_round, _reg_factor, _show_miso_settings);
  //   sw.start();
181

182
  //   //  double time_setup_solve = sw.stop()/1000.0; sw.start();
183

184
185
  //   // restore eliminated vars to fulfill the given conditions
  //   restore_eliminated_vars( _constraints, _x, c_elim, new_idx);
186

187
188
189
190
191
192
193
194
195
196
197
  //   double time_resubstitute = sw.stop()/1000.0; sw.start();

  //   //  double time_total = sw2.stop()/1000.0;

  //   if( _show_timings) std::cerr << "Timings: \n\t" <<
  //     "Gauss Elimination  " << time_gauss          << " s\n\t" <<
  //     "System Elimination " << time_eliminate      << " s\n\t" <<
  //     "Setup              " << time_setup          << " s\n\t" <<
  //    // "Setup + Mi-Solver  " << time_setup_solve    << " s\n\t" <<
  //     "Resubstitution     " << time_resubstitute   << " s\n\t" << std::endl << std::endl;
  //     //"Total              " << time_total          << std::endl;
198
199
200
201
202
203
204
}


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


template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT>
205
206
207
208
209
210
211
212
213
214
void
ConstrainedSolver::solve(
  RMatrixT& _constraints,
  CMatrixT& _A,
  VectorT&  _x,
  VectorT&  _rhs,
  VectorIT& _idx_to_round,
  double    _reg_factor,
  bool      _show_miso_settings,
  bool      _show_timings)
215
{
216
  DEB_enter_func;
217
  // show options dialog
218
  if (_show_miso_settings)
219
220
    miso_.show_options_dialog();

221
222
223
  gmm::size_type nrows = gmm::mat_nrows(_A);
  gmm::size_type ncols = gmm::mat_ncols(_A);
  gmm::size_type ncons = gmm::mat_nrows(_constraints);
224

225
226
227
  DEB_out_if(_show_timings, 1, "Initital dimension: " << nrows << " x " << ncols
    << ", number of constraints: " << ncons
    << " use reordering: " << use_constraint_reordering() << "\n")
228

229
230
    // StopWatch for Timings
    Base::StopWatch sw, sw2; sw.start(); sw2.start();
231
232
233

  // c_elim[i] = index of variable which is eliminated in condition i
  // or -1 if condition is invalid
234
  std::vector<int> c_elim(ncons);
235
236

  // apply sparse gauss elimination to make subsequent _conditions independent
237
238
  if (use_constraint_reordering())
    make_constraints_independent_reordering(_constraints, _idx_to_round, c_elim);
239
  else
240
    make_constraints_independent(_constraints, _idx_to_round, c_elim);
241

242
  double time_gauss = sw.stop() / 1000.0; sw.start();
243
244
245
246
247

  // re-indexing vector
  std::vector<int>                          new_idx;

  gmm::csc_matrix< double > Acsc;
248
249
  eliminate_constraints(_constraints, _A, _x, _rhs, _idx_to_round, c_elim, new_idx, Acsc);
  double time_eliminate = sw.stop() / 1000.0;
250

251
252
253
254
  /// TODO: temporary disable this since it was causing performance issues
  //DEB_out_if( _show_timings, 2,
  //  "Eliminated dimension: " << Acsc.nr << " x " << Acsc.nc 
  //  << "\n#nonzeros: " << gmm::nnz(Acsc) << "\n");
255
256

  sw.start();
257
258
  miso_.solve(Acsc, _x, _rhs, _idx_to_round);
  double time_miso = sw.stop() / 1000.0; sw.start();
259

260
  rhs_update_table_.store(_constraints, c_elim, new_idx);
261
  // restore eliminated vars to fulfill the given conditions
262
  restore_eliminated_vars(_constraints, _x, c_elim, new_idx);
263

264
  double time_resubstitute = sw.stop() / 1000.0; sw.start();
265
  double time_total = time_gauss + time_eliminate + time_miso + time_resubstitute;
266
267
268
269
270
271
  DEB_out_if(_show_timings, 1, "Timings: \n\t" <<
    "\tGauss Elimination  " << time_gauss << " s\n\t" <<
    "\tSystem Elimination " << time_eliminate << " s\n\t" <<
    "\tMi-Solver          " << time_miso << " s\n\t" <<
    "\tResubstitution     " << time_resubstitute << " s\n\t" <<
    "\tTotal              " << time_total << "\n\n");
272
273
274
275
276
277
}


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


278
279
template<class RMatrixT, class VectorT >
void
280
281
282
283
284
ConstrainedSolver::resolve(
  const RMatrixT& _B,
  VectorT&  _x,
  VectorT*  _constraint_rhs,
  bool      _show_timings)
285
286
287
{
  // extract rhs from quadratic system
  VectorT rhs;
288
289
290
  // gmm::col_matrix< gmm::rsvector< double > > A;
  // COMISO_GMM::factored_to_quadratic(_B, A, rhs);
    //TODO only compute rhs, not complete A for efficiency
291

Andreas Fabri's avatar
Andreas Fabri committed
292
293
  gmm::size_type m = gmm::mat_nrows(_B);
  gmm::size_type n = gmm::mat_ncols(_B);
294

295
296
297
298
  typedef typename linalg_traits<RMatrixT>::const_sub_row_type CRowT;
  typedef typename linalg_traits<RMatrixT>::sub_row_type       RowT;
  typedef typename linalg_traits<CRowT>::const_iterator        RIter;
  typedef typename linalg_traits<CRowT>::value_type            VecValT;
299

300
  gmm::resize(rhs, n - 1);
301
  gmm::clear(rhs);
302
  for (unsigned int i = 0; i < m; ++i)
303
304
  {
    // get current condition row
305
306
307
    CRowT row = gmm::mat_const_row(_B, i);
    RIter row_it = gmm::vect_const_begin(row);
    RIter row_end = gmm::vect_const_end(row);
308

309
    if (row_end == row_it) continue;
310
    --row_end;
311
    if (row_end.index() != n - 1) continue;
312
    VecValT n_i = *row_end;
313
    while (row_end != row_it)
314
315
316
317
318
319
320
    {
      --row_end;
      rhs[row_end.index()] -= (*row_end) * n_i;
    }
  }

  // solve
321
  resolve(_x, _constraint_rhs, &rhs,
322
    _show_timings);
323
324
325
326
327
328
}


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


David Bommes's avatar
David Bommes committed
329
template<class VectorT >
330
void
331
332
333
334
335
ConstrainedSolver::resolve(
  VectorT&  _x,
  VectorT*  _constraint_rhs,
  VectorT*  _rhs,
  bool      _show_timings)
336
{
337
  DEB_enter_func;
338
  // StopWatch for Timings
339
  Base::StopWatch sw;
340
341
342

  sw.start();
  // apply stored updates and eliminations to exchanged rhs
343
  if (_constraint_rhs)
344
345
346
347
348
349
  {
    // apply linear transformation of Gaussian elimination
    rhs_update_table_.cur_constraint_rhs_.resize(gmm::mat_nrows(rhs_update_table_.D_));
    gmm::mult(rhs_update_table_.D_, *_constraint_rhs, rhs_update_table_.cur_constraint_rhs_);

    // update rhs of stored constraints
Andreas Fabri's avatar
Andreas Fabri committed
350
    gmm::size_type nc = gmm::mat_ncols(rhs_update_table_.constraints_p_);
351
352
    for (gmm::size_type i = 0; i < rhs_update_table_.cur_constraint_rhs_.size(); ++i)
      rhs_update_table_.constraints_p_(i, nc - 1) = -rhs_update_table_.cur_constraint_rhs_[i];
353
  }
354
  if (_rhs)
355
356
357
358
359
360
    rhs_update_table_.cur_rhs_ = *_rhs;

  std::vector<double> rhs_red = rhs_update_table_.cur_rhs_;

  rhs_update_table_.apply(rhs_update_table_.cur_constraint_rhs_, rhs_red);
  rhs_update_table_.eliminate(rhs_red);
361

362
363
364
365
366
367
368
369
  //  std::cerr << "############### Resolve info ##############" << std::endl;
  //  std::cerr << rhs_update_table_.D_ << std::endl;
  //  std::cerr << rhs_update_table_.cur_rhs_ << std::endl;
  //  std::cerr << rhs_update_table_.cur_constraint_rhs_ << std::endl;
  //  std::cerr << rhs_update_table_.table_.size() << std::endl;
  //  std::cerr << "rhs_red: " << rhs_red << std::endl;

  miso_.resolve(_x, rhs_red);
370

371
  double time_miso = sw.stop() / 1000.0; sw.start();
372
373

  // restore eliminated vars to fulfill the given conditions
374
  restore_eliminated_vars(rhs_update_table_.constraints_p_, _x, rhs_update_table_.c_elim_, rhs_update_table_.new_idx_);
375

376
  double time_resubstitute = sw.stop() / 1000.0; sw.start();
377
  double time_total = time_miso + time_resubstitute;
378
379
380
381
  DEB_out_if(_show_timings, 1, "Timings: \n\t" <<
    "\tMi-Solver          " << time_miso << " s\n\t" <<
    "\tResubstitution     " << time_resubstitute << " s\n\t" <<
    "\tTotal              " << time_total << "\n\n");
382
383
384
385
386
387
}


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


388
template<class RMatrixT, class VectorIT >
389
390
391
392
393
void
ConstrainedSolver::make_constraints_independent(
  RMatrixT&         _constraints,
  VectorIT&         _idx_to_round,
  std::vector<int>& _c_elim)
394
{
395
  DEB_enter_func;
396
  // setup linear transformation for rhs, start with identity
Andreas Fabri's avatar
Andreas Fabri committed
397
  gmm::size_type nr = gmm::mat_nrows(_constraints);
398
399
  gmm::resize(rhs_update_table_.D_, nr, nr);
  gmm::clear(rhs_update_table_.D_);
400
  for (gmm::size_type i = 0; i < nr; ++i) rhs_update_table_.D_(i, i) = 1.0;
401

402
  //  Base::StopWatch sw;
403
  // number of variables
Andreas Fabri's avatar
Andreas Fabri committed
404
  const gmm::size_type n_vars = gmm::mat_ncols(_constraints);
405
406
407

  // TODO Check: HZ added 14.08.09 
  _c_elim.clear();
408
  _c_elim.resize(gmm::mat_nrows(_constraints), -1);
409
410

  // build round map
411
412
  std::vector<bool> roundmap(n_vars, false);
  for (unsigned int i = 0; i < _idx_to_round.size(); ++i)
413
414
415
416
417
418
419
420
421
422
    roundmap[_idx_to_round[i]] = true;

  // copy constraints into column matrix (for faster update via iterators)
  typedef gmm::wsvector<double>      CVector;
  typedef gmm::col_matrix< CVector > CMatrix;
  CMatrix constraints_c;
  gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints_c);

  // for all conditions
423
  for (unsigned int i = 0; i < gmm::mat_nrows(_constraints); ++i)
424
425
426
  {
    // get elimination variable
    int elim_j = -1;
427
    int elim_int_j = -1;
428
429
430
431
432
433

    // iterate over current row, until variable found
    // first search for real valued variable
    // if not found for integers with value +-1
    // and finally take the smallest integer variable

434
435
436
    typedef typename linalg_traits<RMatrixT>::const_sub_row_type CRowT;
    typedef typename linalg_traits<RMatrixT>::sub_row_type       RowT;
    typedef typename linalg_traits<CRowT>::const_iterator        RIter;
437
438

    // get current condition row
439
440
441
    CRowT row = gmm::mat_const_row(_constraints, i);
    RIter row_it = gmm::vect_const_begin(row);
    RIter row_end = gmm::vect_const_end(row);
442
    double elim_val = FLT_MAX;
443
    double max_elim_val = -FLT_MAX;
444

Henrik Zimmer's avatar
   
Henrik Zimmer committed
445
446
    // new: gcd
    std::vector<int> v_gcd;
447
    v_gcd.resize(gmm::nnz(row), -1);
Henrik Zimmer's avatar
   
Henrik Zimmer committed
448
    int n_ints(0);
David Bommes's avatar
   
David Bommes committed
449
    bool gcd_update_valid(true);
Henrik Zimmer's avatar
   
Henrik Zimmer committed
450

451
    for (; row_it != row_end; ++row_it)
452
    {
Andreas Fabri's avatar
Andreas Fabri committed
453
      int cur_j = static_cast<int>(row_it.index());
454
      // do not use the constant part
455
      if (cur_j != (int)n_vars - 1)
456
      {
457
        // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
458
        if (!roundmap[cur_j])
459
        {
460
          if (fabs(*row_it) > max_elim_val)
461
          {
Max Lyon's avatar
Max Lyon committed
462
            elim_j = (int)cur_j;
463
464
465
            max_elim_val = fabs(*row_it);
          }
          //break;
466
467
        }
        else
468
        {
469
          double cur_row_val(fabs(*row_it));
Henrik Zimmer's avatar
   
Henrik Zimmer committed
470
          // gcd
471
472
473
474
475
476
          // If the coefficient of an integer variable is not an integer, then
          // the variable most probably will not be. This is expected if all 
          // coeffs are the same, e.g. 0.5). 
          // This happens quite often in some ReForm test cases, so downgrading
          // the warning below to DEB_line at high verbosity.
          if ((double(int(cur_row_val)) - cur_row_val) != 0.0)
Max Lyon's avatar
Max Lyon committed
477
          {
478
            DEB_line(11, "coefficient of integer variable is NOT integer : " <<
479
              cur_row_val);
Max Lyon's avatar
Max Lyon committed
480
481
            gcd_update_valid = false;
          }
Henrik Zimmer's avatar
   
Henrik Zimmer committed
482

Andreas Fabri's avatar
Andreas Fabri committed
483
          v_gcd[n_ints] = static_cast<int>(cur_row_val);
Henrik Zimmer's avatar
   
Henrik Zimmer committed
484
485
          ++n_ints;

486
          // store integer closest to 1, must be greater than epsilon_
487
488
489
490
          if (fabs(cur_row_val - 1.0) < elim_val && cur_row_val > epsilon_)
          {
            elim_int_j = cur_j;
            elim_val = fabs(cur_row_val - 1.0);
491
          }
492
        }
493
494
495
      }
    }

496
    // first try to eliminate a valid (>epsilon_) real valued variable (safer)
497
498
    if (max_elim_val <= epsilon_)
      elim_j = elim_int_j; // use the best found integer
499

500
501
502
    // if no integer or real valued variable greater than epsilon_ existed, then
    // elim_j is now -1 and this row is not considered as a valid constraint

503
504
505
    // store result
    _c_elim[i] = elim_j;
    // error check result
506
507
508
509
510
511
    if (elim_j == -1)
    {// redundant or incompatible?
      DEB_warning_if((noisy_ > 0) &&
        (fabs(gmm::mat_const_row(_constraints, i)[n_vars - 1]) > epsilon_), 1,
        "incompatible condition: " <<
        fabs(gmm::mat_const_row(_constraints, i)[n_vars - 1]));
512
    }
513
514
515
    else if (roundmap[elim_j] && elim_val > 1e-6)
    {
      if (do_gcd_ && gcd_update_valid)
Henrik Zimmer's avatar
   
Henrik Zimmer committed
516
      {
517
518
519
520
        // perform gcd update
        bool gcd_ok = update_constraint_gcd(_constraints, i, elim_j, v_gcd, n_ints);
        DEB_warning_if((noisy_ > 0) && !gcd_ok, 1, " GCD update failed! "
          << DEB_os_str(gmm::mat_const_row(_constraints, i)));
Henrik Zimmer's avatar
   
Henrik Zimmer committed
521
      }
522
523
524
525
526
527
528
529
530
531
532
      else
      {
        DEB_warning_if((noisy_ > 0) && !do_gcd_, 1,
          "NO +-1 coefficient found, integer rounding cannot be guaranteed. "
          "Try using the GCD option! "
          << DEB_os_str(gmm::mat_const_row(_constraints, i)));
        DEB_warning_if((noisy_ > 0) && do_gcd_, 1,
          "GCD of non-integer cannot be computed! "
          << DEB_os_str(gmm::mat_const_row(_constraints, i)));
      }
    }
533
534

    // is this condition dependent?
535
    if (elim_j != -1)
536
537
    {
      // get elim variable value
538
      double elim_val_cur = _constraints(i, elim_j);
539
540
541
542
543

      // copy col
      CVector col = constraints_c.col(elim_j);

      // iterate over column
544
545
      typename linalg_traits<CVector>::const_iterator c_it = gmm::vect_const_begin(col);
      typename linalg_traits<CVector>::const_iterator c_end = gmm::vect_const_end(col);
546

547
548
549
      for (; c_it != c_end; ++c_it)
      {
        if (c_it.index() > i)
550
        {
551
552
          //          sw.start();
          double val = -(*c_it) / elim_val_cur;
553

Max Lyon's avatar
Max Lyon committed
554
          add_row_simultaneously((int)c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
David Bommes's avatar
David Bommes committed
555
          // make sure the eliminated entry is 0 on all other rows and not 1e-17
556
          _constraints(c_it.index(), elim_j) = 0;
David Bommes's avatar
David Bommes committed
557
          constraints_c(c_it.index(), elim_j) = 0;
558
559
560

          // update linear transition of rhs
          gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
561
            gmm::mat_row(rhs_update_table_.D_, c_it.index()));
David Bommes's avatar
David Bommes committed
562
        }
563
      }
David Bommes's avatar
David Bommes committed
564
565
566
567
568
569
570
571
572
    }
  }
}


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


template<class RMatrixT, class VectorIT >
573
574
575
576
577
void
ConstrainedSolver::make_constraints_independent_reordering(
  RMatrixT&         _constraints,
  VectorIT&         _idx_to_round,
  std::vector<int>& _c_elim)
David Bommes's avatar
David Bommes committed
578
{
579
  DEB_enter_func;
580
  // setup linear transformation for rhs, start with identity
Andreas Fabri's avatar
Andreas Fabri committed
581
  gmm::size_type nr = gmm::mat_nrows(_constraints);
582
583
  gmm::resize(rhs_update_table_.D_, nr, nr);
  gmm::clear(rhs_update_table_.D_);
Max Lyon's avatar
Max Lyon committed
584

585
586
  for (gmm::size_type i = 0; i < nr; ++i)
    rhs_update_table_.D_(i, i) = 1.0;
587

588
  //  Base::StopWatch sw;
David Bommes's avatar
David Bommes committed
589
  // number of variables
Andreas Fabri's avatar
Andreas Fabri committed
590
591
  // AF: Why was n_vars signed? Can it be zero? Later we subtract 1
  const gmm::size_type n_vars = gmm::mat_ncols(_constraints);
David Bommes's avatar
David Bommes committed
592
593
594

  // TODO Check: HZ added 14.08.09 
  _c_elim.clear();
595
  _c_elim.resize(gmm::mat_nrows(_constraints), -1);
David Bommes's avatar
David Bommes committed
596
597

  // build round map
598
599
  std::vector<bool> roundmap(n_vars, false);
  for (size_t i = 0; i < _idx_to_round.size(); ++i)
David Bommes's avatar
David Bommes committed
600
601
602
603
604
605
606
607
608
609
610
    roundmap[_idx_to_round[i]] = true;

  // copy constraints into column matrix (for faster update via iterators)
  typedef gmm::wsvector<double>      CVector;
  typedef gmm::col_matrix< CVector > CMatrix;
  CMatrix constraints_c;
  gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints_c);

  // init priority queue
  MutablePriorityQueueT<unsigned int, unsigned int> queue;
611
612
  queue.clear(static_cast<int>(nr));
  for (unsigned int i = 0; i < nr; ++i)
David Bommes's avatar
David Bommes committed
613
  {
614
615
    gmm::size_type cur_nnz = gmm::nnz(gmm::mat_row(_constraints, i));
    if (_constraints(i, n_vars - 1) != 0.0)
Max Lyon's avatar
Max Lyon committed
616
      --cur_nnz;
David Bommes's avatar
David Bommes committed
617

Andreas Fabri's avatar
Andreas Fabri committed
618
    queue.update(i, static_cast<int>(cur_nnz));
David Bommes's avatar
David Bommes committed
619
  }
620

David Bommes's avatar
David Bommes committed
621
  std::vector<bool> row_visited(nr, false);
Max Lyon's avatar
Max Lyon committed
622
623
  std::vector<gmm::size_type> row_ordering;
  row_ordering.reserve(nr);
David Bommes's avatar
David Bommes committed
624
625
626
627


  // for all conditions
  //  for(unsigned int i=0; i<gmm::mat_nrows(_constraints); ++i)
628
  while (!queue.empty())
David Bommes's avatar
David Bommes committed
629
630
  {
    // get next row
Max Lyon's avatar
Max Lyon committed
631
    unsigned int i = queue.get_next();
David Bommes's avatar
David Bommes committed
632
633
634
635
636
637
638
639
640
641
642
643
    row_ordering.push_back(i);
    row_visited[i] = true;

    // get elimination variable
    int elim_j = -1;
    int elim_int_j = -1;

    // iterate over current row, until variable found
    // first search for real valued variable
    // if not found for integers with value +-1
    // and finally take the smallest integer variable

644
645
646
    typedef typename linalg_traits<RMatrixT>::const_sub_row_type CRowT;
    typedef typename linalg_traits<RMatrixT>::sub_row_type       RowT;
    typedef typename linalg_traits<CRowT>::const_iterator        RIter;
David Bommes's avatar
David Bommes committed
647
648

    // get current condition row
649
650
651
    CRowT row = gmm::mat_const_row(_constraints, i);
    RIter row_it = gmm::vect_const_begin(row);
    RIter row_end = gmm::vect_const_end(row);
David Bommes's avatar
David Bommes committed
652
653
654
655
656
    double elim_val = FLT_MAX;
    double max_elim_val = -FLT_MAX;

    // new: gcd
    std::vector<int> v_gcd;
657
    v_gcd.resize(gmm::nnz(row), -1);
David Bommes's avatar
David Bommes committed
658
659
660
    int n_ints(0);
    bool gcd_update_valid(true);

661
    for (; row_it != row_end; ++row_it)
David Bommes's avatar
David Bommes committed
662
    {
Andreas Fabri's avatar
Andreas Fabri committed
663
      int cur_j = static_cast<int>(row_it.index());
David Bommes's avatar
David Bommes committed
664
      // do not use the constant part
Max Lyon's avatar
Max Lyon committed
665
      if (cur_j != (int)n_vars - 1)
David Bommes's avatar
David Bommes committed
666
667
      {
        // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
668
        if (!roundmap[cur_j])
David Bommes's avatar
David Bommes committed
669
        {
Max Lyon's avatar
Max Lyon committed
670
          if (fabs(*row_it) > max_elim_val)
David Bommes's avatar
David Bommes committed
671
          {
Max Lyon's avatar
Max Lyon committed
672
            elim_j = (int)cur_j;
David Bommes's avatar
David Bommes committed
673
674
675
676
677
678
679
680
            max_elim_val = fabs(*row_it);
          }
          //break;
        }
        else
        {
          double cur_row_val(fabs(*row_it));
          // gcd
681
682
683
684
685
686
          // If the coefficient of an integer variable is not an integer, then
          // the variable most probably will not be. This is expected if all 
          // coeffs are the same, e.g. 0.5). 
          // This happens quite often in some ReForm test cases, so downgrading
          // the warning below to DEB_line at high verbosity.
          if ((double(int(cur_row_val)) - cur_row_val) != 0.0)
Max Lyon's avatar
Max Lyon committed
687
          {
688
            DEB_line(11, "coefficient of integer variable is NOT integer : " <<
689
              cur_row_val);
Max Lyon's avatar
Max Lyon committed
690
691
            gcd_update_valid = false;
          }
David Bommes's avatar
David Bommes committed
692

Andreas Fabri's avatar
Andreas Fabri committed
693
          v_gcd[n_ints] = static_cast<int>(cur_row_val);
David Bommes's avatar
David Bommes committed
694
695
696
          ++n_ints;

          // store integer closest to 1, must be greater than epsilon_
697
          if (fabs(cur_row_val - 1.0) < elim_val && cur_row_val > epsilon_)
David Bommes's avatar
David Bommes committed
698
          {
699
700
            elim_int_j = (int)cur_j;
            elim_val = fabs(cur_row_val - 1.0);
David Bommes's avatar
David Bommes committed
701
702
703
704
705
706
          }
        }
      }
    }

    // first try to eliminate a valid (>epsilon_) real valued variable (safer)
707
708
    if (max_elim_val <= epsilon_)
      elim_j = elim_int_j; // use the best found integer
David Bommes's avatar
David Bommes committed
709
710
711
712
713
714
715

    // if no integer or real valued variable greater than epsilon_ existed, then
    // elim_j is now -1 and this row is not considered as a valid constraint

    // store result
    _c_elim[i] = elim_j;
    // error check result
716
    if (elim_j == -1)
David Bommes's avatar
David Bommes committed
717
    {
718
719
720
721
      DEB_warning_if(noisy_ > 0 && // redundant or incompatible?
        (fabs(gmm::mat_const_row(_constraints, i)[n_vars - 1]) > epsilon_), 1,
        "incompatible condition: " <<
        fabs(gmm::mat_const_row(_constraints, i)[n_vars - 1]))
David Bommes's avatar
David Bommes committed
722
    }
723
724
725
    else if (roundmap[elim_j] && elim_val > 1e-6)
    {
      if (do_gcd_ && gcd_update_valid)
David Bommes's avatar
David Bommes committed
726
      {
727
728
729
730
731
732
733
734
735
736
        // perform gcd update
        bool gcd_ok = update_constraint_gcd(_constraints, (int)i, elim_j, v_gcd, n_ints);
        DEB_warning_if(!gcd_ok && (noisy_ > 0), 1, " GCD update failed! "
          << DEB_os_str(gmm::mat_const_row(_constraints, i)));
      }
      else if (noisy_ > 0)
      {
        if (!do_gcd_)
          DEB_warning(1, "NO +-1 coefficient found, integer rounding cannot be guaranteed. Try using the GCD option! "
            << DEB_os_str(gmm::mat_const_row(_constraints, i)))
David Bommes's avatar
David Bommes committed
737
        else
738
739
          DEB_warning(1, "GCD of non-integer cannot be computed! "
            << DEB_os_str(gmm::mat_const_row(_constraints, i)))
David Bommes's avatar
David Bommes committed
740
      }
741
    }
David Bommes's avatar
David Bommes committed
742
743

    // is this condition dependent?
Max Lyon's avatar
Max Lyon committed
744
    if (elim_j != -1)
David Bommes's avatar
David Bommes committed
745
746
747
748
749
750
751
752
    {
      // get elim variable value
      double elim_val_cur = _constraints(i, elim_j);

      // copy col
      CVector col = constraints_c.col(elim_j);

      // iterate over column
753
754
      typename linalg_traits<CVector>::const_iterator c_it = gmm::vect_const_begin(col);
      typename linalg_traits<CVector>::const_iterator c_end = gmm::vect_const_end(col);
David Bommes's avatar
David Bommes committed
755

Max Lyon's avatar
Max Lyon committed
756
757
      for (; c_it != c_end; ++c_it)
      {
758
        //        if( c_it.index() > i)
Max Lyon's avatar
Max Lyon committed
759
        if (!row_visited[c_it.index()])
David Bommes's avatar
David Bommes committed
760
        {
761
          //          sw.start();
Max Lyon's avatar
Max Lyon committed
762
763
          double val = -(*c_it) / elim_val_cur;
          add_row_simultaneously((int)c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
764
          // make sure the eliminated entry is 0 on all other rows and not 1e-17
Max Lyon's avatar
Max Lyon committed
765
          _constraints(c_it.index(), elim_j) = 0;
766
          constraints_c(c_it.index(), elim_j) = 0;
David Bommes's avatar
David Bommes committed
767

Andreas Fabri's avatar
Andreas Fabri committed
768
          gmm::size_type cur_idx = c_it.index();
769
770
          gmm::size_type cur_nnz = gmm::nnz(gmm::mat_row(_constraints, cur_idx));
          if (_constraints(cur_idx, n_vars - 1) != 0.0)
Max Lyon's avatar
Max Lyon committed
771
            --cur_nnz;
David Bommes's avatar
David Bommes committed
772

Max Lyon's avatar
Max Lyon committed
773
          queue.update(static_cast<int>(cur_idx),
774
            static_cast<int>(cur_nnz));
775
776
777

          // update linear transition of rhs
          gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
Max Lyon's avatar
Max Lyon committed
778
            gmm::mat_row(rhs_update_table_.D_, c_it.index()));
779
        }
Max Lyon's avatar
Max Lyon committed
780
      }
781
782
    }
  }
David Bommes's avatar
David Bommes committed
783
784
785
786
787
788
789
  // // check result
  // for(unsigned int i=0; i<row_visited.size(); ++i)
  //   if( !row_visited[i])
  //     std::cerr <<"FAT ERROR: row " << i << " not visited...\n";

  // correct ordering
  RMatrixT c_tmp(gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
790
791
792
  gmm::copy(_constraints, c_tmp);
  RowMatrix d_tmp(gmm::mat_nrows(rhs_update_table_.D_), gmm::mat_ncols(rhs_update_table_.D_));
  gmm::copy(rhs_update_table_.D_, d_tmp);
David Bommes's avatar
David Bommes committed
793
794
795
796
797
798

  // std::vector<int> elim_temp2(_c_elim);
  // std::sort(elim_temp2.begin(), elim_temp2.end());
  // std::cerr << elim_temp2 << std::endl;

  std::vector<int> elim_temp(_c_elim);
799
  _c_elim.resize(0); _c_elim.resize(elim_temp.size(), -1);
David Bommes's avatar
David Bommes committed
800

801
  for (unsigned int i = 0; i < nr; ++i)
David Bommes's avatar
David Bommes committed
802
  {
803
804
    gmm::copy(gmm::mat_row(c_tmp, row_ordering[i]), gmm::mat_row(_constraints, i));
    gmm::copy(gmm::mat_row(d_tmp, row_ordering[i]), gmm::mat_row(rhs_update_table_.D_, i));
805

David Bommes's avatar
David Bommes committed
806
807
808
809
810
811
812
813
814
815
    _c_elim[i] = elim_temp[row_ordering[i]];
  }

  // // hack
  // elim_temp = _c_elim;
  // std::sort(elim_temp.begin(), elim_temp.end());
  // std::cerr << elim_temp << std::endl;

  // std::sort(row_ordering.begin(), row_ordering.end());
  // std::cerr << "row ordering: " << row_ordering << std::endl;
816
817
}

David Bommes's avatar
David Bommes committed
818

819
820
//-----------------------------------------------------------------------------

Henrik Zimmer's avatar
   
Henrik Zimmer committed
821
822
template<class RMatrixT>
bool
823
824
825
826
827
ConstrainedSolver::update_constraint_gcd(RMatrixT& _constraints,
  int _row_i,
  int& _elim_j,
  std::vector<int>& _v_gcd,
  int& _n_ints)
Henrik Zimmer's avatar
   
Henrik Zimmer committed
828
{
829
  DEB_enter_func;
Henrik Zimmer's avatar
   
Henrik Zimmer committed
830
831
832
  // find gcd
  double i_gcd = find_gcd(_v_gcd, _n_ints);

833
  if (fabs(i_gcd) == 1.0)
Henrik Zimmer's avatar
   
Henrik Zimmer committed
834
    return false;
835

Henrik Zimmer's avatar
   
Henrik Zimmer committed
836
  // divide by gcd
837
838
  typedef typename linalg_traits<RMatrixT>::const_sub_row_type CRowT;
  typedef typename linalg_traits<CRowT>::const_iterator        RIter;
Henrik Zimmer's avatar
   
Henrik Zimmer committed
839
840

  // get current constraint row
841
842
  RIter row_it = gmm::vect_const_begin(gmm::mat_const_row(_constraints, _row_i));
  RIter row_end = gmm::vect_const_end(gmm::mat_const_row(_constraints, _row_i));
Henrik Zimmer's avatar
   
Henrik Zimmer committed
843

844
  for (; row_it != row_end; ++row_it)
Henrik Zimmer's avatar
   
Henrik Zimmer committed
845
  {
Andreas Fabri's avatar
Andreas Fabri committed
846
    gmm::size_type cur_j = row_it.index();
847
    _constraints(_row_i, cur_j) = (*row_it) / i_gcd;
Henrik Zimmer's avatar
   
Henrik Zimmer committed
848
  }
Max Lyon's avatar
Max Lyon committed
849
  gmm::size_type elim_coeff = static_cast<gmm::size_type>(std::abs(_constraints(_row_i, _elim_j)));
850
851
  DEB_error_if(elim_coeff != 1, "elimination coefficient " << elim_coeff
    << " will (most probably) NOT lead to an integer solution!");
Henrik Zimmer's avatar
   
Henrik Zimmer committed
852
853
854
855
  return true;
}

//-----------------------------------------------------------------------------
856
857

template<class SVector1T, class SVector2T, class VectorIT, class SVector3T>
858
859
860
861
862
863
864
865
void
ConstrainedSolver::eliminate_constraints(
  gmm::row_matrix<SVector1T>& _constraints,
  gmm::row_matrix<SVector2T>& _B,
  VectorIT&                   _idx_to_round,
  std::vector<int>&           _c_elim,
  std::vector<int>&           _new_idx,
  gmm::col_matrix<SVector3T>& _Bcol)
866
{
867
  DEB_enter_func;
868
869
  // copy into column matrix
  gmm::resize(_Bcol, gmm::mat_nrows(_B), gmm::mat_ncols(_B));
870
  gmm::copy(_B, _Bcol);
871
872
873

  // store columns which should be eliminated
  std::vector<int> elim_cols;
874
  elim_cols.reserve(_c_elim.size());
875

876
  for (unsigned int i = 0; i < _c_elim.size(); ++i)
877
878
879
  {
    int cur_j = _c_elim[i];

880
    if (cur_j != -1)
881
    {
882
      double cur_val = _constraints(i, cur_j);
883
884
885
886
887
888
889
890

      // store index
      elim_cols.push_back(_c_elim[i]);

      // copy col
      SVector3T col = _Bcol.col(cur_j);

      // iterate over column
891
892
      typename linalg_traits<SVector3T>::const_iterator c_it = gmm::vect_const_begin(col);
      typename linalg_traits<SVector3T>::const_iterator c_end = gmm::vect_const_end(col);
893

894
895
      for (; c_it != c_end; ++c_it)
        add_row(c_it.index(), -(*c_it) / cur_val, gmm::mat_row(_constraints, i), _Bcol);
896
897
898
899
    }
  }

  // eliminate columns
900
  eliminate_columns(_Bcol, elim_cols);
901
902
903
904

  // TODO FIXME Size -1 ?!?!
  // init _new_idx vector
  _new_idx.resize(gmm::mat_ncols(_constraints));
905
  for (unsigned int i = 0; i < _new_idx.size(); ++i)
906
907
908
    _new_idx[i] = i;

  // update _new_idx w.r.t. eliminated cols
909
  COMISO_GMM::eliminate_vars_idx(elim_cols, _new_idx, -1);
910
911
912
913

  // update _idx_to_round (in place)
  std::vector<int> round_old(_idx_to_round);
  unsigned int wi = 0;
914
  for (unsigned int i = 0; i < _idx_to_round.size(); ++i)
915
  {
916
    if (_new_idx[_idx_to_round[i]] != -1)
917
918
919
920
921
    {
      _idx_to_round[wi] = _new_idx[_idx_to_round[i]];
      ++wi;
    }
  }
922

923
924
925
926
  // resize, sort and make unique
  _idx_to_round.resize(wi);

  std::sort(_idx_to_round.begin(), _idx_to_round.end());
927
  _idx_to_round.resize(std::unique(_idx_to_round.begin(), _idx_to_round.end()) - _idx_to_round.begin());
928

929
930
931
932
  DEB_line_if((noisy_ > 2), 2, "remaining         variables: " << 
    gmm::mat_ncols(_Bcol));
  DEB_line_if((noisy_ > 2), 2, "remaining integer variables: " << 
    _idx_to_round.size());
933
934
935
936
937
938
939
}


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


template<class SVector1T, class SVector2T, class VectorIT, class CSCMatrixT>
940
941
942
943
944
945
946
947
948
949
void
ConstrainedSolver::eliminate_constraints(
  gmm::row_matrix<SVector1T>& _constraints,
  gmm::col_matrix<SVector2T>& _A,
  std::vector<double>&        _x,
  std::vector<double>&        _rhs,
  VectorIT&                   _idx_to_round,
  std::vector<int>&           _v_elim,
  std::vector<int>&           _new_idx,
  CSCMatrixT&                 _Acsc)
950
{
951
952
  DEB_enter_func;
  Base::StopWatch sw;
953
954
  sw.start();
  // define iterator on matrix A and on constraints C
955
956
  typedef typename linalg_traits<SVector2T>::const_iterator  AIter;
  typedef typename linalg_traits<SVector1T>::const_iterator  CIter;
957
958
959

  // store variable indices to be eliminated
  std::vector<int> elim_varids;
960
  elim_varids.reserve(_v_elim.size());
961

962
  rhs_update_table_.clear();
963
  std::vector<double> constraint_rhs_vec(_constraints.nrows());
964

965
  for (unsigned int i = 0; i < _v_elim.size(); ++i)
966
967
968
  {
    int cur_j = _v_elim[i];

969
    if (cur_j != -1)
970
971
972
973