Subversion Repositories gelsvn

#include <iostream>

#include "QEM.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

using namespace CGLA;
using namespace std;

namespace Geometry
{
        Vec3f QEM::opt_pos(double sv_thresh) const
        {
                gsl_matrix* A_gsl = gsl_matrix_alloc(3,3);
                gsl_vector *b_gsl = gsl_vector_alloc (3);
                gsl_matrix *V = gsl_matrix_alloc(3,3);
                gsl_vector *S = gsl_vector_alloc (3);
                gsl_vector *work = gsl_vector_alloc (3);
                gsl_vector *x = gsl_vector_alloc (3);
                                                                
                for(int i=0;i<3;++i)
                        {
                                for(int j=0;j<3;++j)
                                        gsl_matrix_set(A_gsl, i, j, A[i][j]);
                                gsl_vector_set(b_gsl,i,b[i]);
                        }
                gsl_linalg_SV_decomp(A_gsl, V, S, work);                                
                float sm = sv_thresh*gsl_vector_get(S,0);
                for(int i=1;i<3;++i)
                        {
                                if(fabs(gsl_vector_get(S,i))< sm)
                                        gsl_vector_set(S,i,0.0);
                        }
                gsl_linalg_SV_solve(A_gsl, V, S, b_gsl, x);
        
                Vec3f p(-gsl_vector_get(x,0),-gsl_vector_get(x,1),-gsl_vector_get(x,2));

                gsl_matrix_free(A_gsl);
                gsl_matrix_free(V);
                gsl_vector_free(b_gsl);
                gsl_vector_free(S);
                gsl_vector_free(work);
                gsl_vector_free(x);

                return p;
        }
}