MechanicalMatrixVisitor.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_SIMULATION_MECHANICALMATRIXVISITOR_H
#define SOFA_SIMULATION_MECHANICALMATRIXVISITOR_H

#if !defined(__GNUC__) || (__GNUC__ > 3 || (_GNUC__ == 3 && __GNUC_MINOR__ > 3))
#pragma once
#endif


#include <sofa/simulation/common/Visitor.h>
#include <sofa/core/componentmodel/behavior/BaseMechanicalState.h>
#include <sofa/core/componentmodel/behavior/BaseMechanicalMapping.h>
#include <sofa/core/componentmodel/behavior/Mass.h>
#include <sofa/core/componentmodel/behavior/ForceField.h>
#include <sofa/core/componentmodel/behavior/InteractionForceField.h>
#include <sofa/core/componentmodel/behavior/InteractionConstraint.h>
#include <sofa/core/componentmodel/behavior/Constraint.h>
#include <sofa/defaulttype/BaseMatrix.h>
#include <sofa/defaulttype/BaseVector.h>
#include <iostream>
using std::cerr;
using std::endl;

namespace sofa
{

  namespace simulation
  {


    using namespace sofa::defaulttype;
    class MechanicalMatrixVisitor : public Visitor
    {
    public:
      typedef sofa::core::componentmodel::behavior::BaseMechanicalState::VecId VecId;



      virtual Result processNodeTopDown(simulation::Node* node);
    
      virtual Result fwdOdeSolver(simulation::Node* /*node*/, core::componentmodel::behavior::OdeSolver* /*solver*/)
      {
        return RESULT_CONTINUE;
      }
    
      virtual Result fwdMechanicalMapping(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalMapping* /*map*/)
      {
        return RESULT_CONTINUE;
      }

      virtual Result fwdMappedMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* /*mm*/)
      {
        return RESULT_PRUNE;
      }

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* /*mm*/)
      {
        return RESULT_CONTINUE;
      }

      virtual Result fwdMass(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMass* /*mass*/)
      {
        return RESULT_CONTINUE;
      }

      virtual Result fwdForceField(simulation::Node* /*node*/, core::componentmodel::behavior::BaseForceField* /*ff*/)
      {
        return RESULT_CONTINUE;
      }


      virtual Result fwdInteractionForceField(simulation::Node* node, core::componentmodel::behavior::InteractionForceField* ff)
      {
        return fwdForceField(node, ff);
      }

      virtual Result fwdConstraint(simulation::Node* /*node*/, core::componentmodel::behavior::BaseConstraint* /*c*/)
      {
        return RESULT_CONTINUE;
      }

      virtual Result fwdInteractionConstraint(simulation::Node* node, core::componentmodel::behavior::InteractionConstraint* c)
      {
        return fwdConstraint(node, c);
      }




      virtual void processNodeBottomUp(simulation::Node* node);
    
      virtual void bwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* /*mm*/)
      {}

      virtual void bwdMappedMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* /*mm*/)
      {}
    
      virtual void bwdMechanicalMapping(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalMapping* /*map*/)
      {}
    
      virtual void bwdOdeSolver(simulation::Node* /*node*/, core::componentmodel::behavior::OdeSolver* /*solver*/)
      {}
    


      virtual const char* getCategoryName() const
      {
        return "animate";
      }

      unsigned int offsetOnEnter, offsetOnExit;
    };


    class MechanicalGetMatrixDimensionVisitor : public MechanicalMatrixVisitor
    {
    public:
      unsigned int * const nbRow;
      unsigned int * const nbCol;
    MechanicalGetMatrixDimensionVisitor(unsigned int * const _nbRow, unsigned int * const _nbCol)
      : nbRow(_nbRow), nbCol(_nbCol)
      {}

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* ms)
      {
    ms->contributeToMatrixDimension(nbRow, nbCol);
    return RESULT_CONTINUE;
      }
    };

    
    class MechanicalAddMBK_ToMatrixVisitor : public MechanicalMatrixVisitor
    {
    public:
      BaseMatrix *mat;
      double m, b, k;
      //    unsigned int offset, offsetBckUp;

    MechanicalAddMBK_ToMatrixVisitor(BaseMatrix *_mat, double _m=0.0, double _b=0.0, double _k=0.0, unsigned int _offset=0)
      : mat(_mat),m(_m),b(_b),k(_k)
      {
    offsetOnEnter = _offset;
    offsetOnExit = _offset;
      }

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* ms)
      {
    ms->setOffset(offsetOnExit);
    return RESULT_CONTINUE;
      }

      virtual Result fwdForceField(simulation::Node* /*node*/, core::componentmodel::behavior::BaseForceField* ff)
      {
    if ((mat != NULL)&&(k!=0.0))
      {
        //offsetOnExit = offsetOnEnter;
          ff->addMBKToMatrix(mat,m,b,k,offsetOnEnter);
      }

        return RESULT_CONTINUE;
      }

      //Masses are now added in the addMBKToMatrix call for all ForceFields
      /*
      virtual Result fwdMass(simulation::Node*, core::componentmodel::behavior::BaseMass* mass)
      {
    if ((mat != NULL)&&(m!=0.0))
      {
        //offsetOnExit = offsetOnEnter;
        mass->addMToMatrix(mat,m,offsetOnEnter);
      }

        return RESULT_CONTINUE;
      }
      */

      virtual Result fwdConstraint(simulation::Node* /*node*/, core::componentmodel::behavior::BaseConstraint* c)
      {
    if (mat != NULL)
      {
        //offsetOnExit = offsetOnEnter;
        c->applyConstraint(mat, offsetOnEnter);
      }

        return RESULT_CONTINUE;
      }
    };

