Subversion Repositories gelsvn

Rev

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

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