Subversion Repositories gelsvn

Rev

Rev 357 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 357 Rev 393
1 
#include <iostream>
 1 
#include <iostream>
2 
 
 2 
 
3 
#include "QEM.h"
 3 
#include "QEM.h"
4 
#include "LinAlg/LapackFunc.h"
 4 
#include "LinAlg/LapackFunc.h"
5 
 
 5 
 
6 
using namespace LinAlg;
 6 
using namespace LinAlg;
7 
using namespace CGLA;
 7 
using namespace CGLA;
8 
using namespace std;
 8 
using namespace std;
9 
 
 9 
 
10 
namespace Geometry
 10 
namespace Geometry
11 
{
 11 
{
12 
	Vec3d QEM::opt_pos(double sv_thresh, const CGLA::Vec3d& p0) const
 12 
	Vec3d QEM::opt_pos(double sv_thresh, const CGLA::Vec3d& p0) const
13 
	{
 13 
	{
14 
			CMatrix U,S,V;
 14 
			CMatrix U,S,V;
15 
			SVD(A,U,S,V);
 15 
			SVD(A,U,S,V);
16 
			CMatrix Sp(3,3,0.0);
 16 
			CMatrix Sp(3,3,0.0);
17
17
18 
			double s00 = S.get(0,0);
 18 
			double s00 = S.get(0,0);
19 
			double limit = sv_thresh * s00;
 19 
			double limit = sv_thresh * s00;
20 
 
 20 
 
21 
			Sp.set(0,0,1.0/s00);
 21 
			Sp.set(0,0,1.0/s00);
22 
			Vec3d diff(0.0);
 22 
			Vec3d diff(0.0);
23 
			for(int i=1;i<3;++i)
 23 
			for(int i=1;i<3;++i)
24 
			{
 24 
			{
25 
					double sii = S.get(i,i);
 25 
					double sii = S.get(i,i);
26 
					if(sii < limit)
 26 
					if(sii < limit)
27 
					{
 27 
					{
28 
						Sp.set(i,i,0.0);
 28 
						Sp.set(i,i,0.0);
29 
						Vec3d vi(V.get(0,i), V.get(1,i), V.get(2,i));
 29 
						Vec3d vi(V.get(0,i), V.get(1,i), V.get(2,i));
30 
						diff += vi*dot(vi, p0);
 30 
						diff += vi*dot(vi, p0);
31 
					}
 31 
					}
32 
					else
 32 
					else
33 
						Sp.set(i,i,1.0/sii);
 33 
						Sp.set(i,i,1.0/sii);
34 
			}
 34 
			}
35 
 
 35 
 
36 
			CMatrix Ap = V * Sp * U.Transposed();
 36 
			CMatrix Ap = V * Sp * U.Transposed();
37 
 
 37 
 
38 
			CVector x(3);
 38 
			CVector x(3);
39 
			x = Ap * (b*0.5);
 39 
			x = Ap * (b*0.5);
40
40
41 
			return diff-Vec3d(x[0],x[1],x[2]);
 41 
			return diff-Vec3d(x[0],x[1],x[2]);
42 
	}
 42 
	}
43 
 
 43 
 
44 
}
 44 
}
45 
 
 45