TranslationConstraint.inl

Go to the documentation of this file.

/*****************************************************************************
 * $CAMITK_LICENCE_BEGIN$
 *
 * CamiTK - Computer Assisted Medical Intervention ToolKit
 * (c) 2001-2025 Univ. Grenoble Alpes, CNRS, Grenoble INP - UGA, TIMC, 38000 Grenoble, France
 *
 * Visit http://camitk.imag.fr for more information
 *
 * This file is part of CamiTK.
 *
 * CamiTK is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * CamiTK 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 version 3 for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with CamiTK.  If not, see <http://www.gnu.org/licenses/>.
 *
 * $CAMITK_LICENCE_END$
 ****************************************************************************/
 
#ifndef TRANSLATION_CONSTRAINT_INL
#define TRANSLATION_CONSTRAINT_INL
 
#include "TranslationConstraint.h"
#include <sofa/helper/system/config.h>
#include <sofa/helper/gl/template.h>
 
using namespace std;
using namespace sofa::core::behavior;
 
template<class DataTypes>
TranslationConstraint<DataTypes>::TranslationConstraint(Loads* loadsList, const map<unsigned int, unsigned int> &atomIndexToDOFIndex, MechanicalState<DataTypes> *mm) : ProjectiveConstraintSet<DataTypes>(mm) {
  atomToDOFIndexes = atomIndexToDOFIndex;
  mmodel = mm;
  
  SReal dirX, dirY, dirZ;
  this->setName("TranslationConstraint");
  initTime=0;
  //for each load, we search which ones are translations applied on the body nodes
  for (unsigned int i = 0 ; i < loadsList->numberOfLoads() ; i++) {
    Load * load = loadsList->getLoad(i);
    if (load->getType() == "Translation") {
      if (load->getDirection().isToward()) {
        std::map<unsigned int, unsigned int>::const_iterator titi = atomIndexToDOFIndex.find(load->getDirection().getToward());
        if (titi != atomIndexToDOFIndex.end()){
          unsigned int dofInd = titi->second;
          dirX = (*mm->getX())[dofInd].x();
          dirY = (*mm->getX())[dofInd].y();
          dirZ = (*mm->getX())[dofInd].z();
        }
      }
      else
        load->getDirection(dirX, dirY, dirZ);
      unsigned int cpt = 0;
      for (unsigned int j = 0 ; j < load->numberOfTargets(); j++) {
        std::map<unsigned int, unsigned int>::const_iterator result = atomIndexToDOFIndex.find(load->getTarget(j));
        if (result != atomIndexToDOFIndex.end()) {
          cpt++;
          if (load->getDirection().isToward())
            addConstraint(result->second, Deriv(dirX - (*mm->getX())[result->second].x(), dirY - (*mm->getX())[result->second].y(), dirZ - (*mm->getX())[result->second].z()));
          else
            addConstraint(result->second, Deriv(dirX, dirY, dirZ));
          // fix targets on the X axe
          if (load->getDirection().isXNull() && load->getValue(0) != 0)
            fixDOF(result->second, 0);
          if (load->getDirection().isYNull() && load->getValue(0) != 0) // fix targets on the Y axe
            fixDOF(result->second, 1);
          if (load->getDirection().isZNull() && load->getValue(0) != 0) // fix targets on the Z axe
            fixDOF(result->second, 2);
        }
      }
      if (cpt > 0)
        loads.push_back(load);
    }
  }
}
 
 
template<class DataTypes>
TranslationConstraint<DataTypes>*  TranslationConstraint<DataTypes>::addConstraint(unsigned int index, Deriv trans)
{
  this->targets.push_back(index);
  trans.normalize();
  this->translations.push_back(trans);
  this->directionsNULLs.push_back(Deriv(1, 1, 1));
  this->initPos.push_back(Deriv(0, 0, 0));
  return this;
}
 
