ExactConstraintSatisfaction.cc 11.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
#include "ExactConstraintSatisfaction.hh"

#include <CoMISo/Config/config.hh>

#include <CoMISo/NSolver/NProblemInterface.hh>
#include <vector>

ExactConstraintSatisfaction::ExactConstraintSatisfaction()
{

}


// ------------------------Helpfull Methods----------------------------------------
Robin Brost's avatar
Robin Brost committed
15
void ExactConstraintSatisfaction::swapRows(Eigen::SparseMatrix<int>& A, Eigen::VectorXi& b, int row1, int row2){
16
  int temp = 0;
Robin Brost's avatar
Robin Brost committed
17
18
19
20
  for(int i = 0; i < A.cols(); i++){
    temp = A.coeffRef(row1,i);
    A.coeffRef(row1,i) = A.coeffRef(row2, i);
    A.coeffRef(row2, i) = temp;
21
22
  }

Robin Brost's avatar
Robin Brost committed
23
24
25
  auto b_Value = b.coeffRef(row1);
  b.coeffRef(row1) = b.coeffRef(row2);
  b.coeffRef(row2) = b_Value;
26
27
28
}

int ExactConstraintSatisfaction::gcd(const int a, const int b){
29
30
31
32
  if(b == 0){
    return std::abs(a);
  }
  return gcd(std::abs(b) , std::abs(a) % std::abs(b));
33
34
35
}

int ExactConstraintSatisfaction::gcdRow(const Eigen::SparseMatrix<int>::RowXpr row, const int b){
36
  int gcdValue = b;
Robin Brost's avatar
Robin Brost committed
37
38
  bool first = true;
  bool is_negativ = 0;
39
  for(int i = 0; i < row.cols(); i ++){
Robin Brost's avatar
Robin Brost committed
40
41
42
43
    if(row.coeff(i) != 0 && first){
      first = false;
      is_negativ = row.coeff(i) < 0;
    }
44
45
46
47
    gcdValue = gcd(gcdValue, row.coeff(i));
  }
  if(gcdValue == 0)
    return 1;
Robin Brost's avatar
Robin Brost committed
48
49
  if(is_negativ)
    gcdValue = std::abs(gcdValue) * -1;
50
  return gcdValue;
51
52
53
}

void ExactConstraintSatisfaction::printMatrix(Eigen::SparseMatrix<int> A){
54
55
56
57
58
59
60
61
  if(A.size() < 50000){
    for(int i = 0; i < A.rows(); i++){
      for(int j = 0; j < A.cols(); j++){
        std::cout << A.coeffRef(i,j) << " ";
      }
      std::cout << std::endl;
    }
  }else{
62
    for(int i = 0; i < A.rows(); i++){
63
64
65
66
67
68
      bool first = true;
      for(int j = 0; j < A.cols(); j++){
        auto val = A.coeffRef(i,j);
        if(val != 0 && first){
          std::cout << j << " x 0 ";
          first = false;
69
        }
70
71
72
73
74
        if(!first && val != 0)
          std::cout << val << " ";
      }
      if(!first){
        std::cout << std::endl;
75
        std::cout << std::endl;
76
77
      }

78
    }
79
  }
80
81
82
83
}

void ExactConstraintSatisfaction::printVector(Eigen::VectorXi b)
{
84
85
86
87
88
  std::cout << "the vector contains the elements: ";
  for(int i = 0; i < b.size(); i++){
    std::cout << b.coeffRef(i) << " ";
  }
  std::cout << std::endl;
89
90
}

Robin Brost's avatar
Robin Brost committed
91
int ExactConstraintSatisfaction::largestExponent(const Eigen::SparseMatrix<int>& A, const Eigen::VectorXd& x)
Robin Brost's avatar
Robin Brost committed
92
{
93
94
  int expo = -65;
  bool empty_col= true;
Robin Brost's avatar
Robin Brost committed
95
96
97
  for(int i = 0; i < x.size(); i++){
    for(int j = 0; j < A.rows(); j++){
      if(A.coeff(j,i) != 0){
98
99
100
101
        empty_col = false;
        break;
      }
    }
Robin Brost's avatar
Robin Brost committed
102
103
    if(!empty_col)
      expo = std::max(expo, static_cast<int>(std::ceil(std::log2(std::abs(x.coeffRef(i)))) + 2));
104
  }
Robin Brost's avatar
Robin Brost committed
105
106
  largest_exponent_ = expo;
  delta_ = std::pow(2, expo);
107
  return expo;
Robin Brost's avatar
Robin Brost committed
108
109
110
111
}

