/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/limits/mbd_reach_cone.h

Go to the documentation of this file.

#ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_LIMITS_MBD_REACH_CONE_H
#define OPENTISSUE_DYNAMICS_MBD_UTIL_LIMITS_MBD_REACH_CONE_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/dynamics/mbd/interfaces/mbd_sub_constraint_interface.h>
#include <OpenTissue/core/math/math_constants.h>


namespace OpenTissue
{
  namespace mbd
  {

    template< typename mbd_types  >
    class ReachCone
      : public SubConstraintInterface<mbd_types>
    {
    public:

      typedef typename mbd_types::math_policy                       math_policy;
      typedef typename mbd_types::math_policy::real_type            real_type;
      typedef typename mbd_types::math_policy::size_type            size_type;
      typedef typename mbd_types::math_policy::value_traits         value_traits;
      typedef typename mbd_types::math_policy::vector3_type         vector3_type;
      typedef typename mbd_types::math_policy::vector_range         vector_range;
      typedef typename mbd_types::math_policy::idx_vector_range     idx_vector_range;
      typedef typename mbd_types::math_policy::matrix_range         matrix_range;
      typedef typename mbd_types::math_policy::vector_type          vector_type;
      typedef typename mbd_types::math_policy::quaternion_type      quaternion_type;

    protected:

      struct cone_constraint_type
      {
        real_type       m_theta_err;    
        vector3_type    m_u;            
      };

      typedef typename std::vector<cone_constraint_type>     constraint_container;
      typedef typename std::vector<vector3_type>             normal_container;

      constraint_container  m_contraints;       


      normal_container      m_cone_normals;     






      size_type          m_cone_sides;       
      size_type          m_rows;             
      vector_type        m_gamma;            
      vector_type        m_solution;         

    public:

      ReachCone()
        : m_cone_sides(0)
        , m_rows(0)
        , m_gamma(0)
        , m_solution(0)
      {}

      virtual ~ReachCone(){}

    public:

      void add_cone(vector3_type const & n_local)
      {
        vector3_type n_unit = normalize(n_local);
        //--- Test whether n_unit and the joint axis from the joint
        //--- frame on body A points in the same direction. I.e. whether
        //--- their dot product is non-negative.
        //--- If the dot product is negative then the user
        //--- is trying to setup and inconsistent reach cone.
        //---
        //--- But, in the local frame of the joint frame wrt. body A, the
        //--- joint axis is always (0,0,1) by convention. So this implies
        //--- that the z-coordinate of n_unit must be non-negative!
        if(n_unit(2)<value_traits::zero())
        {
          std::cout << "ReachCone::add_cone(): Inconsistent cone side, dropping it" << std::endl;
          return;
        }      
        m_cone_normals.push_back(n_unit);
        m_contraints.push_back(cone_constraint_type());
        ++m_cone_sides;
      }

      void evaluate(
        quaternion_type const & Q,
        vector3_type const & s_wcs,
        vector3_type const & s_local
        )
      {
        //std::cout << "ReachCone::evalute(): invoked " << std::endl;
        m_rows = 0;
        for( size_type i=0;i<m_cone_sides;++i)
        {
          //--- Compute the angle between the joint axis and the cone plane normal.
          vector3_type n_local = m_cone_normals[i];

          //---  Given two vectors n and s then the angle between them is given as
          //---
          //---      cos theta = n * s / |n| |s|
          //---  
          //--- If both vectors are unit vectors and we apply the small angle
          //--- approximation then we have
          //---
          //---      theta = n*s
          //---
          real_type theta_err = n_local * s_local;
          //--- If the angle violates the plane ``slope'' then create a angular joint limit.

          //--- The normal points to the inside region of the cone, thus we need to
          //--- test whether the s_local vector have tipped to the other side of the
          //--- cone plane. That is whether the cone face normal and the joint axis
          //--- wrt. body B points in opposite directions.
          if(theta_err<value_traits::zero()) 
          {
            // TODO KE 2006-08-06: If violation is greater than pi/2 radians
            // then we will not see the pi/2 error in the angle!

            m_contraints[m_rows].m_theta_err = theta_err;
            //std::cout << "\ttheta error = " << theta_err << std::endl;

            //--- Compute the current joint angle limit rotation axis. That is
            //--- the axis we need to rotate around to make the joint violation
            //--- disappear.
            vector3_type n_wcs = Q.rotate(n_local);
            m_contraints[m_rows].m_u = cross(n_wcs , s_wcs);
            ++m_rows;
          }
        }
      }

      size_type get_number_of_jacobian_rows() const {return m_rows;}

      void get_linear_jacobian_A(matrix_range & J,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          J(row,0) = value_traits::zero();
          J(row,1) = value_traits::zero();
          J(row,2) = value_traits::zero();
        }
      }

