/Users/jab/Documents/Teaching/02585/GEL2_and_demos/GEL/src/Geometry/QEM.h

#ifndef __GEOMETRY_QEM_H
#define __GEOMETRY_QEM_H

#include <cfloat>
#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), b(0), c(0) {}
                        
                        QEM(const CGLA::Vec3d& p0, const CGLA::Vec3d& n0, double w=1.0f):
                                A(direct_product(n0,n0) * w), 
                                b(-2*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::Vec3d& p) const
                                {
                                        return dot(p,A*p) + dot(b,p)+ c;
                                }

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

namespace GEO = Geometry;

#endif