Vec.h

Go to the documentation of this file.

/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
*                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                              SOFA :: Framework                              *
*                                                                             *
* Authors: M. Adam, J. Allard, B. Andre, P-J. Bensoussan, S. Cotin, C. Duriez,*
* H. Delingette, F. Falipou, F. Faure, S. Fonteneau, L. Heigeas, C. Mendoza,  *
* M. Nesme, P. Neumann, J-P. de la Plata Alcade, F. Poyer and F. Roy          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_DEFAULTTYPE_VEC_H
#define SOFA_DEFAULTTYPE_VEC_H

#include <sofa/helper/fixed_array.h>
#include <sofa/helper/static_assert.h>
#include <sofa/helper/rmath.h>
#include <functional>


#define EQUALITY_THRESHOLD 1e-6

namespace sofa
{

namespace defaulttype
{

enum NoInit { NOINIT }; 

template <int N, typename real=float>
class Vec : public helper::fixed_array<real,N>
{
public:

        Vec()
        {
            this->clear();
        }

        explicit Vec(NoInit)
        {
        }

        explicit Vec(real r1)
        {
            BOOST_STATIC_ASSERT(N==1);
            this->elems[0]=r1;
        }

        void operator=(real r1)
        {
            BOOST_STATIC_ASSERT(N == 1);
            this->elems[0]=r1;
        }

        Vec(real r1, real r2)
        {
                BOOST_STATIC_ASSERT(N == 2);
                this->elems[0]=r1;
                this->elems[1]=r2;
        }

        Vec(real r1, real r2, real r3)
        {
                BOOST_STATIC_ASSERT(N == 3);
                this->elems[0]=r1;
                this->elems[1]=r2;
                this->elems[2]=r3;
        }

        Vec(real r1, real r2, real r3, real r4)
        {
                BOOST_STATIC_ASSERT(N == 4);
                this->elems[0]=r1;
                this->elems[1]=r2;
                this->elems[2]=r3;
                this->elems[3]=r4;
        }

        Vec(real r1, real r2, real r3, real r4, real r5, real r6)
        {
            BOOST_STATIC_ASSERT(N == 6);
            this->elems[0]=r1;
            this->elems[1]=r2;
            this->elems[2]=r3;
            this->elems[3]=r4;
            this->elems[4]=r5;
            this->elems[5]=r6;
        }

        Vec(real r1, real r2, real r3, real r4, real r5, real r6, real r7, real r8)
        {
                BOOST_STATIC_ASSERT(N == 8);
                this->elems[0]=r1;
                this->elems[1]=r2;
                this->elems[2]=r3;
                this->elems[3]=r4;
                this->elems[4]=r5;
                this->elems[5]=r6;
                this->elems[6]=r7;
                this->elems[7]=r8;
        }
        
        Vec(real r1, real r2, real r3, real r4, real r5, real r6, real r7, real r8, real r9)
        {
            BOOST_STATIC_ASSERT(N == 9);
            this->elems[0]=r1;
            this->elems[1]=r2;
            this->elems[2]=r3;
            this->elems[3]=r4;
            this->elems[4]=r5;
            this->elems[5]=r6;
            this->elems[6]=r7;
            this->elems[7]=r8;
            this->elems[8]=r9;
        }
        

        Vec(const Vec<N-1,real>& v, real r1)
        {
                BOOST_STATIC_ASSERT(N > 1);
                for(int i=0;i<N-1;i++)
                        this->elems[i] = v[i];
                this->elems[N-1]=r1;
        }

        template<typename real2>
        Vec(const Vec<N, real2>& p)
        {
                //std::copy(p.begin(), p.end(), this->begin());
                for(int i=0;i<N;i++)
                        this->elems[i] = (real)p(i);
        }

        template<typename real2>
        explicit Vec(const real2* p)
        {
                //std::copy(p, p+N, this->begin());
                for(int i=0;i<N;i++)
                        this->elems[i] = (real)p[i];
        }

        real& x()
        {
                BOOST_STATIC_ASSERT(N >= 1);
                return this->elems[0];
        }
        real& y()
        {
                BOOST_STATIC_ASSERT(N >= 2);
                return this->elems[1];
        }
        real& z()
        {
                BOOST_STATIC_ASSERT(N >= 3);
                return this->elems[2];
        }
        real& w()
        {
                BOOST_STATIC_ASSERT(N >= 4);
                return this->elems[3];
        }

