Subversion Repositories gelsvn

Rev

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

Rev 214 Rev 220
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 
#include "LinAlg/LapackFunc.h"
- 
 
 5 
#include <gsl/gsl_matrix.h>
- 
 
 6 
#include <gsl/gsl_linalg.h>
5 
 
 7 
 
6 
using namespace LinAlg;
 8 
using namespace LinAlg;
7 
using namespace CGLA;
 9 
using namespace CGLA;
8 
using namespace std;
 10 
using namespace std;
9 
 
 11 
 
10 
namespace Geometry
 12 
namespace Geometry
11 
{
 13 
{
12 
	Vec3d QEM::opt_pos(double sv_thresh) const
 14 
	Vec3d QEM::opt_pos(double sv_thresh) const
13 
	{
 15 
	{
- 
 
 16 
			// FIX ME - NOT CLEAR WHAT HAPPENS IN UNDERDETERMINDED CASE.
14 
		CMatrix A_num(A);
 17 
			CMatrix A_num(A);
15 
		CVector b_num(b);
 18 
			CVector b_num(b);
16 
 
 - 
 
17 
		CMatrix U;
 - 
 
18 
		CMatrix V;
 - 
 
19 
		CVector s;
 - 
 
20 
		SVD(A_num,U,s,V);
 - 
 
21 
 
 - 
 
22 
// 		double sm = sv_thresh*s[0];
 - 
 
23 
// 		for(int i=1;i<3;++i)
 - 
 
24 
// 			{
 - 
 
25 
// 				if(fabs(s[i])< sm)
 - 
 
26 
// 					s[i] = 0;
 - 
 
27 
// 			}
 - 
 
28
- 
 
29 
		CMatrix A_inv = (V * s) * U;
 - 
 
30 
 
 - 
 
31 
		CVector x(3);
 19 
			CVector x(3);
32 
		x = A_inv * b_num;
 20 
			x = LinearLSSolve(A,b);
33 
		Vec3d p(-x[0], -x[1], -x[2]);
 21 
			Vec3d p(-x[0], -x[1], -x[2]);
34 
		if(s[0] == 0)
 - 
 
35 
			{
 - 
 
36 
				cout << A << b << s << U << V << endl;
 - 
 
37 
			}
 - 
 
38 
		return p;
 22 
			return p;
39 
	}
 23 
	}
- 
 
 24 
 
40 
}
 25 
}