
    MZd                         d dl mZ d dlmZmZmZmZ d dlmZmZm	Z	 d dlm
Z
 d dlmZmZmZ d dlmZmZ d Zd Zd	 Zd
 Zd Zd Zd Zy)    )symbols)PointReferenceFrameDyadic	RigidBody)dynamicsymbolsouterinertia)inertia_of_point_mass)expandzeros_simplify_matrix)raiseswarns_deprecated_sympyc                    
 t        d      \  } }}}}t        d      
t        d      }t        d      t        d      }t        d      t        d      }t	        d
f      }|j
                  k(  sJ |j                  
k(  sJ |j                  k(  sJ |j                  |j                  fk(  sJ | |_        ||_        ||_        ||j                  f|_        t        t        
fd       t        t        fd	       t        t        
fd
       t        t        
fd       |j                         dk(  sJ |j
                  | k(  sJ |j                  |k(  sJ |j                  |k(  sJ |j                  ||j                  fk(  sJ |j                  |k(  sJ |j                  ||j                  fk(  sJ t        d      }	|j                  |	||	j                  z  ||	j                  z  z   ||	j                  z  z          |j!                  |	      | ||	j                  z  ||	j                  z  z   ||	j                  z  z   z  k(  sJ y )Nzm m2 v1 v2 v3 omegaAA2PP2r   Bc                  &    t         f      S )Nr   r   Ir   ms   N/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_rigidbody.py<lambda>z test_rigidbody.<locals>.<lambda>   s    i1aQF;     c                  &    t        d f      S Nr   r   )r   r   r   s   r   r   z test_rigidbody.<locals>.<lambda>       iQ1q!f= r   c                  &    t        d f      S r    r   )r   r   r   s   r   r   z test_rigidbody.<locals>.<lambda>   r!   r   c                  &    t        d f      S r    r   r   s   r   r   z test_rigidbody.<locals>.<lambda>   r!   r   N)r   r   r   r   r   massframe
masscenterr
   r   	TypeError__str__set_velxyzlinear_momentum)m2v1v2v3omegar   r   I2r   r$   r   r   r   r   s             @@@@r   test_rigidbodyr5   
   s   &'<=Ar2r2usA		Bc
A	tBq	A	B#q!QA'A66Q;;77a<<<<199ALL))))AFAGALQ\\"AI
9;<
9=>
9=>
9=>99;#66R<<77b==<<299Q\\****<<299Q\\**** 	sAJJq"qss(R!##X%QSS01Q2accBH)<rACCx)G#HHHHr   c                  >   t        d      \  } }}}}}t        d      }t        d      }|j                  |||j                  z         t	        d      }t        |j                  |j                        }	|	|f}
t        d||| |
      }|j                  |||j                  z         |j                  ||      ||j                  z  k(  sJ t	        d      }|j                  |||j                  z         |j                  |||j                  z         |j                  ||      ||j                  z  | |z  |z  |j                  z  z
  k(  sJ | |z  |z  |_        |j                  | |z  |z  k(  sJ t        d|j                  |      z        |dz  | |dz  z  z   k(  sJ y )NzM v r omega g hr$   br   r   O   )r   r   set_ang_velr+   r   r	   r   r*   angular_momentumset_posr,   r-   potential_energyr   kinetic_energy)Mvrr3   ghr$   r7   r   r   Inertia_tupler   r8   s                r   test_rigidbody2rE   .   s   )*;<Aq!UAqsAsAMM!UQSS[!c
Aacc133AFM#q!Q.AIIaQSSa#uqss{222c
AIIaQSSIIaQSSa#uqss{QqSU133Y'>>>>QAQ***!a&&q))*eQhQT.AAAAr   c                  &   t        d      \  } }}}t        d      \  }}}t        d      }t        d      }|j                  dd| |j                  g      }	t        d      }
|
j                  |||j                  z  ||j                  z  z   ||j                  z  z          |
j                  d||	j                  z  ||	j                  z  z   ||	j                  z  z         }|j                  |
||	       t        |	j                  |	j                        }t        d	||	|||f      }t        d
||	||t        ||j                  |
      |	      z   |
f      }|j                  |j                  k(  sJ |j!                  |
|      |j!                  |
|      k(  sJ y )Nzq1:5zp1:4r   r   r   axisr8   r   rb1rb2)r   r   r   	orientnewr+   r   r*   r,   r-   	locatenewv2pt_theoryr	   r   r   pos_fromcentral_inertiar;   )q1q2q3q4p1p2p3r   r   r   r8   r   r   rH   rI   s                  r   test_rigidbody3rV   A   sh   #F+NBBJBBAsA	C"acc+Ac
AIIaACC"QSS&2acc6)*	CACC"QSS&2acc612AMM!Qacc133A
E1aQF
+C
E1a.q!**Q-CCQGIC #"5"55551%)=)=a)CCCCr   c                     t        d      \  } }t        d      }t        d      }|j                  dd||j                  g      }|j                  ||j                         |j                  z         t        |d| |dz  z  dz  | |dz  z  dz        }t        d	      }|j                  d
d|z  |j                  z        }|j                  d||j                  z        }t        d||| ||f      }	|j                  |d       |j                  |||       |j                  |||       d| z  |dz  z  dz  |j                         z  |j                  z  |	j                  ||      j                  |      z
  dk(  sJ y)zConsider a pendulum of length OA = 2a, of mass m as a rigid body of
    center of mass G (OG = a) which turn around (O,z). The angle between the
    reference frame R and the rod is q.  The inertia of the body is I =
    (G,0,ma^2/3,ma^2/3). zm, aqRR1Axisr   r9      r8   r   GS   N)r   r   r   rJ   r-   r:   diffr
   r   rK   r+   r   r*   rL   r;   express)