        const real& x() const
        {
                BOOST_STATIC_ASSERT(N >= 1);
                return this->elems[0];
        }
        const real& y() const
        {
                BOOST_STATIC_ASSERT(N >= 2);
                return this->elems[1];
        }
        const real& z() const
        {
                BOOST_STATIC_ASSERT(N >= 3);
                return this->elems[2];
        }
        const real& w() const
        {
                BOOST_STATIC_ASSERT(N >= 4);
                return this->elems[3];
        }

        template<typename real2>
        void operator=(const real2* p)
        {
                //std::copy(p, p+N, this->begin());
                for(int i=0;i<N;i++)
                        this->elems[i] = (real)p[i];
        }

        template<int M, typename real2>
        void operator=(const Vec<M,real2>& v)
        {
                //std::copy(v.begin(), v.begin()+(N>M?M:N), this->begin());
                for(int i=0;i<(N>M?M:N);i++)
                        this->elems[i] = (real)v(i);
        }

    void clear()
    {
    this->assign(real());
    }

        void fill(real r)
        {
                this->assign(r);
        }

        // Access to i-th element.
        // Already in fixed_array
        //real& operator[](int i)
        //{
        //    return this->elems[i];
        //}

        // Access to i-th element.
        // Already in fixed_array
        //const real& operator[](int i) const
        //{
        //    return this->elems[i];
        //}

        real& operator()(int i)
        {
                return this->elems[i];
        }

        const real& operator()(int i) const
        {
                return this->elems[i];
        }

        //operator const real*() const
        //{
        //    return this->elems;
        //}

        //operator real*()
        //{
        //    return this->elems;
        //}

        const real* ptr() const
        {
                return this->elems;
        }

        real* ptr()
        {
                return this->elems;
        }

        // LINEAR ALGEBRA

    template<class real2>
    Vec<N,real> operator*(real2 f) const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i] = this->elems[i]*(real)f;
    return r;
    }

        template<class real2>
        void operator*=(real2 f)
        {
                for (int i=0;i<N;i++)
                        this->elems[i]*=(real)f;
        }

    template<class real2>
    Vec<N,real> operator/(real2 f) const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i] = this->elems[i]/(real)f;
    return r;
    }

        template<class real2>
        void operator/=(real2 f)
        {
                for (int i=0;i<N;i++)
                        this->elems[i]/=(real)f;
        }

        template<class real2>
        real operator*(const Vec<N,real2>& v) const
        {
                real r = (real)(this->elems[0]*v[0]);
                for (int i=1;i<N;i++)
                        r += (real)(this->elems[i]*v[i]);
                return r;
        }

    template<class real2>
    Vec<N,real> linearProduct(const Vec<N,real2>& v) const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i]=this->elems[i]*(real)v[i];
    return r;
    }
    
    
    template<class real2>
    Vec<N,real> linearDivision(const Vec<N,real2>& v) const
    {
      Vec<N,real> r(NOINIT);
      for (int i=0;i<N;i++)
    r[i]=this->elems[i]/(real)v[i];
      return r;
    }

    template<class real2>
    Vec<N,real> operator+(const Vec<N,real2>& v) const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i]=this->elems[i]+(real)v[i];
    return r;
    }

        template<class real2>
        void operator+=(const Vec<N,real2>& v)
        {
                for (int i=0;i<N;i++)
                        this->elems[i]+=(real)v[i];
        }

    template<class real2>
    Vec<N,real> operator-(const Vec<N,real2>& v) const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i]=this->elems[i]-(real)v[i];
    return r;
    }

        template<class real2>
        void operator-=(const Vec<N,real2>& v)
        {
                for (int i=0;i<N;i++)
                        this->elems[i]-=(real)v[i];
        }

    Vec<N,real> operator-() const
    {
    Vec<N,real> r(NOINIT);
    for (int i=0;i<N;i++)
        r[i]=-this->elems[i];
    return r;
    }

        real norm2() const
        {
                real r = this->elems[0]*this->elems[0];
                for (int i=1;i<N;i++)
                        r += this->elems[i]*this->elems[i];
                return r;
        }

    real norm() const
    {
        return helper::rsqrt(norm2());
    }

        void normalize()
        {
                real r = norm();
                if (r>1e-10)
                        for (int i=0;i<N;i++)
                                this->elems[i]/=r;
        }

        Vec cross( const Vec& b ) const
        {
                BOOST_STATIC_ASSERT(N == 3);
                return Vec(
                               (*this)[1]*b[2] - (*this)[2]*b[1],
                               (*this)[2]*b[0] - (*this)[0]*b[2],
                               (*this)[0]*b[1] - (*this)[1]*b[0]
                       );
        }

        
    real sum() const
    {
        real sum = 0.0;
        for (int i=0;i<N;i++)
            sum += this->elems[i];
        return sum;
    }
        
        

    bool operator==(const Vec& b) const
    {
        for (int i=0;i<N;i++)
            if ( fabs( this->elems[i] - b[i] ) > EQUALITY_THRESHOLD ) return false;
        return true;
    }

    bool operator!=(const Vec& b) const
    {
        for (int i=0;i<N;i++)
            if ( fabs( this->elems[i] - b[i] ) > EQUALITY_THRESHOLD ) return true;
        return false;
    }

};

