o
    GZh#/                     @   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bodyZbody_masscenterr   Z	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   ZixxZiyyZizzixyZiyzZizx r   V/var/www/auris/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*   
r)   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   Z_inertiar   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   sD   



r1   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 )	Nr*   r+   r,   r-   ar"   Z_point0r   )r	   r   r
   r   r   r   r0   apply_forcer   lenr   r/   _namer   r   r$   )	r*   r+   r,   r-   r2   force_vectorr   force_pointr   r   r   r   test_particle_body_add_forceY   s*   
r8   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FaZrigidbody_body_point0   c                      s     dS Nr   r3   r   r6   r(   r   r   <lambda>   s    z%test_body_add_force.<locals>.<lambda>c                      
     dS r<   r=   r   )r(   r   r   r?         
 )r	   r   r
   r   r   r   r   r/   r   r0   r   zr3   r4   r   r   r$   r   	TypeError)	r   r    r!   r'   r9   r:   r   r7   r   r   r>   r   test_body_add_forcer   s:   

rD   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                      r@   r<   )apply_torquer   r   r   r   r?      rA   z&test_body_add_torque.<locals>.<lambda>)	r   r   r   r0   rE   r4   r   r   rC   )Ztorque_vectorr   rF   r   test_body_add_torque   s   

rG   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   rB   Zmasscenter_velrI   rJ   rK   r   r   r   test_body_masscenter_vel   s   
rN   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 rH   )r   r   r
   r   Zset_ang_velyZ
ang_vel_inrM   r   r   r   test_body_ang_vel   s   
rP   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 )NrI   rK   
   r   )r   r   r"   )	r   r   r   orient_axisrB   Zdcmr   r   r   )rI   rK   r   r   r   test_body_dcm   s   
>DrS   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 )NrJ   rK   rL   )r
   r   r   r0   rO   rB   )rJ   rK   r   r   r   test_body_axis   s   rT   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PrK   )r   r	   r   r   r0   rO   r3   r   )r2   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 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B2rJ   Zreaction_body)r   r   r	   r   r   r
   r   r   r0   rB   r3   r   r   rO   r   )fgqr0   v1v2rZ   r[   r\   r]   rJ   forceg1g2Zforce2r   r   r   test_apply_force   s2   



 (rg   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tra   r\   r]   rJ   )
r   r   r   r   r
   r0   rE   r   r   rO   )rh   ra   r\   r]   rJ   ZtorqueZtorque2r   r   r   test_apply_torque   s   


ri   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 )Nr2   rU   rK   )r   r	   r   r   rB   r3   r   Zclear_loads)r2   rU   rK   rd   r   r   r   test_clear_load  s   

rj   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 )NrZ   r[   rK   )r	   r   r   r0   rO   r3   r   Zremove_loadrE   crossr   )rZ   r[   rK   rW   rX   r   r   r   test_remove_load  s    

"rl   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WrK   rU   rV   Nzq1 q2zk c g kTzF Tr^   )r   r   r   r   r3   r0   diffr   rO   rE   rB   r   r   r   )rm   rK   rU   rV   q1q2kcr`   ZkTFTr   r   r   9test_apply_loads_on_multi_degree_freedom_holonomic_system  s(   
0860ru   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 )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	   r/   r0   rO   r   r   Zparallel_axisrR   rB   r   Z	to_matrixr   r%   )rJ   mZIxZIyZIzr2   rV   Iorv   rw   rx   ZIpZIp_expectedrI   rU   r   r   r   test_parallel_axis9  s>   
 "*
r|   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)   r1   r8   rD   rG   rN   rP   rS   rT   rY   rg   ri   rj   rl   ru   r|   r   r   r   r   <module>   s*   $ &!

"