double ExactConstraintSatisfaction::F_delta(double x)
{
112
113
114
  int sign = -1;
  if(x >= 0)
    sign = 1;
Robin Brost's avatar
Robin Brost committed
115
  double x_of_F = (x + sign * delta_) - sign * delta_;
116
  return x_of_F;
Robin Brost's avatar
Robin Brost committed
117
118
119
120
}

int ExactConstraintSatisfaction::lcm(const int a, const int b)
{
121
122
123
  if(gcd(a,b) == 0)
    return 0;
  return (a/gcd(a,b)) * b;
Robin Brost's avatar
Robin Brost committed
124
125
}

126
int ExactConstraintSatisfaction::lcm_list(const std::list<int> D)
Robin Brost's avatar
Robin Brost committed
127
{
128
129
130
131
132
  int lcm_D = 1;
  for(int d : D){
    lcm_D = lcm(lcm_D, d);
  }
  return lcm_D;
Robin Brost's avatar
Robin Brost committed
133
134
}

Robin Brost's avatar
Robin Brost committed
135
int ExactConstraintSatisfaction::indexPivot(const Eigen::SparseMatrix<int>& A, int row)
Robin Brost's avatar
Robin Brost committed
136
{
Robin Brost's avatar
Robin Brost committed
137
  auto row1 = A.row(row);
138
139
140
141
142
143
144
145
146
  for(int i = 0; i < row1.size(); i++){
    if(row1.coeff(i) != 0)
      return i;
  }
  return -1;
}

double ExactConstraintSatisfaction::get_delta()
{
Robin Brost's avatar
Robin Brost committed
147
  return delta_;
Robin Brost's avatar
Robin Brost committed
148
149
}

150
// ----------------------Matrix transformation-----------------------------------
Robin Brost's avatar
Robin Brost committed
151
152
153
154
155
156
void ExactConstraintSatisfaction::IREF_Gaussian(Eigen::SparseMatrix<int>& A, Eigen::VectorXi& b){

  number_pivots_ = 0;
  int rows = A.rows();        //number of rows
  int cols = A.cols();        //number of columns
  int col_index = -1;         //save the last column where we found a pivot
157

Robin Brost's avatar
Robin Brost committed
158
  for (int k = 0; k < rows; k++) {                                        //order Matrix after pivot
159
160
161

    //needed for the first line
    if(k == 0){
Robin Brost's avatar
Robin Brost committed
162
      int gcdValue = gcdRow(A.row(k), b.coeffRef(k));                     //compute the gcd to make the values as small as possible
163
      for(int x = 0; x < cols; x++){
Robin Brost's avatar
Robin Brost committed
164
        A.coeffRef(k, x) = A.coeffRef(k,x) / gcdValue;
165
      }
Robin Brost's avatar
Robin Brost committed
166
      b.coeffRef(k) = b.coeffRef(k) / gcdValue;
167
168
169
    }

    if(k < cols){
Robin Brost's avatar
Robin Brost committed
170
      if(A.coeffRef(k,k) == 0){
171
        int pivot_row = -1;
Robin Brost's avatar
Robin Brost committed
172
173
174
        for(col_index += 1; col_index < cols; col_index++){              //find the smallest column with a pivot
          for (int i = k; i < rows; i++) {                               //find row with pivot in this column
            if(A.coeffRef(i,col_index) != 0){
175
              pivot_row = i;
Robin Brost's avatar
Robin Brost committed
176
              number_pivots_++;
177
              break;
178
            }
179
180
181
          }
          if(pivot_row != -1)
            break;
182
        }
183
184
185
        if(pivot_row == -1)
          continue;
        if(pivot_row != k)
Robin Brost's avatar
Robin Brost committed
186
          swapRows(A, b, pivot_row, k);                                  //swap rows so the pivot is in the right row
187
188
      }else{
        col_index ++;
Robin Brost's avatar
Robin Brost committed
189
        number_pivots_++;
190
191
192
      }
    }
    for(int i = k+1; i < rows; i++){
Robin Brost's avatar
Robin Brost committed
193
194
195
      int under_pivot = A.coeffRef(i,col_index);
      for(int j = col_index; j < cols; j++){                                                          //change k+1 to k to eliminate the elements below the pivot
        A.coeffRef(i,j) = A.coeffRef(k,col_index) * A.coeffRef(i,j) - under_pivot * A.coeffRef(k,j);  //eliminate the rows below the row with pivot, only one pivot per column
196
      }
Robin Brost's avatar
Robin Brost committed
197
      b.coeffRef(i) = A.coeffRef(k,col_index) * b.coeffRef(i) - under_pivot * b.coeffRef(k);
198
199
200
    }

    for(int i = k; i < rows; i++){
Robin Brost's avatar
Robin Brost committed
201
      int gcdValue = gcdRow(A.row(i), b.coeffRef(i));                                                 //compute the gcd to make the values as small as possible
202
      for(int j = 0; j < cols; j++){
Robin Brost's avatar
Robin Brost committed
203
        A.coeffRef(i, j) = A.coeffRef(i,j) / gcdValue;
204
      }
Robin Brost's avatar
Robin Brost committed
205
      b.coeffRef(i) = b.coeffRef(i) / gcdValue;
206
    }
Robin Brost's avatar
Robin Brost committed
207

208
  }
209
210
}

