28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#ifndef SEEN_SYSEQ_H
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#define SEEN_SYSEQ_H
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free/*
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free * Auxiliary routines to solve systems of linear equations in several variants and sizes.
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free *
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free * Authors:
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free * Maximilian Albert <Anhalter42@gmx.de>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free *
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free * Copyright (C) 2007 Authors
44ed6dfe15487cdc5a4c78b7d07fcfcd0164bc42Liam P. White *
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free * Released under GNU GPL, read the file 'COPYING' for more information
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free */
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#include <algorithm>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#include <iostream>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#include <iomanip>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#include <vector>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free#include "math.h"
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freenamespace SysEq {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeenum SolutionKind {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free unique = 0,
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free ambiguous,
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free no_solution,
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free solution_exists // FIXME: remove this; does not yield enough information
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free};
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeinline void explain(SolutionKind sol) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free switch (sol) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free case SysEq::unique:
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout << "unique" << std::endl;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free break;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free case SysEq::ambiguous:
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout << "ambiguous" << std::endl;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free break;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free case SysEq::no_solution:
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout << "no solution" << std::endl;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free break;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free case SysEq::solution_exists:
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout << "solution exists" << std::endl;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free break;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free }
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free}
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeinline double
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freedeterminant3x3 (double A[3][3]) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free return (A[0][0]*A[1][1]*A[2][2] +
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free A[0][1]*A[1][2]*A[2][0] +
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free A[0][2]*A[1][0]*A[2][1] -
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free A[0][0]*A[1][2]*A[2][1] -
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free A[0][1]*A[1][0]*A[2][2] -
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free A[0][2]*A[1][1]*A[2][0]);
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free}
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free/* Determinant of the 3x3 matrix having a, b, and c as columns */
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeinline double
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freedeterminant3v (const double a[3], const double b[3], const double c[3]) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free return (a[0]*b[1]*c[2] +
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free a[1]*b[2]*c[0] +
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free a[2]*b[0]*c[1] -
1cd5f4b7d334c460e9bba1ba803c78c1ab1fdaadKris a[0]*b[2]*c[1] -
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free a[1]*b[0]*c[2] -
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free a[2]*b[1]*c[0]);
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free}
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free/* Copy the elements of A into B */
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freetemplate <int S, int T>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeinline void copy_mat(double A[S][T], double B[S][T]) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free for (int i = 0; i < S; ++i) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free for (int j = 0; j < T; ++j) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free B[i][j] = A[i][j];
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free }
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free }
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free}
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freetemplate <int S, int T>
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-freeinline void print_mat (const double A[S][T]) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout.setf(std::ios::left, std::ios::internal);
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free for (int i = 0; i < S; ++i) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free for (int j = 0; j < T; ++j) {
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free printf ("%8.2f ", A[i][j]);
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free }
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free std::cout << std::endl;;
28bf548be956fa98bffa377e2caba5f28fb281adtavmjong-free }
}
/* Multiplication of two matrices */
template <int S, int U, int T>
inline void multiply(double A[S][U], double B[U][T], double res[S][T]) {
for (int i = 0; i < S; ++i) {
for (int j = 0; j < T; ++j) {
double sum = 0;
for (int k = 0; k < U; ++k) {
sum += A[i][k] * B[k][j];
}
res[i][j] = sum;
}
}
}
/*
* Multiplication of a matrix with a vector (for convenience, because with the previous
* multiplication function we would always have to write v[i][0] for elements of the vector.
*/
template <int S, int T>
inline void multiply(double A[S][T], double v[T], double res[S]) {
for (int i = 0; i < S; ++i) {
double sum = 0;
for (int k = 0; k < T; ++k) {
sum += A[i][k] * v[k];
}
res[i] = sum;
}
}
// Remark: Since we are using templates, we cannot separate declarations from definitions (which would
// result in linker errors but have to include the definitions here for the following functions.
// FIXME: Maybe we should rework all this by using vector<vector<double> > structures for matrices
// instead of double[S][T]. This would allow us to avoid templates. Would the performance degrade?
/*
* Find the element of maximal absolute value in row i that
* does not lie in one of the columns given in avoid_cols.
*/
template <int S, int T>
static int find_pivot(const double A[S][T], unsigned int i, std::vector<int> const &avoid_cols) {
if (i >= S) {
return -1;
}
int pos = -1;
double max = 0;
for (int j = 0; j < T; ++j) {
if (std::find(avoid_cols.begin(), avoid_cols.end(), j) != avoid_cols.end()) {
continue; // skip "forbidden" columns
}
if (fabs(A[i][j]) > max) {
pos = j;
max = fabs(A[i][j]);
}
}
return pos;
}
/*
* Performs a single 'exchange step' in the Gauss-Jordan algorithm (i.e., swapping variables in the
* two vectors).
*/
template <int S, int T>
static void gauss_jordan_step (double A[S][T], int row, int col) {
double piv = A[row][col]; // pivot element
/* adapt the entries of the matrix, first outside the pivot row/column */
for (int k = 0; k < S; ++k) {
if (k == row) continue;
for (int l = 0; l < T; ++l) {
if (l == col) continue;
A[k][l] -= A[k][col] * A[row][l] / piv;
}
}
/* now adapt the pivot column ... */
for (int k = 0; k < S; ++k) {
if (k == row) continue;
A[k][col] /= piv;
}
/* and the pivot row */
for (int l = 0; l < T; ++l) {
if (l == col) continue;
A[row][l] /= -piv;
}
/* finally, set the element at the pivot position itself */
A[row][col] = 1/piv;
}
/*
* Perform Gauss-Jordan elimination on the matrix A, optionally avoiding a given column during pivot search
*/
template <int S, int T>
static std::vector<int> gauss_jordan (double A[S][T], int avoid_col = -1) {
std::vector<int> cols_used;
if (avoid_col != -1) {
cols_used.push_back (avoid_col);
}
for (int i = 0; i < S; ++i) {
/* for each row find a pivot element of maximal absolute value, skipping the columns that were used before */
int col = find_pivot<S,T>(A, i, cols_used);
cols_used.push_back(col);
if (col == -1) {
// no non-zero elements in the row
return cols_used;
}
/* if pivot search was successful we can perform a Gauss-Jordan step */
gauss_jordan_step<S,T> (A, i, col);
}
if (avoid_col != -1) {
// since the columns that were used will be needed later on, we need to clean up the column vector
cols_used.erase(cols_used.begin());
}
return cols_used;
}
/* compute the modified value that x[index] needs to assume so that in the end we have x[index]/x[T-1] = val */
template <int S, int T>
static double projectify (std::vector<int> const &cols, const double B[S][T], const double x[T],
const int index, const double val) {
double val_proj = 0.0;
if (index != -1) {
int c = -1;
for (int i = 0; i < S; ++i) {
if (cols[i] == T-1) {
c = i;
break;
}
}
if (c == -1) {
std::cout << "Something is wrong. Rethink!!" << std::endl;
return SysEq::no_solution;
}
double sp = 0;
for (int j = 0; j < T; ++j) {
if (j == index) continue;
sp += B[c][j] * x[j];
}
double mu = 1 - val * B[c][index];
if (fabs(mu) < 1E-6) {
std::cout << "No solution since adapted value is too close to zero" << std::endl;
return SysEq::no_solution;
}
val_proj = sp*val/mu;
} else {
val_proj = val; // FIXME: Is this correct?
}
return val_proj;
}
/**
* Solve the linear system of equations \a A * \a x = \a v where we additionally stipulate
* \a x[\a index] = \a val if \a index is not -1. The system is solved using Gauss-Jordan
* elimination so that we can gracefully handle the case that zero or infinitely many
* solutions exist.
*
* Since our application will be to finding preimages of projective mappings, we provide
* an additional argument \a proj. If this is true, we find a solution of
* \a x[\a index]/\a x[\T - 1] = \a val insted (i.e., we want the corresponding coordinate
* of the _affine image_ of the point with homogeneous coordinate vector \a x to be equal
* to \a val.
*
* Remark: We don't need this but it would be relatively simple to let the calling function
* prescripe the value of _multiple_ components of the solution vector instead of only a single one.
*/
template <int S, int T> SolutionKind gaussjord_solve (double A[S][T], double x[T], double v[S],
int index = -1, double val = 0.0, bool proj = false) {
double B[S][T];
//copy_mat<S,T>(A,B);
SysEq::copy_mat<S,T>(A,B);
std::vector<int> cols = gauss_jordan<S,T>(B, index);
if (std::find(cols.begin(), cols.end(), -1) != cols.end()) {
// pivot search failed for some row so the system is not solvable
return SysEq::no_solution;
}
/* the vector x is filled with the coefficients of the desired solution vector at appropriate places;
* the other components are set to zero, and we additionally set x[index] = val if applicable
*/
std::vector<int>::iterator k;
for (int j = 0; j < S; ++j) {
x[cols[j]] = v[j];
}
for (int j = 0; j < T; ++j) {
k = std::find(cols.begin(), cols.end(), j);
if (k == cols.end()) {
x[j] = 0;
}
}
// we need to adapt the value if we we are in the "projective case" (see above)
double val_new = (proj ? projectify<S,T>(cols, B, x, index, val) : val);
if (index >= 0 && index < T) {
// we want the specified coefficient of the solution vector to have a given value
x[index] = val_new;
}
/* the final solution vector is now obtained as the product B*x, where B is the matrix
* obtained by Gauss-Jordan manipulation of A; we use w as an auxiliary vector and
* afterwards copy the result back to x
*/
double w[S];
SysEq::multiply<S,T>(B,x,w); // initializes w
for (int j = 0; j < S; ++j) {
x[cols[j]] = w[j];
}
if (S + (index == -1 ? 0 : 1) == T) {
return SysEq::unique;
} else {
return SysEq::ambiguous;
}
}
} // namespace SysEq
#endif /* __SYSEQ_H__ */
/*
Local Variables:
mode:c++
c-file-style:"stroustrup"
c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
indent-tabs-mode:nil
fill-column:99
End:
*/
// vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 :