U
    ZÊ¹j#/  ã                   @   sè   d dl 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 d dl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d„ 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 )(é    )ÚSymbolÚsymbolsÚsinÚcosÚMatrixÚzerosÚsimplify)ÚPointÚReferenceFrameÚdynamicsymbolsÚDyadic)ÚinertiaÚBody)ÚraisesÚwarns_deprecated_sympyc            
   	   C   sÈ   t ƒ � tdƒ} W 5 Q R X | jdks(t‚| jg ks6t‚tdƒ}| | jd¡ | j}| j}| 	|¡| 	|¡kspt‚| j
tdƒks‚t‚tdƒ\}}}tdƒ\}}}	| jt| j||||||	ƒ| jfksÄt‚d S )NÚbodyZbody_masscenterr   Z	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   r   ÚnameÚAssertionErrorÚloadsr	   Úset_velÚframeÚ
masscenterÚvelÚmassr   r   r   )
r   ÚpointÚcomr   ZixxZiyyZizzÚixyZiyzZizx© r   úf/var/www/html/bwave/python/venv/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_body.pyÚtest_default   s    ÿr   c               	   C   sö   t dƒ} tdƒ}tdƒ}t|dddƒ}tƒ � td| |||ƒ}W 5 Q R X |j}|j}|  |d¡ | 	|¡|  	|¡ksxt
‚| |¡|  |¡ks�t
‚|j|ksžt
‚|j|| fks°t
‚|jsºt
‚t|dƒsÈt
‚t|dƒsÖt
‚t|d	ƒsät
‚t|d
ƒsòt
‚d S )NÚrigidbody_masscenterÚrigidbody_massÚrigidbody_frameé   r   Úrigidbody_bodyr   r   r   r   )r	   r   r
   r   r   r   r   r   r   r   r   Úpos_fromr   Úis_rigidbodyÚhasattr)r    r!   r"   Úbody_inertiaÚ
rigid_bodyr   r   r   r   r   Útest_custom_rigid_body   s,      ÿ
r*   c               	   C   sÀ  t dƒ} tdƒ}tdƒ}tƒ � td| ||ƒ}W 5 Q R X |j}|j}|  |d¡ | |¡|  |¡ksht	‚| 
|¡|  
|¡ks€t	‚|j|ksŽt	‚t|dƒrœt	‚t|dƒsªt	‚t|dƒs¸t	‚t|d	ƒsÆt	‚|jtdƒ|jfksÞt	‚|jtdƒksðt	‚|jrút	‚t|d
d
d
ƒ|_|jt|d
d
d
ƒk�s$t	‚|j�s0t	‚tƒ � td|d�}W 5 Q R X |j�rZt	‚|j d|j¡}|t|jdd
d
ƒ }||f|_|j||fk�sœt	‚|jtdƒk�s°t	‚|j�s¼t	‚d S )NÚparticle_masscenterÚparticle_massÚparticle_frameÚparticle_bodyr   Z_inertiar   r   r   r#   )r   r   )r	   r   r
   r   r   r   r   r   r   r   r%   r   r'   r   r   Úcentral_inertiar&   Ú	locatenewÚx)r+   r,   r-   r.   r   r   r   Zpoint_inertiar   r   r   Útest_particle_body3   sB     ÿ

r2   c            	   	   C   sì   t dƒ} tdƒ}tdƒ}tƒ � td| ||ƒ}W 5 Q R X tdƒ}||jj }| ||j¡ t	|j
ƒdkslt‚|j |jd d¡}| |jd¡ |j
d d }|j}| |¡| |¡ksºt‚| |¡| |¡ksÒt‚|j
d d |ksèt‚d S )	Nr+   r,   r-   r.   Úar#   Z_point0r   )r	   r   r
   r   r   r   r1   Úapply_forcer   Úlenr   r   r0   Ú_namer   r   r%   )	r+   r,   r-   r.   r3   Úforce_vectorr   Úforce_pointr   r   r   r   Útest_particle_body_add_forceY   s,     ÿ ÿr9   c            	   	      sb  t dƒ} tdƒ}tdƒ}t|dddƒ}tƒ � td| |||ƒ‰W 5 Q R X tdƒ}tdƒ}ˆj d	|ˆjj	 ¡}| 
