linear_solver.hpp 7.04 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1 2 3
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps  *
* version 0.1                                                                  *
4
* Copyright (C) 2009-2012, IGG Team, LSIIT, University of Strasbourg           *
Pierre Kraemer's avatar
Pierre Kraemer committed
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
*                                                                              *
* This library is free software; you can redistribute it and/or modify it      *
* under the terms of the GNU Lesser General Public License as published by the *
* Free Software Foundation; either version 2.1 of the License, or (at your     *
* option) any later version.                                                   *
*                                                                              *
* This library is distributed in the hope that it will be useful, but WITHOUT  *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or        *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License  *
* for more details.                                                            *
*                                                                              *
* You should have received a copy of the GNU Lesser General Public License     *
* along with this library; if not, write to the Free Software Foundation,      *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.           *
*                                                                              *
20
* Web site: http://cgogn.unistra.fr/                                           *
Pierre Kraemer's avatar
Pierre Kraemer committed
21 22 23 24
* Contact information: cgogn@unistra.fr                                        *
*                                                                              *
*******************************************************************************/

25 26
template <typename CoeffType>
LinearSolver<CoeffType>::LinearSolver(unsigned int nb_variables) {
Pierre Kraemer's avatar
Pierre Kraemer committed
27 28 29 30 31 32 33 34 35
	least_squares_ = false ;
	direct_ = false ;
	matrix_already_set_ = false ;
	nb_variables_ = nb_variables ;
	variable_ = new Variable<CoeffType>[nb_variables] ;
	state_ = INITIAL ;
	A_ = NULL ;
	x_ = NULL ;
	b_ = NULL ;
36
	direct_solver_ = new Solver_CHOLESKY<CoeffType>() ;
Pierre Kraemer's avatar
Pierre Kraemer committed
37 38
}

39 40
template <typename CoeffType>
void LinearSolver<CoeffType>::begin_system() {
Pierre Kraemer's avatar
Pierre Kraemer committed
41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58
	current_row_ = 0 ;
	transition(INITIAL, IN_SYSTEM) ;
	
	if(!matrix_already_set_) {
		// Enumerate free variables.
		unsigned int index = 0 ;
		for(unsigned int i = 0; i < nb_variables() ; i++) {
			Variable<CoeffType>& v = variable(i) ;
			if(!v.is_locked()) {
				v.set_index(index) ;
				index++ ;
			}
		}

		// Size of the actual system to solve
		unsigned int n = index ;

		// Allocate and init internal structures
59 60 61
		A_ = new Eigen::SparseMatrix<CoeffType>(n, n) ;
		x_ = new Eigen::Matrix<CoeffType, Eigen::Dynamic, 1>(n) ;
		b_ = new Eigen::Matrix<CoeffType, Eigen::Dynamic, 1>(n) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
62 63 64 65 66 67 68 69 70
		for(unsigned int i = 0; i < n; i++) {
			(*x_)[i] = 0 ;
			(*b_)[i] = 0 ;
		}
	}

	variables_to_vector() ;
}

71 72
template <typename CoeffType>
void LinearSolver<CoeffType>::begin_row() {
Pierre Kraemer's avatar
Pierre Kraemer committed
73 74 75 76 77 78 79 80
	transition(IN_SYSTEM, IN_ROW) ;
	af_.clear() ;
	if_.clear() ;
	al_.clear() ;
	xl_.clear() ;
	bk_ = 0 ;
}

81 82
template <typename CoeffType>
void LinearSolver<CoeffType>::set_right_hand_side(CoeffType b) {
Pierre Kraemer's avatar
Pierre Kraemer committed
83 84 85 86
	check_state(IN_ROW) ;
	bk_ = b ;
}

87 88
template <typename CoeffType>
void LinearSolver<CoeffType>::add_coefficient(unsigned int iv, CoeffType a) {
Pierre Kraemer's avatar
Pierre Kraemer committed
89 90 91 92 93 94 95 96 97 98 99
	check_state(IN_ROW) ;
	Variable<CoeffType>& v = variable(iv) ;
	if(v.is_locked()) {
		al_.push_back(a) ;
		xl_.push_back(v.value()) ;
	} else {
		af_.push_back(a) ;
		if_.push_back(v.index()) ;
	}
}

100 101
template <typename CoeffType>
void LinearSolver<CoeffType>::normalize_row(CoeffType weight) {
Pierre Kraemer's avatar
Pierre Kraemer committed
102 103 104 105 106 107 108 109 110 111 112 113 114 115
	check_state(IN_ROW) ;
	CoeffType norm = 0.0 ;
	unsigned int nf = af_.size() ;
	for(unsigned int i=0; i<nf; i++) {
		norm += af_[i] * af_[i] ;
	}
	unsigned int nl = al_.size() ;
	for(unsigned int i=0; i<nl; i++) {
		norm += al_[i] * al_[i] ;
	}
	norm = sqrt(norm) ;
	scale_row(weight / norm) ;
}

