ExactConstraintSatisfaction.cc 15.1 KB
Newer Older
1
2
3
4
5
#include "ExactConstraintSatisfaction.hh"

#include <CoMISo/Config/config.hh>

#include <CoMISo/NSolver/NProblemInterface.hh>
6
#include <CoMISo/Utils/CoMISoError.hh>
7
8
#include <vector>

9
10
namespace COMISO {

11
12
13
14
15
16
17
ExactConstraintSatisfaction::ExactConstraintSatisfaction()
{

}


// ------------------------Helpfull Methods----------------------------------------
18
19

//row1 belongs to vector b, row2 to a row in the matrix
20
void ExactConstraintSatisfaction::swap_rows(SP_Matrix_R& mat, int row1, int row2){
21
22
23
24
25
26

  Eigen::SparseVector<int> row_2 = mat.row(row2);
  Eigen::SparseVector<int> row_1 = mat.row(row1);
  mat.row(row2) = row_1;
  mat.row(row1) = row_2;

27
//  mat.prune(0.0, 0);
28
29
//  mat.makeCompressed();
//  mat.finalize();
30
31
32
33
34
35

}


//We want to eliminate row1 in mat with the row corresponding to row2 in mat
//the row_2 has a pivot in (row2, col_p)
36
void ExactConstraintSatisfaction::eliminate_row(SP_Matrix_R& mat, Eigen::VectorXi& b, int row1, int row2, int pivot_column)
37
38
39
40
41
{
  if(pivot_column == -1)
    std::cout << "Error in eliminate_row (ExactConstraintSatsfaction.cc) : expected a pivot but didn't find any." << std::endl;

  int pivot_row1 = mat.coeff(row1, pivot_column);     //the element under the pivot
42
43
44
45
46
47

  if(pivot_row1 == 0)
    return;

  int pivot_row2 = mat.coeff(row2, pivot_column);     //the pivot

48
49
  b.coeffRef(row1) *= pivot_row2;
  b.coeffRef(row1) -= pivot_row1 * b.coeffRef(row2);
50

51
52
53
  mat.row(row1) *= pivot_row2;
  mat.row(row1) -= pivot_row1 * mat.row(row2);
  mat.row(row1) = mat.row(row1).pruned(0,0);
54

55
56
57
  int gcdValue = gcd_row(mat, row1, b.coeff(row1));
  mat.row(row1) /= gcdValue;
  b.coeffRef(row1) /= gcdValue;
58

59
60
}

61
62
63
int ExactConstraintSatisfaction::gcd(const int a, const int b)
{
  if(b == 0)
64
65
    return std::abs(a);
  return gcd(std::abs(b) , std::abs(a) % std::abs(b));
66
67
}

68
69
int ExactConstraintSatisfaction::gcd_row(const SP_Matrix_R& A, int row, const int b)
{
70

71
  int gcdValue = b;
Robin Brost's avatar
Robin Brost committed
72
73
  bool first = true;
  bool is_negativ = 0;
74

75
76
77
78
  for(SP_Matrix_R::InnerIterator it(A, row); it; ++it)
  {
    if(it.value() != 0 && first)
    {
Robin Brost's avatar
Robin Brost committed
79
      first = false;
80
      is_negativ = it.value() < 0;
Robin Brost's avatar
Robin Brost committed
81
    }
82
    gcdValue = gcd(gcdValue, it.value());
83
84
85
  }
  if(gcdValue == 0)
    return 1;
Robin Brost's avatar
Robin Brost committed
86
87
  if(is_negativ)
    gcdValue = std::abs(gcdValue) * -1;
88
  return gcdValue;
89
90
}

91
void ExactConstraintSatisfaction::print_matrix(const SP_Matrix_R& A){
92

93
94
95
96
  for(int i = 0; i < A.rows(); i++){
    std::cout << "row: " << i << " ";
    for(SP_Matrix_R::InnerIterator it(A, i); it; ++it){
      std::cout << "(" << it.index() << ", " << it.value() << "), ";
97
    }
98
99

    std::cout << std::endl;
100
  }
101
  std::cout << std::endl;
102
103
}

104
void ExactConstraintSatisfaction::print_vector(Eigen::VectorXi b)
105
{
106
107
108
109
110
  std::cout << "the vector contains the elements: ";
  for(int i = 0; i < b.size(); i++){
    std::cout << b.coeffRef(i) << " ";
  }
  std::cout << std::endl;
111
112
}

113
int ExactConstraintSatisfaction::largest_exponent(const Eigen::VectorXd& x)
Robin Brost's avatar
Robin Brost committed
114
{
115

116
  int expo = -65;
117
118
  for(int i = 0; i < x.size(); i++)
  {
119
    expo = std::max(expo, static_cast<int>(std::ceil(std::log2(std::abs(x.coeffRef(i)))) + 2));
120
  }
Robin Brost's avatar
Robin Brost committed
121
122
  largest_exponent_ = expo;
  delta_ = std::pow(2, expo);
123
  return expo;
Robin Brost's avatar
Robin Brost committed
124
125
126
127
}

double ExactConstraintSatisfaction::F_delta(double x)
{
128
129
130
  int sign = -1;
  if(x >= 0)
    sign = 1;
Robin Brost's avatar
Robin Brost committed
131
  double x_of_F = (x + sign * delta_) - sign * delta_;
132
  return x_of_F;
Robin Brost's avatar
Robin Brost committed
133
134
135
136
}

int ExactConstraintSatisfaction::lcm(const int a, const int b)
{
137
138
139
  if(gcd(a,b) == 0)
    return 0;
  return (a/gcd(a,b)) * b;
Robin Brost's avatar
Robin Brost committed
140
141
}

142
int ExactConstraintSatisfaction::lcm(const std::vector<int>& D)
Robin Brost's avatar
Robin Brost committed
143
{
144
  int lcm_D = 1;
145
146
  for(int d : D)
  {
147
148
149
    lcm_D = lcm(lcm_D, d);
  }
  return lcm_D;
Robin Brost's avatar
Robin Brost committed
150
151
}

152
int ExactConstraintSatisfaction::index_pivot(const sparseVec& row)
Robin Brost's avatar
Robin Brost committed
153
{
154

155
156
  for(sparseVec::InnerIterator it(row); it; ++it)
  {
157
158
    if(it.value() != 0)
      return it.index();
159
160
161
162
163
164
  }
  return -1;
}

double ExactConstraintSatisfaction::get_delta()
{
Robin Brost's avatar
Robin Brost committed
165
  return delta_;
Robin Brost's avatar
Robin Brost committed
166
167
}

168
// ----------------------Matrix transformation-----------------------------------
169
void ExactConstraintSatisfaction::IREF_Gaussian(SP_Matrix_R& A, Eigen::VectorXi& b, const Eigen::VectorXd& x){
Robin Brost's avatar
Robin Brost committed
170
171
172
173

  number_pivots_ = 0;
  int rows = A.rows();        //number of rows
  int cols = A.cols();        //number of columns
174
  int col_index  = 0;         //save the next column where we search for a pivot
175

176
177
178
179
  for (int k = 0; k < rows; k++)
  {                                        //order Matrix after pivot
    if(A.coeff(k, col_index) == 0)
    {
180
      int pivot_row = get_pivot_col_new(A, k, col_index);
181

182
183
184
      if(pivot_row != -1)
        if(k != pivot_row)
          swap_rows(A, k, pivot_row);
185
186
187
188
189
190
191
192
193
194
195

      if(col_index == cols)
        break;

      if(pivot_row == -1)
        continue;
    }
    else
    {
      col_index++;//col_index = k +1;
    }
196
    number_pivots_++;
197

198
    if (number_pivots_ == 1)
199
    {
200
201
202
203
204
205
206
      // first row, divide by gcd to keep numbers small. All other rows will be divided by ther gcd in eliminate row
      int gcdValue = gcd_row(A, 0, b.coeffRef(0));                     //compute the gcd to make the values as small as possible
      if(gcdValue != 1)
      {
        A.row(0) /= gcdValue;
        b.coeffRef(0) /= gcdValue;
      }
207
208
    }

209
    int col_p = col_index -1;
210

211
212
213
214
215
216
    for(int i = k+1; i < rows; ++i)
    {
      SP_Matrix_R::InnerIterator row_iter_i(A,i);
      if (row_iter_i.index() > col_p)
        continue; // first element is right of i, so A(i,col_p) is already 0
      COMISO_THROW_TODO_if(row_iter_i.index() < col_p, "there should be now non zero values lower left of k,col_p");
217

218
      eliminate_row(A, b, i, k, col_p);
219
    }
220
  }
221
222
}

223
void ExactConstraintSatisfaction::IRREF_Jordan(SP_Matrix_R& A, Eigen::VectorXi& b)
224
{
225
226
227
228
  SP_Matrix_C A_colmaj = A; // for more efficient tests which rows need to be eliminated

  for(int k = number_pivots_ - 1; k > 0; k--)
  {
229
    int pivot_col = -1;
230
231
232
233
    for(SP_Matrix_R::InnerIterator it(A, k); it; ++it)
    {
      if(it.value() != 0)
      {
234
        pivot_col = it.index();
235
236
        break;
      }
237
    }
238
    COMISO_THROW_TODO_if(pivot_col == -1, "Could not find a pivot column");
239

240
241
242
243
244
    for (SP_Matrix_C::InnerIterator it(A_colmaj, pivot_col); it; ++it)
    {
      if (it.row() == k)
        break;
      if (it.value() == 0)
245
        continue;
246
      eliminate_row(A, b, it.row(), k, pivot_col);
247
    }
248

249
  }
250
251
252

//  A.makeCompressed();
//  A.finalize();
253
  A.prune(0.0, 0);
254
255
}

Robin Brost's avatar
Robin Brost committed
256
//-------------------------------------Evaluation--------------------------------------------------------------------
257

258
void ExactConstraintSatisfaction::evaluation(SP_Matrix_R& _A, Eigen::VectorXi& b, Eigen::VectorXd& x)
259
{
260
261
262
263
264
265
266
267
  //debug
  double time_G = 0.0;
  double time_J = 0.0;
  double time_e = 0.0;
  double time_count;
  //debug

  time_count = clock();
268
  IREF_Gaussian(_A, b, x);
269
270
  time_G = clock() - time_count;
  time_G = time_G/CLOCKS_PER_SEC;
271
272
  _A.prune(0.0 , 0);
  std::cout << "error is after Gaus: " << (_A.cast<double>() * x).squaredNorm() << std::endl;
273
274
275
276
277

  //debug
  time_count = clock();
  //debug

278
  IRREF_Jordan(_A, b);
279
280
281
282
283
284

  //debug
  time_J = clock() - time_count;
  time_J = time_J/CLOCKS_PER_SEC;
  //debug

285
286
287
//  _A.prune(0.0 , 0);
//  _A.makeCompressed();
//  _A.finalize();
288
289
  std::cout << "error is after Jordan: " << (_A.cast<double>() * x).squaredNorm() << std::endl;

290
291
292
  //debug
  time_count = clock();
  //debug
293

294
  std::cout << "largest Expo" << std::endl;
295
  largest_exponent(x);
296

297
  evaluate(_A, b, x);
298
299
300
301
302
303
304
305
306

  //debug
  time_e = clock() - time_count;
  time_e = time_e/CLOCKS_PER_SEC;

  std::cout.precision(64);
  std::cout << "time for IREF: " << time_G << " Time for IRREF: " << time_J<< " Time for evaluation: " << time_e << std::endl;
  //debug

307
308
}

309
double ExactConstraintSatisfaction::makeDiv(const std::vector<int>& D, double x)
Robin Brost's avatar
Robin Brost committed
310
311
312
313
{
  if(D.empty()){
    return F_delta(x);
  }
314
  int d = lcm(D);
315
316
  double result = F_delta(x/d) * d;
  return result;
317
318
319

}

320
double ExactConstraintSatisfaction::safeDot(const std::vector<std::pair<int, double> >& S)
Robin Brost's avatar
Robin Brost committed
321
{
322
  if (S.empty()) //case all Cij are zero after the pivot
323
    return 0;
324
  int safebreak = 9999999;
325
326
327
  std::vector<std::pair<int, double>> P;
  std::vector<std::pair<int, double>> N;

Robin Brost's avatar
Robin Brost committed
328
329
330
  int k = 0;
  double r = 0;                                         //return value of the dot

331
  for(auto element : S){
Robin Brost's avatar
Robin Brost committed
332
333
    if(element.first * element.second > 0)
    {
334
335
      element.first = std::abs(element.first);
      element.second = std::abs(element.second);
336
      P.push_back(element);
337
    }
338
    else if(element.first * element.second < 0)
Robin Brost's avatar
Robin Brost committed
339
    {
340
341
      element.first = std::abs(element.first);
      element.second = - std::abs(element.second);
342
      N.push_back(element);
343
    }
Robin Brost's avatar
Robin Brost committed
344
  }
Robin Brost's avatar
Robin Brost committed
345

346
347
  while((!P.empty() || !N.empty()) && safebreak > 0)
  {
Robin Brost's avatar
Robin Brost committed
348
    safebreak--;                                        //break out of the while after an amount of time
349
350
351
352
    if(!P.empty() && (r < 0 || N.empty()))
    {
      const std::pair<int, double> element = P.back();
      P.pop_back();
353
      double test_value = element.second;
354
355
      if(test_value < 0.00000001)
      { //to prevent overflow through the dividing
356
        k = element.first;
357
358
359
      }
      else
      {
360
361
        k = std::min(element.first, static_cast<int>(std::floor((delta_ - r)/element.second)));
      }
362
363
      r = r + k * element.second;
      if(k < element.first)
364
365
366
367
368
369
        P.push_back({element.first - k, element.second});
    }
    else
    {
      const std::pair<int, double> element = N.back();
      N.pop_back();
370
      double test_value = element.second;
371
372
      if(std::abs(test_value) < 0.00000001)
      {            //to prevent overflow through the dividing
373
        k = element.first;
374
375
376
      }
      else
      {
377
378
        k = std::min(element.first, static_cast<int>(std::floor((-delta_ - r)/element.second)));
      }
379
380
      r = r + k * element.second;
      if(k < element.first)
381
        N.push_back({element.first - k, element.second});
382
    }
383

384
385
    if(k == 0)
    {
386
387
388
      std::cout << "ERROR: The representable range of double values (delta) is to small (ExactConstraintSatisfaction.cc)" << std::endl;
      return -99999999;
    }
Robin Brost's avatar
Robin Brost committed
389
  }
390
391
  if(safebreak == 0)
    std::cout << "WARNING:: the set number of Iterations in Method : safedot is exceeded" << std::endl;
392

393
  return r;
394
395
}

396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
void ExactConstraintSatisfaction::evaluate(const ExactConstraintSatisfaction::SP_Matrix_R& _A, const Eigen::VectorXi& b, Eigen::VectorXd& x)
{
  SP_Matrix_C A = _A;         //change the matrix type to allow easier iteration

  int cols = A.cols();
  std::cout << "findPivo" << std::endl;
  for(int k = cols -1; k >= 0; k--)
  {
    auto pivot_row = get_pivot_row_new(A, _A, k);

    if(pivot_row == -1)
    {
      //there is no pivot in this column
      auto D = get_divisors_new(A, _A, k);
      x.coeffRef(k) = makeDiv(D, x.coeffRef(k));            //fix free variables so they are in F_delta
    }
    else
    {
      auto S = get_dot_product_elements_new(_A, x, pivot_row);

      double divided_B = b.coeffRef(pivot_row);
      divided_B = F_delta(divided_B / A.coeffRef(pivot_row, k));
      if( divided_B * A.coeffRef(pivot_row, k) !=  static_cast<double>(b.coeffRef(pivot_row)))
        std::cout << "WARNING: Can't handle the right hand side perfectly" << std::endl;
      x.coeffRef(k) = divided_B - safeDot(S);
    }
  }

}

int ExactConstraintSatisfaction::get_pivot_col_student(SP_Matrix_R& _A, int k, int& col_index)
{
  int pivot_row = -1;
  int cols = _A.cols();
  for(; col_index < cols; col_index++)
  {

    Eigen::SparseVector<int> col = _A.col(col_index);
    for(Eigen::SparseVector<int>::InnerIterator it(col); it; ++it)
    {
      if(it.value() != 0 && it.index() >= k)
      {
        pivot_row = it.index();
        break;
      }
    }
    if(pivot_row != -1)
    {
      col_index++;
      break;
    }
  }

  return pivot_row;
}

int ExactConstraintSatisfaction::get_pivot_col_new(ExactConstraintSatisfaction::SP_Matrix_R& _A, int k, int& col_index)
{
  int cols = _A.cols();
  int rows = _A.rows();
  for(; col_index < cols; col_index++)
  {
    for (int i = k; i < rows; ++i)
      if (_A.row(i).coeff(col_index) != 0)
      {
        ++col_index;
        return i;
      }
  }

  return -1;
}

469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
int ExactConstraintSatisfaction::get_pivot_row_student(const SP_Matrix_C& A, int col)
{
  int pivot_row = -1;
  for(SP_Matrix_C::InnerIterator it(A, col); it; ++it)
  {
    if(it.value() != 0)
    {
      int index = index_pivot(A.row(it.index()));
      if(index == col)
      {
        pivot_row = it.index();
        break;
      }
    }
    else
    {
      COMISO_THROW_TODO("There should be no non zero values in the matrix");
    }
  }
  return pivot_row;
}

int ExactConstraintSatisfaction::get_pivot_row_new(const SP_Matrix_C& A, const SP_Matrix_R& _A, int col)
{
  auto collumn = A.col(col);
  if (collumn.nonZeros() != 1) // a pivot is allways the only entry in a column
    return -1;

  auto row = SP_Matrix_C::InnerIterator(A, col).index();

  // check if col is the first element in row
  auto first_index = SP_Matrix_R::InnerIterator(_A, row).index();

  if (first_index == col)
    return row;

  return -1;
}

508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
std::vector<int> ExactConstraintSatisfaction::get_divisors_student(const ExactConstraintSatisfaction::SP_Matrix_C& A, int col)
{
  std::vector<int> D;
  for(SP_Matrix_C::InnerIterator it(A, col); it; ++it)
  {
    COMISO_THROW_TODO_if(it.value() == 0, "There should be no zeros left in the matrix");
    if(it.value() != 0 && it.index() <= col && it.index() < number_pivots_)
    {
      int pivot_col = index_pivot(A.row(it.index()));
      D.push_back(A.coeff(it.index(), pivot_col));
    }
  }

  return D;
}

std::vector<int> ExactConstraintSatisfaction::get_divisors_new(const SP_Matrix_C& A, const SP_Matrix_R& _A, int col)
{
  std::vector<int> D;
  for(SP_Matrix_C::InnerIterator it(A, col); it; ++it)
  {
    COMISO_THROW_TODO_if(it.value() == 0,              "There should be no zeros left in the matrix");
    if (it.index() >= number_pivots_)
      std::cout << A << std::endl;
    COMISO_THROW_TODO_if(it.index() >= number_pivots_, "The matrix should only contain number_pivots non empty rows");
    COMISO_THROW_TODO_if(it.index() > col,             "The matrix should not contain elements below the diagonal");

    D.push_back(SP_Matrix_R::InnerIterator(_A, it.index()).value());
  }
  return D;
}

540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
std::vector<std::pair<int, double> > ExactConstraintSatisfaction::get_dot_product_elements_student(const SP_Matrix_C& A, const Eigen::VectorXd& x,  int k, int pivot_row)
{
  std::vector<std::pair<int, double>> S;
  int cols = A.cols();
  for(int i = k+1; i < cols; i++)
  {                      //construct the list S to do the dot Product
    std::pair<int, double> pair;
    pair.first = A.coeff(pivot_row,i);
    double test = x.coeff(i) / A.coeff(pivot_row,k);
    if(x.coeff(i) != ( test * A.coeff(pivot_row,k)))
      std::cout << "WARNING: can't devide" << " in row : " << i << std::endl;
    pair.second = x.coeff(i) / A.coeff(pivot_row,k);
    if(pair.first != 0)
      S.push_back(pair);
  }

  return S;
}

std::vector<std::pair<int, double> > ExactConstraintSatisfaction::get_dot_product_elements_new(const SP_Matrix_R& A, const Eigen::VectorXd& x, int pivot_row)
{
  std::vector<std::pair<int, double>> S;

  SP_Matrix_R::InnerIterator it(A, pivot_row);
  auto pivot_val = it.value();

  while (++it)
  {
    std::pair<int, double> pair;
    pair.first = it.value();
    auto tmp = x.coeff(it.index());
    COMISO_THROW_TODO_if((tmp / pivot_val) * pivot_val != tmp, "element in x is not divisible by pivot element");
    pair.second = tmp / pivot_val;
    S.push_back(pair);
  }
  return S;
}

578
579
}