SolidTypes.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                             *
******************************************************************************/
// C++ Interface: SolidTypes
//
// Description: Base types for coordinate systems and articulated bodies
//
//
// Author: Francois Faure, INRIA-UJF, (C) 2006
//
// Copyright: See COPYING file that comes with this distribution
//
//
#ifndef SOFA_DEFAULTTYPE_SOLIDTYPES_H
#define SOFA_DEFAULTTYPE_SOLIDTYPES_H

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

namespace sofa
{

namespace defaulttype
{

template< class R=float >
class SolidTypes
{
public:
    typedef R Real;
    typedef defaulttype::Vec<3,Real> Vec;
    typedef helper::Quater<Real> Rot;
    typedef defaulttype::Mat<3,3,Real> Mat;
    typedef defaulttype::Mat<6,6,Real> Mat66;
    typedef defaulttype::Vec<6,Real> DOF;



    class SpatialVector
    {
    public:
        Vec lineVec;
        Vec freeVec;
        void clear();
        SpatialVector();
        SpatialVector( const Vec& l, const Vec& f );
        SpatialVector& operator += (const SpatialVector& v);

        //template<class Real2>
        SpatialVector operator * ( Real a ) const
        {
            return SpatialVector( lineVec *a, freeVec * a);
        }

        //template<class Real2>
        SpatialVector& operator *= ( Real a )
        {
            lineVec *=a;
            freeVec *= a;
            return *this;
        }

        SpatialVector operator + ( const SpatialVector& v ) const;
        SpatialVector operator - ( const SpatialVector& v ) const;
        SpatialVector operator - ( ) const;
        Real operator * ( const SpatialVector& v ) const;
        SpatialVector cross( const SpatialVector& v ) const;
        SpatialVector operator * (const Mat66&) const;
        inline friend std::ostream& operator << (std::ostream& out, const SpatialVector& t )
        {
            out<<t.lineVec<<" "<<t.freeVec;
            return out;
        }
        Vec& getLinearVelocity()
        {
            return freeVec;
        }
        const Vec& getLinearVelocity() const
        {
            return freeVec;
        }
        void setLinearVelocity(const Vec& v)
        {
            freeVec = v;
        }
        Vec& getAngularVelocity()
        {
            return lineVec;
        }
        const Vec& getAngularVelocity() const
        {
            return lineVec;
        }
        void setAngularVelocity(const Vec& v)
        {
            lineVec = v;
        }

        Vec& getTorque()
        {
            return freeVec;
        }
        const Vec& getTorque() const
        {
            return freeVec;
        }
        void setTorque(const Vec& v)
        {
            freeVec = v;
        }
        Vec& getForce() 
        {
            return lineVec;
        }
        const Vec& getForce() const
        {
            return lineVec;
        }
        void setForce(const Vec& v)
        {
            lineVec = v;
        }
    };

    class Transform
    {
    public:
        Transform();
        Transform( const Vec& origin, const Rot& orientation );
        Transform( const Rot& q, const Vec& o );
        void set( const Vec& t, const Rot& q );
        void clear();
        static Transform identity();
        //static Transform inParent(const Vec& t, const Rot& r);
        Transform( const SpatialVector& v );
        Transform inversed() const;
        const Vec& getOriginOfParentInChild() const;
        Vec getOrigin() const;
        void setOrigin( const Vec& );
        const Rot& getOrientation() const;
        void setOrientation( const Rot& );
        Mat getRotationMatrix() const;
        Vec projectVector( const Vec& vectorInChild ) const;
        Vec projectPoint( const Vec& pointInChild ) const;
        Vec backProjectVector( const Vec& vectorInParent ) const;
        Vec backProjectPoint( const Vec& pointInParent ) const;
        Transform operator * (const Transform& f2) const;
        Transform& operator *= (const Transform& f2);
        SpatialVector operator * (const SpatialVector& sv ) const;
        SpatialVector operator / (const SpatialVector& sv ) const;
        void writeOpenGlMatrix( double *m ) const;
        void glDraw() const;
        inline friend std::ostream& operator << (std::ostream& out, const Transform& t )
        {
            out<<"("<<t.getOrigin()<<")";
            out<<"("<<t.getOrientation()<<")";
            return out;
        }
        void printInternal( std::ostream&) const;



        Transform& operator +=(const SpatialVector& a);

        Transform& operator +=(const Transform& a);

        template<class Real2>
        Transform& operator*=(Real2 a)
        {
            std::cout << "SolidTypes<R>::Transform::operator *="<<std::endl;
            origin_ *= a;
            //orientation *= a;
            return *this;
        }

        template<class Real2>
        Transform operator*(Real2 a) const
        {
            Transform r = *this;
            r*=a;
            return r;
        }


    protected:
        Rot orientation_; 
        Vec origin_;  

    };


    class RigidInertia
    {
    public:
        Real m;  
        Vec h;   
        Mat I;  
        RigidInertia();
        RigidInertia( Real m, const Vec& h, const Mat& I );
        SpatialVector operator * (const SpatialVector& v ) const;
        RigidInertia operator * ( const Transform& t ) const;
        inline friend std::ostream& operator << (std::ostream& out, const RigidInertia& r )
        {
            out<<"I= "<<r.I<<std::endl;
            out<<"h= "<<r.h<<std::endl;
            out<<"m= "<<r.m<<std::endl;
            return out;
        }
    };

    class ArticulatedInertia
    {
    public:
        Mat M;
        Mat H;
        Mat I;
        ArticulatedInertia();
        ArticulatedInertia( const Mat& M, const Mat& H, const Mat& I );
        SpatialVector operator * (const SpatialVector& v ) const;
        ArticulatedInertia operator * ( Real r ) const;
        ArticulatedInertia& operator = (const RigidInertia& Ri );
        ArticulatedInertia& operator += (const ArticulatedInertia& Ai );
        ArticulatedInertia operator + (const ArticulatedInertia& Ai ) const;
        ArticulatedInertia operator - (const ArticulatedInertia& Ai ) const;
        inline friend std::ostream& operator << (std::ostream& out, const ArticulatedInertia& r )
        {
            out<<"I= "<<r.I<<std::endl;
            out<<"H= "<<r.H<<std::endl;
            out<<"M= "<<r.M<<std::endl;
            return out;
        }
        void copyTo( Mat66& ) const;
    };

    typedef Transform Coord;
    typedef SpatialVector Deriv;
    typedef Coord VecCoord;
    typedef Deriv VecDeriv;
    typedef Real  VecReal;
    //         typedef std::vector<Coord> VecCoord;
    //         typedef std::vector<SpatialVector> VecDeriv;


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

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

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

    typedef helper::vector<SparseVecDeriv> VecConst;


    static Mat dyad( const Vec& u, const Vec& v );

    static Vec mult( const Mat& m, const Vec& v );

    static Vec multTrans( const Mat& m, const Vec& v );

    static Mat crossM( const Vec& v );

    static ArticulatedInertia dyad ( const SpatialVector& u, const SpatialVector& v );
    
    static const char* Name()
    {
        return "Solid";
    }
};

}// defaulttype

}// sofa



#endif