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

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_PARAMETERS_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_PARAMETERS_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_parameters( 
          skeleton_type const & skeleton
        , ublas::vector<typename skeleton_type::bone_traits::real_type> & 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(theta.size() != dofs )
          theta.resize( dofs );

        //Now retrive 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_theta = subrange(theta,i,i+dof);        

          bone_traits::get_theta( *bone , sub_theta);

          i += dof;
        }
      }


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

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_GET_JOINT_PARAMETERS_H
#endif