template <int N, typename real=float>
class VecNoInit : public Vec<N,real>
{
public:
    VecNoInit()
    : Vec<N,real>(NOINIT)
    {
    }

    template<typename real2>
    void operator=(const real2* p)
    {
    this->Vec<N,real>::operator=(p);
    }

    template<int M, typename real2>
    void operator=(const Vec<M,real2>& v)
    {
    this->Vec<N,real>::operator=(v);
    }
};

template<int N,typename Real>
std::istream& operator >> ( std::istream& in, Vec<N,Real>& v )
{
        for( int i=0; i<N; ++i )
                in>>v[i];
        return in;
}

template<int N,typename Real>
std::ostream& operator << ( std::ostream& out, const Vec<N,Real>& v )
{
        for( int i=0; i<N-1; ++i )
                out<<v[i]<<" ";
        out<<v[N-1];
        return out;
}

template<typename real>
inline Vec<3,real> cross(const Vec<3,real>& a, const Vec<3,real>& b)
{
        return Vec<3,real>(a.y()*b.z() - a.z()*b.y(),
                           a.z()*b.x() - a.x()*b.z(),
                           a.x()*b.y() - a.y()*b.x());
}

template <typename real>
real cross(const defaulttype::Vec<2,real>& a, const defaulttype::Vec<2,real>& b )
{
    return a[0]*b[1] - a[1]*b[0];
}

template<int N,typename real>
inline real dot(const Vec<N,real>& a, const Vec<N,real>& b)
{
        return a*b;
}

typedef Vec<1,float> Vec1f;
typedef Vec<1,double> Vec1d;


typedef Vec<2,float> Vec2f;
typedef Vec<2,double> Vec2d;


typedef Vec<3,float> Vec3f;
typedef Vec<3,double> Vec3d;


typedef Vec<4,float> Vec4f;
typedef Vec<4,double> Vec4d;


typedef Vec<6,float> Vec6f;
typedef Vec<6,double> Vec6d;


#ifdef SOFA_FLOAT
typedef Vec1f Vector1; 
typedef Vec2f Vector2; 
typedef Vec3f Vector3; 
typedef Vec4f Vector4; 
typedef Vec6f Vector6; 
#else
typedef Vec1d Vector1; 
typedef Vec2d Vector2; 
typedef Vec3d Vector3; 
typedef Vec4d Vector4; 
typedef Vec6d Vector6; 
#endif
} // namespace defaulttype

} // namespace sofa

template <int N, typename real>
sofa::defaulttype::Vec<N,real> operator*(real r, const sofa::defaulttype::Vec<N,real>& v)
{
  return v*r;
}


// Specialization of the std comparison function, to use Vec as std::map key
namespace std
{

// template <>
template<int N, class T>
struct less< sofa::defaulttype::Vec<N,T> > : public binary_function< sofa::defaulttype::Vec<N,T>,  sofa::defaulttype::Vec<N,T>, bool>
{
    bool operator()(const  sofa::defaulttype::Vec<N,T>& x, const  sofa::defaulttype::Vec<N,T>& y) const
    {
        //std::cerr<<"specialized std::less, x = "<<x<<", y = "<<y<<std::endl;
        for( unsigned i=0; i<N; ++i ) {
                if( x[i]<y[i] )
                        return true;
                else if( y[i]<x[i] )
                        return false;
        }
        return false;
    }
};

} // namespace std

#endif