ˆjd¡ |ˆjj ‰ ˆ ˆ |¡ tˆjƒdks¦t‚ˆjd d }ˆj}| |¡| |¡ksÒt‚| |¡| |¡ksêt‚ˆjd d ˆ k�st‚ˆ ˆ ¡ tˆjƒd
k�s t‚ˆjd d ˆ k�s8t‚tt‡ ‡fdd„ƒ tt‡fdd„ƒ d S )Nr    r!   r"   r#   r   r$   ÚlÚFaZrigidbody_body_point0é   c                      s   ˆ  ˆ d¡S ©Nr   ©r4   r   ©r7   r)   r   r   Ú<lambda>�   ó    z%test_body_add_force.<locals>.<lambda>c                      s
   ˆ   d¡S r=   r>   r   )r)   r   r   r@   ‘   rA   )r	   r   r
   r   r   r   r   r0   r   r1   r   Úzr4   r5   r   r   r   r%   r   Ú	TypeError)	r    r!   r"   r(   r:   r;   r   r8   r   r   r?   r   Útest_body_add_forcer   s<      ÿ
þ
rD   c               	      sl   t ƒ � tdƒ‰ W 5 Q R X ˆ jj} ˆ  | ¡ tˆ jƒdks>t‚ˆ jd ˆ j| fksVt‚tt	‡ fdd„ƒ d S )Nr   r#   r   c                      s
   ˆ   d¡S r=   )Úapply_torquer   ©r   r   r   r@   ›   rA   z&test_body_add_torque.<locals>.<lambda>)
r   r   r   r1   rE   r5   r   r   r   rC   )Ztorque_vectorr   rF   r   Útest_body_add_torque“   s    
rG   c               	   C   s|   t ƒ � tdƒ} W 5 Q R X tdƒ}t ƒ � td|d�}W 5 Q R X | j ||j¡ |  |¡|jksdt‚|  |¡|jksxt‚d S ©NÚAÚNÚB©r   )r   r   r
   r   r   rB   Zmasscenter_velr   ©rI   rJ   rK   r   r   r   Útest_body_masscenter_vel�   s    rN   c               	   C   s’   t ƒ � tdƒ} W 5 Q R X tdƒ}t ƒ � td|d�}W 5 Q R X | j ||j¡ |  |¡|jksdt‚| | ¡|j kszt‚|  |¡|jksŽt‚d S rH   )r   r   r
   r   Zset_ang_velÚyZ
ang_vel_inr   rM   r   r   r   Útest_body_ang_vel§   s    rP   c               	   C   s¾   t ƒ � tdƒ} tdƒ}W 5 Q R X | j |j|jjd¡ |  |¡ttdƒtdƒdgtdƒ tdƒdgdddggƒksxt	‚|  |j¡ttdƒtdƒdgtdƒ tdƒdgdddggƒksºt	‚d S )NrI   rK   é
   r   r#   )
r   r   r   Úorient_axisrB   Zdcmr   r   r   r   )rI   rK   r   r   r   Útest_body_dcm²   s    @rS   c               	   C   sZ   t dƒ} tƒ � td| d�}W 5 Q R X |j| jks6t‚|j| jksFt‚|j| jksVt‚d S )NrJ   rK   rL   )r
   r   r   r1   r   rO   rB   )rJ   rK   r   r   r   Útest_body_axisº   s    rT   c               	   C   sŠ   t dƒ\} }tdƒ}tƒ � tdƒ}W 5 Q R X | |j }||j }| ||¡ |j||fgksbt‚| ||¡ |j||| fgks†t‚d S )Nza bÚPrK   )	r   r	   r   r   r1   rO   r4   r   r   )r3   ÚbrU   rK   Úf1Úf2r   r   r   Ú#test_apply_force_multiple_one_pointÂ   s    

rY   c               	   C   sœ  t dƒ\} }tdƒ\}}}}tdƒ}tdƒ}tƒ � tdƒ}tdƒ}	W 5 Q R X tdƒ}
| |j||j ¡ | |	j||	j ¡ | | |
j	 }| 
|||	|¡ |j||fgks°t‚|	j|| fgksÆt‚|j| |
j }|	j| |
j }| 
|¡ |	 
|¡ |j||f|j|fgk�st‚|	j|| f|	j|fgk�s8t‚||
j }|j
||	d� |j||f|j|| fgk�srt‚|	j|| f|	j| | fgk�s˜t‚d S )	Nzf gz	q x v1 v2ÚP1ÚP2ÚB1ÚB2rJ   ©Zreaction_body)r   r   r	   r   r   r
   r   r   r1   rB   r4   r   r   r   rO   r   )ÚfÚgÚqr1   Zv1Zv2rZ   r[   r\   r]   rJ   ÚforceÚg1Úg2Zforce2r   r   r   Útest_apply_forceÎ   s0    

 
