HUMIM Tracker: /home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/joints/mbd_wheel

Go to the documentation of this file.
00001 #ifndef OPENTISSUE_DYNAMICS_MBD_UTIL_JOINTS_MBD_WHEEL_JOINT_H
00002 #define OPENTISSUE_DYNAMICS_MBD_UTIL_JOINTS_MBD_WHEEL_JOINT_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 <OpenTissue/dynamics/mbd/interfaces/mbd_joint_interface.h>
00013 #include <OpenTissue/dynamics/mbd/limits/mbd_angular_joint_limit.h>
00014 #include <OpenTissue/dynamics/mbd/motors/mbd_angular_joint_motor.h>
00015 #include <OpenTissue/core/math/math_constants.h>
00016 
00017 
00018 namespace OpenTissue
00019 {
00020   namespace mbd
00021   {
00022 
00038     template< typename mbd_types >
00039     class WheelJoint
00040       : public JointInterface<mbd_types>
00041     {
00042     public:
00043 
00044       typedef typename mbd_types::math_policy                       math_policy;
00045       typedef typename mbd_types::math_policy::index_type           size_type;
00046       typedef typename mbd_types::math_policy::real_type            real_type;
00047       typedef typename mbd_types::math_policy::vector_range         vector_range;
00048       typedef typename mbd_types::math_policy::vector3_type         vector3_type;
00049       typedef typename mbd_types::math_policy::matrix3x3_type       matrix3x3_type;
00050       typedef typename mbd_types::math_policy::quaternion_type      quaternion_type;
00051       typedef typename mbd_types::math_policy::idx_vector_range     idx_vector_range;
00052       typedef typename mbd_types::math_policy::matrix_range         matrix_range;
00053       typedef typename mbd_types::body_type                         body_type;
00054       typedef typename mbd_types::math_policy::vector_type          vector_type;
00055       typedef typename mbd_types::math_policy::value_traits         value_traits;
00056 
00057       typedef AngularJointLimit<mbd_types>    angular_limit_type;
00058       typedef AngularJointMotor<mbd_types>    angular_motor_type;
00059 
00060     protected:
00061 
00062       matrix3x3_type   m_R;               
00063       matrix3x3_type   m_J_A;             
00064       matrix3x3_type   m_J_B;             
00065 
00066       vector3_type     m_anc_A_wcs;       
00067       vector3_type     m_anc_B_wcs;       
00068 
00069       vector3_type     m_s;               
00070       vector3_type     m_t1;              
00071       vector3_type     m_t2;              
00072       real_type        m_erp_susp;        
00073       real_type        m_cfm_susp;        
00074 
00075       real_type        m_cos_theta0;
00076       real_type        m_sin_theta0;
00077       real_type        m_cos_theta;
00078       real_type        m_sin_theta;
00079 
00080       vector3_type     m_v1;               
00081       vector3_type     m_v2;               
00082       vector3_type     m_u;                
00083       size_type        m_rows;             
00084       vector_type      m_gamma;            
00085       vector_type      m_solution;         
00086 
00087       angular_limit_type  *  m_limit;         
00088       angular_motor_type  *  m_motor1;        
00089       angular_motor_type  *  m_motor2;        
00090 
00091       size_type           m_limit_offset;
00092       size_type           m_motor_offset1;
00093       size_type           m_motor_offset2;
00094 
00095     public:
00096 
00130       real_type get_steering_angle() const
00131       {
00132         using std::atan2;
00133 
00134         quaternion_type Q;
00135         get_car_body()->get_orientation(Q);
00136         vector3_type s_motor = conj(Q).rotate( get_motor_axis_world() );
00137 
00138         real_type x = m_v1*s_motor;
00139         real_type y = m_v2*s_motor;
00140         return atan2( y , x );
00141       }
00142 
00148       real_type get_steering_rate() const
00149       {
00150         vector3_type s_wcs = get_steering_axis_world();
00151         this->m_socketA->get_body()->get_spin(this->w_A);
00152         this->m_socketB->get_body()->get_spin(this->w_B);
00153         return s_wcs * (this->w_A - this->w_B);
00154       }
00155 
00161       real_type get_wheel_rate() const
00162       {
00163         vector3_type s_wcs = get_motor_axis_world();
00164         this->m_socketA->get_body()->get_spin(this->w_A);
00165         this->m_socketB->get_body()->get_spin(this->w_B);
00166         return s_wcs * (this->w_A - this->w_B);
00167       }
00168 
00169       vector3_type get_steering_axis_world() const 
00170       {
00171         return this->m_socketA->get_joint_axis_world(); 
00172       }
00173 
00174       vector3_type get_motor_axis_world() const 
00175       {
00176         return this->m_socketB->get_joint_axis_world(); 
00177       }
00178 
00179       body_type * get_wheel_body() const
00180       {
00181         if(this->m_socketB)
00182           return this->m_socketB->get_body();
00183         return 0;
00184       }
00185 
00186       body_type * get_car_body() const
00187       {
00188         if(this->m_socketA)
00189           return this->m_socketA->get_body();
00190         return 0;
00191       }
00192 
00201       void set_suspension(real_type const & gamma,real_type const & erp)
00202       {
00203         assert(gamma>=value_traits::zero() || !"WheelJoint::set_suspension(): gamma less than 0");
00204         assert(gamma<=value_traits::one() || !"WheelJoint::set_suspension(): gamma greater than 1");
00205         assert(erp>=value_traits::zero() || !"WheelJoint::set_suspension(): erp less than 0");
00206         assert(erp<=value_traits::one() || !"WheelJoint::set_suspension(): ero greater than 1");
00207         m_cfm_susp = gamma;
00208         m_erp_susp = erp;
00209       }
00210 
00211       real_type get_suspension_cfm() const 
00212       { 
00213         return m_cfm_susp; 
00214       }
00215 
00216       real_type get_suspension_erp() const   
00217       {
00218         return m_erp_susp;   
00219       }
00220 
00221       void set_steering_limit(angular_limit_type const & limit) 
00222       { 
00223         m_limit = const_cast<angular_limit_type*>(&limit); 
00224       }
00225       
00226       void set_steering_motor(angular_motor_type const & motor) 
00227       { 
00228         m_motor1 = const_cast<angular_motor_type*>(&motor); 
00229       }
00230 
00231       void set_wheel_motor(angular_motor_type const & motor) 
00232       { 
00233         m_motor2 = const_cast<angular_motor_type*>(&motor); 
00234       }
00235 
00236     public:
00237 
00238       WheelJoint()
00239         : m_erp_susp( value_traits::zero() )
00240         , m_cfm_susp( value_traits::zero() )
00241         , m_rows(0)
00242         , m_limit(0)
00243         , m_motor1(0)
00244         , m_motor2(0)
00245       {}
00246 
00247       virtual ~WheelJoint() {}
00248 
00249     public:
00250 
00251       void evaluate()
00252       {
00253         using std::sqrt;
00254 
00255         m_rows = 0;
00256 
00257         if(!(this->m_socketA && this->m_socketB))//--- joint_type must be disconnected nothing to evaluate
00258           return;
00259 
00260         m_rows = 4;
00261 
00262         quaternion_type Q;
00263 
00264         this->m_socketA->get_body()->get_orientation(Q);
00265         vector3_type r_a = Q.rotate(  this->m_socketA->get_anchor_local());
00266 
00267         this->m_socketB->get_body()->get_orientation(Q);
00268         vector3_type r_b = Q.rotate(  this->m_socketB->get_anchor_local());
00269 
00270         //---
00271         //---  Ordinary in WCS coordinate frame we have
00272         //---
00273         //---   v_a + w_a X r_a - (v_b + w_b X r_b)  = 0
00274         //---   v_a - star(r_a) w_a - v_b + star(r_b) w_b  = 0
00275         //---
00276         //---  In local joint frame (JF) given by the basis (s,t1,t2) we have
00277         //---
00278         //---   R v_a - R star(r_a) w_a - R v_b + R star(r_b) w_b  = 0
00279         //---
00280         //---  where
00281         //---
00282         //---           |  s^T   |
00283         //---     R =   |  t1^T  |     : WCS -> JF
00284         //---           |  t2^T  |
00285         //---
00286         //--- Now
00287         //---
00288         //---  J_ang_a =  - R star(r_a)  =    star(r_a) R
00289         //---
00290         //--- from which we see
00291         //---
00292         //---
00293         //---                 |  (r_a X s)^T   |
00294         //---     J_ang_a =   |  (r_a X t1)^T  |
00295         //---                 |  (r_a X t2)^T  |
00296         //---
00297         //--- Similar we find (notice the minus sign):
00298         //---
00299         //---                  |  (r_b X s)^T   |
00300         //---     J_ang_b =  - |  (r_b X t1)^T  |
00301         //---                  |  (r_b X t2)^T  |
00302         //---
00303         //--- The linear parts are simply J_lin_a = R and  J_lin_b = - R
00304         //---
00305         //---
00306         //---  The error term is usually computed as
00307         //---
00308         //---      fps*erp (  r_anc_b_wcs  -   r_anc_a_wcs )
00309         //---
00310         //--- In the rotated frame this is
00311         //---
00312         //---      fps*erp R (  r_anc_b_wcs  -   r_anc_a_wcs )
00313         //---
00314         //--- Furthermore suspension modelling usually means that the erp-factor along
00315         //--- the steering axis is differnt than the one along the other axes. Which
00316         //--- means:
00317         //---
00318         //---              | fps*erp_susp  s*(  r_anc_b_wcs  -   r_anc_a_wcs ) |
00319         //---  b_error =   | fps*erp_norm t1*(  r_anc_b_wcs  -   r_anc_a_wcs ) |
00320         //---              | fps*erp_norm t2*(  r_anc_b_wcs  -   r_anc_a_wcs ) |
00321         //---
00322         m_s  = get_steering_axis_world();
00323         m_t1 = normalize( orthogonal(m_s) );
00324         m_t2 = cross(m_s , m_t1);
00325         m_R(0,0) =  m_s(0);     m_R(0,1) =  m_s(1);   m_R(0,2) =  m_s(2);
00326         m_R(1,0) = m_t1(0);     m_R(1,1) = m_t1(1);   m_R(1,2) = m_t1(2);
00327         m_R(2,0) = m_t2(0);     m_R(2,1) = m_t2(1);   m_R(2,2) = m_t2(2);
00328         m_J_A  =  - m_R*star(r_a);
00329         m_J_B  =    m_R*star(r_b);
00330 
00331         m_anc_A_wcs = this->m_socketA->get_anchor_world();
00332         m_anc_B_wcs = this->m_socketB->get_anchor_world();
00333 
00334         m_u = cross( m_s , get_motor_axis_world());  //--- Constrained axis
00335         m_sin_theta = sqrt(m_u*m_u);
00336         m_cos_theta = m_s * get_motor_axis_world();
00337 
00338 
00339         m_limit_offset  = 0;
00340         m_motor_offset1 = 0;
00341         m_motor_offset2 = 0;
00342         if(m_limit)
00343         {
00344           m_limit->evaluate(get_steering_axis_world(),get_steering_angle());
00345           if(m_limit->is_active())
00346           {
00347             m_limit_offset = m_rows;
00348             ++m_rows;
00349           }
00350         }
00351         if(m_motor1)
00352         {
00353           m_motor1->evaluate(get_steering_axis_world());
00354           if(m_motor1->is_active())
00355           {
00356             m_motor_offset1 = m_rows;
00357             ++m_rows;
00358           }
00359         }
00360         if(m_motor2)
00361         {
00362           m_motor2->evaluate(get_motor_axis_world());
00363           if(m_motor2->is_active())
00364           {
00365             m_motor_offset2 = m_rows;
00366             ++m_rows;
00367           }
00368         }
00369       }
00370 
00371       size_type get_number_of_jacobian_rows() const {return m_rows;}
00372 
00373       void get_linear_jacobian_A(matrix_range & J)const
00374       {
00375         assert(J.size1()==m_rows || !"WheelJoint::get_linear_jacobian_A(): incorrect dimensions");
00376         assert(J.size2()==3 || !"WheelJoint::get_linear_jacobian_A(): incorrect dimensions");
00377         J(0,0) = m_R(0,0); J(0,1) = m_R(0,1); J(0,2) = m_R(0,2);
00378         J(1,0) = m_R(1,0); J(1,1) = m_R(1,1); J(1,2) = m_R(1,2);
00379         J(2,0) = m_R(2,0); J(2,1) = m_R(2,1); J(2,2) = m_R(2,2);
00380         J(3,0) = value_traits::zero();
00381         J(3,1) = value_traits::zero();
00382         J(3,2) = value_traits::zero();
00383         if(m_limit_offset)
00384           m_limit->get_linear_jacobian_A(J,m_limit_offset);
00385         if(m_motor_offset1)
00386           m_motor1->get_linear_jacobian_A(J,m_motor_offset1);
00387         if(m_motor_offset2)
00388           m_motor2->get_linear_jacobian_A(J,m_motor_offset2);
00389       }
00390 
00391       void get_linear_jacobian_B(matrix_range & J)const
00392       {
00393         assert(J.size1()==m_rows || !"WheelJoint::get_linear_jacobian_B(): incorrect dimensions");
00394         assert(J.size2()==3 || !"WheelJoint::get_linear_jacobian_B(): incorrect dimensions");
00395         J(0,0) = -m_R(0,0); J(0,1) = -m_R(0,1); J(0,2) = -m_R(0,2);
00396         J(1,0) = -m_R(1,0); J(1,1) = -m_R(1,1); J(1,2) = -m_R(1,2);
00397         J(2,0) = -m_R(2,0); J(2,1) = -m_R(2,1); J(2,2) = -m_R(2,2);
00398         J(3,0) =  value_traits::zero();
00399         J(3,1) =  value_traits::zero();
00400         J(3,2) =  value_traits::zero();
00401         if(m_limit_offset)
00402           m_limit->get_linear_jacobian_B(J,m_limit_offset);
00403         if(m_motor_offset1)
00404           m_motor1->get_linear_jacobian_B(J,m_motor_offset1);
00405         if(m_motor_offset2)
00406           m_motor2->get_linear_jacobian_B(J,m_motor_offset2);
00407       }
00408 
00409       void get_angular_jacobian_A(matrix_range & J)const
00410       {
00411         assert(J.size1()==m_rows || !"WheelJoint::get_angular_jacobian_A(): incorrect dimensions");
00412         assert(J.size2()==3 || !"WheelJoint::get_angular_jacobian_A(): incorrect dimensions");
00413         J(0,0) = m_J_A(0,0); J(0,1) = m_J_A(0,1); J(0,2) = m_J_A(0,2);
00414         J(1,0) = m_J_A(1,0); J(1,1) = m_J_A(1,1); J(1,2) = m_J_A(1,2);
00415         J(2,0) = m_J_A(2,0); J(2,1) = m_J_A(2,1); J(2,2) = m_J_A(2,2);
00416         J(3,0) = m_u(0);             J(3,1) = m_u(1);             J(3,2) = m_u(2);
00417         if(m_limit_offset)
00418           m_limit->get_angular_jacobian_A(J,m_limit_offset);
00419         if(m_motor_offset1)
00420           m_motor1->get_angular_jacobian_A(J,m_motor_offset1);
00421         if(m_motor_offset2)
00422           m_motor2->get_angular_jacobian_A(J,m_motor_offset2);
00423       }
00424 
00425       void get_angular_jacobian_B(matrix_range & J)const
00426       {
00427         assert(J.size1()==m_rows || !"WheelJoint::get_angular_jacobian_B(): incorrect dimensions");
00428         assert(J.size2()==3 || !"WheelJoint::get_angular_jacobian_B(): incorrect dimensions");
00429         J(0,0) = m_J_B(0,0); J(0,1) = m_J_B(0,1); J(0,2) = m_J_B(0,2);
00430         J(1,0) = m_J_B(1,0); J(1,1) = m_J_B(1,1); J(1,2) = m_J_B(1,2);
00431         J(2,0) = m_J_B(2,0); J(2,1) = m_J_B(2,1); J(2,2) = m_J_B(2,2);
00432         J(3,0) = -m_u(0);           J(3,1) = -m_u(1);           J(3,2) = -m_u(2);
00433         if(m_limit_offset)
00434           m_limit->get_angular_jacobian_B(J,m_limit_offset);
00435         if(m_motor_offset1)
00436           m_motor1->get_angular_jacobian_B(J,m_motor_offset1);
00437         if(m_motor_offset2)
00438           m_motor2->get_angular_jacobian_B(J,m_motor_offset2);
00439       }
00440 
00441       void get_stabilization_term(vector_range & b_error)const
00442       {
00443         assert(b_error.size()==m_rows || !"WheelJoint::get_stabilization_term(): incorrect dimensions");
00444 
00445         real_type k_cor = this->get_frames_per_second()*this->get_error_reduction_parameter();
00446         real_type k_cor_susp = k_cor;
00447         if(m_erp_susp)
00448           k_cor_susp = m_erp_susp*this->get_frames_per_second();
00449 
00450         vector3_type diff = m_anc_B_wcs - m_anc_A_wcs;
00451 
00452         b_error(0) = k_cor_susp * (m_s *diff);
00453         b_error(1) = k_cor      * (m_t1*diff);
00454         b_error(2) = k_cor      * (m_t2*diff);
00455         //(theta0-theta) can be computed using the following small-angle-difference
00456         //  approximation:
00457         //    theta0-theta ~= tan(theta0-theta)
00458         //                  = sin(theta0-theta)/cos(theta0-theta)
00459         //                  = (c*s0 - s*c0) / (c*c0 + s*s0)
00460         //                  = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
00461         //  where c = cos(theta), s = sin(theta)
00462         //        c0 = cos(theta0), s0 = sin(theta0)
00463         //
00464         b_error(3) = k_cor*(m_cos_theta0*m_sin_theta - m_sin_theta0*m_cos_theta);
00465 
00466         if(m_limit_offset)
00467           m_limit->get_stabilization_term(b_error,m_limit_offset);
00468         if(m_motor_offset1)
00469           m_motor1->get_stabilization_term(b_error,m_motor_offset1);
00470         if(m_motor_offset2)
00471           m_motor2->get_stabilization_term(b_error,m_motor_offset2);
00472       }
00473 
00474       void get_low_limits(vector_range & lo)const
00475       {
00476         assert(lo.size()==m_rows || !"WheelJoint::get_low_limits(): incorrect dimensions");
00477         lo(0) = OpenTissue::math::detail::lowest<real_type>();
00478         lo(1) = OpenTissue::math::detail::lowest<real_type>();
00479         lo(2) = OpenTissue::math::detail::lowest<real_type>();
00480         lo(3) = OpenTissue::math::detail::lowest<real_type>();
00481         if(m_limit_offset)
00482           m_limit->get_low_limits(lo,m_limit_offset);
00483         if(m_motor_offset1)
00484           m_motor1->get_low_limits(lo,m_motor_offset1);
00485         if(m_motor_offset2)
00486           m_motor2->get_low_limits(lo,m_motor_offset2);
00487       }
00488 
00489       void get_high_limits(vector_range & hi)const
00490       {
00491         assert(hi.size()==m_rows || !"WheelJoint::get_high_limits(): incorrect dimensions");
00492         hi(0) = OpenTissue::math::detail::highest<real_type>();
00493         hi(1) = OpenTissue::math::detail::highest<real_type>();
00494         hi(2) = OpenTissue::math::detail::highest<real_type>();
00495         hi(3) = OpenTissue::math::detail::highest<real_type>();
00496         if(m_limit_offset)
00497           m_limit->get_high_limits(hi,m_limit_offset);
00498         if(m_motor_offset1)
00499           m_motor1->get_high_limits(hi,m_motor_offset1);
00500         if(m_motor_offset2)
00501           m_motor2->get_high_limits(hi,m_motor_offset2);
00502       }
00503 
00504       void get_dependency_indices(idx_vector_range & dep)const
00505       {
00506         assert(dep.size()==m_rows || !"WheelJoint::get_dependency_indices(): incorrect dimensions");
00507         for(size_type i=0;i<m_rows;++i)
00508           dep(i) = OpenTissue::math::detail::highest<size_type>();
00509       }
00510 
00511       void get_dependency_factors(vector_range & factors)const
00512       {
00513         assert(factors.size()==m_rows || !"WheelJoint::get_dependency_factors(): incorrect dimensions");
00514         for(size_type i=0;i<m_rows;++i)
00515           factors(i) = value_traits::zero();
00516       }
00517 
00518       void set_regularization(vector_range const & gamma)
00519       {
00520         assert(gamma.size()==m_rows || !"WheelJoint::set_regularization(): incorrect dimensions");
00521         math_policy::resize( m_gamma, 4);
00522         for(size_type i=0;i<4;++i)
00523         {
00524           assert(gamma(i)<=value_traits::one()  || !"WheelJoint::set_regularization(): gamma greater than 1");
00525           assert(gamma(i)>=value_traits::zero() || !"WheelJoint::set_regularization(): gamma less than 0");
00526           m_gamma(i) = gamma(i);
00527         }
00528         if(m_limit_offset)
00529           m_limit->set_regularization(gamma,m_limit_offset);
00530         if(m_motor_offset1)
00531           m_motor1->set_regularization(gamma,m_motor_offset1);
00532         if(m_motor_offset2)
00533           m_motor2->set_regularization(gamma,m_motor_offset2);
00534       }
00535 
00536       void get_regularization(vector_range & gamma)const
00537       {
00538         assert(gamma.size()==m_rows || !"WheelJoint::get_regularization(): incorrect dimensions");
00539         if(m_gamma.size()==0)
00540         {
00541           for(size_type i=0;i<4;++i)
00542             gamma(i) = value_traits::zero();
00543         }
00544         else
00545         {
00546           for(size_type i=0;i<4;++i)
00547             gamma(i) = m_gamma(i);
00548         }
00549         if(m_cfm_susp)
00550           gamma(0) = m_cfm_susp;
00551         if(m_limit_offset)
00552           m_limit->get_regularization(gamma,m_limit_offset);
00553         if(m_motor_offset1)
00554           m_motor1->get_regularization(gamma,m_motor_offset1);
00555         if(m_motor_offset2)
00556           m_motor2->get_regularization(gamma,m_motor_offset2);
00557       }
00558 
00559       void set_solution(vector_range const & solution)
00560       {
00561         assert(solution.size()==m_rows || !"WheelJoint::set_solution(): incorrect dimensions");
00562         math_policy::resize( m_solution, 4);
00563         for(size_type i=0;i<4;++i)
00564           m_solution(i) = solution(i);
00565         if(m_limit_offset)
00566           m_limit->set_solution(solution,m_limit_offset);
00567         if(m_motor_offset1)
00568           m_motor1->set_solution(solution,m_motor_offset1);
00569         if(m_motor_offset2)
00570           m_motor2->set_solution(solution,m_motor_offset2);
00571       }
00572 
00573       void get_solution(vector_range & solution)const
00574       {
00575         assert(solution.size()==m_rows || !"WheelJoint::get_solution(): incorrect dimensions");
00576         if(m_solution.size()==0)
00577         {
00578           for(size_type i=0;i<4;++i)
00579             solution(i) = value_traits::zero();
00580         }
00581         else
00582         {
00583           for(size_type i=0;i<4;++i)
00584             solution(i) = m_solution(i);
00585         }
00586         if(m_limit_offset)
00587           m_limit->get_solution(solution,m_limit_offset);
00588         if(m_motor_offset1)
00589           m_motor1->get_solution(solution,m_motor_offset1);
00590         if(m_motor_offset2)
00591           m_motor2->get_solution(solution,m_motor_offset2);
00592       }
00593 
00594       void calibration()
00595       {
00596         using std::sqrt;
00597 
00598         vector3_type s_steering = get_steering_axis_world();
00599         vector3_type s_motor    = get_motor_axis_world();
00600 
00601         //--- verify that joint was set up correctly.
00602         assert(s_steering != s_motor || !"WheelJoint::calibration(): steering and motor axis were the same");
00603 
00604         m_u = cross(s_steering , s_motor);
00605         m_sin_theta0 = sqrt(m_u*m_u);
00606         m_cos_theta0 = s_steering * s_motor;
00607 
00608         m_v1 =  normalize( s_motor - (s_steering * s_motor) * s_steering  );
00609         m_v2 =  cross(s_steering, m_v1);
00610 
00611         quaternion_type Q;
00612         get_car_body()->get_orientation(Q);
00613         m_v1 = conj(Q).rotate( m_v1 );
00614         m_v2 = conj(Q).rotate( m_v2 );
00615       }
00616     };
00617 
00618   } // namespace mbd
00619 } // namespace OpenTissue
00620 // OPENTISSUE_DYNAMICS_MBD_UTIL_JOINTS_MBD_WHEEL_JOINT_H
00621 #endif
/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/dynamics/mbd/joints/mbd_wheel_joint.h
