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

Go to the documentation of this file.

#ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_SEPARATED_COLLISION_CONTACT_FIXED_STEP_SIMULATOR_H
#define OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_SEPARATED_COLLISION_CONTACT_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>
#include <OpenTissue/dynamics/mbd/mbd_stack_propagation.h>

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

      typedef typename mbd_types::math_policy          math_policy;
      typedef typename math_policy::index_type           size_type;
      typedef typename math_policy::real_type            real_type;
      typedef typename math_policy::vector3_type         vector3_type;
      typedef typename math_policy::idx_vector_type      idx_vector_type;
      typedef typename math_policy::matrix_type          matrix_type;
      typedef typename math_policy::vector_type          vector_type;
      typedef typename math_policy::value_traits         value_traits;
      
      typedef typename mbd_types::group_type                       group_type;
      typedef typename mbd_types::group_ptr_container              group_ptr_container;
      typedef typename mbd_types::stepper_policy                   stepper_policy;
      typedef          StackPropagation<mbd_types>            propagation_algorithm;
      typedef typename mbd_types::material_library_type            material_library_type;
      typedef typename material_library_type::material_iterator      material_iterator;

    public:

      class node_traits 
        : public propagation_algorithm::node_traits 
      {};

      class edge_traits 
        : public propagation_algorithm::edge_traits 
      {};

      class constraint_traits 
        : public propagation_algorithm::constraint_traits 
      {};

    protected:

      struct StepperFunctor
      {
        stepper_policy * m_stepper;        
        real_type        m_h;              

        void operator()(group_type & layer)
        {
          assert(m_stepper || !"StepperFunc::operator(): missing stepper");
          m_stepper->run(layer,m_h);
        }
      };

    protected:

      propagation_algorithm      m_propagation;          
      StepperFunctor             m_stepper_functor;      
      vector_type                m_s_cur;                
      vector_type                m_s_predicted;          
      vector_type                m_u;                    
      vector_type                m_F;                    
      matrix_type                m_invM;                 
      vector_type                m_restitution;          
      group_type *               m_all;                  
      group_ptr_container        m_groups;               

    public:

      SeparatedCollisionContactFixedStepSimulator(){}

      virtual ~SeparatedCollisionContactFixedStepSimulator(){}

    public:

      void run(real_type const & time_step)
      {
        assert(time_step>0 || !"SeparatedCollisionContactFixedStepSimulator::run(): time step must be positive");

        m_all = this->get_configuration()->get_all_body_group();

        size_type n = m_all->size_bodies();
        
        math_policy::resize( m_s_cur, 7*n);
        math_policy::resize( m_s_predicted, 7*n);
        math_policy::resize( m_u, 6*n);
        math_policy::resize( m_F, 6*n);

        mbd::get_position_vector(*m_all, m_s_cur);
        mbd::get_inverse_mass_matrix(*m_all,m_invM);

        mbd::compute_scripted_motions(*m_all, this->time() + time_step);

        m_stepper_functor.m_stepper = this->get_stepper();
        m_stepper_functor.m_h = time_step;

        int collision_iterations = 5;
        for(int i=0;i<collision_iterations;++i)
          resolve_collisions(time_step);
        velocity_update(time_step);

        extract_restitution();
        int contact_iterations = 10;
        real_type amount = 1./contact_iterations;
        for(int i=0;i<contact_iterations;++i)
        {
          increase_restitution(amount);
          contact_handling(time_step);
        }
        int shock_iterations = 1;
        for(int i=0;i<shock_iterations;++i)
          shock_propagation(time_step);
        restore_restitution();

        position_update(time_step);
        update_time(time_step);
      }

    protected:

      void velocity_update(real_type const & h)
      {
        assert(m_all || !"SeparatedCollisionContactFixedStepSimulator::velocity_update(): missing all group");
        mbd::get_external_force_vector(*m_all,m_F,true);
        mbd::get_velocity_vector(*m_all, m_u);

        //m_u += ublas::prod(m_invM, m_F)*h;
        math_policy::prod_add(m_invM, m_F,m_u,h);
        
        mbd::set_velocity_vector(*m_all,m_u);
      }

      void position_update(real_type const & h)
      {
        assert(m_all || !"SeparatedCollisionContactFixedStepSimulator::position_update(): missing all group");
        mbd::get_velocity_vector(*m_all, m_u);
        mbd::compute_position_update(*m_all,m_s_cur,m_u,h,m_s_cur);
        mbd::set_position_vector(*m_all,m_s_cur);
      }

      void resolve_collisions(real_type const & h)
      {
        assert(m_all || !"SeparatedCollisionContactFixedStepSimulator::resolve_collisions(): missing all group");

        //--- use predicted postion x' = x + h*(v+h*F)
        mbd::get_external_force_vector(*m_all,m_F, true);
        mbd::get_velocity_vector(*m_all, m_u);

        m_u += prod(m_invM, m_F)*h;

        mbd::compute_position_update(*m_all,m_s_cur,m_u,h,m_s_predicted);
        mbd::set_position_vector(*m_all,m_s_predicted);

        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_stepper()->resolve_collisions(*group);
        }
      }

      void contact_handling(real_type const & h)
      {
        assert(m_all || !"SeparatedCollisionContactFixedStepSimulator::contact_handling(): missing all group");
        //--- use predicted postion x' = x + h*v'
        mbd::get_velocity_vector(*m_all, m_u);
        mbd::compute_position_update(*m_all,m_s_cur,m_u,h,m_s_predicted);
        mbd::set_position_vector(*m_all,m_s_predicted);

        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_stepper()->resolve_collisions(*group);
        }
      }

      void shock_propagation(real_type const & h)
      {
        assert(m_all || !"SeparatedCollisionContactFixedStepSimulator::shock_propagation(): missing all group");

        //--- use predicted postion x' = x + h*v'
        mbd::get_velocity_vector(*m_all, m_u);
        mbd::compute_position_update(*m_all,m_s_cur,m_u,h,m_s_predicted);
        mbd::set_position_vector(*m_all,m_s_predicted);

        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);
          m_propagation.run( 
              *group
            , m_stepper_functor
            , typename propagation_algorithm::fixate_tag()
            , typename propagation_algorithm::upward_tag()  
          );
        }
      }

      void extract_restitution()
      {
        size_type N = this->get_configuration()->get_material_library()->size_materials();

        math_policy::resize( m_restitution, N);
        
        typename vector_type::iterator e_n = m_restitution.begin();

        material_iterator material = this->get_configuration()->get_material_library()->material_begin();
        material_iterator end      = this->get_configuration()->get_material_library()->material_end();
      
        for(;material!=end;++material)
        {
          *e_n++ = material->normal_restitution();
          material->normal_restitution() = -1.;
        }
      }

      void increase_restitution(real_type const & amount)
      {
        using std::min;

        material_iterator material = this->get_configuration()->get_material_library()->material_begin();
        material_iterator end      = this->get_configuration()->get_material_library()->material_end();

        for(;material!=end;++material)
        {
          real_type e_n = material->normal_restitution() + amount;
          e_n = min(value_traits::zero(),e_n);
          material->normal_restitution() = e_n;
        }
      }

      void restore_restitution()
      {
        typename vector_type::iterator e_n = m_restitution.begin();

        material_iterator material = this->get_configuration()->get_material_library()->material_begin();
        material_iterator end      = this->get_configuration()->get_material_library()->material_end();

        for(;material!=end;++material)
        {
          material->normal_restitution() = (*e_n++);
        }
      }

    };

  } // namespace mbd
} // namespace OpenTissue
// OPENTISSUE_DYNAMICS_MBD_UTIL_SIMULATORS_MBD_SEPARATED_COLLISION_CONTACT_FIXED_STEP_SIMULATOR_H
#endif