Subversion Repositories gelsvn

Rev

Rev 230 | Rev 357 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 230 Rev 315
Line 9... Line 9...
9 
 
 9 
 
10 
namespace Geometry
 10 
namespace Geometry
11 
{
 11 
{
12 
	Vec3d QEM::opt_pos(double sv_thresh) const
 12 
	Vec3d QEM::opt_pos(double sv_thresh) const
13 
	{
 13 
	{
14 
			CMatrix A_num(A);
 14 
			CMatrix U,S,V;
- 
 
 15 
			SVD(A,U,S,V);
15 
			CVector b_num(b);
 16 
			CMatrix Sp(3,3,0.0);
- 
 
 17
- 
 
 18 
			double s00 = S.get(0,0);
- 
 
 19 
			double limit = sv_thresh * s00;
- 
 
 20 
 
- 
 
 21 
			Sp.set(0,0,1.0/s00);
- 
 
 22 
			for(int i=1;i<3;++i)
- 
 
 23 
			{
- 
 
 24 
					double sii = S.get(i,i);
- 
 
 25 
					if(sii < limit)
- 
 
 26 
							Sp.set(i,i,0.0);
- 
 
 27 
					else
- 
 
 28 
							Sp.set(i,i,1.0/sii);
- 
 
 29 
			}
- 
 
 30 
 
- 
 
 31 
			CMatrix Ap = V * Sp * U.Transposed();
- 
 
 32 
 
16 
			CVector x(3);
 33 
			CVector x(3);
17 
			x = LinearLSSolve(A,b);
 34 
			x = Ap * (b*0.5);
18 
			/* Actually LSSolve uses SVD, but we cannot set a cut off value for
 - 
 
19 
				 the singular values. Might be worth fixing */
 - 
 
- 
 
 35
20 
			Vec3d p(-x[0], -x[1], -x[2]);
 36 
			Vec3d p(-x[0], -x[1], -x[2]);
21 
			return p;
 37 
			return p;
22 
	}
 38 
	}
23 
 
 39 
 
24 
}
 40 
}