116 117
template <typename CoeffType>
void LinearSolver<CoeffType>::scale_row(CoeffType s) {
Pierre Kraemer's avatar
Pierre Kraemer committed
118 119 120 121 122 123 124 125 126 127 128 129
	check_state(IN_ROW) ;
	unsigned int nf = af_.size() ;
	for(unsigned int i=0; i<nf; i++) {
		af_[i] *= s ;
	}
	unsigned int nl = al_.size() ;
	for(unsigned int i=0; i<nl; i++) {
		al_[i] *= s ;
	}
	bk_ *= s ; 
}

130 131
template <typename CoeffType>
void LinearSolver<CoeffType>::end_row() {
Pierre Kraemer's avatar
Pierre Kraemer committed
132 133 134 135 136 137 138
	if(least_squares_) {
		unsigned int nf = af_.size() ;
		unsigned int nl = al_.size() ;
		// Construct the matrix coefficients of the current row
		if(!matrix_already_set_) {
			for(unsigned int i = 0; i < nf; i++) {
				for(unsigned int j = 0; j < nf; j++) {
139
					A_->coeffRef(if_[i], if_[j]) += af_[i] * af_[j] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
				}
			}
		}
		// Construct the right-hand side of the current row
		CoeffType S = - bk_ ;
		for(unsigned int j = 0; j < nl; j++) {
			S += al_[j] * xl_[j] ;
		}
		for(unsigned int i = 0; i < nf; i++) {
			(*b_)[if_[i]] -= af_[i] * S ;
		}
	} else {
		unsigned int nf = af_.size() ;
		unsigned int nl = al_.size() ;
		// Construct the matrix coefficients of the current row
		if(!matrix_already_set_) {
			for(unsigned int i = 0; i < nf; i++) {
157
				A_->coeffRef(current_row_, if_[i]) += af_[i] ;
Pierre Kraemer's avatar
Pierre Kraemer committed
158 159 160
			}
		}
		// Construct the right-hand side of the current row
161
		(*b_)[current_row_] = -bk_ ;
Pierre Kraemer's avatar
Pierre Kraemer committed
162 163 164 165 166 167 168 169
		for(unsigned int i = 0; i < nl; i++) {
			(*b_)[current_row_] -= al_[i] * xl_[i] ;
		}
	}
	current_row_++ ;
	transition(IN_ROW, IN_SYSTEM) ;
}

170 171
template <typename CoeffType>
void LinearSolver<CoeffType>::end_system() {
172 173
	if(least_squares_ && direct_ && !direct_solver_->factorized())
		direct_solver_->factorize(*A_) ;
174

175
	transition(IN_SYSTEM, CONSTRUCTED) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
176 177
}

178 179
template <typename CoeffType>
void LinearSolver<CoeffType>::solve() {
Pierre Kraemer's avatar
Pierre Kraemer committed
180 181 182 183
	check_state(CONSTRUCTED) ;
	if(least_squares_)
	{
		if(direct_) {
184
			direct_solver_->solve(*b_, *x_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
185
		} else {
186 187 188 189 190 191
			Eigen::ConjugateGradient<Eigen::MatrixXf > solver(*A_) ;
//			int n = x_->rows() ;
//			Eigen::Matrix<CoeffType, Eigen::Dynamic, 1> guess(n) ;
//			for(int i = 0; i < n; ++i)
//				guess[i] = (*x_)[i] ;
//			*x_ = solver.solveWithGuess(*b_, guess) ;
192
			*x_ = solver.solve(*b_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
193 194
		}
	} else {
195
		Eigen::BiCGSTAB<Eigen::MatrixXf > solver(*A_) ;
196
		*x_ = solver.solve(*b_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
197
	}
198

Pierre Kraemer's avatar
Pierre Kraemer committed
199 200 201 202
	vector_to_variables() ;
	transition(CONSTRUCTED, SOLVED) ;
}

203 204
template <typename CoeffType>
void LinearSolver<CoeffType>::reset(bool keep_matrix) {
205 206 207 208 209 210 211 212 213 214 215 216 217 218
	if(keep_matrix) {
		x_->setZero() ;
		b_->setZero() ;
		matrix_already_set_ = true ;
	} else {
		delete A_ ; A_ = NULL ;
		delete x_ ; x_ = NULL ;
		delete b_ ; b_ = NULL ;
		direct_solver_->reset();
		matrix_already_set_ = false ;
		for(unsigned int i = 0; i < nb_variables_; ++i) {
			variable_[i].unlock() ;
		}
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
219 220 221
	state_ = INITIAL ;
}

222 223
template <typename CoeffType>
void LinearSolver<CoeffType>::vector_to_variables() {
Pierre Kraemer's avatar
Pierre Kraemer committed
224
	for(unsigned int i=0; i < nb_variables(); i++) {
225
		Variable<CoeffType>& v = variable(i) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
226 227 228 229 230 231
		if(!v.is_locked()) {
			v.set_value((*x_)[v.index()]) ;
		}
	}
}

232 233
template <typename CoeffType>
void LinearSolver<CoeffType>::variables_to_vector() {
Pierre Kraemer's avatar
Pierre Kraemer committed
234
	for(unsigned int i=0; i < nb_variables(); i++) {
235
		Variable<CoeffType>& v = variable(i) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
236 237 238 239 240
		if(!v.is_locked()) {
			(*x_)[v.index()] = v.value() ;
		}
	}
}