linear_solver.hpp 7.24 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 37 38
	symmetric_solver_ = NULL;
	nonsymmetric_solver_ = NULL;
	direct_solver_ = NULL;
Pierre Kraemer's avatar
Pierre Kraemer committed
39 40
}

41 42
template <typename CoeffType>
void LinearSolver<CoeffType>::begin_system() {
Pierre Kraemer's avatar
Pierre Kraemer committed
43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
	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
61 62 63
		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
64 65 66 67 68 69 70 71 72
		for(unsigned int i = 0; i < n; i++) {
			(*x_)[i] = 0 ;
			(*b_)[i] = 0 ;
		}
	}

	variables_to_vector() ;
}

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

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

89 90
template <typename CoeffType>
void LinearSolver<CoeffType>::add_coefficient(unsigned int iv, CoeffType a) {
Pierre Kraemer's avatar
Pierre Kraemer committed
91 92 93 94 95 96 97 98 99 100 101
	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()) ;
	}
}

102 103
template <typename CoeffType>
void LinearSolver<CoeffType>::normalize_row(CoeffType weight) {
Pierre Kraemer's avatar
Pierre Kraemer committed
104 105 106 107 108 109 110 111 112 113 114 115 116 117
	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) ;
}

118 119
template <typename CoeffType>
void LinearSolver<CoeffType>::scale_row(CoeffType s) {
Pierre Kraemer's avatar
Pierre Kraemer committed
120 121 122 123 124 125 126 127 128 129 130 131
	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 ; 
}

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

172 173 174
template <typename CoeffType>
void LinearSolver<CoeffType>::end_system() {
	if(least_squares_ && direct_ && !direct_solver_)
175
		direct_solver_ = new Eigen::SimplicialLLT<Eigen::SparseMatrix<CoeffType> >(*A_);
176

Pierre Kraemer's avatar
Pierre Kraemer committed
177 178 179
    transition(IN_SYSTEM, CONSTRUCTED) ;
}

180 181
template <typename CoeffType>
void LinearSolver<CoeffType>::solve() {
Pierre Kraemer's avatar
Pierre Kraemer committed
182 183 184 185
	check_state(CONSTRUCTED) ;
	if(least_squares_)
	{
		if(direct_) {
186
			*x_ = direct_solver_->solve(*b_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
187
		} else {
188 189
			symmetric_solver_ = new Eigen::ConjugateGradient<Eigen::SparseMatrix<CoeffType> >(*A_) ;
			*x_ = symmetric_solver_->solve(*b_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
190 191
		}
	} else {
192 193
		nonsymmetric_solver_ = new Eigen::BiCGSTAB<Eigen::SparseMatrix<CoeffType> >(*A_) ;
		*x_ = nonsymmetric_solver_->solve(*b_) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
194 195 196 197 198
	}
	vector_to_variables() ;
	transition(CONSTRUCTED, SOLVED) ;
}

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

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

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