/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/kinematics/inverse/inverse_get_joint_limits.h

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_LIMITS_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_LIMITS_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/core/math/big/big_types.h>

namespace OpenTissue
{
  namespace kinematics
  {
    namespace inverse
    {

      template< typename skeleton_type>
      inline void get_joint_limits( 
          skeleton_type const & skeleton
        , ublas::vector<typename skeleton_type::bone_traits::real_type> & min_theta
        , ublas::vector<typename skeleton_type::bone_traits::real_type> & max_theta                
        )
      {
        using ublas::subrange;

        typedef typename skeleton_type::bone_traits              bone_traits;
        typedef typename bone_traits::real_type                  real_type;
        typedef          ublas::vector<real_type>                vector_type;
        typedef          ublas::vector_range<vector_type>        vector_range;

        // Determine size of theta
        size_t dofs = 0u;

        typename skeleton_type::const_bone_iterator bone = skeleton.begin();
        typename skeleton_type::const_bone_iterator end  = skeleton.end();

        for(;bone!=end;++bone)
        {
          dofs += bone->active_dofs(); 
        }

        //make sure theta has proper size
        if(min_theta.size() != dofs)
          min_theta.resize( dofs );
        if(max_theta.size() != dofs)
          max_theta.resize( dofs );

        //Now retrieve joint parameter values from each bone and store it into theta
        size_t i   = 0u;
        size_t dof = 0u;

        bone = skeleton.begin();
        for(;bone!=end;++bone)
        {
          dof = bone->active_dofs(); 

          vector_range sub_min = subrange(min_theta,i,i+dof);
          vector_range sub_max = subrange(max_theta,i,i+dof);
          bone_traits::get_joint_limits( *bone , sub_min, sub_max);

          i += dof;
        }
      }

    } // namespace inverse
  } // namespace kinematics
} // namespace OpenTissue

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_LIMITS_H
#endif