Robin Brost's avatar
Robin Brost committed
211
void ExactConstraintSatisfaction::IRREF_Jordan(Eigen::SparseMatrix<int> &A, Eigen::VectorXi &b)
212
{
Robin Brost's avatar
Robin Brost committed
213
214
  int cols = A.cols();
  for(int k = number_pivots_ - 1; k > 0; k--){
215
    int p_col = k;
Robin Brost's avatar
Robin Brost committed
216
217
    for(p_col = k; p_col < cols; p_col++){              //find pivot
      if(A.coeffRef(k,p_col) != 0)
218
219
        break;
    }
Robin Brost's avatar
Robin Brost committed
220
221
    for(int i = k-1; i >= 0; i--){                      //eliminate row i with row k
      int above_p = A.coeffRef(i,p_col);                //element in rows above the pivot
222
223
      for(int j = cols-1; j >= i; j--){
        if(j >= p_col){
Robin Brost's avatar
Robin Brost committed
224
          A.coeffRef(i,j) = A.coeffRef(k,p_col) * A.coeffRef(i,j) - above_p * A.coeffRef(k,j);
225
        }else if(j >= i){
Robin Brost's avatar
Robin Brost committed
226
          A.coeffRef(i,j) = A.coeffRef(k,p_col) * A.coeffRef(i,j);
227
        }
228
      }
Robin Brost's avatar
Robin Brost committed
229
      b.coeffRef(i) = A.coeffRef(k,p_col) * b.coeffRef(i) - above_p * b.coeffRef(k);
230

Robin Brost's avatar
Robin Brost committed
231
      int gcdValue = gcdRow(A.row(i), b.coeffRef(i));   //compute the gcd to make the values as small as possible
232
      for(int j = 0; j < cols; j++){
Robin Brost's avatar
Robin Brost committed
233
        A.coeffRef(i, j) = A.coeffRef(i,j) / gcdValue;
234
      }
Robin Brost's avatar
Robin Brost committed
235
      b.coeffRef(i) = b.coeffRef(i) / gcdValue;
236
    }
Robin Brost's avatar
Robin Brost committed
237

238
  }
239
240
}

Robin Brost's avatar
Robin Brost committed
241
//-------------------------------------Evaluation--------------------------------------------------------------------
242

