diff options
Diffstat (limited to 'intern/iksolver/intern/IK_ConjugateGradientSolver.cpp')
| -rw-r--r-- | intern/iksolver/intern/IK_ConjugateGradientSolver.cpp | 162 |
1 files changed, 162 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp b/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp new file mode 100644 index 00000000000..028b7dfe2b0 --- /dev/null +++ b/intern/iksolver/intern/IK_ConjugateGradientSolver.cpp @@ -0,0 +1,162 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "IK_ConjugateGradientSolver.h" + + +#define EPS 1.0e-10 + + IK_ConjugateGradientSolver * +IK_ConjugateGradientSolver:: +New( +){ + return new IK_ConjugateGradientSolver(); +}; + +IK_ConjugateGradientSolver:: +~IK_ConjugateGradientSolver( +){ + //nothing to do +}; + +// Compute the minimum of the potenial function +// starting at point p. On return p contains the +// computed minima, iter the number of iterations performed, +// fret the potenial value at the minima + + void +IK_ConjugateGradientSolver:: +Solve( + TNT::Vector<MT_Scalar> &p, + MT_Scalar ftol, + int &iter, + MT_Scalar &fret, + DifferentiablePotenialFunctionNd &potenial, + int max_its +){ + + int j; + MT_Scalar gg,gam,fp,dgg; + + int n = potenial.Dimension(); + ArmVectors(n); + + // initialize --- FIXME we probably have these + // values to hand already. + + fp = potenial.Evaluate(p); + potenial.Derivative(p,m_xi); + + for (j = 1; j<=n; j++) { + m_g(j) = -m_xi(j); + m_xi(j) = m_h(j) = m_g(j); + } + for (iter =1;iter <= max_its; iter++) { + LineMinimize(p,m_xi,fret,potenial); + + if (2 *TNT::abs(fret-fp) <= ftol*(TNT::abs(fret) + TNT::abs(fp) + EPS)) { + return; + } + fp = fret; + potenial.Derivative(p,m_xi); + dgg = gg = 0.0; + + for (j = 1; j<=n;j++) { + gg += m_g(j)*m_g(j); + //dgg+= xi(j)*xi(j); + dgg += (m_xi(j) + m_g(j))*m_xi(j); + } + if (gg == 0.0) { + return; + } + gam = dgg/gg; + + for (j = 1; j<=n; j++) { + m_g(j) = -m_xi(j); + m_xi(j) = m_h(j) = m_g(j) + gam*m_h(j); + } + } + // FIXME throw exception + //assert(false); +}; + + +IK_ConjugateGradientSolver:: +IK_ConjugateGradientSolver( +){ + //nothing to do +} + + void +IK_ConjugateGradientSolver:: +ArmVectors( + int dimension +){ + m_g.newsize(dimension); + m_h.newsize(dimension); + m_xi.newsize(dimension); + m_xi_temp.newsize(dimension); +}; + + void +IK_ConjugateGradientSolver:: +LineMinimize( + TNT::Vector<MT_Scalar> & p, + const TNT::Vector<MT_Scalar> & xi, + MT_Scalar &fret, + DifferentiablePotenialFunctionNd &potenial +){ + MT_Scalar ax(0),bx(1); // initial bracket guess + MT_Scalar cx,fa,fb,fc; + + MT_Scalar xmin(0); // the 1d function minima + + potenial.SetLineVector(p,xi); + IK_LineMinimizer::InitialBracket1d(ax,bx,cx,fa,fb,fc,potenial); + fret = IK_LineMinimizer::DerivativeBrent1d(ax,bx,cx,potenial,xmin,0.001); + + // x_min in minimum along line and corresponds with + // p[] + x_min *xi[] + + TNT::vectorscale(m_xi_temp,xi,xmin); + TNT::vectoradd(p,m_xi_temp); +}; + + +#undef EPS + + + + + + + + |