VecTypes.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_VECTYPES_H
#define SOFA_DEFAULTTYPE_VECTYPES_H

#include <sofa/defaulttype/Vec.h>
#include <sofa/core/objectmodel/BaseContext.h>
#include <sofa/helper/vector.h>
#include <iostream>
#include <algorithm>



namespace sofa
{

namespace defaulttype
{

using helper::vector;

template<class TCoord, class TDeriv, class TReal = typename TCoord::value_type>
class StdVectorTypes
{
public:
    typedef TCoord Coord;
    typedef TDeriv Deriv;
    typedef TReal Real;
    typedef vector<Coord> VecCoord;
    typedef vector<Deriv> VecDeriv;
    typedef vector<Real> VecReal;


    template <class T>
    class SparseData
    {
    public:
        SparseData(unsigned int _index, const T& _data): index(_index), data(_data){};
        SparseData(){};
        unsigned int index;
        T data;
    };

    typedef SparseData<Coord> SparseCoord;
    typedef SparseData<Deriv> SparseDeriv;

    typedef vector<SparseCoord> SparseVecCoord;
    typedef vector<SparseDeriv> SparseVecDeriv;

    typedef vector<SparseVecDeriv> VecConst;
    
    template<typename T>
    static void set(Coord& c, T x, T y, T z)
    {
    
        if (c.size() > 0)
            c[0] = (Real)x;
        if (c.size() > 1)
            c[1] = (Real)y;
        if (c.size() > 2)
            c[2] = (Real)z;
    }
    
    template<typename T>
    static void get(T& x, T& y, T& z, const Coord& c)
    {
      x = (c.size() > 0) ? (T) c[0] : (T) 0.0;
      y = (c.size() > 1) ? (T) c[1] : (T) 0.0;
      z = (c.size() > 2) ? (T) c[2] : (T) 0.0;
    }
    
    template<typename T>
    static void add(Coord& c, T x, T y, T z)
    {
        if (c.size() > 0)
            c[0] += (Real)x;
        if (c.size() > 1)
            c[1] += (Real)y;
        if (c.size() > 2)
            c[2] += (Real)z;
    }
    
    static const char* Name();

    static Coord interpolate(const helper::vector< Coord > &ancestors, const helper::vector< Real > &coefs)
    {
        assert(ancestors.size() == coefs.size());
            
        Coord c;
        
        for (unsigned int i = 0; i < ancestors.size(); i++)
        {
            c += ancestors[i] * coefs[i];
        }

        return c;
    }
};

template<class T>
class ExtVectorAllocator
{
public:
    typedef T              value_type;
    typedef unsigned int   size_type;
    virtual ~ExtVectorAllocator() {}
    virtual void resize(value_type*& data, size_type size, size_type& maxsize, size_type& cursize)=0;
    virtual void close(value_type* data)=0;
};

template<class T>
class ExtVector
{
public:
    typedef T              value_type;
    typedef unsigned int   size_type;

protected:
    value_type* data;
    size_type   maxsize;
    size_type   cursize;
    ExtVectorAllocator<T>* allocator;
public:
    explicit ExtVector(ExtVectorAllocator<T>* alloc = NULL) : data(NULL), maxsize(0), cursize(0), allocator(alloc) {}
    ExtVector(int size, ExtVectorAllocator<T>* alloc) : data(NULL), maxsize(0), cursize(0), allocator(alloc) { resize(size); }
    ~ExtVector() { if (allocator) allocator->close(data); }
    void setAllocator(ExtVectorAllocator<T>* alloc)
    {
        if (alloc != allocator)
        {
            if (cursize)
            {
                value_type* oldData = data;
                size_type size = cursize;
                data = NULL;
                maxsize = 0;
                cursize = 0;
                alloc->resize(data, size, maxsize, cursize);
                std::copy(oldData, oldData+size, data);
                if (allocator)
                    allocator->close(oldData);
            }
            allocator = alloc;
        }
    }
    void setData(value_type* d, size_type s) { data=d; maxsize=s; cursize=s; }
    T* getData() { return this->data; }
    const T* getData() const { return this->data; }