"re   c               	   C   s´   t dƒ} tdƒ}tƒ � tdƒ}tdƒ}W 5 Q R X tdƒ}| | |j }| ||¡ |j|j|fgksjt	‚|j|j| fgks‚t	‚| |j
 }| |¡ |j|j|| fgks°t	‚d S )NÚtra   r\   r]   rJ   )r   r   r   r   r
   r1   rE   r   r   r   rO   )rf   ra   r\   r]   rJ   ZtorqueZtorque2r   r   r   Útest_apply_torqueð   s    

rg   c               	   C   sn   t dƒ} tdƒ}tƒ � tdƒ}W 5 Q R X | |j }| ||¡ |j||fgksTt‚| ¡  |jg ksjt‚d S )Nr3   rU   rK   )	r   r	   r   r   rB   r4   r   r   Zclear_loads)r3   rU   rK   rb   r   r   r   Útest_clear_load  s    
rh   c               	   C   sØ   t dƒ} t dƒ}tƒ � tdƒ}W 5 Q R X |j}|j}| || ¡ | ||¡ |j| |f||fgksht‚| |¡ |j| |fgks†t‚| 	| 
|¡¡ |j| |f|j| 
|¡fgks¸t‚| ¡  |j| |fgksÔt‚d S )NrZ   r[   rK   )r	   r   r   r1   rO   r4   r   r   Zremove_loadrE   Úcrossr   )rZ   r[   rK   rW   rX   r   r   r   Útest_remove_load  s    
"rj   c               	   C   sÈ  t ƒ �& tdƒ} tdƒ}tdƒ}tdƒ}W 5 Q R X tdƒ\}}tdƒ\}}}}	tdƒ\}
}| |
| j ¡ | j|| | j |d� | j|| ¡  | j |d� | |j| | j ¡ | |j| | j ¡ |j	|	| | j
 |d� | 	|| j
 ¡ |j|j|
||  || ¡   | j fgk�s$t‚|j|j|j| | j f|j||	|  | j
 fgk�s^t‚|j|j|j| | j f|j|	 | | j
 fgk�s–t‚| j| j|| ¡  ||  | j fgk�sÄt‚d	S )
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlÚWrK   rU   rV   zq1 q2zk c g kTzF Tr^   N)r   r   r   r   r4   r1   Údiffr   rO   rE   rB   r   r   r   r   )rk   rK   rU   rV   Úq1Úq2ÚkÚcr`   ZkTÚFÚTr   r   r   Ú9test_apply_loads_on_multi_degree_freedom_holonomic_system  s&    2:8rs   c               	   C   s¾  t dƒ} tdƒ\}}}}}}t| |||ƒ}tdƒ}| d|| j || j  ¡}	tƒ � td|| ||d�}
W 5 Q R X |
 	|	¡}t| |||d   |||d   |||d |d    | | | d�}||ksÐt
‚t d	ƒ}| | | jd
¡ t|
 	|	|¡|  |¡ƒtddƒk�st
‚tdƒ}| d|| j || j  ¡}	tƒ � td||| d�}W 5 Q R X | 	|	| ¡}t| ||d  ||d  ||d |d   | | | d�}|j�r¬t
‚||k�sºt
‚d S )NrJ   zm, I_x, I_y, I_z, a, bÚoÚpÚR)r   r   r   r/   r<   )r   rI   r#   é   rU   )r   r   r   )r
   r   r   r	   r0   r1   rO   r   r   Zparallel_axisr   rR   rB   r   Z	to_matrixr   r&   )rJ   ÚmZIxZIyZIzr3   rV   ÚIort   ru   rv   ZIpZIp_expectedrI   rU   r   r   r   Útest_parallel_axis9  s<    
  ÿÿÿ
*ÿrz   N)$Zsympyr   r   r   r   r   r   r   Zsympy.physics.vectorr	   r
   r   r   Zsympy.physics.mechanicsr   r   Zsympy.testing.pytestr   r   r   r*   r2   r9   rD   rG   rN   rP   rS   rT   rY   re   rg   rh   rj   rs   rz   r   r   r   r   Ú<module>   s(   $&!

"