/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/skeleton_state.h

Go to the documentation of this file.

// Copyright (C) 2011 Soren Hauberg
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, see <http://www.gnu.org/licenses/>.
 
/*
 ********************************** READ THIS ***********************************
 * The only function that actually copies the 'theta' vector into the internal  *
 * skeleton structure is 'compute_pose'. Thus, if you want to do stuff with the *
 * skeleton (like IK), you have to copy the 'theta' vector back and forth       *
 * yourself. This was done to simplify the code (and hence increase the chance  *
 * of making it correct).                                                       *
 ********************************************************************************
 */

#ifndef SKELETON_STATE_H
#define SKELETON_STATE_H

// If ONE_BONE is defined a stupid debug mode is enabled that allows to similate brownian motion on a sphere
//#define ONE_BONE

#include "state.h"
#include "bone_appearance.h"
#include "options.h"
#include "math_types.h"
#include "calibration.h"
#include "auxil.h"
#include "ellipsoid.h"

#include <OpenTissue/kinematics/skeleton/io/skeleton_cal3d_xml_read.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/containers/grid/grid.h>
#include <OpenTissue/core/containers/mesh/mesh.h>
#include <OpenTissue/core/containers/grid/util/grid_idx2coord.h>

// Bunch of typedefs because OpenTissue _really_ likes namespaces
//typedef OpenTissue::math::default_math_types math_types;
typedef OpenTissue::kinematics::inverse::BoneTraits <bone_appearance> bone_traits;
typedef OpenTissue::skeleton::Types <math_types, bone_traits> skeleton_types;
   
typedef skeleton_types::skeleton_type skeleton_type;
typedef skeleton_types::bone_type bone_type; 
typedef OpenTissue::kinematics::inverse::NonlinearSolver <skeleton_type> solver_type;

typedef skeleton_type::bone_iterator bone_iter;
typedef skeleton_type::const_bone_iterator const_bone_iter;
typedef solver_type::chain_iterator chain_iter;
typedef solver_type::chain_type chain_type;
typedef solver_type::const_chain_iterator const_chain_iter;
typedef solver_type::matrix_type matrix_type;
typedef solver_type::vector_type vector_type;

//typedef OpenTissue::math::BasicMathTypes<double,size_t>   math_types;
//   typedef math_types::real_type                             real_type;
//   typedef math_types::vector3_type                          vector3_type;
//   typedef math_types::matrix3x3_type                        matrix3x3_type;
//   typedef float                                             value_type;

typedef OpenTissue::grid::Grid<float ,math_types> grid_type;
typedef OpenTissue::trimesh::TriMesh<> mesh_type;

template <class observation_type>
class skeleton_state : public state<observation_type>
{
public:
  skeleton_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : calib (_calib),
      root (0, 0, 0),
      cam (calib.camera_centre ()),
      #ifdef WITH_LINE
      line_offset (1),
      #else
      line_offset (0),
      #endif
      for_show (_for_show)
    {
      // Initialise inverse kinematics
      OpenTissue::skeleton::cal3d_xml_read (opts.skeleton_xsf (), skeleton);
  
      OpenTissue::kinematics::inverse::set_default_joint_settings (skeleton);
      skeleton.set_bind_pose ();
    
      #ifndef ONE_BONE
      set_joint_types ();
      #endif // ! ONE_BONE
  
      //solver = OpenTissue::kinematics::inverse::make_solver (skeleton);
      solver.init (skeleton);
      solver.chain_begin ()->only_position () = true; //false; // XXX: What is the point of this one?
    
      OpenTissue::kinematics::inverse::get_joint_parameters (skeleton, theta);
      load (opts.initial_pose ());
    
      try
        {
          limits_file = opts.joint_limits ();
        }
      catch (missing_option)
        {
          limits_file = "";
        }
      set_joint_limits ();
      
      // Load appearance (XXX: names should come from OT and the std::map in opts should be used)
      std::ifstream file (opts.bone_appearance ().c_str ());
      if (!file.is_open ())
        {
          std::cerr << "skeleton_state::skeleton_state: could not open '"
                    << opts.bone_appearance () << "' for reading." << std::endl;
        }
      
      for (bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter ++)
        {
          if (!iter->is_root ())
            {
              std::string name;
              file >> name;
              iter->set_name (name);
    
              double width;
              file >> width;
              
              const vector3 a = iter->absolute ().T ();
              const vector3 b = iter->parent ()->absolute ().T ();
              const double length = sqrt ((a-b)*(a-b));
    
              iter->set_size (width, length);
            }
        }
      file.close ();
    
      // Get joint limits
      OpenTissue::kinematics::inverse::get_joint_limits (skeleton, min_limits, max_limits);
      
      if (for_show)
        prepare_mesh ();
    };

