JointSpringForceField.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_JOINTSPRINGFORCEFIELD_H
#define SOFA_COMPONENT_FORCEFIELD_JOINTSPRINGFORCEFIELD_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 JointSpringForceFieldInternalData
{
public:
};

template<class DataTypes>
class JointSpringForceField : 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> Vector;


  class Spring
  {
    public:
      int  m1, m2;          
      Real kd;              
      Vector  initTrans;        
      Quat initRot;         
      Quat lawfulTorsion;   
      Quat extraTorsion;    

      sofa::defaulttype::Vec<6,bool> freeMovements; 
      Real softStiffnessTrans;  
      Real hardStiffnessTrans;  
      Real softStiffnessRot;    
      Real hardStiffnessRot;    
      Real blocStiffnessRot;    

      sofa::defaulttype::Vec<6,Real> limitAngles; 

      Vector bloquage;


      Spring()
      : m1(0), m2(0), kd(0), lawfulTorsion(0,0,0,1), extraTorsion(0,0,0,1)
      , softStiffnessTrans(0), hardStiffnessTrans(10000), softStiffnessRot(0), hardStiffnessRot(10000), blocStiffnessRot(100)
      //, freeMovements(0,0,0,1,1,1), limitAngles(-100000, 100000, -100000, 100000, -100000, 100000)
      {
          freeMovements = sofa::defaulttype::Vec<6,bool>(0,0,0,1,1,1);
          limitAngles = sofa::defaulttype::Vec<6,Real>(-100000, 100000, -100000, 100000, -100000, 100000);
          initTrans = Vector(0,0,0);
          initRot = Quat(0,0,0,1);
      }

      Spring(int m1, int m2)
      : m1(m1), m2(m2), kd(0), lawfulTorsion(0,0,0,1), extraTorsion(0,0,0,1)
      , softStiffnessTrans(0), hardStiffnessTrans(10000), softStiffnessRot(0), hardStiffnessRot(10000), blocStiffnessRot(100)
      //, freeMovements(0,0,0,1,1,1), limitAngles(-100000, 100000, -100000, 100000, -100000, 100000)
      {
          freeMovements = sofa::defaulttype::Vec<6,bool>(0,0,0,1,1,1);
          limitAngles = sofa::defaulttype::Vec<6,Real>(-100000, 100000, -100000, 100000, -100000, 100000);
          initTrans = Vector(0,0,0);
          initRot = Quat(0,0,0,1);
      }

      Spring(int m1, int m2, Real softKst, Real hardKst, Real softKsr, Real hardKsr, Real blocKsr, Real axmin, Real axmax, Real aymin, Real aymax, Real azmin, Real azmax, Real kd)
      : m1(m1), m2(m2), kd(kd), lawfulTorsion(0,0,0,1), extraTorsion(0,0,0,1)
      //,limitAngles(axmin,axmax,aymin,aymax,azmin,azmax)
      , softStiffnessTrans(softKst), hardStiffnessTrans(hardKst), softStiffnessRot(softKsr), hardStiffnessRot(hardKsr), blocStiffnessRot(blocKsr)
      {
          limitAngles = sofa::defaulttype::Vec<6,Real>(axmin,axmax,aymin,aymax,azmin,azmax);
          freeMovements = sofa::defaulttype::Vec<6,bool>(false, false, false, true, true, true);
          for (unsigned int i=0; i<3; i++){
            if(limitAngles[2*i]==limitAngles[2*i+1])
                freeMovements[3+i] = false;
          }
          initTrans = Vector(0,0,0);
          initRot = Quat(0,0,0,1);
      }
      
      //accessors
      Real getHardStiffnessRotation() {return hardStiffnessRot;}
      Real getSoftStiffnessRotation() {return softStiffnessRot;}
      Real getHardStiffnessTranslation() {return hardStiffnessTrans;}
      Real getSoftStiffnessTranslation() {return softStiffnessTrans;}
      Real getBlocStiffnessRotation() { return blocStiffnessRot; }
      sofa::defaulttype::Vec<6,Real> getLimitAngles() { return limitAngles;}
      sofa::defaulttype::Vec<6,bool> getFreeAxis() { return freeMovements;}
      Vector getInitLength() { return initTrans; }
      Quat getInitOrientation() { return initRot; }

      //affectors
      void setHardStiffnessRotation(Real ksr){    hardStiffnessRot = ksr;  }
      void setSoftStiffnessRotation(Real ksr){    softStiffnessRot = ksr;  }
      void setHardStiffnessTranslation(Real kst){ hardStiffnessTrans = kst;  }
      void setSoftStiffnessTranslation(Real kst){ softStiffnessTrans = kst;  }
      void setBlocStiffnessRotation(Real ksb){    blocStiffnessRot = ksb;  }
      void setLimitAngles(const sofa::defaulttype::Vec<6,Real>& lims) { 
          limitAngles = lims; 
            if(lims[0]==lims[1]) freeMovements[3]=false;
            if(lims[2]==lims[3]) freeMovements[4]=false;
            if(lims[4]==lims[5]) freeMovements[5]=false;
      }
      void setLimitAngles(Real minx, Real maxx, Real miny, Real maxy, Real minz, Real maxz) {
          limitAngles = sofa::defaulttype::Vec<6,Real>(minx, maxx, miny, maxy, minz, maxz);
          if(minx==maxx) freeMovements[3]=false;
          if(miny==maxy) freeMovements[4]=false;
          if(minz==maxz) freeMovements[5]=false;
      }
      void setInitLength( const Vector& l) { initTrans=l; }
      void setInitOrientation( const Quat& o) { initRot=o; }
      void setInitOrientation( const Vector& o) { initRot=Quat::createFromRotationVector(o); }
      void setFreeAxis(const sofa::defaulttype::Vec<6,bool>& axis) { freeMovements = axis; }
      void setFreeAxis(bool isFreeTx, bool isFreeTy, bool isFreeTz, bool isFreeRx, bool isFreeRy, bool isFreeRz)  {
         freeMovements = sofa::defaulttype::Vec<6,bool>(isFreeTx, isFreeTy, isFreeTz, isFreeRx, isFreeRy, isFreeRz);
      }
      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
        s.freeMovements = sofa::defaulttype::Vec<6,bool>(false, false, false, true, true, true);
        s.initTrans = Vector(0,0,0);
        s.initRot = Quat(0,0,0,1);
        s.blocStiffnessRot = 0.0;
        //by default no angle limitation is set (bi values for initialisation)
        s.limitAngles = sofa::defaulttype::Vec<6,Real>(-100000., 100000., -100000., 100000., -100000., 100000.);

        std::string str;
        in>>str;
        if(str == "BEGIN_SPRING"){
            in>>s.m1>>s.m2; //read references
            in>>str;
            while(str != "END_SPRING"){
                if(str == "FREE_AXIS")
                      in>>s.freeMovements;
                else if(str == "KS_T")
                      in>>s.softStiffnessTrans>>s.hardStiffnessTrans;
                else if(str == "KS_R")
                      in>>s.softStiffnessRot>>s.hardStiffnessRot;
                else if(str == "KS_B")
                      in>>s.blocStiffnessRot;
                else if(str == "KD")
                      in>>s.kd;
                else if(str == "R_LIM_X")
                      in>>s.limitAngles[0]>>s.limitAngles[1];
                else if(str == "R_LIM_Y")
                      in>>s.limitAngles[2]>>s.limitAngles[3];
                else if(str == "R_LIM_Z")
                      in>>s.limitAngles[4]>>s.limitAngles[5];
                else if(str == "REST_T")
                      in>>s.initTrans;
                else if(str == "REST_R")
                      in>>s.initRot;
                else{
                    std::cerr<<"Error parsing Spring : Unknown Attribute "<<str<<std::endl;
                      return in;
                }

                in>>str;
            }
        }

        //if no blocStiffnessRot was specified (typically 0), we use hardStiffnessRot/100
        if(s.blocStiffnessRot == 0.0)
            s.blocStiffnessRot = s.hardStiffnessRot/100;

        //if limit angle were specified, free rotation axis are set from them
        for (unsigned int i=0; i<3; i++){
            if(s.limitAngles[2*i]==s.limitAngles[2*i+1])
                s.freeMovements[3+i] = false;
        }
      
        return in;
      }
      
      friend std::ostream& operator << ( std::ostream& out, const Spring& s )
      {
          out<<"BEGIN_SPRING  "<<s.m1<<" "<<s.m2<<"  ";

          if (s.freeMovements[0]!=false || s.freeMovements[1]!=false || s.freeMovements[2]!=false || s.freeMovements[3]!=true || s.freeMovements[4]!=true || s.freeMovements[5]!=true)
              out<<"FREE_AXIS "<<s.freeMovements<<"  ";
          if (s.softStiffnessTrans != 0.0 || s.hardStiffnessTrans != 10000.0)
              out<<"KS_T "<<s.softStiffnessTrans<<" "<<s.hardStiffnessTrans<<"  ";
          if (s.softStiffnessRot != 0.0 || s.hardStiffnessRot != 10000.0)
              out<<"KS_R "<<s.softStiffnessRot<<" "<<s.hardStiffnessRot<<"  ";
          if (s.blocStiffnessRot != s.hardStiffnessRot/100)
              out<<"KS_B "<<s.blocStiffnessRot<<"  ";
          if (s.kd != 0.0)
              out<<"KD "<<s.kd<<"  ";
          if (s.limitAngles[0]!=-100000 || s.limitAngles[1] != 100000)
              out<<"R_LIM_X "<<s.limitAngles[0]<<" "<<s.limitAngles[1]<<"  ";
          if (s.limitAngles[2]!=-100000 || s.limitAngles[3] != 100000)
              out<<"R_LIM_Y "<<s.limitAngles[2]<<" "<<s.limitAngles[3]<<"  ";
          if (s.limitAngles[4]!=-100000 || s.limitAngles[5] != 100000)
              out<<"R_LIM_Z "<<s.limitAngles[4]<<" "<<s.limitAngles[5]<<"  ";
          if (s.initTrans!= Vector())
              out<<"REST_T "<<s.initTrans<<"  ";
          if (s.initRot[3]!= 1)
              out<<"REST_R "<<s.initRot<<"  ";

          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;
    
    JointSpringForceFieldInternalData<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:
    JointSpringForceField(MechanicalState* object1, MechanicalState* object2);  
    JointSpringForceField();
    
    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)
    {
        helper::vector<Spring>& springs = *this->springs.beginEdit();
        springs.clear();
        if (reserve) springs.reserve(reserve);
        this->springs.endEdit();
    }

    void addSpring(const Spring& s)
    {
        springs.beginEdit()->push_back(s);
        springs.endEdit();
    }


    void addSpring(int m1, int m2, Real softKst, Real hardKst, Real softKsr, Real hardKsr, Real blocKsr, Real axmin, Real axmax, Real aymin, Real aymax, Real azmin, Real azmax, Real kd)
    {
        Spring s(m1,m2,softKst,hardKst,softKsr,hardKsr, blocKsr, axmin, axmax, aymin, aymax, azmin, azmax, kd);
        s.initTrans = Vector(0,0,0);
        s.initRot = Quat(0,0,0,1);

        springs.beginEdit()->push_back(s);
        springs.endEdit();
    }

};

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif