Subversion Repositories gelsvn

Rev

Rev 23 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 23 Rev 31
Line 1... Line 1...
1 
#include <iostream>
 1 
#include <iostream>
2 
 
 2 
 
3 
#include "QEM.h"
 3 
#include "QEM.h"
- 
 
 4 
#include "LinAlg/LapackFunc.h"
4 
 
 5 
 
5 
#include <gsl/gsl_matrix.h>
 - 
 
6 
#include <gsl/gsl_linalg.h>
 6 
using namespace LinAlg;
7 
 
 - 
 
8 
using namespace CGLA;
 7 
using namespace CGLA;
9 
using namespace std;
 8 
using namespace std;
10 
 
 9 
 
11 
namespace Geometry
 10 
namespace Geometry
12 
{
 11 
{
13 
	Vec3f QEM::opt_pos(double sv_thresh) const
 12 
	Vec3f QEM::opt_pos(double sv_thresh) const
14 
	{
 13 
	{
15 
		gsl_matrix* A_gsl = gsl_matrix_alloc(3,3);
 14 
		CMatrix A_num(A);
16 
		gsl_vector *b_gsl = gsl_vector_alloc (3);
 15 
		CVector b_num(b);
- 
 
 16 
 
- 
 
 17 
		CMatrix U;
17 
		gsl_matrix *V = gsl_matrix_alloc(3,3);
 18 
		CMatrix V;
18 
		gsl_vector *S = gsl_vector_alloc (3);
 19 
		CVector s;
19 
		gsl_vector *work = gsl_vector_alloc (3);
 20 
		SVD(A_num,U,s,V);
- 
 
 21 
 
20 
		gsl_vector *x = gsl_vector_alloc (3);
 22 
// 		double sm = sv_thresh*s[0];
21
- 
 
22 
		for(int i=0;i<3;++i)
 23 
// 		for(int i=1;i<3;++i)
23 
			{
 24 
// 			{
24 
				for(int j=0;j<3;++j)
 25 
// 				if(fabs(s[i])< sm)
25 
					gsl_matrix_set(A_gsl, i, j, A[i][j]);
 26 
// 					s[i] = 0;
26 
				gsl_vector_set(b_gsl,i,b[i]);
 27 
// 			}
27 
			}
 28
28 
		gsl_linalg_SV_decomp(A_gsl, V, S, work);
29 
		CMatrix A_inv = (V * s) * U;
- 
 
 30 
 
- 
 
 31 
		CVector x(3);
- 
 
 32 
		x = A_inv * b_num;
29 
		float sm = sv_thresh*gsl_vector_get(S,0);
 33 
		Vec3f p(-x[0], -x[1], -x[2]);
30 
		for(int i=1;i<3;++i)
 34 
		if(s[0] == 0)
31 
			{
 35 
			{
32 
				if(fabs(gsl_vector_get(S,i))< sm)
 36 
				cout << A << b << s << U << V << endl;
33 
					gsl_vector_set(S,i,0.0);
 - 
 
34 
			}
 37 
			}
35 
		gsl_linalg_SV_solve(A_gsl, V, S, b_gsl, x);
 - 
 
36
38
37 
		Vec3f p(-gsl_vector_get(x,0),-gsl_vector_get(x,1),-gsl_vector_get(x,2));
 - 
 
38 
 
 - 
 
39 
		gsl_matrix_free(A_gsl);
 - 
 
40 
		gsl_matrix_free(V);
 - 
 
41 
		gsl_vector_free(b_gsl);
 - 
 
42 
		gsl_vector_free(S);
 - 
 
43 
		gsl_vector_free(work);
 - 
 
44 
		gsl_vector_free(x);
 - 
 
45 
 
 39 
 
46 
		return p;
 40 
		return p;
47 
	}
 41 
	}
48 
}
 42 
}