o
    g#/                     @   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 d    n1 sw   Y  | jdksJ | jg ks%J td}|| jd | j}| j}||||ksBJ | j	t
dksKJ td\}}}td\}}}	| jt| j||||||	| jfkslJ d S )Nbodybody_masscenterr   	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   r   nameloadsr	   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ixxiyyizzixyiyzizx r#   ^/var/www/visachat/venv/lib/python3.10/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 d    n1 s)w   Y  |j}|j}| |d |	|| 	|ksFJ |
|| 
|ksRJ |j|ksYJ |j|| fksbJ |jsgJ t|dsnJ t|dsuJ t|d	s|J t|d
sJ 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   pos_fromr   is_rigidbodyhasattr)r&   r'   r(   body_inertia
rigid_bodyr   r   r#   r#   r$   test_custom_rigid_body   s*   
r0   c                  C   s  t d} td}td}t  td| ||}W d    n1 s!w   Y  |j}|j}| |d ||| |ks>J |	|| 	|ksJJ |j
|ksQJ t|drXJ t|ds_J t|dsfJ t|d	smJ |jtd|jfksyJ |jtdksJ |jrJ t|d
d
d
|_|jt|d
d
d
ksJ |jsJ t  td|d}W d    n1 sw   Y  |jrJ |jd|j}|t|jdd
d
 }||f|_|j||fksJ |jtdksJ |jsJ d S )Nparticle_masscenterparticle_massparticle_frameparticle_bodyr   _inertiar   r   r   r)   )r   r   )r	   r   r
   r   r   r   r   r   r   r+   r   r-   r   r   central_inertiar,   	locatenewx)r1   r2   r3   r4   r   r   r   point_inertiar#   r#   r$   test_particle_body3   sD   



r:   c            	      C   s   t d} td}td}t  td| ||}W d    n1 s!w   Y  td}||jj }|||j t	|j
dks@J |j|jd d}||jd |j
d d }|j}||||ksgJ ||||kssJ |j
d d |ks~J d S )	Nr1   r2   r3   r4   ar)   _point0r   )r	   r   r
   r   r   r   r8   apply_forcer   lenr   r7   _namer   r   r+   )	r1   r2   r3   r4   r;   force_vectorr   force_pointr   r#   r#   r$   test_particle_body_add_forceY   s*   
rB   c            	         sp  t d} td}td}t|ddd}t  td| |||W d    n1 s)w   Y  td}td}jd	|jj	 }|
jd |jj   | tjdks]J jd d }j}||||kssJ ||||ksJ jd d  ksJ   tjd
ksJ jd d  ksJ tt fdd ttfdd d S )Nr&   r'   r(   r)   r   r*   lFarigidbody_body_point0   c                      s     dS Nr   r=   r#   r@   r/   r#   r$   <lambda>   s    z%test_body_add_force.<locals>.<lambda>c                      
     dS rG   rH   r#   )r/   r#   r$   rJ         
 )r	   r   r
   r   r   r   r   r7   r   r8   r   zr=   r>   r   r   r+   r   	TypeError)	r&   r'   r(   r.   rC   rD   r   rA   r   r#   rI   r$   test_body_add_forcer   s:   

rO   c                     s   t   td W d    n1 sw   Y   jj}  |  t jdks)J  jd  j| fks5J tt fdd d S )Nr   r)   r   c                      rK   rG   )apply_torquer#   r   r#   r$   rJ      rL   z&test_body_add_torque.<locals>.<lambda>)	r   r   r   r8   rP   r>   r   r   rN   )torque_vectorr#   rQ   r$   test_body_add_torque   s   

rS   c                  C   s   t   td} W d    n1 sw   Y  td}t   td|d}W d    n1 s/w   Y  | j||j | ||jksFJ | ||jksPJ d S NANBr   )r   r   r
   r   r   rM   masscenter_velrU   rV   rW   r#   r#   r$   test_body_masscenter_vel   s   
r[   c                  C   s   t   td} W d    n1 sw   Y  td}t   td|d}W d    n1 s/w   Y  | j||j | ||jksFJ || |j ksQJ | ||jks[J d S rT   )r   r   r
   r   set_ang_vely
ang_vel_inrZ   r#   r#   r$   test_body_ang_vel   s   
r_   c                  C   s   t   td} td}W d    n1 sw   Y  | j|j|jjd | |ttdtddgtd tddgg dgksEJ | |jttdtddgtd tddgg dgkseJ d S )NrU   rW   
   r   )r   r   r)   )	r   r   r   orient_axisrM   dcmr   r   r   )rU   rW   r#   r#   r$   test_body_dcm   s   
