Subversion Repositories gelsvn

Rev

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

Rev 601 Rev 630
1 
/* ----------------------------------------------------------------------- *
 1 
/* ----------------------------------------------------------------------- *
2 
 * This file is part of GEL, http://www.imm.dtu.dk/GEL
 2 
 * This file is part of GEL, http://www.imm.dtu.dk/GEL
3 
 * Copyright (C) the authors and DTU Informatics
 3 
 * Copyright (C) the authors and DTU Informatics
4 
 * For license and list of authors, see ../../doc/intro.pdf
 4 
 * For license and list of authors, see ../../doc/intro.pdf
5 
 * ----------------------------------------------------------------------- */
 5 
 * ----------------------------------------------------------------------- */
6 
 
 6 
 
7 
#include <iostream>
 7 
#include <iostream>
8 
 
 8 
 
9 
#include "QEM.h"
 9 
#include "QEM.h"
10 
#include "../LinAlg/LapackFunc.h"
 10 
#include "../LinAlg/LapackFunc.h"
11 
 
 11 
 
12 
using namespace LinAlg;
 12 
using namespace LinAlg;
13 
using namespace CGLA;
 13 
using namespace CGLA;
14 
using namespace std;
 14 
using namespace std;
15 
 
 15 
 
16 
namespace Geometry
 16 
namespace Geometry
17 
{
 17 
{
18 
	Vec3d QEM::opt_pos(double sv_thresh, const CGLA::Vec3d& p0) const
 18 
	Vec3d QEM::opt_pos(double sv_thresh, const CGLA::Vec3d& p0) const
19 
	{
 19 
	{
20 
        CMatrix U,S,V;
 20 
        CMatrix U,S,V;
21 
        SVD(A,U,S,V);
 21 
        SVD(A,U,S,V);
22 
        CMatrix Sp(3,3,0.0);
 22 
        CMatrix Sp(3,3,0.0);
23
23
24 
        double s00 = S.get(0,0);
 24 
        double s00 = S.get(0,0);
25 
        double limit = sv_thresh * s00;
 25 
        double limit = sv_thresh * s00;
26
26
27 
        Sp.set(0,0,1.0/s00);
 27 
        Sp.set(0,0,1.0/s00);
28 
        Vec3d diff(0.0);
 28 
        Vec3d diff(0.0);
29 
        for(int i=1;i<3;++i)
 29 
        for(int i=1;i<3;++i)
30 
        {
 30 
        {
31 
            double sii = S.get(i,i);
 31 
            double sii = S.get(i,i);
32 
            if(sii < limit)
 32 
            if(sii < limit)
33 
            {
 33 
            {
34 
                Sp.set(i,i,0.0);
 34 
                Sp.set(i,i,0.0);
35 
                Vec3d vi(V.get(0,i), V.get(1,i), V.get(2,i));
 35 
                Vec3d vi(V.get(0,i), V.get(1,i), V.get(2,i));
36 
                diff += vi*dot(vi, p0);
 36 
                diff += vi*dot(vi, p0);
37 
            }
 37 
            }
38 
            else
 38 
            else
39 
                Sp.set(i,i,1.0/sii);
 39 
                Sp.set(i,i,1.0/sii);
40 
        }
 40 
        }
41
41
42 
        CMatrix Ap = V * Sp * U.Transposed();
 42 
        CMatrix Ap = V * Sp * U.Transposed();
43
43
44 
        CVector x(3);
 44 
        CVector x(3);
45 
        x = Ap * CVector(b*0.5);
 45 
        x = Ap * CVector(b*0.5);
46
46
47 
        return diff-Vec3d(x[0],x[1],x[2]);
 47 
        return diff-Vec3d(x[0],x[1],x[2]);
48 
	}
 48 
	}
49
49
50 
}
 50 
}
51 
 
 51