00001 #ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_CHAIN_H 00002 #define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_CHAIN_H 00003 // 00004 // OpenTissue Template Library 00005 // - A generic toolbox for physics-based modeling and simulation. 00006 // Copyright (C) 2008 Department of Computer Science, University of Copenhagen. 00007 // 00008 // OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php 00009 // 00010 #include <OpenTissue/configuration.h> 00011 00012 #include <boost/iterator/indirect_iterator.hpp> 00013 00014 #include <stdexcept> 00015 #include <list> 00016 00017 namespace OpenTissue 00018 { 00019 namespace kinematics 00020 { 00021 namespace inverse 00022 { 00023 00043 template<typename skeleton_type_ > 00044 class Chain 00045 { 00046 public: 00047 00048 typedef skeleton_type_ skeleton_type; 00049 typedef typename skeleton_type::math_types math_types; 00050 typedef typename skeleton_type::bone_type bone_type; 00051 typedef typename skeleton_type::bone_traits bone_traits; 00052 00053 typedef typename math_types::real_type real_type; 00054 typedef typename math_types::vector3_type vector3_type; 00055 00056 protected: 00057 00058 typedef typename math_types::value_traits value_traits; 00059 00060 typedef std::list<bone_type*> bone_container; 00061 typedef typename bone_container::iterator bone_ptr_iterator; 00062 typedef typename bone_container::reverse_iterator reverse_bone_ptr_iterator; 00063 00064 public: 00065 00066 typedef boost::indirect_iterator<bone_ptr_iterator> bone_iterator; 00067 typedef boost::indirect_iterator<reverse_bone_ptr_iterator> reverse_bone_iterator; 00068 00069 00070 protected: 00071 00072 bone_container m_bones; 00073 00074 bool m_only_position; 00075 00076 vector3_type m_p_local; 00077 vector3_type m_x_local; 00078 vector3_type m_y_local; 00079 00080 vector3_type m_p_global; 00081 vector3_type m_x_global; 00082 vector3_type m_y_global; 00083 00084 real_type m_weight_p; 00085 real_type m_weight_x; 00086 real_type m_weight_y; 00087 00088 public: 00089 00097 bool & only_position() { return m_only_position; } 00098 bool const & only_position() const { return m_only_position; } 00099 00105 vector3_type const & p_local() const { return m_p_local; } 00106 vector3_type & p_local() { return m_p_local; } 00107 00114 vector3_type const & x_local() const { return m_x_local; } 00115 vector3_type & x_local() { return m_x_local; } 00116 00123 vector3_type const & y_local() const { return m_y_local; } 00124 vector3_type & y_local() { return m_y_local; } 00125 00132 vector3_type const & p_global() const { return m_p_global; } 00133 vector3_type & p_global() { return m_p_global; } 00134 00141 vector3_type const & x_global() const { return m_x_global; } 00142 vector3_type & x_global() { return m_x_global; } 00143 00150 vector3_type const & y_global() const { return m_y_global; } 00151 vector3_type & y_global() { return m_y_global; } 00152 00158 real_type const & weight_p() const { return m_weight_p; } 00159 real_type const & weight_x() const { return m_weight_x; } 00160 real_type const & weight_y() const { return m_weight_y; } 00161 00167 void set_weight_p(real_type const & value){ this->m_weight_p = value >= value_traits::zero() ? value : value_traits::zero(); } 00168 void set_weight_x(real_type const & value){ this->m_weight_x = value >= value_traits::zero() ? value : value_traits::zero(); } 00169 void set_weight_y(real_type const & value){ this->m_weight_y = value >= value_traits::zero() ? value : value_traits::zero(); } 00170 00171 public: 00172 00180 bool operator==(Chain const & chain) 00181 { 00182 if (this->m_bones.empty() || chain.m_bones.empty() ) 00183 return false; 00184 00185 if( this->m_bones.front() != chain.m_bones.front()) 00186 return false; 00187 00188 if( this->m_bones.back() == chain.m_bones.back() ) 00189 return false; 00190 00191 return true; 00192 } 00193 00194 public: 00195 00205 void init(bone_type const * root_, bone_type const * end_effector_) 00206 { 00207 // Strip away const qualifier 00208 bone_type * end_effector = const_cast<bone_type*>(end_effector_); 00209 bone_type * root = const_cast<bone_type*>( root_ ); 00210 00211 if(root==0) 00212 throw std::invalid_argument("root was null"); 00213 if(end_effector==0) 00214 throw std::invalid_argument("end_effector was null"); 00215 00216 this->m_bones.push_front(end_effector); 00217 bone_type * bone = end_effector; 00218 while(bone!=root && bone!=0) 00219 { 00220 bone_type * parent = const_cast<bone_type*>( bone->parent() ); 00221 this->m_bones.push_front( parent); 00222 bone = parent; 00223 } 00224 00225 if(bone==0) 00226 throw std::logic_error("root was not an ancestor of end-effector"); 00227 00228 // Setup default goal positions and weights 00229 00230 this->m_weight_p = value_traits::one(); 00231 this->m_weight_x = value_traits::one(); 00232 this->m_weight_y = value_traits::one(); 00233 00234 this->m_p_local = vector3_type( value_traits::zero(), value_traits::zero(), value_traits::zero()); 00235 this->m_x_local = vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero()); 00236 this->m_y_local = vector3_type( value_traits::zero(), value_traits::one(), value_traits::zero()); 00237 00238 typename math_types::coordsys_type goal = bone_traits::convert( end_effector->absolute() ); 00239 typename math_types::quaternion_type Q = goal.Q(); 00240 typename math_types::matrix3x3_type M; 00241 00242 M = Q; 00243 00244 this->m_p_global = goal.T(); 00245 00246 this->m_x_global = M.column(0); 00247 this->m_y_global = M.column(1); 00248 00249 this->m_only_position = true; 00250 } 00251 00257 bone_iterator bone_begin() { return bone_iterator( this->m_bones.begin() ); } 00258 00264 bone_iterator bone_end() { return bone_iterator( this->m_bones.end() ); } 00265 00272 reverse_bone_iterator rbone_begin() { return reverse_bone_iterator(this->m_bones.rbegin()); } 00273 00280 reverse_bone_iterator rbone_end() { return reverse_bone_iterator(this->m_bones.rend()); } 00281 00288 bone_type * const get_root() const 00289 { 00290 if(this->m_bones.empty()) 00291 return 0; 00292 return *(this->m_bones.begin()); 00293 } 00294 00301 bone_type * const get_end_effector() const 00302 { 00303 if(this->m_bones.empty()) 00304 return 0; 00305 return *(this->m_bones.rbegin()); 00306 } 00307 00313 size_t get_goal_dimension() const 00314 { 00315 return (this->m_only_position?3u:9u); 00316 } 00317 00318 }; 00319 00320 } // namespace inverse 00321 } // namespace kinematics 00322 } // namespace OpenTissue 00323 00324 //OPENTISSUE_KINEMATICS_INVERSE_INVERSE_CHAIN_H 00325 #endif
1.7.1