    value_type& operator[](size_type i) { return data[i]; }
    const value_type& operator[](size_type i) const { return data[i]; }
    size_type size() const { return cursize; }
    bool empty() const { return cursize==0; }
    void reserve(size_type size)
    {
        if (size <= maxsize)
            return;
        size_type temp = cursize;
        if (allocator)
            allocator->resize(data, size, maxsize, temp);
        else
        {
            std::cerr << "Error: invalid reserve request ("<<size<<">"<<maxsize<<") on external vector without allocator.\n";
        }
    }
    void resize(size_type size)
    {
        if (size <= maxsize)
            cursize = size;
        else if (allocator)
            allocator->resize(data, size, maxsize, cursize);
        else
        {
            cursize = maxsize;
            std::cerr << "Error: invalid resize request ("<<size<<">"<<maxsize<<") on external vector without allocator.\n";
        }
    }
    void clear()
    {
        resize(0);
    }
    void push_back(const T& v)
    {
        int i = this->size();
        resize(i+1);
        (*this)[i] = v;
    }
    T* begin() { return getData(); }
    const T* begin() const { return getData(); }
    T* end() { return getData()+size(); }
    const T* end() const { return getData()+size(); }


  inline friend std::ostream& operator<< ( std::ostream& os, const ExtVector<T>& vec )
  {
      if( vec.size()>0 ){
          for( unsigned int i=0; i<vec.size()-1; ++i ) os<<vec[i]<<" ";
          os<<vec[vec.size()-1];
      }
      return os;
  }

  inline friend std::istream& operator>> ( std::istream& in, ExtVector<T>& vec )
  {
      T t;
      vec.clear();
      while(in>>t){
          vec.push_back(t);
      }
      if( in.rdstate() & std::ios_base::eofbit ) { in.clear(); }
      return in;
  }

};

template<class T>
class DefaultAllocator : public ExtVectorAllocator<T>
{
public:
    typedef typename ExtVectorAllocator<T>::value_type value_type;
    typedef typename ExtVectorAllocator<T>::size_type size_type;
    virtual void close(value_type* data)
    {
            if (data!=NULL) delete[] data;
            delete this;
    }
    virtual void resize(value_type*& data, size_type size, size_type& maxsize, size_type& cursize)
    {
        if (size > maxsize)
        {
            T* oldData = data;
            maxsize = (size > 2*maxsize ? size : 2*maxsize);
            data = new T[maxsize];
            if (cursize)
                std::copy(oldData, oldData+cursize, data);
            //for (size_type i = 0 ; i < cursize ; ++i)
            //    data[i] = oldData[i];
            if (oldData!=NULL) delete[] oldData;
        }
        cursize = size;
    }
};

template<class T>
class ResizableExtVector : public ExtVector<T>
{
public:
    typedef typename ExtVector<T>::value_type value_type;
    typedef typename ExtVector<T>::size_type size_type;
    ResizableExtVector()
    : ExtVector<T>(new DefaultAllocator<T>)
    {
    }
};

template<class TCoord, class TDeriv, class TReal = typename TCoord::value_type>
class ExtVectorTypes
{
public:
    typedef TCoord Coord;
    typedef TDeriv Deriv;
    typedef TReal Real;
    typedef ResizableExtVector<Coord> VecCoord;
    typedef ResizableExtVector<Deriv> VecDeriv;
    typedef ResizableExtVector<Real> VecReal;

    template <class T>
    class SparseData
    {
    public:
        SparseData(unsigned int _index, const T& _data): index(_index), data(_data){};
        unsigned int index;
        T data;
    };

    typedef SparseData<Coord> SparseCoord;
    typedef SparseData<Deriv> SparseDeriv;

    typedef vector<SparseCoord> SparseVecCoord;
    typedef vector<SparseDeriv> SparseVecDeriv;

