vec.h

#pragma once
 
#include <iostream>
#include <cmath>
#include <cassert>
#include <algorithm>
#include <functional>
#include <iterator>
#include <limits>
#include <string.h>
#include <vector>
 
#ifdef max
#undef max
#endif
 
#ifdef min
#undef min
#endif
 
namespace cgv {
        namespace math { 
 
 
template <typename T>
class vec
{       
 
protected:
        T *_data;
        unsigned _size;
        bool data_is_external;
public:
        typedef T              value_type;
        typedef T&             reference;
    typedef const T&       const_reference;
    typedef std::size_t    size_type;
        typedef std::ptrdiff_t difference_type;
        typedef T*             pointer;
        typedef const T*       const_pointer;
 
    typedef T*             iterator;
    typedef const T*       const_iterator;
    
   
        typedef std::reverse_iterator<iterator> reverse_iterator;
        typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
 
        iterator begin()
        {
                return _data;
        }
 
        iterator end()
        {
                return _data+_size;
        }
 
        const_iterator begin() const
        {
                return _data;
        }
 
        const_iterator end() const
        {
                return _data+_size;
        }
 
        reverse_iterator rbegin()
        {
                return reverse_iterator(end());
        }
 
        reverse_iterator rend()
        {
                return reverse_iterator(begin());
        }
 
        const_reverse_iterator rbegin() const
        {
                return const_reverse_iterator(end());
        }
 
        const_reverse_iterator rend() const
        {
                return const_reverse_iterator(begin());
        }
 
 
 
 
        unsigned size() const
        {
                return _size;
        }
 
        unsigned dim() const
        {
                return _size;
        }
        
        vec()
        {
                _data = NULL;
                _size = 0;
                data_is_external = false;
        }
 
        explicit vec(unsigned dim, const T& value = T(0))
        {
                _size=dim;
                if (dim > 0) {
                        _data = new T[_size];
                        std::fill(_data, _data + _size, value);
                }
                else
                        _data = NULL;
                data_is_external = false;
        }
 
 
        vec(unsigned dim, const T* marray)
        {
                _size = dim;
                if(dim > 0)
                {               
                        _data = new T[_size];
                        memcpy(_data,marray,_size*sizeof(T));
                }else
                        _data = NULL;
                data_is_external = false;
        }
 
        vec(const vec<T>&v)
        {
                _size = v.size();
                if (v._data) {
                        _data = new T[v.size()];
                        memcpy(_data,v._data,_size*sizeof(T));
                }
                else
                        _data=NULL;
                data_is_external = false;
        }
 
        template <typename S>
        vec(const vec<S>& v)
        {
                _size = v.size();
                if(v.size() > 0) {
                        _data = new T[v.size()];
                        for(unsigned i = 0; i < _size;i++)
                                _data [i]=(T)v(i);
                }
                else
                        _data =NULL;
                data_is_external = false;
        }
 
        vec(const T& c0, const T& c1)
        {
                _size = 2;
                _data = new T[2];
                _data[0] = c0; 
                _data[1] = c1;  
                data_is_external = false;
        }
 
        
        vec(const T& c0, const T& c1, const T& c2)
        {
                _size = 3;
                _data = new T[3];
                _data[0] = c0; 
                _data[1] = c1; 
                _data[2] = c2;
                data_is_external = false;
        }
 
        vec(const T& c0, const T& c1, const T& c2, const T& c3)
        {
                _size = 4;
                _data = new T[4];
                _data[0] = c0; 
                _data[1] = c1; 
                _data[2] = c2;
                _data[3] = c3;
                data_is_external = false;
        }
 
        void set(const T& c0, const T& c1) 
        {
                assert(_size == 2);
                _data[0] = c0; 
                _data[1] = c1; 
        }
 
        void set(const T& c0, const T& c1, const T& c2) 
        {
                assert(_size == 3);
                _data[0] = c0; 
                _data[1] = c1; 
                _data[2] = c2;
        }
 