template<class DataTypes>
TranslationConstraint<DataTypes>*  TranslationConstraint<DataTypes>::removeConstraint(int index)
{
  std::vector<unsigned int>::iterator it1 = targets.begin();
  VecDerivIterator it2 = translations.begin();
  VecDerivIterator it3 = directionsNULLs.begin();
  VecDerivIterator it4 = initPos.begin();
  while (it1 != targets.end() && *it1 != (unsigned)index) {
    ++it1;
    ++it2;
    ++it3;
    ++it4;
  }
 
  targets.erase(it1);
  translations.erase(it2);
  directionsNULLs.erase(it3);
  initPos.erase(it4);
 
  return this;
}
 
 
template<class DataTypes>
void TranslationConstraint<DataTypes>::fixDOF(int index, int axe)
{
  //set the value to 1 on the corrects vector component
  std::vector<unsigned int>::iterator it1 = targets.begin();
  VecDerivIterator it2 = directionsNULLs.begin();
  while (it1 != targets.end() && *it1 != (unsigned)index) {
    ++it1;
    ++it2;
  }
 
  (*it2)[axe] = 0;
}
 
 
template<class DataTypes>
void TranslationConstraint<DataTypes>::projectResponse(VecDeriv& dx) {
  
  SReal time = this->getContext()->getTime()-initTime;
  //SReal prevTime = time - this->getContext()->getDt();
 
  std::vector<unsigned int>::iterator it1 = targets.begin();
  VecDerivIterator it2 = translations.begin();
  VecDerivIterator it3 = directionsNULLs.begin();
  Load * load;
  SReal valTime; //, prevValTime;
 
  for (unsigned int i = 0 ; i < loads.size() ; i++) {
    load = loads[i];
    // the current value
    valTime = load->getValue(time);
    // the value at last time we updated the load
    //prevValTime = (prevTime < 0.0) ? 0.0 : load->getValue(prevTime);
    for (unsigned int j = 0 ; j < load->numberOfTargets();j++) {
      if (atomToDOFIndexes.find(load->getTarget(j)) != atomToDOFIndexes.end())
      {
        // nullified all other acceleration
        if ((load->getDirection().isXNull() || load->getDirection().isXSpecified()) && valTime != 0)
          dx[*it1][0] = 0; // fix targets on the X axis
        if ((load->getDirection().isYNull() || load->getDirection().isYSpecified()) && valTime != 0)
          dx[*it1][1] = 0; // fix targets on the Y axis
        if ((load->getDirection().isZNull() || load->getDirection().isZSpecified()) && valTime != 0)
          dx[*it1][2] = 0; // fix targets on the Z axis
        
        ++it1;
        ++it2;
        ++it3;
      }
    }
  }
}
 
template <class DataTypes>
void TranslationConstraint<DataTypes>::projectVelocity(VecDeriv& v) {
  for (std::vector<unsigned int>::iterator it1 = targets.begin();it1 != targets.end();++it1) {
    v[*it1][0] = 0.0;
    v[*it1][1] = 0.0;
    v[*it1][2] = 0.0;
  }
}
 
template<class DataTypes>
void TranslationConstraint<DataTypes>::projectPosition(VecCoord& x) {
 
  SReal time = this->getContext()->getTime()-initTime;
  SReal prevTime = time - this->getContext()->getDt();
 
  std::vector<unsigned int>::iterator it1 = targets.begin();
  VecDerivIterator it2 = translations.begin();
  VecDerivIterator it3 = initPos.begin();
  Load * load;
 
  for (unsigned int i = 0 ; i < loads.size() ; i++)
  {
    load = loads[i];
    for (unsigned int j = 0 ; j < load->numberOfTargets();j++)
    {
      if (atomToDOFIndexes.find(load->getTarget(j)) != atomToDOFIndexes.end()) {
        
  //initPos is the current position
  *it3 = x[*it1];
 
        if (load->getValue(time) != 0.0){
          if (load->getDirection().isToward()) {
            std::map<unsigned int, unsigned int>::const_iterator titi = atomToDOFIndexes.find(load->getDirection().getToward());
            if (titi != atomToDOFIndexes.end()){
              (*it2) = (*mmodel->getX())[titi->second] - (*mmodel->getX())[*it1];
              it2->normalize();
            }
          }
          // the last time we updated the load
          double lastValue = (prevTime < 0.0) ? 0.0 : load->getValue(prevTime);
          // multiply by the difference between the current value and the last time value,
          // so that when t is equal to the date of the current value event, the total
          // of displacement will sum up to value.
          x[*it1] = (*it3) + (*it2) * (load->getValue(time) - lastValue);
        }
 
        ++it1;
        ++it2;
        ++it3;
      }
    }
  }
}
 
 
 
// -- VisualModel interface
template<class DataTypes>
void TranslationConstraint<DataTypes>::draw()
{
  /*
  // TODO update this to show constraints if this class is integrated as a Sofa Plugin
  
  if (!this->getContext()->getShowBehaviorModels()) return;
 
  VecCoord x = *mmodel->getX();
  glDisable(GL_LIGHTING);
  glColor4f(1, 0.5, 0.5, 1);
 
  glPointSize(10);
 
  //for Fixed points, display a big red point
  glBegin(GL_POINTS);
  VecDerivIterator it2 = directionsNULLs.begin();
  for (std::vector<unsigned int>::const_iterator it = this->targets.begin(); it != this->targets.end(); ++it)
  {
    if ((*it2)[0] == 0 && (*it2)[1] == 0 && (*it2)[2] == 0)
      sofa::helper::gl::glVertexT(x[*it]);
 
    it2++;
  }
  glEnd();
 
  //for translated points, display a little red segment with translation direction
  glPointSize(1);
  glBegin(GL_LINES);
  VecDerivIterator it3 = translations.begin();
  it2 = directionsNULLs.begin();
  for (std::vector<unsigned int>::const_iterator it = this->targets.begin(); it != this->targets.end(); ++it)
  {
    if ((*it2)[0] == 1 || (*it2)[1] == 1 || (*it2)[2] == 1) {
      sofa::helper::gl::glVertexT(x[*it]);
      sofa::helper::gl::glVertexT(x[*it] + *it3);
    }
    it3++;
    it2++;
  }
  glEnd();
*/
 
  
}
 
template<class DataTypes>
void TranslationConstraint<DataTypes>::setInitTime(double time){
  initTime=time;
}
 
#endif //TRANSLATION_CONSTRAINT_INL