    typedef vector<SparseVecDeriv> VecConst;
    
    template<typename T>
    static void set(Coord& c, T x, T y, T z)
    {
    
        if (c.size() > 0)
            c[0] = (Real)x;
        if (c.size() > 1)
            c[1] = (Real)y;
        if (c.size() > 2)
            c[2] = (Real)z;
    }
    
    template<typename T>
    static void get(T& x, T& y, T& z, const Coord& c)
    {
      x = (c.size() > 0) ? (T) c[0] : (T) 0.0;
      y = (c.size() > 1) ? (T) c[1] : (T) 0.0;
      z = (c.size() > 2) ? (T) c[2] : (T) 0.0;
    }
    
    template<typename T>
    static void add(Coord& c, T x, T y, T z)
    {
        if (c.size() > 0)
            c[0] += (Real)x;
        if (c.size() > 1)
            c[1] += (Real)y;
        if (c.size() > 2)
            c[2] += (Real)z;
    }
    
    static const char* Name();

    static Coord interpolate(const helper::vector< Coord > & ancestors, const helper::vector< Real > & coefs)
    {
        assert(ancestors.size() == coefs.size());
            
        Coord c;
        
        for (unsigned int i = 0; i < ancestors.size(); i++)
        {
            c += ancestors[i] * coefs[i];
        }

        return c;
    }
};

//
// 3D
//

typedef StdVectorTypes<Vec3d,Vec3d,double> Vec3dTypes;
typedef StdVectorTypes<Vec3f,Vec3f,float> Vec3fTypes;

template<> inline const char* Vec3dTypes::Name() { return "Vec3d"; }
template<> inline const char* Vec3fTypes::Name() { return "Vec3f"; }

typedef ExtVectorTypes<Vec3d,Vec3d,double> ExtVec3dTypes;
typedef ExtVectorTypes<Vec3f,Vec3f,float> ExtVec3fTypes;

template<> inline const char* ExtVec3dTypes::Name() { return "ExtVec3d"; }
template<> inline const char* ExtVec3fTypes::Name() { return "ExtVec3f"; }

//
// 2D
//

typedef StdVectorTypes<Vec2d,Vec2d,double> Vec2dTypes;
typedef StdVectorTypes<Vec2f,Vec2f,float> Vec2fTypes;

template<> inline const char* Vec2dTypes::Name() { return "Vec2d"; }
template<> inline const char* Vec2fTypes::Name() { return "Vec2f"; }

typedef ExtVectorTypes<Vec2d,Vec2d,double> ExtVec2dTypes;
typedef ExtVectorTypes<Vec2f,Vec2f,float> ExtVec2fTypes;

template<> inline const char* ExtVec2dTypes::Name() { return "ExtVec2d"; }
template<> inline const char* ExtVec2fTypes::Name() { return "ExtVec2f"; }

//
// 1D
//

typedef StdVectorTypes<Vec1d,Vec1d,double> Vec1dTypes;
typedef StdVectorTypes<Vec1f,Vec1f,float> Vec1fTypes;

template<> inline const char* Vec1dTypes::Name() { return "Vec1d"; }
template<> inline const char* Vec1fTypes::Name() { return "Vec1f"; }

typedef ExtVectorTypes<Vec1d,Vec1d,double> ExtVec1dTypes;
typedef ExtVectorTypes<Vec1f,Vec1f,float> ExtVec1fTypes;

template<> inline const char* ExtVec1dTypes::Name() { return "ExtVec1d"; }
template<> inline const char* ExtVec1fTypes::Name() { return "ExtVec1f"; }

//
// 6D (3 coordinates + 3 angles)
//

typedef StdVectorTypes<Vec6d,Vec6d,double> Vec6dTypes;
typedef StdVectorTypes<Vec6f,Vec6f,float> Vec6fTypes;

template<> inline const char* Vec6dTypes::Name() { return "Vec6d"; }
template<> inline const char* Vec6fTypes::Name() { return "Vec6f"; }

typedef ExtVectorTypes<Vec6d,Vec6d,double> ExtVec6dTypes;
typedef ExtVectorTypes<Vec6f,Vec6f,float> ExtVec6fTypes;

template<> inline const char* ExtVec6dTypes::Name() { return "ExtVec6d"; }
template<> inline const char* ExtVec6fTypes::Name() { return "ExtVec6f"; }

#ifdef SOFA_FLOAT
typedef Vec6fTypes Vec6Types;
typedef Vec3fTypes Vec3Types;
typedef Vec2fTypes Vec2Types;
typedef Vec1fTypes Vec1Types;
typedef ExtVec6fTypes ExtVec6Types;
typedef ExtVec3fTypes ExtVec3Types;
typedef ExtVec2fTypes ExtVec2Types;
typedef ExtVec1fTypes ExtVec1Types;
#else
typedef Vec6dTypes Vec6Types;
typedef Vec3dTypes Vec3Types;
typedef Vec2dTypes Vec2Types;
typedef Vec1dTypes Vec1Types;
typedef ExtVec6dTypes ExtVec6Types;
typedef ExtVec3dTypes ExtVec3Types;
typedef ExtVec2dTypes ExtVec2Types;
typedef ExtVec1dTypes ExtVec1Types;
#endif
} // namespace defaulttype

namespace core
{
namespace componentmodel
{
namespace behavior
{

template<class Coord, class Deriv, class Vec, class M, class SV>
Deriv inertiaForce( const SV& /*sv*/, const Vec& /*a*/, const M& /*m*/, const Coord& /*x*/, const Deriv& /*v*/ );

template <>
inline defaulttype::Vec<3, double> inertiaForce<
    defaulttype::Vec<3, double>,
    defaulttype::Vec<3, double>,
    objectmodel::BaseContext::Vec3,
    double,
    objectmodel::BaseContext::SpatialVector
>
(
    const objectmodel::BaseContext::SpatialVector& sv,
    const objectmodel::BaseContext::Vec3& a,
    const double& m,
    const defaulttype::Vec<3, double>& x,
    const defaulttype::Vec<3, double>& v
)
{
    const objectmodel::BaseContext::Vec3& omega=sv.getAngularVelocity();
    //std::cerr<<"inertiaForce, sv = "<<sv<<", omega ="<<omega<<", a = "<<a<<", m= "<<m<<", x= "<<x<<", v= "<<v<<std::endl;
    return -( a + omega.cross( omega.cross(x) + v*2 ))*m;
}

template <>
inline defaulttype::Vec<3, float> inertiaForce<
    defaulttype::Vec<3, float>,
    defaulttype::Vec<3, float>,
    objectmodel::BaseContext::Vec3,
    float,
    objectmodel::BaseContext::SpatialVector
>
(
    const objectmodel::BaseContext::SpatialVector& sv,
    const objectmodel::BaseContext::Vec3& a,
    const float& m,
    const defaulttype::Vec<3, float>& x,
    const defaulttype::Vec<3, float>& v
)
{
    const objectmodel::BaseContext::Vec3& omega=sv.getAngularVelocity();
    //std::cerr<<"inertiaForce, sv = "<<sv<<", omega ="<<omega<<", a = "<<a<<", m= "<<m<<", x= "<<x<<", v= "<<v<<std::endl;
    return -( a + omega.cross( omega.cross(x) + v*2 ))*m;
}

template <>
inline defaulttype::Vec<2, double> inertiaForce<
    defaulttype::Vec<2, double>,
    defaulttype::Vec<2, double>,
    objectmodel::BaseContext::Vec3,
    double,
    objectmodel::BaseContext::SpatialVector
>
(
    const objectmodel::BaseContext::SpatialVector& sv,
    const objectmodel::BaseContext::Vec3& a,
    const double& m,
    const defaulttype::Vec<2, double>& x,
    const defaulttype::Vec<2, double>& v
)
{
    double omega=(double)sv.getAngularVelocity()[2]; // z direction
    defaulttype::Vec<2, double> a2( (double)a[0], (double)a[1] );
    return -( a2 -( x*omega + v*2 )*omega )*m;
}

template <>
inline defaulttype::Vec<2, float> inertiaForce<
    defaulttype::Vec<2, float>,
    defaulttype::Vec<2, float>,
    objectmodel::BaseContext::Vec3,
    float,
    objectmodel::BaseContext::SpatialVector
>
(
    const objectmodel::BaseContext::SpatialVector& sv,
    const objectmodel::BaseContext::Vec3& a,
    const float& m,
    const defaulttype::Vec<2, float>& x,
    const defaulttype::Vec<2, float>& v
)
{
    float omega=(float)sv.getAngularVelocity()[2]; // z direction
    defaulttype::Vec<2, float> a2( (float)a[0], (float)a[1] );
    return -( a2 -( x*omega + v*2 )*omega )*m;
}

} // namespace behavoir

} // namespace componentmodel



namespace objectmodel
{

// Specialization of Field::getValueTypeString() method to display smaller aliases 
// The next line hides all those methods from the doxygen documentation

template<>
inline std::string BaseData::typeName< defaulttype::Vec1f >(const defaulttype::Vec1f *) { return "Vec1f"; }

template<>
inline std::string BaseData::typeName< std::vector< defaulttype::Vec1f > >(const std::vector< defaulttype::Vec1f > *) { return "vector<Vec1f>"; }

template<>
inline std::string BaseData::typeName< helper::vector< defaulttype::Vec1f > >(const helper::vector< defaulttype::Vec1f > *) { return "vector<Vec1f>"; }

template<>
inline std::string BaseData::typeName< defaulttype::Vec1d >(const defaulttype::Vec1d *) { return "Vec1d"; }

template<>
inline std::string BaseData::typeName< std::vector< defaulttype::Vec1d > >(const std::vector< defaulttype::Vec1d > *) { return "vector<Vec1d>"; }

template<>
inline std::string BaseData::typeName< helper::vector< defaulttype::Vec1d > >(const helper::vector< defaulttype::Vec1d > *) { return "vector<Vec1d>"; }

template<>
inline std::string BaseData::typeName< defaulttype::Vec2d >(const defaulttype::Vec2d *) { return "Vec2d"; }

template<>
inline std::string BaseData::typeName< std::vector< defaulttype::Vec2d > >(const std::vector< defaulttype::Vec2d > *) { return "vector<Vec2d>"; }

template<>
inline std::string BaseData::typeName< helper::vector< defaulttype::Vec2d > >(const helper::vector< defaulttype::Vec2d > *) { return "vector<Vec2d>"; }

template<>
inline std::string BaseData::typeName< defaulttype::Vec3d >(const defaulttype::Vec3d *) { return "Vec3d"; }

template<>
inline std::string BaseData::typeName< std::vector< defaulttype::Vec3d > >(const std::vector< defaulttype::Vec3d > *) { return "vector<Vec3d>"; }

template<>
inline std::string BaseData::typeName< helper::vector< defaulttype::Vec3d > >(const helper::vector< defaulttype::Vec3d > *) { return "vector<Vec3d>"; }

template<>
inline std::string BaseData::typeName< defaulttype::Vec6d >(const defaulttype::Vec6d *) { return "Vec6d"; }

template<>
inline std::string BaseData::typeName< std::vector< defaulttype::Vec6d > >(const std::vector< defaulttype::Vec6d > *) { return "vector<Vec6d>"; }

template<>
inline std::string BaseData::typeName< helper::vector< defaulttype::Vec6d > >(const helper::vector< defaulttype::Vec6d > *) { return "vector<Vec6d>"; }


} // namespace objectmodel

} // namespace core

} // namespace sofa

#endif