qem.h

#pragma once
 
#include "vec.h"
#include "mat.h"
#include "inv.h"
 
namespace cgv {
        namespace math { 
 
 
template <typename T>
class qem : public vec<T>
{
public:
        qem(int d = -1) : vec<T>((d+1)*(d+2)/2)
        {
                if (d>=0)
                        this->zeros();
        }
        qem(const vec<T>& p, const vec<T>& n) : vec<T>((p.size()+1)*(p.size()+2)/2)
        {
                set(n, -dot(p,n));
        }
        qem(const vec<T>& n, T d) : vec<T>((n.size()+1)*(n.size()+2)/2)
        {
                set(n,d);
        }
        void set(const vec<T>& n, T d)
        {
                this->first() = d*d;
                unsigned int i,j,k=n.size()+1;
                for (i=0;i<n.size(); ++i) {
                        (*this)(i+1) = d*n(i);
                        for (j=i;j<n.size(); ++j, ++k)
                                (*this)(k) = n(i)*n(j);
                }
        }
        unsigned dim() const
        {
                return (unsigned int)sqrt(2.0*this->size())-1;
        }
        qem<T>& operator = (const qem<T>& v) 
        { 
                *static_cast<vec<T>*>(this) = v;
                return *this; 
        }
        const T& scalar_part() const
        {
                return this->first();
        }
        vec<T> vector_part() const
        {
                vec<T> v(dim());
                for (unsigned int i=0; i<v.size(); ++i)
                        v(i) = (*this)(i+1);
                return v;
        }
        mat<T> matrix_part() const
        {
                unsigned int d = dim();
                mat<T> m(d,d);
                unsigned int i,j,k=d+1;
                for (i=0; i<d; ++i)
                        for (j=i; j<d; ++j,++k)
                                m(i,j) = m(j,i) = (*this)(k);
                return m;
        }
        T evaluate(const vec<T>& p) const
        {
                return dot(matrix_part()*p+2.0*vector_part(),p)+scalar_part();
        }
        static bool inside(const vec<T>& p, const vec<T>& minp, const vec<T>& maxp)
        {
                for (unsigned int i=0; i<p.size(); ++i) {
                        if (p(i) < minp(i))
                                return false;
                        if (p(i) > maxp(i))
                                return false;
                }
                return true;
        }
        vec<T> minarg(const vec<T>& p_ref, T relative_epsilon, T max_distance = -1, T epsilon = 1e-10) const
        {
                unsigned int d = p_ref.size();
                assert(d == dim());
                T max_distance2 = max_distance*max_distance;
                mat<T> U,V,A = matrix_part();
                diag_mat<T> W,iW;
                svd(A,U,W,V);
                U.transpose();
                iW = inv(W);
                vec<T> y_solve = -(inv(W)*(U*vector_part()));
                vec<T> y_ref   = transpose(V)*p_ref;
                vec<T> y(3);
                for (unsigned int i = d; i > 0; ) {
                        --i;
                        if (fabs(W(i)) > epsilon && fabs(W(i)*iW(0)) > relative_epsilon) {
                                unsigned int j;
                                for (j = 0; j <= i; ++j)
                                        y(j) = y_solve(j);
                                for (; j < d; ++j)
                                        y(j) = y_ref(j);
                                vec<T> p = V*y;
                                if (max_distance != -1 && (p-p_ref).sqr_length() <= max_distance2)
                                        return p;
                        }
                }
                return p_ref;
        }
        template <typename S> 
        qem<T>& operator += (const qem<S>& v) 
        { 
                assert(this->size() == v.size());
                for (unsigned i=0;i<this->size();++i) (*this)(i) += v(i); return *this;
        }
 
        template <typename S> 
        qem<T>& operator -= (const qem<S>& v) 
        {
                assert(this->size() == v.size());
                for (unsigned i=0;i<this->size();++i) (*this)(i) -= v(i); return *this;
        }
        
        template <typename S> 
        const qem<T>  operator +  (const qem<S>& v) const 
        { 
                vec<T> r = *this; r += v; return r; 
        }
 
        template <typename S> 
        qem<T>  operator -  (const qem<S>& v) const 
        {
                 qem<T> r = *this; r -= v; return r; 
        }
 
        qem<T>  operator-(void) const 
        {
                qem<T> r=(*this);
                r=(T)(-1)*r;
                return r; 
        }
 
        qem<T>  operator * (const T& s) const 
        {
                 qem<T> r = *this; r *= s; return r; 
        }
 
        
        qem<T>  operator / (const T& s)const 
        { 
                qem<T> r = *this;
                r /= s;
                return r; 
        }
        
        void resize(unsigned d)
        {
                vec<T>::resize((d+1)*(d+2)/2);
        }
 
        template <typename S> 
        bool operator == (const qem<S>& v) const
        { 
                for (unsigned i=0;i<this->size();++i)
                        if((*this)(i) != (T)v(i)) return false;
                return true; 
        }
 
        template <typename S> 
        bool operator != (const qem<S>& v) const
        { 
                for (unsigned i=0;i<this->size();++i)
                        if((*this)(i) != (T)v(i)) return true;
                return false; 
        }
 
};
 
template <typename T>
const qem<T> operator * (const T& s, const qem<T>& v) 
{
        qem<T> r = v; r *= s; return r; 
}
 
        }
}