svd_eigen_HH.hpp
Go to the documentation of this file.00001 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 00021 00022 00023 //Based on the svd of the KDL-0.2 library by Erwin Aertbelien 00024 #ifndef SVD_EIGEN_HH_HPP 00025 #define SVD_EIGEN_HH_HPP 00026 00027 00028 #include <Eigen/Core> 00029 #include <algorithm> 00030 00031 using namespace Eigen; 00032 00033 namespace KDL 00034 { 00035 inline double PYTHAG(double a,double b) { 00036 double at,bt,ct; 00037 at = fabs(a); 00038 bt = fabs(b); 00039 if (at > bt ) { 00040 ct=bt/at; 00041 return at*sqrt(1.0+ct*ct); 00042 } else { 00043 if (bt==0) 00044 return 0.0; 00045 else { 00046 ct=at/bt; 00047 return bt*sqrt(1.0+ct*ct); 00048 } 00049 } 00050 } 00051 00052 00053 inline double SIGN(double a,double b) { 00054 return ((b) >= 0.0 ? fabs(a) : -fabs(a)); 00055 } 00056 00069 int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300); 00070 } 00071 #endif