Robin Brost's avatar
Robin Brost committed
243
void ExactConstraintSatisfaction::evaluation(Eigen::SparseMatrix<int>& A, Eigen::VectorXi& b, Eigen::VectorXd& x)
244
{
245
246
247
  IREF_Gaussian(A, b);
  IRREF_Jordan(A, b);

Robin Brost's avatar
Robin Brost committed
248
  int cols = A.cols();
249
250
251
252
253
  std::cout << "largest Expo" << std::endl;
  largestExponent(A, x);
  std::cout << "findPivo" << std::endl;
  for(int k = cols -1; k >= 0; k--){
    int pivot = -1;
254

Robin Brost's avatar
Robin Brost committed
255
256
257
//new faster Version test, iterates over the non zero (non empty) elements of one col
//    for(iteratorV it(static_cast<sparsVec>(A.col(k))); it; ++it){
//      int i = it.index();
258

Robin Brost's avatar
Robin Brost committed
259
260
261

      for(int i = 0; i <= k; i++){                      //find row with pivot in this column
      if(i < A.rows()){
262
263
        int pivotIndex = indexPivot(A,i);
        if(pivotIndex == k){
Robin Brost's avatar
Robin Brost committed
264
          pivot = i;                                    //the row with the pivot
265
          break;
Robin Brost's avatar
Robin Brost committed
266
        }
267
      }
268

269
270
    }

Robin Brost's avatar
Robin Brost committed
271
    if(pivot == -1){                                    //there is no pivot in this column
272
273
      std::list<int> D;
      D.clear();
Robin Brost's avatar
Robin Brost committed
274
      for(int i = 1; i <= std::min(k, number_pivots_ - 1); i ++){
275
        int pivot_col = indexPivot(A, i);
Robin Brost's avatar
Robin Brost committed
276
277
        if(A.coeffRef(i,k) != 0 && pivot_col < k){
          D.push_front(A.coeffRef(i, pivot_col));
Robin Brost's avatar
Robin Brost committed
278
        }
279
      }
Robin Brost's avatar
Robin Brost committed
280
      x.coeffRef(k) = makeDiv(D, x.coeffRef(k));        //fix free variables so they are in F_delta
281
282
283
284
285
286


    }else{                                              //compute now the implied variables

      std::list<std::pair<int, double>> S;
      S.clear();
Robin Brost's avatar
Robin Brost committed
287
      for(int i = k+1; i < cols; i++){                  //construct the list S to do the dot Product
288
        std::pair<int, double> tuple;
Robin Brost's avatar
Robin Brost committed
289
290
291
        tuple.first = A.coeffRef(pivot,i);
        double test =x.coeffRef(i) / A.coeffRef(pivot,k);
        if(x.coeffRef(i) != ( test * A.coeffRef(pivot,k)))
292
           std::cout << "WARNING: can't devide" << " in row : " << i << std::endl;
Robin Brost's avatar
Robin Brost committed
293
        tuple.second = x.coeffRef(i) / A.coeffRef(pivot,k);
294
295
296
297
        if(tuple.first != 0)
          S.push_front(tuple);

      }
Robin Brost's avatar
Robin Brost committed
298
299
300
      double divided_B = b.coeffRef(pivot);
      divided_B = F_delta(divided_B / A.coeffRef(pivot, k));
      if( divided_B * A.coeffRef(pivot, k) !=  static_cast<double>(b.coeffRef(pivot)))
301
        std::cout << "WARNING: Can't handle the right hand side perfectly" << std::endl;
Robin Brost's avatar
Robin Brost committed
302
      x.coeffRef(k) = divided_B - safeDot(S);
Robin Brost's avatar
Robin Brost committed
303
    }
304
  }
305
306
}

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

}

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

329
  for(std::pair<int, double> element : S){
Robin Brost's avatar
Robin Brost committed
330
331
    if(element.first * element.second > 0)
    {
332
333
334
      element.first = std::abs(element.first);
      element.second = std::abs(element.second);
      P.push_front(element);
335
    }
336
    else if(element.first * element.second < 0)
Robin Brost's avatar
Robin Brost committed
337
    {
338
339
340
      element.first = std::abs(element.first);
      element.second = - std::abs(element.second);
      N.push_front(element);
341
    }
Robin Brost's avatar
Robin Brost committed
342
  }
Robin Brost's avatar
Robin Brost committed
343

344
  while((!P.empty() || !N.empty()) && safebreak > 0){
Robin Brost's avatar
Robin Brost committed
345
346

    safebreak--;                                        //break out of the while after an amount of time
Robin Brost's avatar
Robin Brost committed
347
    if(!P.empty() && (r < 0 || N.empty())){
348
      const std::pair<int, double> element = P.front();
349
      P.pop_front();
Robin Brost's avatar
Robin Brost committed
350
      k = std::min(element.first, static_cast<int>(std::floor((delta_ - r)/element.second)));
351
352
      if(k <=0)
        std::cout << "k == 0" << std::endl;
353
354
355
      r = r + k * element.second;
      if(k < element.first)
        P.push_front({element.first - k, element.second});
Robin Brost's avatar
Robin Brost committed
356

357
    }else{
Robin Brost's avatar
Robin Brost committed
358

359
      const std::pair<int, double> element = N.front();
360
      N.pop_front();
Robin Brost's avatar
Robin Brost committed
361
      k = std::min(element.first, static_cast<int>(std::floor((-delta_ - r)/element.second)));
362
363
      if(k <= 0)
        std::cout << "k == 0" << std::endl;
364
365
366
      r = r + k * element.second;
      if(k < element.first)
        N.push_front({element.first - k, element.second});
367
    }
Robin Brost's avatar
Robin Brost committed
368
  }
369
370
  if(safebreak == 0)
    std::cout << "WARNING:: the set number of Iterations in Method : safedot is exceeded" << std::endl;
371
  return r;
372
373
}