ó
~9­\c           @   sÆ  d  d l  m Z m Z m Z m Z m Z m Z m Z m Z d  d l	 m
 Z
 m Z m Z m Z m Z d  d l	 m Z m Z m Z m Z m Z m Z m Z m Z m Z m Z d  d l m Z m Z d  d l m Z d  d l m Z e e _  e d ƒ \ Z! Z" Z# Z$ Z% e d ƒ Z& e& j' d	 d
 e! e& j( g ƒ Z) e) j' d d
 e" e) j* g ƒ Z+ e+ j' d d
 e# e+ j, g ƒ Z- d „  Z. d „  Z/ d „  Z0 d „  Z1 d „  Z2 d „  Z3 d „  Z4 d „  Z5 d „  Z6 d „  Z7 d S(   iÿÿÿÿ(   t   sint   cost   tant   pit   symbolst   Matrixt   zerost   S(   t   Particlet   Pointt   ReferenceFramet	   RigidBodyt   Vector(
   t   angular_momentumt   dynamicsymbolst   inertiat   inertia_of_point_masst   kinetic_energyt   linear_momentumt   outert   potential_energyt   msubst   find_dynamicsymbols(   t   gravityt   center_of_mass(   R   (   t   raisess   q1 q2 q3 q4 q5t   Nt   At   Axist   Bt   Cc          C   s„  t  d ƒ }  t d ƒ \ } } } t d ƒ \ } } } t |  | | | ƒ | |  j |  j B| |  j |  j B| |  j |  j Bk s‰ t ‚ t |  d d d ƒ d |  j |  j Bk s¸ t ‚ t |  | | | | | | ƒ | |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j B| |  j |  j Bk s€t ‚ d  S(   NR   s   ixx iyy izzs   ixy iyz izxi    (   R
   R   R   t   xt   yt   zt   AssertionError(   R   t   ixxt   iyyt   izzt   ixyt   iyzt   izx(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_inertia   s    A/c    
      C   s{  t  d ƒ \ }  } } } t d ƒ } |  | j } t | | | ƒ } | | |  d | j | j B| |  d | j | j Bk s… t ‚ | | j } t | | | ƒ } | | | d | j | j B| | d | j | j Bk sæ t ‚ | | j } t | | | ƒ } | | | d | j | j B| | d | j | j Bk sGt ‚ | | | }	 t | |	 | ƒ } | | | d | d | j | j B| |  | | j | j B| |  | | j | j B| |  | | j | j B| |  d | d | j | j B| | | | j | j B| |  | | j | j B| | | | j | j B| |  d | d | j | j Bk swt ‚ d  S(   Ns   r s t mR   i   (   R   R
   R   R   R    R!   R"   (
   t   rt   st   tt   mR   t   pxt   It   pyt   pzt   p(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_inertia_of_point_mass#   s     BBBßc          C   sÎ   t  d ƒ }  t d ƒ } | j |  d |  j ƒ t |  j |  j ƒ } t d | |  d | | f ƒ } t d ƒ } t d | d ƒ } | j j |  d	 |  j ƒ t	 |  | | ƒ d	 |  j d
 |  j k sÊ t
 ‚ d  S(   NR   t   Aci   R   i   t   Pt   Pai   i
   iô  (   R
   R	   t   set_velR    R   R   R   R   t   pointR   R"   (   R   R4   R/   R   R5   R6   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_linear_momentum@   s    c       	   C   s­  t  d ƒ \ }  } } } t d ƒ } t d ƒ } t d ƒ } t d ƒ } | j d | | j ƒ } | j d | | j ƒ }	 | j | d | j ƒ | j | | | j ƒ | j	 | | | ƒ |	 j	 | | | ƒ t
 d	 |	 |  ƒ }
 t d
 | | | | t | j | j ƒ | f ƒ } d |  | | | j | | | | j } t | | |
 ƒ | k s[t ‚ | | | d d |  | d | | j } t | | | |
 ƒ | k s©t ‚ d S(   s   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.s
   m, M, l, It   omegaR   t   at   OR4   R5   i    R6   R   i   i   N(   R   R   R
   R	   t	   locatenewR   R7   t   set_ang_velR!   t   v2pt_theoryR   R   R   R    R   R"   R   (   R-   t   Mt   lR/   R:   R   R;   R<   R4   R5   R6   R   t   expected(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt)   test_angular_momentum_and_linear_momentumL   s"    1,-c          C   st  t  d ƒ \ }  } } t d ƒ } t d ƒ } t d ƒ } | j | d | j ƒ | j d | | j ƒ } | j d | | j ƒ } t d ƒ } | j | | | j ƒ | j	 | | | ƒ | j	 | | | ƒ t
 d	 | |  ƒ }	 t | j | j ƒ }
 t d
 | | | |
 | f ƒ } d t | |	 | ƒ | | d | d d d | d |  | d | d d j ƒ  k spt ‚ d  S(   Ns   m M l1R:   R   R<   i    R4   R5   R;   R6   R   i   (   R   R   R
   R	   R7   R   R=   R>   R!   R?   R   R   R   R   t   expandR"   (   R-   R@   t   l1R:   R   R<   R4   R5   R;   R6   R/   R   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_kinetic_energyd   s     c          C   sn  t  d ƒ \ }  } } } } } t d ƒ } t d ƒ } t d ƒ } | j | d | j ƒ | j d | | j ƒ }	 |	 j d | | j ƒ }
 t d ƒ } | j | | | j ƒ |	 j	 | | | ƒ |
 j	 | | | ƒ t
 d	 |
 |  ƒ } t | j | j ƒ } t d
 |	 | | | |	 f ƒ } |  | | | _ | | | | _ t | | ƒ |  | | | | | k sjt ‚ d  S(   Ns   m M l1 g h HR:   R   R<   i    R4   R5   R;   R6   R   (   R   R   R
   R	   R7   R   R=   R>   R!   R?   R   R   R   R   R"   (   R-   R@   RE   t   gt   ht   HR:   R   R<   R4   R5   R;   R6   R/   R   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_potential_energyw   s"    c          C   s  t  d ƒ \ }  } t d ƒ \ } } } t |  | | | | j ƒ  | g | j ƒ  j ƒ  | t | j ƒ  ƒ g g ƒ } t |  | | g | j ƒ  j ƒ  d g g ƒ } i d | 6d | 6d | j ƒ  6d | j ƒ  6} t | | ƒ | k sî t ‚ t | | ƒ t | | ƒ | | j ƒ  } i d | 6t	 d | 6d | j ƒ  6} t | | d t
 ƒ| d k sdt ‚ t d ƒ } | | j | | j }	 | | j | j B| | j | j B}
 d | j } d | j | j B} i d | 6d | 6} t |	 | ƒ | k sþt ‚ t |
 | ƒ | k st ‚ d  S(   Ns   a, bs   x, y, zi   i    i   t   smartR   (   R   R   R   t   diffR    R   R"   R   R   R   t   TrueR
   R   R    (   R;   t   bR   R    R!   t   exprt   solt   sdR   t   vt   dt   v_solt   d_sol(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt
   test_msubs‹   s&    "..,%%&c             sž  t  d ƒ \ }  } t d ƒ \ } } } t |  | | | | j ƒ  | g | j ƒ  j ƒ  | t | j ƒ  ƒ g g ƒ } | | j ƒ  | | j ƒ  j ƒ  | | j ƒ  h } t | ƒ | k s¿ t ‚ | | | g } | j ƒ  | j ƒ  j ƒ  | j ƒ  h } t | d | ƒ| k st ‚ t d ƒ \ } }	 }
 t d ƒ } | | j |	 | j	 |
 | j
 ‰  | |	 |
 h } t ˆ  d | ƒ| k s„t ‚ t t ‡  f d †  ƒ d  S(   Ns   a, bs   x, y, zt   excludes   d, e, fR   t   reference_framec              s
   t  ˆ  ƒ S(   N(   R   (    (   RR   (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   <lambda>¶   t    (   R   R   R   RL   R    R   R"   R
   R   R    R!   R   t
   ValueError(   R;   RN   R   R    R!   RO   RP   t   exclude_listRS   t   et   fR   (    (   RR   sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_find_dynamicsymbols£   s    ".0'#c          C   sx  t  d ƒ }  t d ƒ \ } } } t d ƒ \ } } t d ƒ } t d | | ƒ } t  d ƒ } t d ƒ }	 t | j | j ƒ }
 t d |	 | | |
 |	 f ƒ } | | f |	 | f g } | j t	 | |  j
 | | ƒ ƒ | | f |	 | f | | | |  j
 f |	 | | |  j
 f g } x] t t | ƒ ƒ D]I } x@ t t | | ƒ ƒ D]( } | | | | | | k sDt ‚ qDWq'Wd  S(	   NR   s   m M gs   F1 F2t   pot   paR   R5   R   (   R
   R   R   R	   R   R   R   R   t   extendR   R    t   ranget   lenR"   (   R   R-   R@   RG   t   F1t   F2R`   Ra   R   R5   R/   R   t	   forceListRA   t   it   j(    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_gravity¹   s     @c       	   C   sÿ  t  d ƒ }  t d d t ƒ} t d t d ƒ t d ƒ ƒ } t d t d ƒ t d	 ƒ ƒ } t d
 t d ƒ t d ƒ ƒ } t d t d ƒ | ƒ } t  d ƒ } t d ƒ } t d ƒ } t d | | | t | j | j ƒ | f ƒ }	 | j	 j
 | j	 |  j ƒ | j	 j
 | j	 |  j |  j ƒ | j	 j
 | j	 |  j ƒ |	 j j
 | j	 |  j |  j ƒ t d ƒ }
 |
 j
 | j	 t | j	 | | | | |	 ƒ ƒ d | | d |  j | | d | | d |  j | | | d |  j } |
 j | j	 ƒ | d k sût ‚ d  S(   NR;   R-   t   realt   p1t   p1_pti   t   p2t   p2_pti   t   p3t   p3_pti   t   p4t   p4_ptt   b_ft   b_cmt   mbRN   t   oi   i   i    (   R
   R   RM   R   R	   R   R   R   R   R8   t   set_posR    t
   masscenterR!   R   t   pos_fromR"   (   R;   R-   Rl   Rn   Rp   Rr   Rt   Ru   Rv   RN   t   point_oRO   (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   test_center_of_massÎ   s$    -  +ON(8   t   sympy.core.backendR    R   R   R   R   R   R   R   t   sympy.physics.mechanicsR   R	   R
   R   R   R   R   R   R   R   R   R   R   R   R   t!   sympy.physics.mechanics.functionsR   R   t   sympy.physics.vector.vectort   sympy.utilities.pytestR   RM   t   simpt   q1t   q2t   q3t   q4t   q5R   t	   orientnewR!   R   R   R   R    R   R)   R3   R9   RC   RF   RJ   RV   R_   Rj   R|   (    (    (    sK   lib/python2.7/site-packages/sympy/physics/mechanics/tests/test_functions.pyt   <module>   s*   :(F										