/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/simulators/mbd_implicit_fixed_step_simulator.h

Go to the documentation of this file.

#ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_IMPLICIT_FIXED_STEP_SIMULATOR_H
#define OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_IMPLICIT_FIXED_STEP_SIMULATOR_H
//
// OpenTissue Template Library
// - A generic toolbox for physics-based modeling and simulation.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/dynamics/mbd/interfaces/mbd_simulator_interface.h>

namespace OpenTissue
{
  namespace mbd
  {
    template< typename mbd_types >
    class ImplicitFixedStepSimulator 
      : public SimulatorInterface<mbd_types>
    {
    protected:

      typedef typename mbd_types::math_policy                   math_policy;
      typedef typename mbd_types::math_policy::real_type        real_type;
      typedef typename mbd_types::math_policy::vector_type      vector_type;
      typedef typename mbd_types::math_policy::matrix_type      matrix_type;
      typedef typename mbd_types::group_type                    group_type;
      typedef typename mbd_types::group_ptr_container           group_ptr_container;

    public:

      class node_traits{};
      class edge_traits{};
      class constraint_traits{};

    protected:

      vector_type                m_s;                    
      vector_type                m_ss;                   
      vector_type                m_u;                    
      vector_type                m_f_ext;                
      matrix_type                m_invM;                 
      group_type *               m_all;                  
      group_ptr_container        m_groups;               


    public:

      ImplicitFixedStepSimulator(){}

      virtual ~ImplicitFixedStepSimulator(){}

    public:

      void run(real_type const & time_step)
      {
        m_all = this->get_configuration()->get_all_body_group();
        
        mbd::compute_scripted_motions(*m_all,this->time());
        mbd::get_position_vector(*m_all, m_s);
        mbd::get_velocity_vector(*m_all, m_u);
        mbd::get_external_force_vector(*m_all,m_f_ext,true);
        mbd::get_inverse_mass_matrix(*m_all,m_invM);

        //--- velocity update, compute effect of gravity: h*inv(M)f_ext
        //m_u += ublas::prod(m_invM, m_f_ext)*time_step;
        math_policy::prod_add(m_invM, m_f_ext, m_u, time_step);
        
        //--- fake position update
        mbd::compute_position_update(*m_all,m_s,m_u,time_step,m_ss);
        mbd::set_position_vector(*m_all,m_ss);
        mbd::compute_scripted_motions(*m_all,this->time() + time_step);

        this->get_collision_detection()->run( m_groups );
        for(typename group_ptr_container::iterator tmp=m_groups.begin();tmp!=m_groups.end();++tmp)
        {
          group_type * group = (*tmp);
          this->get_sleepy()->evaluate(group->body_begin(),group->body_end());
          this->get_stepper()->run(*group,time_step);
        }

        //--- get constrained velocities
        mbd::get_velocity_vector(*m_all, m_u);
        //--- perform true position update
        mbd::compute_position_update(*m_all,m_s,m_u,time_step,m_ss);
        mbd::set_position_vector(*m_all,m_ss);
        update_time(time_step);
      }

    };

  } // namespace mbd
} // namespace OpenTissue
// OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_IMPLICIT_FIXED_STEP_SIMULATOR_H
#endif