libsofa1-dev 1.0~beta4-12 / usr / include / sofa / helper / LCPSolver.inl

/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* 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.          *
*******************************************************************************
*                              SOFA :: Framework                              *
*                                                                             *
* Authors: M. Adam, J. Allard, B. Andre, P-J. Bensoussan, S. Cotin, C. Duriez,*
* H. Delingette, F. Falipou, F. Faure, S. Fonteneau, L. Heigeas, C. Mendoza,  *
* M. Nesme, P. Neumann, J-P. de la Plata Alcade, F. Poyer and F. Roy          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_HELPER_LCPSOLVER_INL
#define SOFA_HELPER_LCPSOLVER_INL

#include <sofa/helper/LCPSolver.h>
#include <cstdio>

namespace sofa
{

namespace helper
{

#define EPS     0.00001	// epsilon pour tests = 0
#define EPSP    0.00000000001	// epsilon pour pivot
#define MAX_BOU 50	// nombre maximal de boucles de calcul

// -----------------------------------------------------------------
// --- Resoud un LCP �rit sous la forme U = q + M.F
// ---   dim : dimension du pb
// ---   res[0..dim-1] = U
// ---   res[dim..2*dim-1] = F
// -----------------------------------------------------------------
template <int dim> bool LCPSolver<dim>::solve(const double *q, const Matrix &M, double *res)
{
	int         ii, jj;	
	int         ligPiv;	// ligne du pivot
	int         colPiv;	// colonne du pivot
	double      pivot;	// pivot
	double      min;	// recherche du minimum pour le pivot
	double      coeff;	// valeur du coefficient de la combinaison lin�ire
	int         boucles;	// ii du nombre de passages dans la boucle
	double      mat[dim][2*dim+1];
	int         base[dim];		// base des variables non nulles

	// matrix initialization 
	for (ii=0; ii<dim; ii++) {
		// colonnes correspondantes ?w
		for(jj=0;jj<dim;jj++) {
			if(jj==ii) {
				mat[ii][jj] = 1;
			}
			else {
				mat[ii][jj] = 0;
			}
		}
		// colonnes correspondantes ?z
		for (;jj<2*dim;jj++) {
			mat[ii][jj] = -(M[ii][jj-dim]);
		}
		// colonne correspondante ?q
		mat[ii][jj] = q[ii];
	}

	// initialisation de la base
	for(ii=0;ii<dim;ii++) {
		base[ii]=ii;
	}

	// initialisation du nombre de boucles
	boucles=0;

	// recherche de la ligne du pivot
	ligPiv=-1;
	min = -EPS;
	for(ii=0;ii<dim;ii++) {
		if (mat[ii][2*dim]<min) {
			ligPiv=ii;
			min=mat[ii][2*dim];
		}
	}

	// tant que tous les q[i] ne sont pas > 0 et qu'on ne boucle pas trop
	while ((ligPiv>=0) && (boucles<MAX_BOU)) {
		// augmentation du nombre de passages dans cette boucle
		boucles++;
		// recherche de la colonne du pivot
		if (base[ligPiv]<dim) {
			// c'est un wi dans la base
			colPiv=dim+base[ligPiv];
		}
		else {
			// c'est un zi dans la base
			colPiv=base[ligPiv]-dim;
		}

		// stockage de la valeur du pivot
		pivot=mat[ligPiv][colPiv];

		// si le pivot est nul, le LCP echoue
		if (fabs(pivot)<EPSP) {
		//	printInfo(q,M,base, mat, dim);
 			printf("  + No solution to LCP + \n");
			boucles=MAX_BOU;
			return false;
		} else {
			// division de la ligne du pivot par le pivot
			for(ii=0;ii<2*dim+1;ii++) {
				mat[ligPiv][ii]/=pivot;
			}

			// combinaisons lin�ires mettant la colonne du pivot a 0
			for(ii=0;ii<dim;ii++) {
				if (ii!=ligPiv) {
					coeff=mat[ii][colPiv];
					for(jj=0;jj<2*dim+1;jj++) {
						mat[ii][jj]-=coeff*mat[ligPiv][jj];
					}
				}
			}

			// on rentre dans la base la nouvelle variable
			base[ligPiv]=colPiv;

			// recherche de la nouvelle ligne du pivot
			ligPiv=-1;
			min = -EPS;
			for(ii=0;ii<dim;ii++) {
				if (mat[ii][2*dim]<min) {
					ligPiv=ii;
					min=mat[ii][2*dim];
				}
			}

		}
	}

	// stockage du resultat
	for(ii=0;ii<2*dim;ii++) {
		res[ii]=0;
	}
	// si on est arrive a r�oudre le pb, seules les variables en base sont non nulles
	if (boucles<MAX_BOU) {
		for(ii=0;ii<dim;ii++) {
			res[base[ii]]=mat[ii][2*dim];
		}
	}

	return true;
}


// -----------------------------------------------------------------
// ---
// -----------------------------------------------------------------
template <int dim> void LCPSolver<dim>::printInfo(double *q, Matrix &M)
{
	int ii, jj;

	// affichage de la matrice du LCP
	printf("M = [");
	for(ii=0;ii<dim;ii++) {
		for(jj=0;jj<dim;jj++) {
			printf("\t%.4f",M[ii][jj]);
		}
		printf("\n");
	}
	printf("      ]\n\n");

	// affichage de q
	printf("q = [");
	for (ii=0;ii<dim;ii++) {
		printf("\t%.4f\n",q[ii]);
	}
	printf("      ]\n\n");

	// afficahge base courante
/*	printf("B = [");
	for(ii=0;ii<dim;ii++) {
		printf("\t%d",base[ii]);
	}
	printf("\t]\n\n");

	// affichage matrice courante
	printf("mat = [");
	for(ii=0;ii<dim;ii++) {
		for(jj=0;jj<2*dim+1;jj++) {
			printf("\t%.4f",mat[ii][jj]);
		}
		printf("\n");
	} 
	printf("      ]\n\n"); */
}

//template<> class LCPSolver<3>;
//template<> class LCPSolver<5>;

} // namespace helper

} // namespace sofa

#endif