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

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_COMPUTE_JACOBIAN_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_COMPUTE_JACOBIAN_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>

#include <vector>

namespace OpenTissue
{
  namespace kinematics
  {
    namespace inverse
    {

      template <  typename chain_iterator, typename bone_iterator, typename matrix_type>
      inline void compute_jacobian(
          chain_iterator const & chain_begin
        , chain_iterator const & chain_end
        , bone_iterator const & bone_begin
        , bone_iterator const & bone_end
        , matrix_type & J
        )
      {
        typedef typename matrix_type::value_type                     real_type;
        typedef typename ublas::vector<real_type>                    vector_type;
        typedef typename ublas::matrix_range< matrix_type >          matrix_range;

        typedef typename chain_iterator::value_type                  chain_type;
        typedef typename chain_type::bone_traits                     bone_traits;

        // Determine how many chains we have
        size_t max_chains = std::distance( chain_begin, chain_end );
        size_t max_bones  = std::distance( bone_begin, bone_end   );

        // Determine how many rows and columns we have in the Jacobian
        std::vector<size_t> row_index(max_chains);      
        std::vector<size_t> column_index(max_bones);    

        size_t rows    = 0;              
        size_t columns = 0;              

        std::vector<size_t> dof_array(max_bones);   
        {
          size_t index = 0;
          for(chain_iterator chain = chain_begin;chain!=chain_end;++chain)
          {
            row_index[index++] = rows;
            rows += chain->get_goal_dimension();
          }

          bone_iterator bone     = bone_begin;
          for(;bone!=bone_end;++bone)
          {
            dof_array[ bone->get_number()] = bone->active_dofs();
          }

          for (size_t i=0;i<max_bones;++i)
          {
            column_index[i] = columns;
            columns += dof_array[i];
          }
        }

        // Allocate space for Jacobian
        if(J.size1() != rows || J.size2() != columns)
        {
          J.resize( rows, columns, false);
        }
        J.clear(); // Empty all data from J 

        // Now we are ready to fill in data into the Jacobian matrix.
        size_t chain_index = 0;
        for(chain_iterator chain = chain_begin;chain!=chain_end;++chain)
        {
          typename chain_type::bone_iterator bend  = chain->bone_end();
          typename chain_type::bone_iterator bone  = chain->bone_begin();

          for(; bone != bend ; ++bone)
          {
            size_t bone_index = bone->get_number();

            size_t begin_row      = row_index[chain_index];
            size_t end_row        = begin_row + chain->get_goal_dimension();
            size_t begin_column   = column_index[bone_index];
            size_t end_column     = begin_column + dof_array[bone_index];

            matrix_range J_block = ublas::subrange(
              J
              , begin_row
              , end_row
              , begin_column
              , end_column
              );

            bone_traits::compute_jacobian( (*bone) , (*chain), J_block); //fix to use different bones
          }
          ++chain_index;
        }

      }

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

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_COMPUTE_JACOBIAN_H
#endif