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

Go to the documentation of this file.

#ifndef OPENTISSUE_DYNAMICS_MBD_MBD_BODY_H
#define OPENTISSUE_DYNAMICS_MBD_MBD_BODY_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/mbd_update_inertia_tensor.h>

namespace OpenTissue
{
  namespace mbd
  {

    template< typename mbd_types  >
    class Body
      : public mbd_types::identifier_type
      , public mbd_types::node_traits
    {
    public:

      typedef typename mbd_types::math_policy                   math_policy;
      typedef typename mbd_types::math_policy::real_type        real_type;
      typedef typename mbd_types::math_policy::vector3_type     vector3_type;
      typedef typename mbd_types::math_policy::matrix3x3_type   matrix3x3_type;
      typedef typename mbd_types::math_policy::quaternion_type  quaternion_type;
      typedef typename mbd_types::math_policy::size_type        size_type;
      typedef typename mbd_types::math_policy::value_traits     value_traits;
      typedef typename mbd_types::node_traits                   node_traits;

    protected:

      typedef typename mbd_types::geometry_type                 geometry_type;
      typedef typename mbd_types::body_type                     body_type;
      typedef typename mbd_types::joint_type                    joint_type;
      typedef typename mbd_types::edge_type                     edge_type;
      typedef typename mbd_types::force_type                    force_type;
      typedef typename mbd_types::scripted_motion_type          scripted_motion_type;
      typedef typename mbd_types::joint_ptr_container           joint_ptr_container;
      typedef typename mbd_types::edge_ptr_container            edge_ptr_container;
      typedef typename mbd_types::force_ptr_container           force_ptr_container;

      typedef typename edge_ptr_container::iterator         edge_ptr_iterator;
      typedef typename edge_ptr_container::const_iterator   const_edge_ptr_iterator;
      typedef typename joint_ptr_container::iterator        joint_ptr_iterator;
      typedef typename joint_ptr_container::const_iterator  const_joint_ptr_iterator;

    public:

      joint_ptr_container    m_joints;           
      edge_ptr_container     m_edges;            
      size_type              m_tag;              

    protected:

      force_ptr_container    m_forces;           
      scripted_motion_type * m_scripted_motion;  

    public:

      typedef typename mbd_types::indirect_joint_iterator          indirect_joint_iterator;
      typedef typename mbd_types::const_indirect_joint_iterator    const_indirect_joint_iterator;
      typedef typename mbd_types::indirect_edge_iterator           indirect_edge_iterator;
      typedef typename mbd_types::indirect_force_iterator          indirect_force_iterator;
      typedef typename mbd_types::const_indirect_force_iterator    const_indirect_force_iterator;

      indirect_joint_iterator         joint_begin()       { return indirect_joint_iterator(m_joints.begin());        }
      indirect_joint_iterator         joint_end()         { return indirect_joint_iterator(m_joints.end());          }
      const_indirect_joint_iterator   joint_begin() const { return const_indirect_joint_iterator(m_joints.begin());  }
      const_indirect_joint_iterator   joint_end()   const { return const_indirect_joint_iterator(m_joints.end());    }
      indirect_edge_iterator          edge_begin()        { return indirect_edge_iterator(m_edges.begin()); }
      indirect_edge_iterator          edge_end()          { return indirect_edge_iterator(m_edges.end());   }
      indirect_force_iterator         force_begin()       { return indirect_force_iterator(m_forces.begin());        }
      indirect_force_iterator         force_end()         { return indirect_force_iterator(m_forces.end());          }
      const_indirect_force_iterator   force_begin() const { return const_indirect_force_iterator(m_forces.begin());  }
      const_indirect_force_iterator   force_end()   const { return const_indirect_force_iterator(m_forces.end());    }

    protected:

      size_type m_material_idx;             
      bool      m_active;                   


      bool      m_sleepy;                   
      bool      m_fixed;                    
      bool      m_scripted;                 
      bool      m_finite_rotation_update;   

      vector3_type    m_r_axis;             
      vector3_type    m_r;                  
      quaternion_type m_Q;                  
      vector3_type    m_P;                  
      vector3_type    m_L;                  
      vector3_type    m_V;                  
      vector3_type    m_W;                  
      matrix3x3_type  m_invI_WCS;           
      matrix3x3_type  m_invI_BF;            
      matrix3x3_type  m_I_WCS;              
      matrix3x3_type  m_I_BF;               
      real_type       m_inv_mass;           
      real_type       m_mass;               
      matrix3x3_type  m_R;                  

      geometry_type * m_geometry;           

    public:

      Body(){ this->clear(); }

      ~Body()
      {
        m_edges.clear();
        m_forces.clear();
      }


      Body (Body const & body)
      {
        assert(body.m_edges.empty() || !"Body(body): Could not make a copy of a body already in a configuration! It has live edges!");

        this->m_joints          = body.m_joints;
        this->m_edges           = body.m_edges;
        this->m_tag             = body.m_tag;
        this->m_forces          = body.m_forces;
        this->m_scripted_motion = body.m_scripted_motion;

        this->m_material_idx           = body.m_material_idx;
        this->m_active                 = body.m_active;
        this->m_sleepy                 = body.m_sleepy;
        this->m_fixed                  = body.m_fixed;
        this->m_scripted               = body.m_scripted;
        this->m_finite_rotation_update = body.m_finite_rotation_update;

        this->m_r_axis   = body.m_r_axis;
        this->m_r        = body.m_r;
        this->m_Q        = body.m_Q;
        this->m_P        = body.m_P;
        this->m_L        = body.m_L;
        this->m_V        = body.m_V;
        this->m_W        = body.m_W;
        this->m_invI_WCS = body.m_invI_WCS;
        this->m_invI_BF  = body.m_invI_BF;
        this->m_I_WCS    = body.m_I_WCS;
        this->m_I_BF     = body.m_I_BF;
        this->m_inv_mass = body.m_inv_mass;
        this->m_mass     = body.m_mass;
        this->m_R        = body.m_R;

        this->m_geometry = body.m_geometry;
      }

    public:

      void clear()
      {
        m_forces.clear();

        m_geometry = 0;

        m_scripted_motion = 0;
        m_scripted = false;

        m_active = true;
        m_sleepy = false;
        m_material_idx = 0;
        m_r_axis.clear();
        m_finite_rotation_update = false;
        m_fixed = false;
        m_tag = 0;

        m_r.clear();
        m_Q.identity();
        m_R = OpenTissue::math::diag(value_traits::one());
        m_I_WCS = OpenTissue::math::diag(value_traits::one());
        m_I_BF = OpenTissue::math::diag(value_traits::one());
        m_invI_WCS = OpenTissue::math::diag(value_traits::one());
        m_invI_BF = OpenTissue::math::diag(value_traits::one());
        m_inv_mass = value_traits::one();
        m_mass = value_traits::one();
        m_V.clear();
        m_W.clear();
        m_P.clear();
        m_L.clear();

        if(!m_edges.empty())
        {
          std::cout << "Body::clear(): WARNING clear was invoked on a body that still got edges?" << std::endl;
        }
      }

      void compute_forces_and_torques(vector3_type & F,vector3_type & T)const
      {
        //--- Clear force accumulators
        F.clear();
        T.clear();

        vector3_type f,t;
        //--- Compute effects of all external forces
        for(const_indirect_force_iterator force = force_begin();force!=force_end();++force)
        {
          force->compute(*this,f,t);
          F += f;
          T += t;
        }
      }

      void attach(force_type * force)
      {
        assert(force || !"Body::detach(): force_type pointer was null");
        if  ( find( m_forces.begin( ), m_forces.end( ), force ) == m_forces.end( ) )
        {
          m_forces.push_back(force);
        }
        else
        {
          std::cout << "Body::attach(): Sorry, you already added that force to this body!" << std::endl;
        }
      }

      void detach(force_type * force)
      {
        assert(force || !"Body::detach(): force_type pointer was null");

        if  ( find( m_forces.begin( ), m_forces.end( ), force ) == m_forces.end( ) )
        {
          std::cout << "Body::detach(): Sorry, the force wer not attached to this body!" << std::endl;
        }
        else
        {
          m_forces.remove(force);
        }

      }

      void set_position(vector3_type const & r)    {      m_r = r;    }

      void set_orientation(quaternion_type const & Q)
      {
        m_Q = unit(Q);
        m_R = Q;

        mbd::update_inertia_tensor(m_R,m_I_BF,m_I_WCS);
        mbd::update_inertia_tensor(m_R,m_invI_BF,m_invI_WCS);
      }

      void set_velocity(vector3_type const & V)
      {
        if(m_fixed)
          return;
        m_V = V;
        m_P = V *m_mass;
      }

      void set_spin(vector3_type const & W)
      {
        if(m_fixed)
          return;
        m_W = W;
        m_L = m_I_WCS * m_W;
      }

      void set_mass(real_type const & mass)
      {
        assert(mass>value_traits::zero() || !"Body::set_mass(): mass must be positive");
        m_mass = mass;
        m_inv_mass = value_traits::one() / mass;
        m_P = m_V*m_mass;
      }

      void set_inertia_bf(matrix3x3_type const & I_BF)
      {
        m_I_BF = I_BF;
        m_invI_BF = inverse(I_BF);

        mbd::update_inertia_tensor(m_R,m_I_BF,m_I_WCS);
        mbd::update_inertia_tensor(m_R,m_invI_BF,m_invI_WCS);

        m_L = m_I_WCS * m_W;
      }

      void get_position(vector3_type & r) const { r = m_r; }

      void get_orientation(quaternion_type & Q) const
      {
        Q = m_Q;
      }

      void get_orientation(matrix3x3_type & R) const
      {
        R = m_R;
      }

      void get_velocity(vector3_type & V) const
      {
        if(m_fixed)
          V.clear();
        else
          V = m_V;
      }

      void get_spin(vector3_type & W) const
      {
        if(m_fixed)
          W.clear();
        else
          W = m_W;
      }

      real_type get_inverse_mass() const { return (m_fixed || m_scripted) ? value_traits::zero() : m_inv_mass; }

      real_type get_mass() const { return (m_fixed || m_scripted) ? value_traits::infinity() : m_mass;  }

      void get_inverse_inertia_bf(matrix3x3_type & invI_BF) const
      {
        if(m_fixed || m_scripted)
          invI_BF.clear();
        else
          invI_BF = m_invI_BF;
      }

      void get_inverse_inertia_wcs(matrix3x3_type & invI_WCS) const
      {
        if(m_fixed || m_scripted)
          invI_WCS.clear();
        else
          invI_WCS = m_invI_WCS;
      }

      void get_inertia_wcs(matrix3x3_type & I_WCS) const
      {
        if(m_fixed || m_scripted)
        {
          I_WCS = OpenTissue::math::diag( value_traits::infinity() );
        }
        else
          I_WCS = m_I_WCS;
      }

      bool is_active() const { return m_active;  }

      void set_active(bool const & active) { this->m_active = active; }

      bool is_sleepy() const {  return m_sleepy;  }

      void set_sleepy(bool const & sleepy){ this->m_sleepy = sleepy; }

      void set_material_idx(size_type const & idx){  m_material_idx = idx;  }

      size_type get_material_idx() const { return m_material_idx; }

      bool has_finite_rotation_update()const { return m_finite_rotation_update; }

      void set_finite_rotation_update(bool const & active) { m_finite_rotation_update = active; }

      bool has_finite_rotation_axis() const {  return (m_r_axis(0)||m_r_axis(1)||m_r_axis(2));  }

      void get_finite_rotation_axis(vector3_type & r_axis) const    {      r_axis = m_r_axis;    }

      void set_finite_rotation_axis(vector3_type const & r_axis)
      {
        m_r_axis.clear();
        if(r_axis(0) || r_axis(1) ||r_axis(2) )
        {
          m_finite_rotation_update = true;
          m_r_axis = unit(r_axis);
        }
      }


      bool is_scripted() const { return m_scripted; }

      void set_scripted_motion(scripted_motion_type * motion)
      {
        m_scripted_motion = motion;
        if(motion)
          m_scripted = true;
        else
          m_scripted = false;
      }

      void compute_scripted_motion(real_type const & time)
      {
        if(m_scripted_motion)
        {
          vector3_type r,v,w;
          quaternion_type q;
          m_scripted_motion->compute((*this),time,r,q,v,w);
          set_position(r);
          set_orientation(q);
          set_velocity(v);
          set_spin(w);
        }
      }

      bool is_fixed() const {  return m_fixed; }



      void set_fixed(bool const & fixed) { this->m_fixed = fixed; }

      real_type compute_kinetic_energy() const
      {
        real_type Ekin = value_traits::zero();
        if(!m_fixed)
          Ekin = (( (m_V*m_V) /m_inv_mass) + m_W*m_L)/value_traits::two();
        return Ekin;
      }

      bool has_joints() const { return (!m_joints.empty()); }

      bool has_active_joints() const
      {
        for(const_indirect_joint_iterator joint = joint_begin();joint!=joint_end();++joint)
        {
          if(joint->is_active())
            return true;
        }
        return false;
      }

      bool has_joint_to(body_type const * body) const
      {
        assert(body       || !"Body::has_joint_to(): body was null");
        assert(this!=body || !"Body::has_joint_to(): body was the same as this body");
        for(const_indirect_joint_iterator joint = joint_begin();joint!=joint_end();++joint)
        {
          if(joint->get_socket_A()->get_body()==body || joint->get_socket_B()->get_body()==body)
            return true;
        }
        return false;
      }

      void set_geometry( geometry_type * geometry )
      {
        assert( geometry || !"Body::set_geometry(): geometry was null?");

        m_geometry = geometry;
      }

      geometry_type * get_geometry() const { return m_geometry; }


      void compute_collision_aabb(
        vector3_type const & r
        , matrix3x3_type const & R
        , vector3_type & min_coord
        , vector3_type & max_coord
        , real_type const & envelope
        ) const
      {
        assert(m_geometry || !"Body::compute_collision_aabb(): Geometry was null!");

        //--- Initialize to inifinite AABB
        min_coord(0) = OpenTissue::math::detail::lowest<real_type>();
        min_coord(1) = OpenTissue::math::detail::lowest<real_type>();
        min_coord(2) = OpenTissue::math::detail::lowest<real_type>();
        max_coord(0) = OpenTissue::math::detail::highest<real_type>();
        max_coord(1) = OpenTissue::math::detail::highest<real_type>();
        max_coord(2) = OpenTissue::math::detail::highest<real_type>();

        m_geometry->compute_collision_aabb( r, R, min_coord, max_coord );

        // Enlarge by envelope!
        min_coord(0) -= envelope;
        min_coord(1) -= envelope;
        min_coord(2) -= envelope;
        max_coord(0) += envelope;
        max_coord(1) += envelope;
        max_coord(2) += envelope;
      }


    };

  } // namespace mbd
} // namespace OpenTissue
// OPENTISSUE_DYNAMICS_MBD_MBD_BODY_H
#endif