00001 #ifndef OPENTISSUE_DYNAMICS_MBD_MBD_JOINT_SOCKET_H 00002 #define OPENTISSUE_DYNAMICS_MBD_MBD_JOINT_SOCKET_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 namespace OpenTissue 00013 { 00014 namespace mbd 00015 { 00021 template<typename mbd_types> 00022 class JointSocket 00023 { 00024 public: 00025 00026 typedef typename mbd_types::body_type body_type; 00027 typedef typename mbd_types::socket_type socket_type; 00028 typedef typename mbd_types::math_policy::vector3_type vector3_type; 00029 typedef typename mbd_types::math_policy::matrix3x3_type matrix3x3_type; 00030 typedef typename mbd_types::math_policy::quaternion_type quaternion_type; 00031 typedef typename mbd_types::math_policy::coordsys_type coordsys_type; 00032 00033 protected: 00034 00035 body_type * m_body; 00036 coordsys_type m_joint_frame; 00037 00038 public: 00039 00040 JointSocket() 00041 : m_body(0) 00042 {} 00043 00044 public: 00045 00046 void init(body_type const & body, coordsys_type const & joint_frame) 00047 { 00048 m_body = const_cast<body_type*>(&body); 00049 m_joint_frame = joint_frame; 00050 } 00051 00052 body_type * get_body() { return m_body; } 00053 body_type const * get_body() const { return m_body; } 00054 00055 vector3_type get_anchor_local() const { return m_joint_frame.T(); } 00056 vector3_type get_axis1_local() const { return m_joint_frame.Q().rotate(vector3_type(1,0,0)); } 00057 vector3_type get_axis2_local() const { return m_joint_frame.Q().rotate(vector3_type(0,1,0)); } 00058 vector3_type get_joint_axis_local() const { return m_joint_frame.Q().rotate(vector3_type(0,0,1)); } 00059 00060 vector3_type get_anchor_world() const 00061 { 00062 assert(m_body || !"JointSocket::get_anchor_world(): body was null"); 00063 matrix3x3_type R; 00064 vector3_type r; 00065 m_body->get_orientation(R); 00066 m_body->get_position(r); 00067 return (R*m_joint_frame.T() + r); 00068 } 00069 00070 vector3_type get_axis1_world() const 00071 { 00072 assert(m_body || !"JointSocket::get_axis1_world(): body was null"); 00073 matrix3x3_type R; 00074 m_body->get_orientation(R); 00075 return R*get_axis1_local(); 00076 } 00077 00078 vector3_type get_axis2_world() const 00079 { 00080 assert(m_body || !"JointSocket::get_axis2_world(): body was null"); 00081 matrix3x3_type R; 00082 m_body->get_orientation(R); 00083 return R*get_axis2_local(); 00084 } 00085 00086 vector3_type get_joint_axis_world() const 00087 { 00088 assert(m_body || !"JointSocket::get_axis2_world(): body was null"); 00089 matrix3x3_type R; 00090 m_body->get_orientation(R); 00091 return R*get_joint_axis_local(); 00092 } 00093 00094 coordsys_type get_joint_frame() const { return m_joint_frame; } 00095 00096 }; 00097 00098 } // namespace mbd 00099 } // namespace OpenTissue 00100 // OPENTISSUE_DYNAMICS_MBD_MBD_JOINT_SOCKET_H 00101 #endif
1.7.1