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

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_JOINT_PARAMETERS_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_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>
      void set_joint_parameters(
          skeleton_type & skeleton
        , ublas::vector<typename skeleton_type::bone_traits::real_type> const & 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<const vector_type>  const_vector_range;

        size_t i   = 0u;
        size_t dof = 0u;

        typename skeleton_type::bone_iterator bone = skeleton.begin();
        typename skeleton_type::bone_iterator end  = skeleton.end();
        for(;bone!=end;++bone)
        {
          dof = bone->active_dofs();
          // Get the sub-block of theta corresponding to the current bone
          const_vector_range sub_theta = subrange(theta,i,(i+dof));
          // Set the relative bone transform of each bone
          bone_traits::set_theta( *bone, sub_theta);
          i += dof;
        }
        // Finally compute absolute bone transformations
        skeleton.compute_pose();
      }

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

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_JOINT_PARAMETERS_H
#endif