
    Rie                         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dS )    )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           |                                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        z  ||	j        z  z   ||	j        z  z              |                    |	          | ||	j        z  ||	j        z  z   ||	j        z  z   z  k    sJ d S )Nzm m2 v1 v2 v3 omegaAA2PP2r   Bc                  .    t           f          S )Nr   r   Ir   ms   Llib/python3.11/site-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6Q;;;;7a<<<<<19AL)))))AFAGALQ\"AI
9;;;;;;;<<<
9======>>>
9======>>>
9=======>>>99;;#6R<<<<7b====<29Q\*****<29Q\***** 	sAJJq"qs(R!#X%QS0111Q2acBH)<rACx)G#HHHHHHHr   c                  \   t          d          \  } }}}}}t          d          }t          d          }|                    |||j        z             t	          d          }t          |j        |j                  }	|	|f}
t          d||| |
          }|                    |||j        z             |                    ||          ||j        z  k    sJ t	          d          }|                    |||j        z             |	                    |||j
        z             |                    ||          ||j        z  | |z  |z  |j        z  z
  k    sJ | |z  |z  |_        |j        | |z  |z  k    sJ t          d|                    |          z            |dz  | |dz  z  z   k    sJ d S )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!UQS[!!!c

Aac13AFM#q!Q..AIIaQSa##uqs{2222c

AIIaQSIIaQSa##uqs{QqSU13Y'>>>>>QAQ****!a&&q)))**eQhQT.AAAAAAAr   c                  >   t          d          \  } }}}t          d          \  }}}t          d          }t          d          }|                    dd| |j        g          }	t          d          }
|
                    |||j        z  ||j        z  z   ||j        z  z              |
	                    d||	j        z  ||	j        z  z   ||	j        z  z             }|
                    |
||	           t          |	j        |	j                  }t          d	||	|||f          }t          d
||	||t          ||                    |
          |	          z   |
f          }|j        |j        k    sJ |                    |
|          |                    |
|          k    sJ d S )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   s   #F++NBBJBBAsA	C"ac++Ac

AIIaAC"QS&2ac6)***	CAC"QS&2ac6122AMM!Qac13A
E1aQF
+
+C
E1a.q!**Q--CCCQGI IC #"555551%%)=)=a)C)CCCCCCCr   c                  N   t          d          \  } }t          d          }t          d          }|                    dd||j        g          }|                    ||                                |j        z             t          |d| |dz  z  dz  | |dz  z  dz            }t          d	          }|	                    d
d|z  |j
        z            }|	                    d||j
        z            }t          d||| ||f          }	|                    |d           |                    |||           |                    |||           d| z  |dz  z  dz  |                                z  |j        z  |	                    ||                              |          z
  dk    sJ dS )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	
T6Aqs8	,	,BNN1affhhn%%%Aq1a4x!|QAX\22Ac

A	C1rt$$A	CRT""A#q"a!Q((AIIaOOOMM!QMM!QEAqDL1qvvxx'!#-q!$$,,Q//0456 6 6 6 6 6r   c            
      J   t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    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 d S )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   so   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++A#q!QQ((Aaa1fqj"qAvz/1Qa00!a%!)= =G9Q''''A9Q""""QAI9Q''''''r   c            
         t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    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            }||k    sJ t          d          }|	                    | | j
        d	           t          |
                    |	|          |z
                      |                    t          d
d
          k    sJ d S )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      s   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++A#q!QQ((A	
		B!R!ad(]BQTMqAqD1a4K00qb1fqjB B BKsAMM!QS!	
A			,77::< <?DQ{{K K K K K Kr   c                  4   t          d          \  } }}t          d          }t          d          }t          d          }t	          d||| ||f          }t                      5  |                    | |z  |z             d d d            d S # 1 swxY w Y   d S )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      s    gGAq!sAc

Aq		A#q!QA''A		!	! & &	qs1u%%%& & & & & & & & & & & & & & & & & &s   $BBB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      s!   % % % % % % L L L L L L L L L L L L B B B B B B B B B B 9 9 9 9 9 9 > > > > > > > > > > ? ? ? ? ? ? ? ?!I !I !IHB B B&D D D,6 6 6<( ( (&K K K$& & & & &r   