  void prepare_mesh ()
    {
      // Draw bones and skin
      // (Bounding box computation)
      vector3 bb_min = skeleton.begin ()->absolute ().T ();
      vector3 bb_max = bb_min;
      for (const_bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter++)
        {
          const vector3 p = iter->absolute ().T ();
          for (size_t k = 0; k < 3; k++)
            {
              bb_min [k] = std::min (bb_min [k], p [k]-1);
              bb_max [k] = std::max (bb_max [k], p [k]+1);
            }
        }

      const size_t grid_size = 32;
      grid.create (bb_min, bb_max, grid_size, grid_size, grid_size);
      for (size_t k = 0; k < grid_size ; k++)
        for (size_t j = 0; j < grid_size; j++)
          for (size_t i = 0; i < grid_size; i++)
            grid (i, j, k) = 0;

      for (const_bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter++)
        {
          if (!iter->is_root () && !iter->parent ()->is_root ())
            {
              ellipsoid E;
              if (bone_to_ellipsoid (iter, E))
                {
                  for (size_t k = 0; k < grid_size ; k++)
                    for (size_t j = 0; j < grid_size; j++)
                      for (size_t i = 0; i < grid_size; i++)
                        {
                          vector3 coord, p;
                          double N;
                          OpenTissue::grid::idx2coord (grid, i, j, k, coord);
                          E.project_point (coord, p, N);
                          
                          const double dist2 = E.euc_mahalanobis2 (coord, p);
                          const float radial = (N > 1.0) ? exp (-dist2*1) : 1.0;
                          grid (i, j, k) += radial;
                        }
                
                }
            }
        }
      OpenTissue::mesh::smooth_isosurface (grid, 0.9, isosurface);
    }
  
  bool bone_to_ellipsoid (const_bone_iter &bone, ellipsoid &E) const
    {
      const vector3 absolute  = bone->absolute ().T () - root;
      const vector3 pabsolute = bone->parent ()->absolute ().T () - root;
      const vector3 C = 0.5 * (absolute + pabsolute);

      const double rx = 0.8 * bone->get_width ();
    
      const OpenTissue::math::CoordSys<double> coord_sys = bone->parent ()->absolute ();
      vector3 a0 = absolute - C;
      const double ry = norm (a0);

      if (ry > 0.1)
        {
          const double rz = (bone->get_number () < 5) ? 0.5 * rx : rx;

          vector3 x (1.0, 0.0, 0.0);
          vector3 y (0.0, 1.0, 0.0);
          vector3 z (0.0, 0.0, 1.0);
          coord_sys.xform_vector (x);
          coord_sys.xform_vector (y);
          coord_sys.xform_vector (z);
                
          E.init (C, x, y, z, rx, ry, rz);
          
          return true;
        }
      else
        {
         return false;
        }
    }

