Subversion Repositories gelsvn

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
23 jab 1 
#include <iostream>
2 
 
3 
#include "QEM.h"
4 
 
5 
#include <gsl/gsl_matrix.h>
6 
#include <gsl/gsl_linalg.h>
7 
 
8 
using namespace CGLA;
9 
using namespace std;
10 
 
11 
namespace Geometry
12 
{
13 
	Vec3f QEM::opt_pos(double sv_thresh) const
14 
	{
15 
		gsl_matrix* A_gsl = gsl_matrix_alloc(3,3);
16 
		gsl_vector *b_gsl = gsl_vector_alloc (3);
17 
		gsl_matrix *V = gsl_matrix_alloc(3,3);
18 
		gsl_vector *S = gsl_vector_alloc (3);
19 
		gsl_vector *work = gsl_vector_alloc (3);
20 
		gsl_vector *x = gsl_vector_alloc (3);
21 
 
22 
		for(int i=0;i<3;++i)
23 
			{
24 
				for(int j=0;j<3;++j)
25 
					gsl_matrix_set(A_gsl, i, j, A[i][j]);
26 
				gsl_vector_set(b_gsl,i,b[i]);
27 
			}
28 
		gsl_linalg_SV_decomp(A_gsl, V, S, work);
29 
		float sm = sv_thresh*gsl_vector_get(S,0);
30 
		for(int i=1;i<3;++i)
31 
			{
32 
				if(fabs(gsl_vector_get(S,i))< sm)
33 
					gsl_vector_set(S,i,0.0);
34 
			}
35 
		gsl_linalg_SV_solve(A_gsl, V, S, b_gsl, x);
36 
 
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 
 
46 
		return p;
47 
	}
48 
}