        void set(const T& c0, const T& c1, const T& c2, const T& c3) 
        {
                assert(_size == 4);
                _data[0] = c0; 
                _data[1] = c1; 
                _data[2] = c2;
                _data[3] = c3;
        }
        void set_extern_data(unsigned dim, T* data)
        {
                destruct();
                _size = dim;
                _data = data;
                data_is_external = true;
        }
        virtual ~vec()
        {
                destruct();
        }
 
        void destruct()
        {
                if(_data && !data_is_external)
                {
                        delete[] _data;
                        _data=NULL;
                        _size=0;
                }
        }
 
        operator T*()
        {
                return _data;
        }
 
        operator const T*() const
        {
                return _data;
        }
 
        vec<T>& operator = (const vec<T>& v) 
        { 
                if(v.size() == 0)
                        destruct();
                else {
                        if (size() != v.size())
                                resize(v.size());
                        memcpy(_data,v._data,_size*sizeof(T));          
                }
                return *this; 
        }
 
        vec<T>& operator = (const T& s) 
        { 
                fill(s);
                return *this; 
        }
 
        template <typename S> 
        vec<T>& operator = (const vec<S>& v) 
        { 
                resize(v.size());
                for (unsigned i=0;i<_size;++i) _data[i]=(T)v(i); 
                return *this; 
        }
 
        T& operator () (unsigned i)  
        {
                assert(i < _size);
                return _data[i]; 
        }
 
        const T& operator () (unsigned i) const 
        {
                assert(i < _size);
                return _data[i]; 
        }
 
        T& first()   
        {
                assert( _size > 0);
                return _data[0]; 
        }
 
        const T& first()  const 
        {
                assert( _size > 0);
                return _data[0]; 
        }
 
 
 
        T& last()   
        {
                assert( _size > 0);
                return _data[_size-1]; 
        }
 
        const T& last()  const 
        {
                assert( _size > 0);
                return _data[_size-1]; 
        }
        
        T& x()   
        {
                assert( _size > 0);
                return _data[0]; 
        }
 
        const T& x()  const 
        {
                assert( _size > 0);
                return _data[0]; 
        }
 
        T& y()   
        {
                assert( _size > 1);
                return _data[1]; 
        }
 
        const T& y()  const 
        {
                assert( _size > 1);
                return _data[1]; 
        }
 
        T& z()   
        {
                assert( _size > 2);
                return _data[2]; 
        }
 
        const T& z()  const 
        {
                assert( _size > 2);
                return _data[2]; 
        }
 
        T& w()   
        {
                assert( _size > 3);
                return _data[3]; 
        }
 
        const T& w()  const 
        {
                assert( _size > 3);
                return _data[3]; 
        }
 
        vec<T>& operator += (const T& s)                        
        {
                 for (unsigned i=0;i<_size;++i) 
                        _data[i] += s; 
                return *this; 
        }
 
        vec<T>& operator -= (const T& s)
        {
                for (unsigned i=0;i<_size; ++i) 
                        _data[i] -= s; 
                return *this; 
        }
 
        vec<T>& operator *= (const T& s)
        { 
                for (unsigned i=0;i<_size;++i) _data[i] *= s; return *this; 
        }
 
        vec<T>& operator /= (const T& s)                        
        {               
                for (unsigned i=0;i<_size;++i) _data[i] /= s; return *this; 
        }
 
        template <typename S> 
        vec<T>& operator += (const vec<S>& v) 
        { 
                assert(_size == v._size);
                for (unsigned i=0;i<_size;++i) _data[i] += v(i); return *this; 
        }
 
        template <typename S> 
        vec<T>& operator -= (const vec<S>& v) 
        {
                assert(_size == v._size);
                for (unsigned i=0;i<_size;++i) _data[i] -= v(i); return *this; 
        }
 
        template <typename S> 
        vec<T>& operator *= (const vec<S>& v) 
        {
                assert(_size == v._size);
                 for (unsigned i=0;i<_size;++i) _data[i] *= v(i); return *this; 
        }
 
        template <typename S> 
        vec<T>& operator /= (const vec<S>& v) 
        {
                assert(_size == v._size);
                 for (unsigned i=0;i<_size;++i) _data[i] /= v(i); return *this; 
        }
        
        template <typename S> 
        const vec<T>  operator +  (const vec<S>& v) const 
        { 
                vec<T> r = *this; r += v; return r; 
        }
 
        const vec<T>  operator +  (const T& s) const 
        { 
                vec<T> r = *this; r += s; return r; 
        }
 
        const vec<T>  operator -  (const T& s) const 
        { 
                vec<T> r = *this; r -= s; return r; 
        }
 
        template <typename S> 
        vec<T>  operator -  (const vec<S>& v) const 
        {
                 vec<T> r = *this; r -= v; return r; 
        }
 
        template <typename S> 
        const vec<T>  operator *  (const vec<S>& v) const 
        { 
                vec<T> r = *this; r *= v; return r; 
        }
 
        
        template <typename S> 
        const vec<T>  operator / (const vec<S>& v) const 
        { 
                vec<T> r = *this; r /= v; return r; 
        }
 
 
 
        vec<T>  operator-(void) const 
        {
                vec<T> r=(*this);
                r=(T)(-1)*r;
                return r; 
        }
 
        vec<T>  operator * (const T& s) const 
        {
                 vec<T> r = *this; r *= s; return r; 
        }
 
        
        vec<T>  operator / (const T& s)const 
        { 
                vec<T> r = *this;
                r /= s;
                return r; 
        }
        
 
        void fill(const T& v)
        {
                for (unsigned i=0; i<_size; ++i) 
                        _data[i]= (T)v; 
        }
 
        void zeros()
        {
                fill((T)0);
        }
 
        void ones()
        {
                fill((T)1);
        }
 
        void zeros(unsigned n)
        {
                resize(n);
                fill((T)0);
        }
 
        void ones(unsigned n)
        {
                resize(n);
                fill((T)1);
        }
 
        void resize(unsigned dim)
        {
                
                if(_data)
                {
                        if(dim != _size)
                        {
                                destruct();
                                _size=dim;
                                if(dim > 0)
                                        _data = new T[dim];     
                                else
                                        _data = NULL;
                                data_is_external = false;
                        }
                }else
                {
                                _size=dim;
                                if(dim > 0)
                                        _data = new T[dim];     
                                else
                                        _data = NULL;
                                data_is_external = false;
                }               
        }
 
        template <typename S> 
        bool operator == (const vec<S>& v) const
        { 
                for (unsigned i=0;i<_size;++i) 
                        if(operator()(i) != (T)v(i)) return false; 
                return true; 
        }
 
        template <typename S> 
        bool operator != (const vec<S>& v) const
        { 
                for (unsigned i=0;i<_size;++i) 
                        if(operator()(i) != (T)v(i)) return true; 
                return false; 
        }
 
        T length() const
        {
                return (T)sqrt((double)sqr_length());
        }
 
        void abs()
        {       
                if(std::numeric_limits<T>::is_signed)
                {
                        for(unsigned i = 0; i < _size;i++)
                                _data[i]=(T)std::abs((double)_data[i]);
                }
        }
 
        void ceil()
        {
                for(unsigned i = 0; i < _size;i++)
                        _data[i]=(T)::ceil((double)_data[i]);
        }
 
        void floor()
        {
                for(unsigned i = 0; i < _size;i++)
                        _data[i]=(T)::floor((double)_data[i]);
        }
 
        void round()
        {
                for(unsigned i = 0; i < _size;i++)
                        _data[i]=(T)::floor((double)_data[i]+0.5);      
        }
 
 
        T sqr_length() const
        {
                T l=0;          
                for(unsigned i = 0; i!=_size;i++)
                        l+= operator()(i)*operator()(i);
                return l;
        }
 
