U
    L?h#/                     @   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sttd\}}}td\}}}	| jt| j||||||	| jfkst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   Y/var/www/html/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st
|j|kst
|j|| fkst
|jst
t|dst
t|dst
t|d	st
t|d
st
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st	|j|kst	t|drt	t|dst	t|dst	t|d	st	|jtd|jfkst	|jtdkst	|jrt	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st	|jtdkst	|jst	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st||||kst|j
d d |kst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stjd d }j}||||kst||||kst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st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st	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st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st|	j|| fgkst|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st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st	| |j
 }|| |j|j|| fgkst	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st|	|
| |j| |f|j|
|fgkst|  |j| |fgkst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st| j| j||  ||  | j fgkst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st
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rt
||kst
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(   $&!

"