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(0), _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 = 0;
        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;
        }
    }
 
    T* data()
    {
        return _data;
    }
 
    const T* data() 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& 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);
    }
 
    void safe_normalize()
    {
        T l = length();
        if(std::abs(l) > std::numeric_limits<T>::epsilon()) {
            l = (T)1.0 / l;
            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);     
    }
 
 
 
};
 
#define CGV_MATH_VEC_DECLARED
 
template<typename T>
vec<T> normalize(const vec<T>& v)
{
    vec<T> r = v;
    r.normalize();
    return r;
}
 
 
template<typename T>
vec<T> safe_normalize(const vec<T>& v)
{
    vec<T> r = v;
    r.safe_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 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;
}
 
} // namespace math
 
 
typedef cgv::math::vec<float> vecn;
typedef cgv::math::vec<double> dvecn;
 
 
}// namespace cgv