src/syseq.h

   1 #ifndef SEEN_SYSEQ_H
   2 #define SEEN_SYSEQ_H
   3
   4 /*
   5  * Auxiliary routines to solve systems of linear equations in several variants and sizes.
   6  *
   7  * Authors:
   8  *   Maximilian Albert <Anhalter42@gmx.de>
   9  *
  10  * Copyright (C) 2007  Authors
  11  *
  12  * Released under GNU GPL, read the file 'COPYING' for more information
  13  */
  14
  15 #include <iostream>
  16 #include <iomanip>
  17 #include <vector>
  18 #include "math.h"
  19
  20 namespace SysEq {
  21
  22 enum SolutionKind {
  23     unique = 0,
  24     ambiguous,
  25     no_solution,
  26     solution_exists // FIXME: remove this; does not yield enough information
  27 };
  28
  29 inline void explain(SolutionKind sol) {
  30     switch (sol) {
  31         case SysEq::unique:
  32             std::cout << "unique" << std::endl;
  33             break;
  34         case SysEq::ambiguous:
  35             std::cout << "ambiguous" << std::endl;
  36             break;
  37         case SysEq::no_solution:
  38             std::cout << "no solution" << std::endl;
  39             break;
  40         case SysEq::solution_exists:
  41             std::cout << "solution exists" << std::endl;
  42             break;
  43     }
  44 }
  45
  46 inline double
  47 determinant3x3 (double A[3][3]) {
  48     return (A[0][0]*A[1][1]*A[2][2] +
  49             A[0][1]*A[1][2]*A[2][0] +
  50             A[0][2]*A[1][0]*A[2][1] -
  51             A[0][0]*A[1][2]*A[2][1] -
  52             A[0][1]*A[1][0]*A[2][2] -
  53             A[0][2]*A[1][1]*A[2][0]);
  54 }
  55
  56 /* Determinant of the 3x3 matrix having a, b, and c as columns */
  57 inline double
  58 determinant3v (const double a[3], const double b[3], const double c[3]) {
  59     return (a[0]*b[1]*c[2] +
  60             a[1]*b[2]*c[0] +
  61             a[2]*b[0]*c[1] -
  62             a[0]*b[2]*c[1] -
  63             a[1]*b[0]*c[2] -
  64             a[2]*b[1]*c[0]);
  65 }
  66
  67 /* Fills the matrix A with random values between lower and upper */
  68 template <int S, int T>
  69 inline void fill_random (double A[S][T], double lower = 0.0, double upper = 1.0) {
  70     srand(time(NULL));
  71     double range = upper - lower;
  72     for (int i = 0; i < S; ++i) {
  73         for (int j = 0; j < T; ++j) {
  74             A[i][j] = range*(random()/(RAND_MAX + 1.0)) - lower;
  75         }
  76     }
  77 }
  78
  79 /* Copy the elements of A into B */
  80 template <int S, int T>
  81 inline void copy_mat(double A[S][T], double B[S][T]) {
  82     for (int i = 0; i < S; ++i) {
  83         for (int j = 0; j < T; ++j) {
  84             B[i][j] = A[i][j];
  85         }
  86     }
  87 }
  88
  89 template <int S, int T>
  90 inline void print_mat (const double A[S][T]) {
  91     std::cout.setf(std::ios::left, std::ios::internal);
  92     for (int i = 0; i < S; ++i) {
  93         for (int j = 0; j < T; ++j) {
  94             printf ("%8.2f ", A[i][j]);
  95         }
  96         std::cout << std::endl;;
  97     }
  98 }
  99
 100 /* Multiplication of two matrices */
 101 template <int S, int U, int T>
 102 inline void multiply(double A[S][U], double B[U][T], double res[S][T]) {
 103     for (int i = 0; i < S; ++i) {
 104         for (int j = 0; j < T; ++j) {
 105             double sum = 0;
 106             for (int k = 0; k < U; ++k) {
 107                 sum += A[i][k] * B[k][j];
 108             }
 109             res[i][j] = sum;
 110         }
 111     }
 112 }
 113
 114 /*
 115  * Multiplication of a matrix with a vector (for convenience, because with the previous
 116  * multiplication function we would always have to write v[i][0] for elements of the vector.
 117  */
 118 template <int S, int T>
 119 inline void multiply(double A[S][T], double v[T], double res[S]) {
 120     for (int i = 0; i < S; ++i) {
 121         double sum = 0;
 122         for (int k = 0; k < T; ++k) {
 123             sum += A[i][k] * v[k];
 124         }
 125         res[i] = sum;
 126     }
 127 }
 128
 129 // Remark: Since we are using templates, we cannot separate declarations from definitions (which would
 130 //         result in linker errors but have to include the definitions here for the following functions.
 131 // FIXME: Maybe we should rework all this by using vector<vector<double> > structures for matrices
 132 //        instead of double[S][T]. This would allow us to avoid templates. Would the performance degrade?
 133
 134 /*
 135  * Find the element of maximal absolute value in row i that
 136  * does not lie in one of the columns given in avoid_cols.
 137  */
 138 template <int S, int T>
 139 static int find_pivot(const double A[S][T], unsigned int i, std::vector<int> const &avoid_cols) {
 140     if (i >= S) {
 141         return -1;
 142     }
 143     int pos = -1;
 144     double max = 0;
 145     for (int j = 0; j < T; ++j) {
 146         if (std::find(avoid_cols.begin(), avoid_cols.end(), j) != avoid_cols.end()) {
 147             continue; // skip "forbidden" columns
 148         }
 149         if (fabs(A[i][j]) > max) {
 150             pos = j;
 151             max = fabs(A[i][j]);
 152         }
 153     }
 154     return pos;
 155 }
 156
 157 /*
 158  * Performs a single 'exchange step' in the Gauss-Jordan algorithm (i.e., swapping variables in the
 159  * two vectors).
 160  */
 161 template <int S, int T>
 162 static void gauss_jordan_step (double A[S][T], int row, int col) {
 163     double piv = A[row][col]; // pivot element
 164     /* adapt the entries of the matrix, first outside the pivot row/column */
 165     for (int k = 0; k < S; ++k) {
 166         if (k == row) continue;
 167         for (int l = 0; l < T; ++l) {
 168             if (l == col) continue;
 169             A[k][l] -= A[k][col] * A[row][l] / piv;
 170         }
 171     }
 172     /* now adapt the pivot column ... */
 173     for (int k = 0; k < S; ++k) {
 174         if (k == row) continue;
 175         A[k][col]  /= piv;
 176     }
 177     /* and the pivot row */
 178     for (int l = 0; l < T; ++l) {
 179         if (l == col) continue;
 180         A[row][l]  /= -piv;
 181     }
 182     /* finally, set the element at the pivot position itself */
 183     A[row][col] = 1/piv;
 184 }
 185
 186 /*
 187  * Perform Gauss-Jordan elimination on the matrix A, optionally avoiding a given column during pivot search
 188  */
 189 template <int S, int T>
 190 static std::vector<int> gauss_jordan (double A[S][T], int avoid_col = -1) {
 191     std::vector<int> cols_used;
 192     if (avoid_col != -1) {
 193         cols_used.push_back (avoid_col);
 194     }
 195     int col;
 196     for (int i = 0; i < S; ++i) {
 197         /* for each row find a pivot element of maximal absolute value, skipping the columns that were used before */
 198         col = find_pivot<S,T>(A, i, cols_used);
 199         cols_used.push_back(col);
 200         if (col == -1) {
 201             // no non-zero elements in the row
 202             return cols_used;
 203         }
 204
 205         /* if pivot search was successful we can perform a Gauss-Jordan step */
 206         gauss_jordan_step<S,T> (A, i, col);
 207     }
 208     if (avoid_col != -1) {
 209         // since the columns that were used will be needed later on, we need to clean up the column vector
 210         cols_used.erase(cols_used.begin());
 211     }
 212     return cols_used;
 213 }
 214
 215 /* compute the modified value that x[index] needs to assume so that in the end we have x[index]/x[T-1] = val */
 216 template <int S, int T>
 217 static double projectify (std::vector<int> const &cols, const double B[S][T], const double x[T],
 218                           const int index, const double val) {
 219     double val_proj = 0.0;
 220     if (index != -1) {
 221         int c = -1;
 222         for (int i = 0; i < S; ++i) {
 223             if (cols[i] == T-1) {
 224                 c = i;
 225                 break;
 226             }
 227         }
 228         if (c == -1) {
 229             std::cout << "Something is wrong. Rethink!!" << std::endl;
 230             return SysEq::no_solution;
 231         }
 232
 233         double sp = 0;
 234         for (int j = 0; j < T; ++j) {
 235             if (j == index) continue;
 236             sp += B[c][j] * x[j];
 237         }
 238         double mu = 1 - val * B[c][index];
 239         if (fabs(mu) < 1E-6) {
 240             std::cout << "No solution since adapted value is too close to zero" << std::endl;
 241             return SysEq::no_solution;
 242         }
 243         val_proj = sp*val/mu;
 244     } else {
 245         val_proj = val; // FIXME: Is this correct?
 246     }
 247     return val_proj;
 248 }
 249
 250 /**
 251  * Solve the linear system of equations \a A * \a x = \a v where we additionally stipulate
 252  * \a x[\a index] = \a val if \a index is not -1. The system is solved using Gauss-Jordan
 253  * elimination so that we can gracefully handle the case that zero or infinitely many
 254  * solutions exist.
 255  *
 256  * Since our application will be to finding preimages of projective mappings, we provide
 257  * an additional argument \a proj. If this is true, we find a solution of
 258  * \a x[\a index]/\a x[\T - 1] = \a val insted (i.e., we want the corresponding coordinate
 259  * of the _affine image_ of the point with homogeneous coordinate vector \a x to be equal
 260  * to \a val.
 261  *
 262  * Remark: We don't need this but it would be relatively simple to let the calling function
 263  * prescripe the value of _multiple_ components of the solution vector instead of only a single one.
 264  */
 265 template <int S, int T> SolutionKind gaussjord_solve (double A[S][T], double x[T], double v[S],
 266                                                       int index = -1, double val = 0.0, bool proj = false) {
 267     double B[S][T];
 268     //copy_mat<S,T>(A,B);
 269     SysEq::copy_mat<S,T>(A,B);
 270     std::vector<int> cols = gauss_jordan<S,T>(B, index);
 271     if (std::find(cols.begin(), cols.end(), -1) != cols.end()) {
 272         // pivot search failed for some row so the system is not solvable
 273         return SysEq::no_solution;
 274     }
 275
 276     /* the vector x is filled with the coefficients of the desired solution vector at appropriate places;
 277      * the other components are set to zero, and we additionally set x[index] = val if applicable
 278      */
 279     std::vector<int>::iterator k;
 280     for (int j = 0; j < S; ++j) {
 281         x[cols[j]] = v[j];
 282     }
 283     for (int j = 0; j < T; ++j) {
 284         k = std::find(cols.begin(), cols.end(), j);
 285         if (k == cols.end()) {
 286             x[j] = 0;
 287         }
 288     }
 289
 290     // we need to adapt the value if we we are in the "projective case" (see above)
 291     double val_new = (proj ? projectify<S,T>(cols, B, x, index, val) : val);
 292
 293     if (index != -1 && index >= 0 && index < T) {
 294         // we want the specified coefficient of the solution vector to have a given value
 295         x[index] = val_new;
 296     }
 297
 298     /* the final solution vector is now obtained as the product B*x, where B is the matrix
 299      * obtained by Gauss-Jordan manipulation of A; we use w as an auxiliary vector and
 300      * afterwards copy the result back to x
 301      */
 302     double w[S];
 303     SysEq::multiply<S,T>(B,x,w);
 304     for (int j = 0; j < S; ++j) {
 305         x[cols[j]] = w[j];
 306     }
 307
 308     if (S + (index == -1 ? 0 : 1) == T) {
 309         return SysEq::unique;
 310     } else {
 311         return SysEq::ambiguous;
 312     }
 313 }
 314
 315 } // namespace SysEq
 316
 317 #endif /* __SYSEQ_H__ */
 318
 319 /*
 320   Local Variables:
 321   mode:c++
 322   c-file-style:"stroustrup"
 323   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 324   indent-tabs-mode:nil
 325   fill-column:99
 326   End:
 327 */
 328 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :