AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

massspringsolver.inl

Go to the documentation of this file.

/***************************************************************************
                          massspringsolver.inl  -  description
                             -------------------
    begin                : Thu Nov 27 2003
    copyright            : (C) 2003 by François Faure, GRAVIR
    email                : Francois.Faure@imag.fr
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include "massspringsolver.h"
#include <animal/physicalSolver.inl>
#include <animal/odeImplicitSolver.inl>
#include<iostream>
using std::cerr; using std::endl;

namespace animal {

template<class P, class V, class Im, class R, class I>
typename MassSpringSolver<P,V,Im,R,I>::Real MassSpringSolver<P,V,Im,R,I>::_thresholdDistance=1.e-12;

template<class P, class V, class Im, class R, class I>
bool MassSpringSolver<P,V,Im,R,I>::_debug=false;


template<class P, class V, class Im, class R, class I>
MassSpringSolver<P,V,Im,R,I>::MassSpringSolver()
    : _links(0)
    , _stiffnesses(0)
    , _restLengths(0)
    , _own_restLengths(false)
    , _isotropy(0.03)
{
//  cerr<<"MassSpringSolver<P,V,Im,R,I>::MassSpringSolver()"<<endl;
}

template<class P,class V,class Im,class R,class I>
void MassSpringSolver<P,V,Im,R,I>::solveODE(
    Positions& pos,
    Vector& vel,
    Real dt
)
{
  //cerr<<"MassSpringSolver<P,V,Im,R,I>::solveODE "<< endl;
    _OdeImplicitSolver::solveODE(pos,vel,dt);
    this->applyExponentialDamping(vel,dt);
}


template<class P, class V, class Im, class R, class I>
MassSpringSolver<P,V,Im,R,I>::~MassSpringSolver()
{
    if( _own_restLengths ) delete _restLengths;
}





template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::setMethod( int m ){
    this->_OdeImplicitSolver::setMethod(m);
}
template<class P, class V, class Im, class R, class I>
int MassSpringSolver<P,V,Im,R,I>::getMethod() const {
    return this->_OdeImplicitSolver::method();
}
template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::setIsotropy( Real isot ){
    this->_isotropy = isot;
}
template<class P, class V, class Im, class R, class I>
typename MassSpringSolver<P,V,Im,R,I>::Real MassSpringSolver<P,V,Im,R,I>::getIsotropy() const {
    return this->_isotropy;
}
template<class P, class V, class Im, class R, class I>
const typename MassSpringSolver<P,V,Im,R,I>::Real& MassSpringSolver<P,V,Im,R,I>::get_common_damping_ratio(){
    return this->_dampingRatio;
}
template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::set_common_damping_ratio( const Real& _newVal){
    this->_dampingRatio = _newVal;
}

template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::set_spring_indices( VecIndex* newVal){ _links=newVal; }

template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::set_stiffnesses( VecReal* newVal){ _stiffnesses = newVal; }

//template<class P, class V, class Im, class R, class I>
//void MassSpringSolver<P,V,Im,R,I>::set_rest_lengths( R* r )
//{
//  assert( size(*_links)%2==size(*r) );
//  this->_restLengths = r;
//  _own_restLengths = false;
//}

//  /** Set the rest lengths of the springs according to the current particle positions. */
//template<class P, class V, class Im, class R, class I>
//void MassSpringSolver<P,V,Im,R,I>::create_rest_lengths( const Positions& p )
//{
//  assert(false);
//  assert( size(*_links)%2==0 );
//  if(!_restLengths) _restLengths = new VecReal;
//  _own_restLengths = true;
//  for( unsigned int i=0; i<_links->size(); i+=2 )
//  {
//    Position x =  p[(*_links)[i]];
//    cerr<<"p0 = " << x <<", p1 = "<< p[(*_links)[i+1]]
//    << endl;
//    v_meq( x, p[(*_links)[i+1]] );   // x==p[a]-p[b]
//    _restLengths->push_back( v_norm( x ) );
//  }
//  cerr<<"MassSpringSolver<P,V,Im,R,I>::create_rest_lengths: "
//  << *_restLengths
//  << endl;
//}