#if 0 // deprecated: as dx is stored in MechanicalState, compute df there and then convert to BaseVector using MechanicalMultiVector2BaseVectorVisitor

    class MechanicalAddMBKdx_ToVectorVisitor : public MechanicalMatrixVisitor
    {
    public:
      BaseVector *vect;
      VecId dx;
      double m, b, k;
      //    unsigned int offset, offsetBckUp;

    MechanicalAddMBKdx_ToVectorVisitor(BaseVector *_vect, VecId _dx, double _m=0.0, double _b=0.0, double _k=0.0, unsigned int _offset=0)
      : vect(_vect), dx(_dx), m(_m),b(_b),k(_k)
      {
    offsetOnEnter = _offset;
    offsetOnExit = _offset;
      }

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* mm)
      {
    mm->setOffset(offsetOnExit);

        if (!dx.isNull())
      mm->setDx(dx);
        return RESULT_CONTINUE;
      }

      virtual Result fwdForceField(simulation::Node* /*node*/, core::componentmodel::behavior::BaseForceField* ff)
      {
    if ((vect != NULL)&&(k != 0.0))
      {
        //  offsetOnExit = offsetOnEnter;
        ff->addKDxToVector(vect,k,offsetOnEnter);
      }

        return RESULT_CONTINUE;
      }

      virtual Result fwdMass(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMass* mass)
      {
    if ((vect != NULL)&&(m != 0.0))
      {
        //  offsetOnExit = offsetOnEnter;
        if (dx.isNull())
          std::cout << "Dx Null\n";
        else
          std::cout << "Dx Not Null\n";
            
        mass->addMDxToVector(vect,m,offsetOnEnter,dx.isNull());
      }

        return RESULT_CONTINUE;
      }

      virtual Result fwdConstraint(simulation::Node* /*node*/, core::componentmodel::behavior::BaseConstraint* c)
      {
    if (vect != NULL)
      {
        //  offsetOnExit = offsetOnEnter;
        c->applyConstraint(vect, offsetOnEnter);
      }

        return RESULT_CONTINUE;
      }
    };
#endif

    class MechanicalMultiVector2BaseVectorVisitor : public MechanicalMatrixVisitor
    {
    public:
      VecId src;
      BaseVector *vect;
      unsigned int offset;

    MechanicalMultiVector2BaseVectorVisitor(VecId _src, defaulttype::BaseVector * _vect, unsigned int _offset=0)
      : src(_src),vect(_vect),offset(_offset)
      {
      }

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* mm)
      {
    if (vect!= NULL)
      {
        mm->loadInBaseVector(vect, src, offset);
      }

        return RESULT_CONTINUE;
      }
    };

    class MechanicalMultiVectorPeqBaseVectorVisitor : public MechanicalMatrixVisitor
    {
    public:
      BaseVector *src;
      VecId dest;
      unsigned int offset;

    MechanicalMultiVectorPeqBaseVectorVisitor(VecId _dest, defaulttype::BaseVector * _src, unsigned int _offset=0)
      : src(_src),dest(_dest),offset(_offset)
      {
      }

      virtual Result fwdMechanicalState(simulation::Node* /*node*/, core::componentmodel::behavior::BaseMechanicalState* mm)
      {
    if (src!= NULL)
      {
        mm->addBaseVectorToState(dest, src, offset);
      }

        return RESULT_CONTINUE;
      }
    };

  } // namespace simulation

} // namespace sofa

#endif