Subversion Repositories gelsvn

#include <iostream>

#include "QEM.h"

#include "LinAlg/LapackFunc.h"

using namespace LinAlg;

using namespace CGLA;

using namespace std;

namespace Geometry

{

	Vec3d QEM::opt_pos(double sv_thresh) const

	{

			CMatrix U,S,V;

			SVD(A,U,S,V);

			CMatrix Sp(3,3,0.0);

			double s00 = S.get(0,0);

			double limit = sv_thresh * s00;

			Sp.set(0,0,1.0/s00);

			for(int i=1;i<3;++i)

			{

					double sii = S.get(i,i);

					if(sii < limit)

							Sp.set(i,i,0.0);

					else

							Sp.set(i,i,1.0/sii);

			}

			CMatrix Ap = V * Sp * U.Transposed();

			CVector x(3);

			x = Ap * (b*0.5);

			Vec3d p(-x[0], -x[1], -x[2]);

			return p;

	}

}