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; 
}
 
    }
}