template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::accumulateSpringForce(Vec& fa, Vec& fb,
    const Vec& pa, const Vec& pb,
    const Vec& va, const Vec& vb,
    const Real& stiffness, const Real& dampingRatio, const Real& restLength)
{
   Real distance;
   Vec direction;
   if( computeDistanceAndDirection(distance, direction, pa, pb ) )
   {
   Real elongation = distance-restLength;
         Vec relativeVelocity = vb;
         v_meq(relativeVelocity,va);
         Real elongationVelocity = v_dot(direction,relativeVelocity);
         Vec f = direction;
         v_teq( f, stiffness*(elongation + dampingRatio*elongationVelocity) );
         v_peq( fa, f );
         v_meq( fb, f );
//   cerr <<"MassSpringSolver<P,V,Im,R,I>::accumulateSpringForce" << endl
        if (_debug){
        cerr << ", point " << pa <<", force += "<< fa << endl   
            << ", point " << pb <<", force -= "<< fb << endl
            << ", distance = " << distance <<", elongation = "<< elongation <<endl
            << ", direction = " << direction << endl
        << endl;
        }
   }
}

template<class P, class Vec, class Im, class Real, class I>
void MassSpringSolver<P,Vec,Im,Real,I>::accumulateSpringForceAndGradientMatrix
    ( Vec& fa, Vec& fb, Vec& grad,
    const Vec& pa, const Vec& pb,
    const Vec& va, const Vec& vb,
    const Real& stiffness, const Real& dampingRatio, const Real& restLength)
{
   Real distance;
   Vec direction;
   if( computeDistanceAndDirection(distance, direction, pa, pb) )
   {
     grad = direction;
         Real elongation = distance-restLength;
         Vec relativeVelocity = vb;
         v_meq(relativeVelocity,va);
         Real elongationVelocity = v_dot(direction,relativeVelocity);
         Vec f = direction;
         v_teq( f, stiffness*(elongation + dampingRatio*elongationVelocity) );
         //cerr<<"MassSpringSolver<P,V,Im,R,I>::accumulateSpringForceAndGradientMatrix, f= "<< f << endl;
         v_peq( fa, f );
         v_meq( fb, f );
   }
}

template<class P, class V, class Im, class R, class I>
bool MassSpringSolver<P,V,Im,R,I>::computeDistanceAndDirection(Real& d, Vec& u,
const Vec& pa, const Vec& pb){
    u = pb-pa;
    d = v_norm(u);
    //cerr << "massSpringSolver::_thresholdDistance " << _thresholdDistance << endl;
    if( d<get_thresholdDistance() ) return false;
    v_teq( u, 1/d);
    return true;
}

template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::computeForces( Vector& f, const Positions& p, const Vector& v)
{
    _PhysicalSolver::computeForces(f,p,v);
    // f==0
  assert( size(*_links)%2 ==0);
    for( unsigned int i=0; i<_links->size(); i+=2 )
    {
    Index& a = (*_links)[i  ];
    Index& b = (*_links)[i+1];
        if (_debug){
            cerr << "index du point a= " << a << endl
            << "index du point b= " << b << endl;
        }
        accumulateSpringForce( f[a], f[b], p[a], p[b], v[a], v[b], (*_stiffnesses)[i/2], _dampingRatio, (*_restLengths)[i/2] );
    }
   //cerr << "MassSpringSolver<P,V,Im,R,I>::computeForces: " << f << endl;
}

template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::computeAccelerations( Vector& a, const Positions& p, const Vector& v)
{
    _PhysicalSolver::computeAccelerations(a,p,v);
    //cerr<<"MassSpringSolver<P,V,Im,R,I>::computeAccelerations: "<< a << endl;
}

template<class P, class V, class Im, class R, class I>
void MassSpringSolver<P,V,Im,R,I>::computeAccelerationsAndStiffness( Vector& f, const Positions& p, const Vector& v)
{
      //typedef container_traits<Vector>::value_type VectorEntry;
    _PhysicalSolver::computeForces(f,p,v);
    // f==0
  assert( size(*_links)%2 ==0);
  assert( size(*_stiffnesses)==size(*_links)/2 );
  assert( size(*_restLengths)==size(*_links)/2 );
  assert( size(p)==size(f) );
  assert( size(v)==size(f) );
  //cerr<<"computeAccelerationsAndGradient, size(p)== "<<size(p)<<endl;
  //cerr<<"computeAccelerationsAndGradient, restLengths== "<<*_restLengths<<endl;
  _directions.resize( size(*_links)/2 );
    for( unsigned int i=0; i<_links->size(); i+=2 )
    {
    Index& a = (*_links)[i  ];
    Index& b = (*_links)[i+1];
        accumulateSpringForceAndGradientMatrix(
            f[a],
            f[b],
            _directions[i/2],
            p[a],
            p[b],
            v[a],
            v[b],
            (*_stiffnesses)[i/2],
            _dampingRatio,
            (*_restLengths)[i/2] );
    }
  //cerr<<"computeAccelerationsAndGradient, f== "<<f<<endl;

  this->applyForces(f,f);
  // f==f/m

  this->applyGravity(f);
  // f== g + f/m

    //cerr<<"MassSpringSolver<P,V,Im,R,I>::computeAccelerationsAndGradient, a= "<< f << endl;

}

//  /** Compute the force applied to the particles and update the gradient matrix.
//  \param f force vector
//  \param p current positions
//  \param v current velocities
//  */
//template<class Positions, class Vector, class Im, class R, class I>
//  void MassSpringSolver<Positions,Vector,Im,R,I>
//  ::computeForce( Vector& f, const Positions& p, const Vector& v )
//  {
//      //typedef container_traits<Vector>::value_type VectorEntry;
//  _PhysicalSolver::computeForces(f,p,v);
//  // f==0
//  assert( size(*_links)%2 ==0);
//  assert( size(*_stiffnesses)==size(*_links)/2 );
//  assert( size(*_restLengths)==size(*_links)/2 );
//  assert( size(p)==size(f) );
//  assert( size(v)==size(f) );
//  cerr<<"::computeForce, size(p)== "<<size(p)<<endl;
//  cerr<<"::computeForce, restLengths== "<<*_restLengths<<endl;
//  _directions.resize( size(*_links)/2 );
//  for( unsigned int i=0; i<_links->size(); i+=2 )
//  {
//    Index& a = (*_links)[i  ];
//    Index& b = (*_links)[i+1];
//      accumulateSpringForceAndGradientMatrix(
//          f[a],
//          f[b],
//          _directions[i/2],
//          p[a],
//          p[b],
//          v[a],
//          v[b],
//          (*_stiffnesses)[i/2],
//          _dampingRatio,
//          (*_restLengths)[i/2] );
//  }
//  cerr<<"::computeForce, f== "<<f<<endl;
//
//  }