        void normalize() 
        {
                T l = (T)1.0/length();
                for(unsigned i = 0; i<_size; i++)
                         operator()(i)=l*operator()(i);
        }
 
        vec<T> sub_vec(unsigned ifrom, unsigned size) const
        {
                
                vec<T> vnew(size);
        
                for(unsigned i = 0; i < size; i++)
                                vnew(i)=operator()(i+ifrom);            
                
                return vnew;
        }
 
        void copy(unsigned ifrom, unsigned s,vec<T>& subvec) const
        {
                assert(subvec.size() == s); 
                assert(ifrom+s <=size());
                
                for(unsigned i = 0; i < s; i++)
                                subvec(i)=operator()(i+ifrom);          
        }
 
        void paste(unsigned ifrom,const vec<T>& v) 
        {
                
                assert(ifrom+v.size() <= size());
                for(unsigned i = 0; i < v.size(); i++)
                                operator()(i+ifrom) = v(i);             
        }
 
 
 
};
 
template<typename T>
vec<T> normalize(const vec<T>& v)
{
        vec<T> r = v;
        r.normalize();
        return r;
}
 
 
template <typename T,typename S>
T p_norm(const vec<T>& values,const S& p=1)
{       
        assert(p > 0);
        unsigned N = values.size();
 
        T n=0;  
 
        for(unsigned i = 0; i < N;i++)
        {
                n+=pow(fabs(values(i)),(T)p);
        }
 
        return pow(n,(T)(1.0/(T)p));
}
 
template <typename T>
T inf_norm(const vec<T>& values)
{
        vec<T> r = abs(values); 
 
        return max_value(r);
}
 
 
template<typename T>
T length(const vec<T>& v)
{
        return v.length();
}
 
template<typename T>
T sqr_length(const vec<T>& v)
{
        return v.sqr_length();
}
 
template<typename T>
std::ostream& operator<<(std::ostream& out, const vec<T>& v)
{
 
        for (unsigned i=0;i<v.size()-1;++i)
        {
                out << v(i)<<" ";       
        }
        out << v(v.size()-1);
        return out;
 
}
 
template<typename T>
std::istream& operator>>(std::istream& in, vec<T>& v)
{
 
        for (unsigned i=0;i<v.size();++i)
        {
                in >> v(i);     
        }
        
        return in;
 
}
 
 
template <typename T>
const vec<T> operator * (const T& s, const vec<T>& v) 
{
        vec<T> r = v; r *= s; return r; 
}
 
template <typename T>
inline T dot(const vec<T>& v, const vec<T>& w) 
{ 
        T r = 0; 
        for (unsigned i=0;i<v.size();++i) r += v(i)*(T)w(i); 
        return r; 
}
 
template < typename T>
inline vec<T> cross(const vec<T>& v, const vec<T>& w) 
{ 
        vec<T> r(3);
        r(0)=  v(1)*(T)w(2) - v(2)*(T)w(1);
        r(1)= -v(0)*(T)w(2) + v(2)*(T)w(0);
        r(2)=  v(0)*(T)w(1) - v(1)*(T)w(0);
        return r;
}
 
template < typename T, typename S, typename U>
vec<T> dbl_cross(const vec<T> &a, const vec<S> &b, vec<U> &c)
{
        return dot(a,c)*b - dot(a,b)*c;
}
 
template < typename T, typename S, typename U>
T spat(const vec<T> &a,const vec<S> &b,const vec<U> &c)
{
         return dot(cross(a,b),c);
}
 
template <typename T>
vec<T> project(const vec<T> &v, const vec<T> &n)
{
        return dot(v,n)/dot(n,n)*n;
}
 
 
template <typename T>
vec<T> reflect(const vec<T> &v, const vec<T> &n)
{
        return v-(T)2.0*dot(v,n)/dot(n,n)*n;
}
 
template <typename T>
vec<T> refract(const vec<T> &v,const vec<T> &n,T c1, T c2,bool* total_reflection=NULL)
{
        
        T NdotV =-dot(n,v)/dot(n,n);
        T c = c2/c1;
 
        T cosasqr = (T)1.0-(c*c)*((T)1.0-NdotV*NdotV);
        
        //total reflection
        if(cosasqr < 0)
        {
                if(total_reflection)
                        *total_reflection=true;
                return reflect(v,n);
        }
        else
        {
                if(total_reflection)
                        *total_reflection=false;
                return  c*v + (c*NdotV - sqrt(cosasqr)/dot(n,n) )*n;
                 
        }
 
}
 
 
 
template <typename T>
vec<T> zeros(const unsigned dim)
{
        vec<T> v;
        v.zeros(dim);
        return v;
}
 
template <typename T>
vec<T> ones(const unsigned dim)
{
        vec<T> v;
        v.ones(dim);
        return v;
}
 
 
template <typename T>
vec<T> floor(const vec<T> &v)
{
        vec<T> r(v.size());
        for(unsigned i = 0; i < v.size();i++)
                r(i)=::floor((T)v(i));
        
        return r;
}
 
 
template <typename T>
vec<T> ceil(const vec<T> &v)
{
        vec<T> r(v.size());
        for(unsigned i = 0; i < v.size();i++)
                r(i)=::ceil(v(i));
        
        return r;
}
 
template <typename T>
vec<T> round(const vec<T> &v)
{
        vec<T> r(v.size());
        for(unsigned i = 0; i < v.size();i++)
                r(i)=::floor(v(i)+0.5);
        
        return r;
}
 
 
template <typename T>
vec<T> abs(const vec<T> &v)
{
        vec<T> r(v.size());
        for(unsigned i = 0; i < v.size();i++)
                r(i)=std::abs(v(i));
 
        return r;
}
 
template <typename T>
T min_value(const vec<T> &v)
{
        return *(std::min_element(&v(0),&v(v.size()-1)+1));
}
 
 
 
template <typename T>
unsigned min_index(const vec<T> &v)
{
        return (unsigned) (std::min_element(&v(0),&v(v.size()-1)+1)-&v(0));
}
 
template <typename T>
unsigned max_index(const vec<T> &v)
{
        return (unsigned) (std::max_element(&v(0),&v(v.size()-1)+1)-&v(0));
}
 
template <typename T>
T max_value(const vec<T> &v)
{
        return *(std::max_element(&v(0),&v(v.size()-1)+1));
}
 
 
template<typename T>
T mean_value(const vec<T>& values)
{
 
        unsigned N = values.size();
 
        T mu=0;
 
        for(unsigned i = 0; i < N;i++)
        {
                mu+=values(i);
        }
        mu/=(T)N;
        return mu;
}
 
template<typename T>
T var_value(const vec<T>& values)
{
        unsigned N = values.size();     
 
        T mu=mean_value(values);
        T v = 0;
 
        for(unsigned i = 0; i < N;i++)
        {
                v+=(values(i)-mu)*(values(i)-mu);       
        }
 
        if(N > 1)
                v/=(T)(N-1);
 
        return v;
 
}
 
template<typename T>
T range_value(const vec<T>& values)
{
        return max_value(values)-min_value(values);
}
 
template<typename T>
T mad_value(const vec<T>& values)
{
        return  median_value(abs(values-median_value(values)));
}
 
 
template<typename T>
T std_value(const vec<T>& values)
 
{
        T v = var_value(values);
        return sqrt(v);
}
 
 
 
template<typename T>
void var_and_mean_value(const vec<T>& values, T& mu, T&var)
{
 
        unsigned N = values.size();     
        mu=0;
 
        for(unsigned i = 0; i < N;i++)
        {
                mu+=values(i);  
        }
 
        mu/=(T)N;
        var = 0;
        for(unsigned i = 0; i < N;i++)
        {
                var+=sqr(values(i)-mu); 
        }
 
        var/=(T)(N-1);  
}
 
 
template <typename T>
void sort_values(vec<T>& values, bool ascending=true)
{
        
        if(ascending)
                std::sort(&values(0),&values(values.size()-1)+1,std::less<T>());
        else
                std::sort(&values(0),&values(values.size()-1)+1,std::greater<T>());
}
 
 
 
template <typename T>
T sum_values(const vec<T>& values)
{
        T v =0;
        for(unsigned i = 0; i < values.size(); i++)
                v+=values(i);
        return v;
}
 
template <typename T>
T cumsum_values(const vec<T>& values, vec<T>& cumsumvalues)
{
        cumsumvalues.resize(values.size());
        T v =0;
        for(unsigned i = 0; i < values.size(); i++)
        {
                cumsumvalues(i)=v;
                v+=values(i);
        }
        return v;
}
 
 
 
template <typename T>
T prod_values(vec<T>& values)
{
        T v =1;
        for(unsigned i = 0; i < values.size(); i++)
                v*=values(i);
        return v;
}
 
 
template <typename T>
T select_value(unsigned k, vec<T>& values)
{
        if (k >= values.size())
                k = values.size()-1;
        std::nth_element(&values(0),&values(k),&values(values.size()-1)+1);
        return values(k);
}
 
 
 
template <typename T>
T select_median_value( vec<T>& values)
{       
        return select_value((values.size())/2,values);
}
 
 
 
template <typename T>
T median_value(const vec<T>& values)
{
        vec<T> c = values;
        return select_value((c.size())/2,c);
}
 
 
 
template <typename T>
const vec<T>  lin_space(const T& first_val, const T& last_val, unsigned N=10)
{
        vec<T> lv(N);
        if(N == 1)
        {
                lv(0) = last_val;
                return lv;
        }
        T diff = last_val-first_val;
 
        for(unsigned i = 0; i < N; i++)
        {
                lv(i) = first_val + i*diff/((T)N-(T)1.0);
        }
        return lv;
}
 
//create N chebychev sample points for interval [first_val,last_val]
template <typename T>
const vec<T>  cheb_points(const T& first_val, const T& last_val,unsigned N=10)
{
        vec<T> lv(N) ;
        if(N == 1)
        {
                lv(0) = (T)(first_val+last_val)/2.0;
                return lv;
        }
        T diff = (last_val-first_val)/(T)2.0;
 
        for(unsigned i = 0; i < N; i++)
        {
                lv(i) =  diff*((first_val+1.0)-cos((T)((2*i+1)*3.14159)/((T)(2.0*(N-1)+2))));
        }
        return lv;
}
 
 
 
template <typename T>
const vec<T>  log_space(const T& first_pow_of_10, const T& last_pow_of_10, unsigned N=10)
{
 
        vec<T> lv(N);
        if(N == 1)
        {
                lv(0) = pow((T)10.0,last_pow_of_10);
                return lv;
        }
        T diff = last_pow_of_10 - first_pow_of_10;
 
        for(unsigned i = 0; i < N; i++)
        {
                lv(i) = pow((T)10.0,(T)first_pow_of_10 + (T)i*diff/((T)N-(T)1.0));
        }
        return lv;
}
 
 
 
template <typename T>
const vec<T> lerp(const vec<T>& v1, const vec<T>& v2, T t)
{
        return (1-t)*v1+t*v2;
}
 
 
 
template <typename T>
const T lerp(const T& s1, const T& s2, T t)
{
        return (1-t)*s1+t*s2;
}
 
 
 
template <typename T>
const vec<T> slerp(const vec<T>& v0, const vec<T>& v1, T t)
{
        T dotv0v1 = dot(v0,v1);
        //clamp between [-1,1]
        if(dotv0v1 < -1)
                dotv0v1 = -1;
 
        if(dotv0v1 > 1)
                dotv0v1 = 1;
 
        T theta = acos(dotv0v1)*t;
        cgv::math::vec<T> v2 = normalize(v1 - (dotv0v1)*v0);
        return cos(theta)*v0 + sin(theta)*v2;
}
 
 
 
        }
}