  void show (const double gray_level = 0.25) const
    {
      glPushMatrix ();
      glTranslatef (root (0), root (1), root (2));
      
      // Draw end-effectors
      /*
      size_t k = 0;
      for (const_chain_iter iter = solver.chain_begin (); iter != solver.chain_end (); iter++)
        {
          if (k == 69)
            OpenTissue::gl::ColorPicker (1.0, 0.0, 0.0);
          else
            OpenTissue::gl::ColorPicker (0.0, 1.0, 0.0);
          const vector3 endeffec = iter->get_end_effector ()->absolute ().T ();
          OpenTissue::gl::DrawPoint (endeffec, 0.2);
    
          k += 3;

          //OpenTissue::gl::ColorPicker (0.9, 0.9, 0.9, 1.0);
          //const vector3 tmp = iter->p_global ();
          //OpenTissue::gl::DrawPoint (tmp, 0.3);
        }
      */

      for (const_bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter++)
        {
          if (!iter->is_root () && !iter->parent ()->is_root ())
            {
              OpenTissue::gl::DrawStickBone (*iter, gray_level, 0, 0); //gray_level, gray_level);
              //OpenTissue::gl::ColorPicker (0.1, 0.7, 0.1);

              // Draw cylinder
              //const vector3 absolute = iter->absolute ().T ();
              //const vector3 pabsolute = iter->parent ()->absolute ().T ();
              //const double r = iter->get_width ();
              //draw_cylinder (absolute, pabsolute, r);

              // Draw capsule endings
              //OpenTissue::gl::DrawPoint (absolute, r);
              //OpenTissue::gl::DrawPoint (pabsolute, r);

              // Draw ellipsoid
/*
              if (for_show)
                {
                  ellipsoid E;
                  if (bone_to_ellipsoid (iter, E))
                    E.show ();
                }
*/
            }
        }

      if (for_show)
        {
          OpenTissue::gl::ColorPicker (1.0, 0.3, 0.0);
          //OpenTissue::gl::ColorPicker (0.1, 0.7, 0.1);
          OpenTissue::gl::DrawMesh (isosurface, GL_LINE_LOOP);
          //OpenTissue::gl::DrawMesh (isosurface, GL_POLYGON);
        }

      glPopMatrix ();
    }
  
  void make_every_bone_an_end_effector ()
    {
      this->compute_pose ();
      
      // Add everything as end-effectors
      for (bone_iter iter = this->bone_begin (); iter != this->bone_end (); iter++)
        {
          if (iter->is_root ())
            continue;
        
          chain_type chain;
          chain.init (this->skeleton.root (), &(*iter));
          this->solver.add_chain (chain);
        }
    }

