lwr_server/lwrserv/include/Vec.h

#ifndef _VEC_H_
#define _VEC_H_
#include <math.h>
#include <ostream>
#include <cmath>
#include "Mat.h"

template<class T, unsigned SIZE> class Vec;
template <class T, unsigned SIZE> class Mat;


// Vector class for SIMPLE data types
template <class T, unsigned SIZE> class Vec 
{
public:

    // standard constructor
    Vec<T,SIZE> ()  
    {   // initialize all elements with zero
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = T(0);
    }
    
    // construction with data array
    Vec<T, SIZE> (const T tData) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = tData;
    }

    // construction with data array
    Vec<T, SIZE> (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = atData[i];
    }

    // copy constructor
    Vec<T, SIZE> (const Vec<T, SIZE> &vec) 
    {
        if (this==&vec)
            return; // nothing to do, it's me
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = vec.m_atData[i];
    }

    // destructor
    ~Vec () 
    {   // nothing to do here ...
    }

    void setData (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] = atData[i];
    }

    void getData (T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            atData[i] = m_atData[i];
        
    }

    unsigned getDimension () 
    {
        return SIZE;
    }


    Vec<T, SIZE> &operator = (const Vec<T, SIZE> &vec) 
    {
        if (this==&vec)
            return (*this); // ok, it's me, so no L-value action
        for (unsigned int i=0; i<SIZE; i++)     // not me, so L-Value action: copy data
            m_atData[i] = vec.m_atData[i];

        return (*this); // also an R-value in e.g
                        // vec1    =       vec2      = vec3;
                        // L-Value = R-Value/L-Value = R-Value
    }

    Vec<T, SIZE> &operator = (const T atData[SIZE]) 
    {
        for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
            m_atData[i] = atData[i];

        return (*this); // also an R-value in e.g.
                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
                        // L-Val = R-Val +       R-Val
                        //   "   =   "   + (L-Val = R-Val)
    }

    // usage of operator:
    // vec(i) = var;    // 0 <= i <= SIZE-1
    // var = vec(i);
    // vec1(i) = vec2(j);
    T &operator () (unsigned i) 
    {
        if (i>=SIZE)
            i = SIZE-1; // !!! operator clips index ...
        return m_atData[i];         // ... so we can safely return a reference
    }

    T operator () (unsigned i) const 
    {
        if (i>=SIZE)
            i = SIZE-1;
        return m_atData[i];
    }

    void operator += (const Vec<T, SIZE> &vec) 
    {
        for (int i=0; i<SIZE; i++)
            m_atData[i] += vec.m_atData[i];
    }

    Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec) 
    {
        Vec<T, SIZE> buf (m_atData);
        for (unsigned int i=0; i<SIZE; i++)
            buf.m_atData[i] += vec.m_atData[i];
        return buf;
    }

    void operator -= (const Vec<T, SIZE> &vec) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] -= vec.m_atData[i];
    }

    // binary -
    // vec1 - vec2 i.e. (*this) - vec
    Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec) 
    {
        Vec<T, SIZE> buf (m_atData);
        for (unsigned int i=0; i<SIZE; i++)
            buf.m_atData[i] -= vec.m_atData[i];
        return buf;
    }

    // unary -
    // -vec i.e. -(*this)
    Vec<T, SIZE> operator - () 
    {
        T atBuffer[SIZE];
        for (int i=0; i<SIZE; i++)
            atBuffer[i] = -m_atData[i];
        return Vec<T, SIZE> (atBuffer);
    }

    T operator * (const Vec<T, SIZE> & vec) 
    {
        T dp = T(0);
        for (int i=0; i<SIZE; i++)
            dp += m_atData[i]*vec.m_atData[i];
        return dp;
    }

    void operator *= (T tScale) 
    {
        for (unsigned int i=0; i<SIZE; i++)
            m_atData[i] *= tScale;
    }

    Vec<T, SIZE> operator * (T tScale) 
    {
        Vec<T, SIZE> vec;
        for (unsigned int i=0; i<SIZE; i++)
            vec(i) = m_atData[i]*tScale;
        return vec;
    }
    Vec<T, SIZE> operator / (T tScale) 
    {
        Vec<T, SIZE> vec;
        for (unsigned int i=0; i<SIZE; i++)
            vec(i) = m_atData[i]/tScale;
        return vec;
    }

    Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
    {
        Vec<T, SIZE> vec;
        for (unsigned int j=0; j<SIZE; j++)
            for (unsigned int i=0; i<SIZE; i++)
               vec(j) += m_atData[i]*mat(i,j);
        return vec;
    }

    Vec<T, SIZE> celldivide (const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = m_atData[i]/vec.m_atData[i];
        return buff;
    }
    Vec<T, SIZE> cellmultiply (const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
        return buff;
    }
    Vec<T, SIZE> cellmin(const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::min(m_atData[i],vec.m_atData[i]);
        return buff;
    }
    Vec<T, SIZE> cellmax(const Vec<T, SIZE> & vec) 
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::max(m_atData[i],vec.m_atData[i]);
        return buff;
    }

    Vec<T, SIZE> abs ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::abs(m_atData[i]);
        return buff;
    }
    
    Vec<T, SIZE> sqrt()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::sqrt(m_atData[i]);
        return buff;
    }
    Vec<T, SIZE> floor ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = floor(m_atData[i]);
        return buff;
    }
    Vec<T, SIZE> ceil ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = std::ceil(m_atData[i]);
        return buff;
    }
    float sgn(float x) 
    { 
        if ( x == 0 ) 
            return 0.0f; 

        if ( x > 0)
            return 1.0f;
        else
            return -1.0f; 
    }
    Vec<T, SIZE> sgn ()
    {
        Vec<T, SIZE> buff;
        for (int i=0; i<SIZE; i++)
            buff.m_atData[i] = sgn(m_atData[i]);
        return buff;
    }
    
    T max ()
    {
        T retval = m_atData[0];

        // this * this
        for (int i=0; i<SIZE; i++)
            if (retval < m_atData[i])
                retval =  m_atData[i];
        return retval;
    }
    T min ()
    {
        T retval = m_atData[0];

        // this * this
        for (int i=0; i<SIZE; i++)
            if (retval > m_atData[i])
                retval =  m_atData[i];
        return retval;
    }

    T norm ()
    {
        T retval = 0;

        // this * this
        for (int i=0; i<SIZE; i++)
            retval += m_atData[i] * m_atData[i];
        sqrt(retval);
        return retval;
    }

    Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
    {
        T temp[SIZE];
        temp[0] = m_atData[1]*vec(2) - m_atData[2]*vec(1);
        temp[1] = m_atData[2]*vec(0) - m_atData[0]*vec(2);
        temp[2] = m_atData[0]*vec(1) - m_atData[1]*vec(0);
        temp[3] = vec(3);
        return Vec<T, SIZE> (temp);
    }

private:

    T m_atData[SIZE];

}; // class Vec


// some common vector classes (abbr. names)
typedef Vec<float, 2> Vec2f;
typedef Vec<float, 3> Vec3f;
typedef Vec<float, 4> Vec4f;

typedef Vec<double, 2> Vec2d;
typedef Vec<double, 3> Vec3d;
typedef Vec<double, 4> Vec4d;

typedef Vec<int, 2> Vec2i;
typedef Vec<int, 3> Vec3i;
typedef Vec<int, 4> Vec4i;

typedef Vec<bool, 2> Vec2b;
typedef Vec<bool, 3> Vec3b;
typedef Vec<bool, 4> Vec4b;


template <class T, unsigned SIZE>
static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
{
    output << "( ";
    for(unsigned int i = 0 ; i< SIZE; ++i)
    {
        output << vec(i);
        if (i<SIZE-1)
            output << " , ";
        else
            output << " )";
    }
    return output;
}

#endif