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

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_DEFAULT_JOINT_SETTINGS_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_DEFAULT_JOINT_SETTINGS_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/math_euler_angles.h> // needed for ZYZ_euler_angles

#include <cmath>
#include <cassert>

namespace OpenTissue
{
  namespace kinematics
  {
    namespace inverse
    {

      template<typename skeleton_type>
      inline void  set_default_joint_settings( skeleton_type & skeleton )
      {
        using std::atan2;

        typedef typename skeleton_type::bone_type             bone_type;
        typedef typename skeleton_type::bone_iterator         bone_iterator;
        typedef typename skeleton_type::bone_traits           bone_traits;
        typedef typename skeleton_type::math_types            math_types;
        typedef typename bone_traits::transform_type          transform_type;

        typedef typename math_types::value_traits             value_traits;
        typedef typename math_types::vector3_type                         vector3_type;
        typedef typename math_types::quaternion_type                    quaternion_type;
        typedef typename math_types::real_type                real_type;

        bone_iterator    bone = skeleton.begin();  
        bone_iterator    end  = skeleton.end();
        for( ; bone!=end; ++bone)
        {
          transform_type const & T = bone->bind_pose();

          if(bone->type() == bone_traits::hinge_type)
          {
            quaternion_type Q = T.Q();
            vector3_type u = unit( Q.v() );
            // we know
            //
            // Q = [cos(theta/2), sin(theta/2) u]
            //
            real_type ct2 = Q.s();           //---   cos(theta/2)
            real_type st2 = length( Q.v() ); //---  |sin(theta/2)|
            real_type theta = value_traits::zero();
            theta = value_traits::two()* atan2(st2,ct2);
            // of course it might have been -|sin(theta/2)| that were the correct solution?
            bone->u() = u;
            bone->theta(0) = theta;
          }
          else if(bone->type() == bone_traits::slider_type)
          {
            vector3_type u  = unit( T.T() );
            real_type theta = length( T.T() );
            bone->u() = u;
            bone->theta(0) = theta;
          }
          else if(bone->type() == bone_traits::ball_type)
          {
            real_type phi   = value_traits::zero(); // bone->theta(0)
            real_type psi   = value_traits::zero(); // bone->theta(1)
            real_type theta   = value_traits::zero(); // bone->theta(2)

            quaternion_type Q = T.Q();

            OpenTissue::math::ZYZ_euler_angles(Q,phi,psi,theta);

            // Now we can store the correct joint parameter values into the bone
            bone->theta(0) = phi;
            bone->theta(1) = psi;
            bone->theta(2) = theta;

                                 
          }
          else
          {
            assert(false || !"set_default_joint_settings(): unsupported joint type encountered!");
          }

        }
      }

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

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_SET_DEFAULT_JOINT_SETTINGS_H
#endif