FrameSpringForceField.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 :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#ifndef SOFA_COMPONENT_FORCEFIELD_FRAMESPRINGFORCEFIELD_H
#define SOFA_COMPONENT_FORCEFIELD_FRAMESPRINGFORCEFIELD_H

#include <sofa/core/componentmodel/behavior/PairInteractionForceField.h>
#include <sofa/core/componentmodel/behavior/MechanicalState.h>
#include <sofa/defaulttype/Vec.h>
#include <vector>
#include <sofa/defaulttype/Mat.h>
using namespace sofa::defaulttype;

namespace sofa
{

  namespace component
  {

    namespace forcefield
    {

      template<class DataTypes>
      class FrameSpringForceFieldInternalData
      {
        public:
      };

      template<class DataTypes>
      class FrameSpringForceField : public core::componentmodel::behavior::PairInteractionForceField<DataTypes>, public virtual core::objectmodel::BaseObject
      {
        public:
          typedef typename core::componentmodel::behavior::PairInteractionForceField<DataTypes> Inherit;
          typedef typename DataTypes::VecCoord VecCoord;
          typedef typename DataTypes::VecDeriv VecDeriv;
          typedef typename DataTypes::Coord Coord;
          typedef typename DataTypes::Deriv Deriv;
          typedef typename Coord::value_type Real;
          typedef core::componentmodel::behavior::MechanicalState<DataTypes> MechanicalState;
          enum { N=Coord::static_size };
          typedef defaulttype::Mat<N,N,Real> Mat;
          typedef Vec<N,Real> VecN;


          class Spring
          {
            public:
              int  m1, m2;             
              VecN vec1, vec2;
              Real kd;                 

              Real stiffnessTrans;  
              Real stiffnessRot;    

              Spring ( int m1=0, int m2=0, Real softKst=0, Real softKsr=0, Real kd=0 )
                  : m1 ( m1 ), m2 ( m2 ), kd ( kd ), stiffnessTrans ( softKst ), stiffnessRot ( softKsr )
              {
              }

              //accessors
              Real getStiffnessRotation() {return stiffnessRot;}
              Real getStiffnessTranslation() {return stiffnessTrans;}
              VecN getInitVec1() {return vec1;}

              //affectors
              void setStiffnessRotation ( Real ksr ) {    stiffnessRot = ksr;  }
              void setStiffnessTranslation ( Real kst ) { stiffnessTrans = kst;  }
              void setInitVec1 ( const VecN& l ) { vec1=l; }
              void setInitVec2 ( const VecN& l ) { vec2=l; }
              void setDamping ( Real _kd ) {  kd = _kd;   }


              inline friend std::istream& operator >> ( std::istream& in, Spring& s )
              {
                //default joint is a free rotation joint --> translation is bloqued, rotation is free

                std::string str;
                in>>str;
                if ( str == "BEGIN_SPRING" )
                {
                  in>>s.m1>>s.m2; //read references
                  in>>str;
                  while ( str != "END_SPRING" )
                  {
                    if ( str == "KS_T" )
                      in>>s.stiffnessTrans;
                    else if ( str == "KS_R" )
                      in>>s.stiffnessRot;
                    else if ( str == "KD" )
                      in>>s.kd;
                    else if ( str == "VEC1" )
                      in>>s.vec1;
                    else if ( str == "VEC2" )
                      in>>s.vec2;
                    else
                    {
                      std::cerr<<"Error parsing Spring : Unknown Attribute "<<str<<std::endl;
                      return in;
                    }

                    in>>str;
                  }
                }

                return in;
              }

              friend std::ostream& operator << ( std::ostream& out, const Spring& s )
              {
                out<<"BEGIN_SPRING  "<<s.m1<<" "<<s.m2<<"  ";

                if ( s.stiffnessTrans != 0.0 )
                  out<<"KS_T "<<s.stiffnessTrans<<"  ";
                if ( s.stiffnessRot != 0.0 )
                  out<<"KS_R "<<s.stiffnessRot<<"  ";
                if ( s.kd != 0.0 )
                  out<<"KD "<<s.kd<<"  ";
                if ( s.vec1!= VecN ( 0, 0, 0 ) )
                  out<<"VEC1 "<<s.vec1<<"  ";
                if ( s.vec2!= VecN ( 0, 0, 0 ) )
                  out<<"VEC2 "<<s.vec2<<"  ";

                out<<"END_SPRING"<<std::endl;
                return out;
              }

          };
// end inner class spring



        protected:

          double m_potentialEnergy;
          Data<sofa::helper::vector<Spring> > springs;
          VecCoord springRef;
          Data<bool> showLawfulTorsion;
          Data<bool> showExtraTorsion;

          FrameSpringForceFieldInternalData<DataTypes> data;


          void addSpringForce ( double& potentialEnergy, VecDeriv& f1, const VecCoord& p1, const VecDeriv& v1, VecDeriv& f2, const VecCoord& p2, const VecDeriv& v2, int i, /*const*/ Spring& spring );
          void addSpringDForce ( VecDeriv& df1, const VecDeriv& dx1, VecDeriv& df2, const VecDeriv& dx2, int i, /*const*/ Spring& spring );



        public:
          FrameSpringForceField ( MechanicalState* object1, MechanicalState* object2 );
          FrameSpringForceField();

          core::componentmodel::behavior::MechanicalState<DataTypes>* getObject1() { return this->mstate1; }
          core::componentmodel::behavior::MechanicalState<DataTypes>* getObject2() { return this->mstate2; }

          virtual void init();

          virtual void addForce ( VecDeriv& f1, VecDeriv& f2, const VecCoord& x1, const VecCoord& x2, const VecDeriv& v1, const VecDeriv& v2 );

          virtual void addDForce ( VecDeriv& df1, VecDeriv& df2, const VecDeriv& dx1, const VecDeriv& dx2 );

          virtual double getPotentialEnergy ( const VecCoord&, const VecCoord& ) { return m_potentialEnergy; }

          sofa::helper::vector<Spring> * getSprings() { return springs.beginEdit(); }

          void draw();

          // -- Modifiers

          void clear ( int reserve=0 );

          void addSpring ( const Spring& s );

          void addSpring ( int m1, int m2, Real softKst, Real softKsr, Real kd );

      };

    } // namespace forcefield

  } // namespace component

} // namespace sofa

#endif