template<class Positions, class Vector, class Im, class R, class I>
    void MassSpringSolver<Positions,Vector,Im,R,I>
	::applyForces( Vector& a, const Vector& f)
    {
    this->_PhysicalSolver::applyForces(a,f);
    }

    /* old stuff
template<class Positions, class Vector, class Im, class Real, class I>
    void MassSpringSolver<Positions,Vector,Im,Real,I>
    ::v_eq_h_dfdx_x( Vector& v, Real h, const Vector& x )
    {
     assert( size(v)==size(x) );

     v_assign( v, Value<Vec>::zero() );
     // v==0
     assert( size(_directions)==size(*_links)/2);
     assert( size(*_stiffnesses)==size(*_links)/2);
     //cerr<<"::v_eq_h_dfdx_x, directions= "<< _directions << endl;

         for( unsigned int i=0, iend=size(_directions); i!=iend; ++i )
         {
              Real u; Vec p;
              Real hh = -h * (*_stiffnesses)[i];

        // first endpoint
              u = v_dot(_directions[i],x[ (*_links)[i/2] ]);
              v_eq_ab( p, hh*u, _directions[i] );
              v_peq( v[ (*_links)[i/2  ] ], p );
              v_meq( v[ (*_links)[i/2+1] ], p );

        // second endpoint
        u = v_dot(_directions[i],x[i/2+1]);
              v_eq_ab( p, hh*u, _directions[i] );
              v_peq( v[ (*_links)[i/2+1] ], p );
              v_meq( v[ (*_links)[i/2  ] ], p );
         }
    }

template<class Positions, class Vector, class Im, class Real, class I>
    void MassSpringSolver<Positions,Vector,Im,Real,I>
    ::v_peq_h_dfdx_x( Vector& v, Real h, const Vector& x )
    {
     assert( size(v)==size(x) );
     assert( size((*_stiffnesses))==size(_directions) );
     assert( size(_directions)==size(*_links)/2 );

         for( unsigned int i=0, iend=size(_directions); i!=iend; ++i )
         {
              Real u; Vec p;
            Real hh = -h * (*_stiffnesses)[i];

        // first endpoint
              u = v_dot(_directions[i],x[ (*_links)[i/2] ]);
              v_eq_ab( p, hh*u, _directions[i] );
              v_peq( v[ (*_links)[i/2  ] ], p );
              v_meq( v[ (*_links)[i/2+1] ], p );

        // second endpoint
        u = v_dot(_directions[i],x[ (*_links)[i/2+1] ]);
              v_eq_ab( p, hh*u, _directions[i] );
              v_peq( v[ (*_links)[i/2+1] ], p );
              v_meq( v[ (*_links)[i/2  ] ], p );
         }

    }
old stuff */


template<class Positions, class Vector, class Im, class Real, class I>
    void MassSpringSolver<Positions,Vector,Im,Real,I>
	::v_eq_h_dfdx_x( Vector& v, Real h, const Vector& x )
    {
        v_assign( v, Value<Vec>::zero() );
        v_peq_h_dfdx_x(v,h,x);
    }

template<class Positions, class Vector, class Im, class Real, class I>
    void MassSpringSolver<Positions,Vector,Im,Real,I>
	::v_peq_h_dfdx_x( Vector& v, Real h, const Vector& x )
    {
     assert( size(v)==size(x) );
     assert( size((*_stiffnesses))==size(_directions) );
     assert( size(_directions)==size(*_links)/2 );
     //cerr<<"MassSpringSolver:v_peq_h_dfdx_x, links= "<< *_links << endl;

         for( unsigned int i=0, iend=size(_directions); i!=iend; ++i )
         {
            Real u; Vec p, piso;
            Real hk = -h * (*_stiffnesses)[i];
            Real hiso = _isotropy * hk;
            Real hh = hk -  hiso;
            std::size_t i1=(*_links)[i*2], i2=(*_links)[(i*2)+1];
            //cerr<<"MassSpringSolver:v_peq_h_dfdx_x, i1= "<< i1<<", i2= "<< i2 << endl;

            // === first endpoint
            // anisotropic part
            u = v_dot(_directions[i],x[i1]);
            v_eq_ab( p, hh*u, _directions[i] );
            v_peq( v[i1], p );
            v_meq( v[i2], p );
            // isotropic part
            v_eq_ab( piso, hiso, x[i1] );
            v_peq( v[i1], piso );
            v_meq( v[i2], piso );

            // === second endpoint
            // anisotropic part
            u = v_dot(_directions[i],x[i2]);
            v_eq_ab( p, hh*u, _directions[i] );
            v_peq( v[i2], p );
            v_meq( v[i1], p );
            // isotropic part
            v_eq_ab( piso, hiso, x[i2] );
            v_peq( v[i2], piso );
            v_meq( v[i1], piso );
         }

    }

}//animal

---