r   arX   rY   rZ   r   r8   r   r]   r^   s
             r   test_pendulum_angular_momentumrc   W   s]    6?DAqsAsA	
T6Aqss8	,BNN1affhn%Aq1a4x!|QAX\2Ac
A	C1rtt$A	CRTT"A#q"a!Q(AIIaOMM!QMM!QEAqDL1qvvx'!##-q!$,,Q/0456 6 6r   c            
      T   t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t        d|| |||	f      }
t        | ||dz  |z  z
  ||dz  |z  z
  |||dz  |dz  z   z  z
  ||z  |z        }|
j                  ||	fk(  sJ |
j                  |k(  sJ ||
_        |
j                  ||fk(  sJ |
j                  |k(  sJ ||	f|
_        |
j                  ||	fk(  sJ |
j                  |k(  sJ y )Nr$   m, I_x, I_y, I_z, a, boprY   r9   )	r   r   r
   r   rK   r+   r,   r   rN   )r$   r   IxIyIzrb   r7   Iorf   rg   rY   I_checks               r   test_rigidbody_inertiarm   u   sT   sA!":;Ar2r1a	BB	Bc
A	CQSS1qss7*+A#q!QQ(Aaa1fqj"qAvz/1Qa00!a%!)=G99Q'''A99Q"""QAI99Q'''r   c            
      X   t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t        d|| |||f      }
|
j                  |	      }t        | |||dz  z  z   |||dz  z  z   |||dz  |dz  z   z  z   | |z  |z        }||k(  sJ t        d      }|j                  | | j                  d	       t        |
j                  |	|      |z
  j                  |            t        d
d
      k(  sJ y )Nr$   re   rf   rg   rY   r9   )ixyr      r\   )r   r   r
   r   rK   r+   r,   r   parallel_axisorient_axisr-   r   	to_matrixr   )r$   r   rh   ri   rj   rb   r7   rk   rf   rg   rY   IpIp_expectedr   s                 r   test_parallel_axisrv      sA   sA!":;Ar2r1a	BB	Bc
A	CQSS1qss7*+A#q!QQ(A	
	B!R!ad(]BQTMqAqD1a4K00qb1fqjBKsAMM!QSS!	
A		,77:<?DQ{K K Kr   c                      t        d      \  } }}t        d      }t        d      }t        d      }t	        d||| ||f      }t               5  |j                  | |z  |z         d d d        y # 1 sw Y   y xY w)Nzm g hr   r   r   r   )r   r   r   r   r   r   set_potential_energy)r   rB   rC   r   r   r   r   s          r   $test_deprecated_set_potential_energyry      su    gGAq!sAc
Aq	A#q!QA'A		! &	qs1u%& & &s   A--A6N)sympy.core.symbolr   sympy.physics.mechanicsr   r   r   r   r   r	   r
   r   sympy.core.backendr   r   r   sympy.testing.pytestr   r   r5   rE   rV   rc   rm   rv   ry    r   r   <module>r      sH    % L L B B 9 > > ?!IHB&D,6<(&K$&r   