Subversion Repositories gelsvn

#include <iostream>

#include "QEM.h"
#include "LinAlg/LapackFunc.h"

using namespace LinAlg;
using namespace CGLA;
using namespace std;

namespace Geometry
{
        Vec3f QEM::opt_pos(double sv_thresh) const
        {
                CMatrix A_num(A);
                CVector b_num(b);

                CMatrix U;
                CMatrix V;
                CVector s;
                SVD(A_num,U,s,V);

//              double sm = sv_thresh*s[0];
//              for(int i=1;i<3;++i)
//                      {
//                              if(fabs(s[i])< sm)
//                                      s[i] = 0;
//                      }
                
                CMatrix A_inv = (V * s) * U;

                CVector x(3);
                x = A_inv * b_num;
                Vec3f p(-x[0], -x[1], -x[2]);
                if(s[0] == 0)
                        {
                                cout << A << b << s << U << V << endl;
                        }
                         

                return p;
        }
}