U
    ZÊ¹j,  ã                   @   s¨   d dl mZmZmZmZ d dl mZmZmZmZ d dl m	Z	 d dl
mZmZmZmZ d dlmZmZ dd„ Zdd	„ Zd
d„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ ZdS )é    )ÚPointÚReferenceFrameÚDyadicÚ	RigidBody)ÚdynamicsymbolsÚouterÚinertiaÚInertia)Úinertia_of_point_mass)ÚexpandÚzerosÚsimplifyÚsymbols)ÚraisesÚwarns_deprecated_sympyc                  C   s¢   t dƒ} t| jftdƒžŽ }| jdks*t‚| jtdƒks<t‚| jjdksLt‚| j|| jfks`t‚| j|ksnt‚| jjdks~t‚|  	¡ dksŽt‚|  
¡ dksžt‚d S )NÚBz#B_ixx B_iyy B_izz B_ixy B_iyz B_izxZB_massZB_masscenterZB_framea|  RigidBody('B', masscenter=B_masscenter, frame=B_frame, mass=B_mass, inertia=Inertia(dyadic=B_ixx*(B_frame.x|B_frame.x) + B_ixy*(B_frame.x|B_frame.y) + B_izx*(B_frame.x|B_frame.z) + B_ixy*(B_frame.y|B_frame.x) + B_iyy*(B_frame.y|B_frame.y) + B_iyz*(B_frame.y|B_frame.z) + B_izx*(B_frame.z|B_frame.x) + B_iyz*(B_frame.z|B_frame.y) + B_izz*(B_frame.z|B_frame.z), point=B_masscenter)))r   r   Úframer   ÚnameÚAssertionErrorÚmassÚ
masscenterÚcentral_inertiaÚ__str__Ú__repr__)ÚbÚI© r   úk/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_rigidbody.pyÚtest_rigidbody_default   s    ÿr   c            
         sÞ  t dƒ\‰} }}}}tdƒ‰ tdƒ}tdƒ‰tdƒ}tdƒ‰tdƒ}tdˆˆ ˆˆˆfƒ}|jˆksft‚|jˆ kstt‚|jˆks‚t‚|j	ˆ|jfks–t‚| |_||_||_||jf|_	t
t‡ ‡‡‡fdd	„ƒ t
t‡‡‡fd
d	„ƒ t
t‡ ‡‡fdd	„ƒ t
t‡ ‡‡‡fdd	„ƒ | ¡ dk�s"t‚|j| k�s2t‚|j|k�sBt‚|j|k�sRt‚|j	||jfk�sht‚t|j	tƒ�szt‚tdƒ}	| |	||	j ||	j  ||	j  ¡ | |	¡| ||	j ||	j  ||	j   k�sÚt‚d S )Nzm m2 v1 v2 v3 omegaÚAÚA2ÚPÚP2r   r   c                      s   t ˆˆˆ ˆˆˆfƒS )N©r   r   ©r   r   r!   Úmr   r   Ú<lambda>/   ó    z test_rigidbody.<locals>.<lambda>c                      s   t dˆˆˆˆ ˆfƒS ©Nr   r#   r   )r   r!   r%   r   r   r&   0   r'   c                      s   t dˆˆ ˆˆˆfƒS r(   r#   r   )r   r!   r%   r   r   r&   1   r'   c                      s   t dˆˆ ˆˆˆfƒS r(   r#   r   r$   r   r   r&   2   r'   ÚN)r   r   r   r   r   r   r   r   r   r   r   Ú	TypeErrorr   Ú
isinstancer	   Úset_velÚxÚyÚzZlinear_momentum)
Úm2Zv1Zv2Zv3Úomegar    r"   ZI2r   r)   r   r$   r   Útest_rigidbody   s:    &r2   c                  C   s>  t dƒ\} }}}}}tdƒ}tdƒ}| |||j ¡ tdƒ}t|j|jƒ}	|	|f}
td||| |
ƒ}| |||j ¡ | ||¡||j ks�t	‚tdƒ}| |||j ¡ | 
|||j ¡ | ||¡||j | | | |j  ksèt	‚| | | |_|j| | | k�st	‚td| |¡ ƒ|d | |d   k�s:t	‚d S )NzM v r omega g hr)   r   r!   r   ÚOé   )r   r   Úset_ang_velr-   r   r   r   r,   Úangular_momentumr   Zset_posr.   r/   Zpotential_energyr   Zkinetic_energy)ÚMÚvÚrr1   ÚgÚhr)   r   r!   r   ZInertia_tupler   r3   r   r   r   Útest_rigidbody2@   s"    ,r<   c                  C   s  t dƒ\} }}}tdƒ\}}}tdƒ}tdƒ}| dd| |jg¡}	tdƒ}
|
 |||j ||j  ||j  ¡ |
 	d||	j ||	j  ||	j  ¡}| 
|
||	¡ t|	j|	jƒ}td	||	|||fƒ}td
||	||t|| |
¡|	ƒ |
fƒ}|j|jksüt‚| |
|¡| |
|¡k�st‚d S )Nzq1:5zp1:4r%   r   r   Zaxisr3   r!   Úrb1Úrb2)r   r   r   Ú	orientnewr-   r   r,   r.   r/   Ú	locatenewÚv2pt_theoryr   r   r
   Zpos_fromr   r   r6   )Úq1Úq2Zq3Zq4Úp1Zp2Zp3r%   r   r   r3   r!   r   r=   r>   r   r   r   Útest_rigidbody3T   s     &&
ÿrE   c            
      C   s  t dƒ\} }tdƒ}tdƒ}| dd||jg¡}| || ¡ |j ¡ t|d| |d  d | |d  d ƒ}td	ƒ}| 	d
d| |j
 ¡}| 	d||j
 ¡}td||| ||fƒ}	| |d¡ | |||¡ | |||¡ d|  |d  d | ¡  |j |	 ||¡ |¡ dk�st‚dS )zòConsider 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ÚR1ZAxisr   r4   é   r3   r   ÚGÚSé   N)r   r   r   r?   r/   r5   Údiffr   r   r@   r-   r   r,   rA   r6   Zexpressr   )
r%   ÚarF   rG   rH   r   r3   r   rJ   rK   r   r   r   Útest_pendulum_angular_momentumj   s$    & ÿÿrO   c               	   C   sN  t dƒ} tdƒ\}}}}}}t| |||ƒ}tdƒ}| d|| j || j  ¡}	td|| |||	fƒ}
t| ||d |  ||d |  |||d |d    || | ƒ}t|
jt	ƒs¶t
‚|
j||	fksÈt
‚|
j|ksÖt
‚||
_|
j||fksît
‚|
j|ksüt
‚||	f|
_|
j||	fk�st
‚|
j|k�s*t
‚t	||ƒ|
_|
j||fk�sJt
‚d S )Nr)   úm, I_x, I_y, I_z, a, bÚoÚprG   r4   )r   r   r   r   r@   r-   r.   r   r+   r	   r   r   )r)   r%   ÚIxÚIyÚIzrN   r   ÚIorQ   rR   rG   ZI_checkr   r   r   Útest_rigidbody_inertiaˆ   s*      
ÿ
rW   c               	   C   s  t dƒ} tdƒ\}}}}}}t| |||ƒ}tdƒ}| d|| j || j  ¡}	td|| |||fƒ}
|
 |	¡}t| |||d   |||d   |||d |d    | | | d�}||ksÀt	‚t dƒ}| 
| | jd	¡ t|
 |	|¡|  |¡ƒtd
d
ƒk�st	‚d S )Nr)   rP   rQ   rR   rG   r4   )Zixyr   é   rI   )r   r   r   r   r@   r-   r.   r   Zparallel_axisr   Zorient_axisr/   r   Z	to_matrixr   )r)   r%   rS   rT   rU   rN   r   rV   rQ   rR   rG   ZIpZIp_expectedr   r   r   r   Útest_parallel_axisŸ   s&    
  ÿÿÿrY   c               	   C   sb   t dƒ\} }}tdƒ}tdƒ}tdƒ}td||| ||fƒ}tƒ � | | | | ¡ W 5 Q R X d S )Nzm g hr   r!   r   r   )r   r   r   r   r   r   Zset_potential_energy)r%   r:   r;   r   r!   r   r   r   r   r   Ú$test_deprecated_set_potential_energy±   s    rZ   N)Zsympy.physics.mechanicsr   r   r   r   r   r   r   r	   r
   Zsympyr   r   r   r   Zsympy.testing.pytestr   r   r   r2   r<   rE   rO   rW   rY   rZ   r   r   r   r   Ú<module>   s   #