Subversion Repositories gelsvn

#ifndef __QEM_H
#define __QEM_H

#include <cfloat>
#include "CGLA/Vec3f.h"
#include "CGLA/Vec3d.h"
#include "CGLA/Mat3x3d.h"

namespace
{
        inline const CGLA::Mat3x3d direct_product(const CGLA::Vec3d& v0, const CGLA::Vec3d& v1)
                {
                        CGLA::Mat3x3d m;
                        for(int i=0;i<3;++i)
                                for(int j=0;j<3;++j)
                                        m[i][j] = v0[i]*v1[j];
                        return m;
                }
}

namespace Geometry
{
        class QEM
                {
                        CGLA::Mat3x3d A;
                        CGLA::Vec3d   b;
                        double   c;
                public:
                        
                        QEM(): A(0), c(0), b(0) {}
                        
                        QEM(const CGLA::Vec3d& p0, const CGLA::Vec3d& n0, double w=1.0f):
                                A(direct_product(n0,n0) * w), 
                                b(-n0*dot(n0,p0) * w), 
                                c(dot(p0,n0)*dot(p0,n0) * w) {}

                        
                        void operator+=(const QEM& q)
                                {
                                        A += q.A;
                                        b += q.b;
                                        c += q.c;
                                }
                        
                        float error(const CGLA::Vec3f& _p) const
                                {
                                        CGLA::Vec3d p(_p);
                                        return dot(p,A*p) + 2*dot(b,p)+ c;
                                }

                        double determinant() const
                                {
                                        return CGLA::determinant(A);
                                }
                        
                        const CGLA::Vec3f grad(const CGLA::Vec3f& _p) const
                                {
                                        CGLA::Vec3d p(_p);
                                        CGLA::Vec3f g(2*A*p+2*b);
                                        return g;
                                }
                        
                        CGLA::Vec3f opt_pos(double QEM_thresh = 0.005) const;
                        
                };
}

namespace GEO = Geometry;

#endif