>Drc   c                  C   sn   t d} t  td| d}W d    n1 sw   Y  |j| jks%J |j| jks-J |j| jks5J d S )NrV   rW   rX   )r
   r   r   r8   r]   rM   )rV   rW   r#   r#   r$   test_body_axis   s   rd   c                  C   s   t d\} }td}t  td}W d    n1 sw   Y  | |j }||j }||| |j||fgks;J ||| |j||| fgksMJ d S )Nza bPrW   )r   r	   r   r   r8   r]   r=   r   )r;   bre   rW   f1f2r#   r#   r$   #test_apply_force_multiple_one_point   s   


ri   c                  C   s  t d\} }td\}}}}td}td}t  td}td}	W d    n1 s,w   Y  td}
||j||j  ||	j||	j  | | |
j	 }|
|||	| |j||fgksbJ |	j|| fgksmJ |j| |
j }|	j| |
j }|
| |	
| |j||f|j|fgksJ |	j|| f|	j|fgksJ ||
j }|j
||	d |j||f|j|| fgksJ |	j|| f|	j| | fgksJ d S )	Nzf gz	q x v1 v2P1P2B1B2rV   reaction_body)r   r   r	   r   r   r
   r   r   r8   rM   r=   r   r   r]   r   )fgqr8   v1v2rj   rk   rl   rm   rV   forceg1g2force2r#   r#   r$   test_apply_force   s2   



 (ry   c                  C   s   t d} td}t  td}td}W d    n1 sw   Y  td}| | |j }||| |j|j|fgks?J |j|j| fgksKJ | |j	 }|| |j|j|| fgksbJ d S )Ntrr   rl   rm   rV   )
r   r   r   r   r
   r8   rP   r   r   r]   )rz   rr   rl   rm   rV   torquetorque2r#   r#   r$   test_apply_torque   s   


r}   c                  C   s   t d} td}t  td}W d    n1 sw   Y  | |j }||| |j||fgks4J |  |jg ks?J d S )Nr;   re   rW   )r   r	   r   r   rM   r=   r   clear_loads)r;   re   rW   ru   r#   r#   r$   test_clear_load  s   

r   c                  C   s   t d} t d}t  td}W d    n1 sw   Y  |j}|j}|||  ||| |j| |f||fgks>J || |j| |fgksMJ ||	| |j| |f|j
|	|fgksfJ |  |j| |fgkstJ d S )Nrj   rk   rW   )r	   r   r   r8   r]   r=   r   remove_loadrP   crossr   )rj   rk   rW   rg   rh   r#   r#   r$   test_remove_load  s    

"r   c                  C   s  t   td} td}td}td}W d   n1 sw   Y  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sJ |j|j|j| | j f|j||	|  | j
 fgksJ |j|j|j| | j f|j|	 | | j
 fgksJ | j| j||  ||  | j fgksJ dS )
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlWrW   re   rf   Nzq1 q2zk c g kTzF Trn   )r   r   r   r   r=   r8   diffr   r]   rP   rM   r   r   r   )r   rW   re   rf   q1q2kcrq   kTFTr#   r#   r$   9test_apply_loads_on_multi_degree_freedom_holonomic_system  s(   
0860r   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	t  td|| ||d}
W d    n1 s>w   Y  |
	|	}t| |||d   |||d   |||d |d    | | | d}||ksrJ t d	}|
| | jd
 t|
	|	|| |tddksJ td}|d|| j || j  }	t  td||| d}W d    n1 sw   Y  |	|	| }t| ||d  ||d  ||d |d   | | | d}|jrJ ||ksJ d S )NrV   zm, I_x, I_y, I_z, a, bopR)r   r   r   r6   rF   )r    rU   r)      re   )r   r   r   )r
   r   r   r	   r7   r8   r]   r   r   parallel_axisra   rM   r   	to_matrixr   r,   )rV   mIxIyIzr;   rf   Ior   r   r   IpIp_expectedrU   re   r#   r#   r$   test_parallel_axis9  s>   
 "*
r   N)$sympyr   r   r   r   r   r   r   sympy.physics.vectorr	   r
   r   r   sympy.physics.mechanicsr   r   sympy.testing.pytestr   r   r%   r0   r:   rB   rO   rS   r[   r_   rc   rd   ri   ry   r}   r   r   r   r   r#   r#   r#   r$   <module>   s*   $ &!

"