Code

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