  size_t count_num_end_effectors () const
    {
      size_t retval = 0;
      for (const_chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        retval++;
      return retval;
    }
  
  void save (const std::string filename) const
    {
      std::ofstream file (filename.c_str ());
      if (!file.is_open ())
        std::cerr << "skeleton_state::save: could not save to " << filename 
                  << std::endl;
      else
        {
          save (file);
          file.close ();
        }
    };
  
  void save (std::ofstream &file) const
    {
      for (size_t k = 0; k < 3; k++)
        file << root [k] << " ";
      for (size_t k = 0; k < theta.size (); k++)
        file << theta [k] << " ";
      file << std::endl;
    };

  bool load (const std::string filename)
    {
      bool success = false;
      std::ifstream file (filename.c_str ());
      if (!file.is_open ())
        std::cerr << "skeleton_state::load: could not open " << filename << std::endl;
      else
        {
          success = load (file);
          file.close ();
        }
      
      return success;
    };

  bool load (std::ifstream &file)
    {
      //OpenTissue::kinematics::inverse::get_joint_parameters (skeleton, theta);
      for (size_t k = 0; k < 3; k++)
        file >> root [k];
      for (size_t k = 0; k < theta.size (); k++)
        file >> theta [k];
      //OpenTissue::kinematics::inverse::set_joint_parameters (skeleton, theta);

      compute_pose ();
      
      return true;
    };

  inline  
  void compute_pose ()
    {
      //project_limits ();
      OpenTissue::kinematics::inverse::set_joint_parameters (skeleton, theta);
      OpenTissue::kinematics::inverse::get_joint_parameters (skeleton, theta);
      skeleton.compute_pose ();
    };
  
  skeleton_state& operator= (skeleton_state &new_state)
    {
      theta.resize (new_state.theta.size ());
      for (size_t k = 0; k < theta.size (); k++)
        theta [k] = new_state.theta [k];
    
      root = new_state.root;
      
      return *this;
    };

  inline  
  solver_type::vector_type& get_theta ()
    {
      return theta;
    };
  
  inline  
  const solver_type::vector_type& get_theta () const
    {
      return theta;
    };
  
  inline  
  double get_theta (const int idx) const
    {
      return theta [idx];
    };
  
  inline  
  void set_theta (const solver_type::vector_type &new_theta)
    {
      for (size_t k = 0; k < new_theta.size (); k++)
        theta [k] = new_theta [k];
    };
 
  inline  
  void set_theta (const int idx, const double val)
    {
      theta [idx] = val;
    };
  
  inline  
  int theta_size () const
    {
      return theta.size ();
    };
  
  inline  
  skeleton_type& get_skeleton ()
    {
      return skeleton;
    };
  
  inline  
  const skeleton_type& get_skeleton () const
    {
      return skeleton;
    };

  inline
  double get_min_limit (unsigned int idx) const
    {
      return min_limits [idx];
    };
    
  inline
  double get_max_limit (unsigned int idx) const
    {
      return max_limits [idx];
    };
  
  inline
  void project_limits ()
    {
      OpenTissue::kinematics::inverse::compute_joint_limits_projection (skeleton, theta);
    };
  
  inline
  const vector3& get_root () const
    {
      return root;
    };
  
  inline
  void set_root (vector3 &new_root)
    {
      for (int k = 0; k < 3; k++)
        root (k) = new_root (k);
    };
  
  inline
  bone_iter bone_begin ()
    {
      return skeleton.begin ();
    };
    
  inline
  bone_iter bone_end ()
    {
      return skeleton.end ();
    };

  inline
  const_bone_iter bone_begin () const
    {
      return skeleton.begin ();
    };

  inline
  const_bone_iter bone_end () const
    {
      return skeleton.end ();
    };
  
  inline
  solver_type& get_solver ()
    {
      return solver;
    }
  
  inline
  const mesh_type& get_mesh () const
    {
      return isosurface;
    }
  
private:
  void set_joint_types ()
    {
      // Make elbows to hinge joints
      skeleton.get_bone (9+line_offset)->type () =  bone_type::bone_traits::hinge_type;
      skeleton.get_bone (13+line_offset)->type () =  bone_type::bone_traits::hinge_type;
      
      // Optional: lower part of the body
      if (skeleton.size () > 16)
        {
          skeleton.get_bone (17+line_offset)->type () = bone_type::bone_traits::hinge_type; // knee
          skeleton.get_bone (18+line_offset)->type () = bone_type::bone_traits::hinge_type; // foot
          skeleton.get_bone (22+line_offset)->type () = bone_type::bone_traits::hinge_type; // knee
          skeleton.get_bone (23+line_offset)->type () = bone_type::bone_traits::hinge_type; // foot
        }
    };

  void set_joint_limits ()
    {
      //#define PELVIS_LIMITS
      #define GENERAL_PURPOSE_LIMITS
      //#define FULL_BODY
      //#define STICKS
      //#define FILE_BASED
      
      #ifdef PELVIS_LIMITS
      #ifdef STICKS
      const double default_delta = 0.3;
      #else
      const double default_delta = 0.2;
      #endif // STICKS
  
      // Set bone limits
      size_t idx = line_offset;
      bone_iter iter = skeleton.begin ();
      for (size_t n = 0; n < line_offset; n++) iter++;
      
      for ( ; iter != skeleton.end (); iter ++)
        {
          // Set 'hacky' joint limits for bones that don't move too much
          for (size_t k = 0; k < iter->active_dofs (); k++)
            {
              if (idx == 4+line_offset)
                {
                  iter->min_joint_limit (k) = -0.1;
                  iter->max_joint_limit (k) =  0.7;
                }
              else if (idx == 13+line_offset)
                {
                  iter->min_joint_limit (k) = 1.5;
                  iter->max_joint_limit (k) = 2.5;
                }
              else if (idx <= 40+line_offset)
                {
                  iter->min_joint_limit (k) = iter->theta (k) - default_delta;
                  iter->max_joint_limit (k) = iter->theta (k) + default_delta;
                }
              else
                {
                  iter->min_joint_limit (k) = iter->theta (k) - 4.0 * default_delta;
                  iter->max_joint_limit (k) = iter->theta (k) + 4.0 * default_delta;
                }
              idx ++;
            }
        }
      #endif // PELVIS_LIMITS

      #ifdef GENERAL_PURPOSE_LIMITS
      const double default_delta = 0.1; //0.25; //0.15;
      //const double default_delta = 0.3; //XXX KILL ME!!! FOR OPTIMISATION
  
      // Set bone limits
      bone_iter iter = skeleton.begin ();
      for (size_t n = 0; n < line_offset; n++)
        iter++;

      size_t idx = line_offset;
      for ( ; iter != skeleton.end (); iter ++)
        {
          // Set 'hacky' joint limits for bones that don't move too much
          for (unsigned int k = 0; k < iter->active_dofs (); k++)
            {
              // XXX: SHOULD WE BE DOING THIS (THIS IS A FAIRLY NEW CHANGE!!!) ???
              /*if (idx <= 2 + line_offset)
                {
                  iter->min_joint_limit (k) = iter->theta (k) - default_delta / 10.0;
                  iter->max_joint_limit (k) = iter->theta (k) + default_delta / 10.0;
                }
              else*/
                {
              iter->min_joint_limit (k) = iter->theta (k) - default_delta;
              iter->max_joint_limit (k) = iter->theta (k) + default_delta;
                }
            }
          
          idx++;
        }
      
      // Set the more motional bones (Full body limits)
      // left shoulder
      #ifdef FULL_BODY
      skeleton.get_bone (8+line_offset)->min_joint_limit (0) = -2.0;
      skeleton.get_bone (8+line_offset)->max_joint_limit (0) =  0.0;
      skeleton.get_bone (8+line_offset)->min_joint_limit (1) = -0.5;
      skeleton.get_bone (8+line_offset)->max_joint_limit (1) =  2.3;
      skeleton.get_bone (8+line_offset)->min_joint_limit (2) = -0.5;
      skeleton.get_bone (8+line_offset)->max_joint_limit (2) =  2.0;
      
      // left elbow
      skeleton.get_bone (9+line_offset)->min_joint_limit (0) = -0.1;
      skeleton.get_bone (9+line_offset)->max_joint_limit (0) =  2.6;
      
      // right shoulder
      skeleton.get_bone (12+line_offset)->min_joint_limit (0) = -3.1;
      skeleton.get_bone (12+line_offset)->max_joint_limit (0) = -1.1;
      skeleton.get_bone (12+line_offset)->min_joint_limit (1) = -0.5;
      skeleton.get_bone (12+line_offset)->max_joint_limit (1) =  2.3;      
      skeleton.get_bone (12+line_offset)->min_joint_limit (2) =  2.0;
      skeleton.get_bone (12+line_offset)->max_joint_limit (2) =  4.1;      
      
      // right elbow
      skeleton.get_bone (13+line_offset)->min_joint_limit (0) = -2.6;
      skeleton.get_bone (13+line_offset)->max_joint_limit (0) =  0.1;
      #endif // FULL_BODY
    
      // Set the more motional bones (Upper body limits)
      #ifndef FULL_BODY
      // These limits seem to work with the hands2 sequence
      // left shoulder
      /*
      skeleton.get_bone (8+line_offset)->min_joint_limit (0) = -1.2;
      skeleton.get_bone (8+line_offset)->max_joint_limit (0) =  1.2;
      skeleton.get_bone (8+line_offset)->min_joint_limit (1) = -4.8;
      skeleton.get_bone (8+line_offset)->max_joint_limit (1) = -2.5;
      skeleton.get_bone (8+line_offset)->min_joint_limit (2) = -1.5;
      skeleton.get_bone (8+line_offset)->max_joint_limit (2) =  1.5;
      
      // left elbow
      skeleton.get_bone (9+line_offset)->min_joint_limit (0) = 0;
      skeleton.get_bone (9+line_offset)->max_joint_limit (0) =  2.7;
      
      // right shoulder
      skeleton.get_bone (12+line_offset)->min_joint_limit (0) = -1.3;
      skeleton.get_bone (12+line_offset)->max_joint_limit (0) =  1.3;
      skeleton.get_bone (12+line_offset)->min_joint_limit (1) = -3.2;
      skeleton.get_bone (12+line_offset)->max_joint_limit (1) =  1.2;      
      skeleton.get_bone (12+line_offset)->min_joint_limit (2) = -1.1;
      skeleton.get_bone (12+line_offset)->max_joint_limit (2) =  1.3;
      
      // right elbow
      skeleton.get_bone (13+line_offset)->min_joint_limit (0) = -2.5;
      skeleton.get_bone (13+line_offset)->max_joint_limit (0) =  0.0;
*/

      // These limits seem to work with the stick1_051310 sequence
      // left shoulder
      skeleton.get_bone (7+line_offset)->min_joint_limit (0) = -1.2;
      skeleton.get_bone (7+line_offset)->max_joint_limit (0) = -0.5;
      skeleton.get_bone (7+line_offset)->min_joint_limit (1) =  2.8;
      skeleton.get_bone (7+line_offset)->max_joint_limit (1) =  4.3;
      skeleton.get_bone (7+line_offset)->min_joint_limit (2) =  0.2;
      skeleton.get_bone (7+line_offset)->max_joint_limit (2) =  1.2;

      skeleton.get_bone (8+line_offset)->min_joint_limit (0) = -0.4;
      skeleton.get_bone (8+line_offset)->max_joint_limit (0) =  1.7;
      skeleton.get_bone (8+line_offset)->min_joint_limit (1) = -3.0;
      skeleton.get_bone (8+line_offset)->max_joint_limit (1) = -0.8;
      skeleton.get_bone (8+line_offset)->min_joint_limit (2) = -0.5;
      skeleton.get_bone (8+line_offset)->max_joint_limit (2) =  1.2;
      
      // left elbow
      skeleton.get_bone (9+line_offset)->min_joint_limit (0) = 0;
      skeleton.get_bone (9+line_offset)->max_joint_limit (0) =  2.7;
      
      // right shoulder
      skeleton.get_bone (11+line_offset)->min_joint_limit (0) = -1.8;
      skeleton.get_bone (11+line_offset)->max_joint_limit (0) = -1.0;
      skeleton.get_bone (11+line_offset)->min_joint_limit (1) = -1.1;
      skeleton.get_bone (11+line_offset)->max_joint_limit (1) =  1.1;      
      skeleton.get_bone (11+line_offset)->min_joint_limit (2) =  2.0;
      skeleton.get_bone (11+line_offset)->max_joint_limit (2) =  3.5;

      skeleton.get_bone (12+line_offset)->min_joint_limit (0) = -6.6;
      skeleton.get_bone (12+line_offset)->max_joint_limit (0) = -5.2;
      //skeleton.get_bone (12+line_offset)->min_joint_limit (1) = -2.0;
      //skeleton.get_bone (12+line_offset)->max_joint_limit (1) =  0.2;      
      skeleton.get_bone (12+line_offset)->min_joint_limit (1) = -3.6;
      skeleton.get_bone (12+line_offset)->max_joint_limit (1) = -0.8;      
      skeleton.get_bone (12+line_offset)->min_joint_limit (2) = -2.9;
      skeleton.get_bone (12+line_offset)->max_joint_limit (2) =  0.6;
      
      // right elbow
      skeleton.get_bone (13+line_offset)->min_joint_limit (0) = -2.5;
      skeleton.get_bone (13+line_offset)->max_joint_limit (0) =  0.0;

      #endif // !FULL_BODY
    #endif // GENERAL_PURPOSE_LIMITS
    
      #ifdef FILE_BASED
      std::ifstream file (limits_file.c_str ());
      if (!file.is_open ())
        {
          std::cerr << "skeleton_state::set_joint_limits: could not open '"
                    << limits_file << "' for reading." << std::endl;

          for (bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter ++)
            {
              for (size_t k = 0; k < iter->active_dofs (); k++)
                {
                  iter->min_joint_limit (k) = iter->theta (k) - 100000.0;
                  iter->max_joint_limit (k) = iter->theta (k) + 100000.0;
                }
            }
        }
      else
        {
          for (bone_iter iter = skeleton.begin (); iter != skeleton.end (); iter ++)
            {
              for (size_t k = 0; k < iter->active_dofs (); k++)
                {
                  double min_limit, max_limit;
                  file >> min_limit;
                  file >> max_limit;
              
                  iter->min_joint_limit (k) = min_limit;
                  iter->max_joint_limit (k) = max_limit;
                }
            }
          file.close ();
        }
      #endif // FILE_BASED
    }

  void draw_cylinder (const vector3 &a, const vector3 &b, const double radius) const
    {
      //const double z_scale = 2.0 / 3.0; // XXX: this need to match with 'skeleton_cloud_measure.h'
    
      const double delta_t = M_PI / 10.0;
      const vector3 v1 = a - b;
      const vector3 ab2 = b + v1 / 2.0 - cam;
      const vector3 v2 = unit (v1 % ab2); // orthogonal to v1 and ab2
    
      glBegin (GL_QUADS);
      const matrix3 Rt = OpenTissue::math::Ru (delta_t, v1);
      vector3 Rv2 = radius * v2;
      //Rv2 (2) *= z_scale;
      for (double t = 0; t <= M_PI; t += delta_t)
      //for (double t = 0; t <= 2.0 * M_PI; t += delta_t) // This draws full cylinders
        {
          glNormal3f (Rv2 (0), Rv2 (1), Rv2 (2));
    
          const vector3 a1 = a + Rv2;
          const vector3 b1 = b + Rv2;
          glVertex3f (a1 (0), a1 (1), a1 (2));
          glVertex3f (b1 (0), b1 (1), b1 (2));
          
          Rv2 = Rt * Rv2;
          
          const vector3 b2 = b + Rv2;
          const vector3 a2 = a + Rv2;
          glVertex3f (b2 (0), b2 (1), b2 (2));
          glVertex3f (a2 (0), a2 (1), a2 (2));

          //for (double alpha = 0; alpha <= 1; alpha+= 0.1)
          //  std::cout << (1.0 - alpha) * a1 + alpha * b1 << std::endl << (1.0 - alpha) * a1 + alpha * b2 << std::endl;
        }
      glEnd ();
    };

protected:
  const calibration calib;
  solver_type::vector_type theta, min_limits, max_limits;
  skeleton_type skeleton;
  vector3 root;
  solver_type solver;
  const vector3 cam;
  const size_t line_offset;
  mesh_type isosurface;
  grid_type grid;
  const bool for_show;
  std::string limits_file;
};


#endif // SKELETON_STATE_H