      void get_linear_jacobian_B(matrix_range & J,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          J(row,0) = value_traits::zero();
          J(row,1) = value_traits::zero();
          J(row,2) = value_traits::zero();
        }
      }

      void get_angular_jacobian_A(matrix_range & J,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          J(row,0) = -m_contraints[i].m_u(0);
          J(row,1) = -m_contraints[i].m_u(1);
          J(row,2) = -m_contraints[i].m_u(2);
        }
      }

      void get_angular_jacobian_B(matrix_range & J,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          J(row,0) = m_contraints[i].m_u(0);
          J(row,1) = m_contraints[i].m_u(1);
          J(row,2) = m_contraints[i].m_u(2);
        }
      }

      void get_stabilization_term(vector_range & b_error,size_type const & offset)const
      {
        real_type k_cor = this->get_frames_per_second()*this->get_error_reduction_parameter();
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          b_error(row) = - k_cor*m_contraints[i].m_theta_err;
        }
      }

      void get_low_limits(vector_range & lo,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          lo(row) = value_traits::zero();
        }
      }

      void get_high_limits(vector_range & hi,size_type const & offset)const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          hi(row) = OpenTissue::math::detail::highest<real_type>();
        }
      }

      void get_dependency_indices(idx_vector_range & dep,size_type const & offset) const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          dep(row) = OpenTissue::math::detail::highest<size_type>();
        }
      }

      void get_dependency_factors(vector_range & factors,size_type const & offset) const
      {
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          factors(row) = value_traits::zero();
        }
      }

      void set_regularization(vector_range const & gamma,size_type const & offset)
      {
        math_policy::resize( m_gamma, m_rows);
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
        {
          assert(gamma(row)<=1 || !"ReachCone::set_regularization(): gamma value out of range");
          assert(gamma(row)>=0 || !"ReachCone::set_regularization(): gamma value out of range");
          m_gamma(i) = gamma(row);
        }
      }

      void get_regularization(vector_range & gamma,size_type const & offset)const
      {
        size_type row = offset;
        if(m_gamma.size()==0)
        {
          for(size_type i=0;i<m_rows;++i,++row)
            gamma(row) = value_traits::zero();
        }
        else
        {
          for(size_type i=0;i<m_rows;++i,++row)
            gamma(row) = m_gamma(i);
        }
      }

      void set_solution(vector_range const & solution,size_type const & offset)
      {
        assert(solution.size()==m_rows || !"ReachCone::set_solution(): solution array size did not match number of active cone constraints");
        math_policy::resize( m_solution, m_rows);
        size_type row = offset;
        for(size_type i=0;i<m_rows;++i,++row)
          m_solution(i) = solution(row);
      }

      void get_solution(vector_range & solution,size_type const & offset)const
      {
        size_type row = offset;
        if(m_solution.size()==0)
        {
          for(size_type i=0;i<m_rows;++i,++row)
            solution(row) = value_traits::zero();
        }
        else
        {
          for(size_type i=0;i<m_rows;++i,++row)
            solution(row) = m_solution(i);
        }
      }

    };

  } // namespace mbd
} // namespace OpenTissue
// OPENTISSUE_DYNAMICS_MBD_UTIL_LIMITS_MBD_REACH_CONE_H
#endif