a
    kh#/                     @   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$0    Y  | jdks<J | jg ksJJ td}|| jd | j}| j}||||ksJ | j	t
dksJ td\}}}td\}}}	| jt| j||||||	| jfksJ 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   U/var/www/auris/lib/python3.9/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 sR0    Y  |j}|j}| |d |	|| 	|ksJ |
|| 
|ksJ |j|ksJ |j|| fksJ |jsJ t|dsJ t|dsJ t|d	sJ t|d
sJ 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 sB0    Y  |j}|j}| |d ||| |ks|J |	|| 	|ksJ |j
|ksJ t|drJ t|dsJ t|dsJ t|d	sJ |jtd|jfksJ |jtdksJ |jrJ t|d
d
d
|_|jt|d
d
d
ks<J |jsHJ t  td|d}W d    n1 sr0    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   s@    ",
r1   c            	      C   s   t d} td}td}t  td| ||}W d    n1 sB0    Y  td}||jj }|||j t	|j
dksJ |j|jd d}||jd |j
d d }|j}||||ksJ ||||ksJ |j
d d |ksJ 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            	         sv  t d} td}td}t|ddd}t   td| |||W d    n1 sR0    Y  td}td}jd	|jj	 }|
jd |jj   | tjdksJ jd d }j}||||ksJ ||||ksJ jd d  ksJ   tjd
ks4J jd d  ksLJ 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>       z%test_body_add_force.<locals>.<lambda>c                      s
     dS r<   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   s8    "

rC   c                     s   t   td W d    n1 s$0    Y   jj}  |  t jdksRJ  jd  j| fksjJ tt fdd d S )Nr   r"   r   c                      s
     dS r<   )apply_torquer   r   r   r   r?      r@   z&test_body_add_torque.<locals>.<lambda>)	r   r   r   r0   rD   r4   r   r   rB   )Ztorque_vectorr   rE   r   test_body_add_torque   s    &
rF   c                  C   s   t   td} W d    n1 s$0    Y  td}t   td|d}W d    n1 s^0    Y  | j||j | ||jksJ | ||jksJ d S NANBr   )r   r   r
   r   r   rA   Zmasscenter_velrH   rI   rJ   r   r   r   test_body_masscenter_vel   s    &*rM   c                  C   s   t   td} W d    n1 s$0    Y  td}t   td|d}W d    n1 s^0    Y  | j||j | ||jksJ || |j ksJ | ||jksJ d S rG   )r   r   r
   r   Zset_ang_velyZ
ang_vel_inrL   r   r   r   test_body_ang_vel   s    &*rO   c                  C   s   t    td} td}W d    n1 s,0    Y  | j|j|jjd | |ttdtddgtd tddgg dgksJ | |jttdtddgtd tddgg dgksJ d S )NrH   rJ   
   r   )r   r   r"   )	r   r   r   orient_axisrA   Zdcmr   r   r   )rH   rJ   r   r   r   test_body_dcm   s    &>rR   c                  C   sn   t d} t  td| d}W d    n1 s00    Y  |j| jksJJ |j| jksZJ |j| jksjJ d S )NrI   rJ   rK   )r
   r   r   r0   rN   rA   )rI   rJ   r   r   r   test_body_axis   s    *rS   c                  C   s   t d\} }td}t  td}W d    n1 s80    Y  | |j }||j }||| |j||fgksvJ ||| |j||| fgksJ d S )Nza bPrJ   )r   r	   r   r   r0   rN   r3   r   )r2   brT   rJ   f1f2r   r   r   #test_apply_force_multiple_one_point   s    &

rX   c                  C   s  t d\} }td\}}}}td}td}t   td}td}	W d    n1 sX0    Y  td}
||j||j  ||	j||	j  | | |
j	 }|
|||	| |j||fgksJ |	j|| fgksJ |j| |
j }|	j| |
j }|
| |	
| |j||f|j|fgks,J |	j|| f|	j|fgksLJ ||
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B2rI   Zreaction_body)r   r   r	   r   r   r
   r   r   r0   rA   r3   r   r   rN   r   )fgqr0   Zv1Zv2rY   rZ   r[   r\   rI   forceZg1Zg2Zforce2r   r   r   test_apply_force   s0    &

 
"rb   c                  C   s   t d} td}t   td}td}W d    n1 s<0    Y  td}| | |j }||| |j|j|fgks~J |j|j| fgksJ | |j	 }|| |j|j|| fgksJ d S )Ntr`   r[   r\   rI   )
r   r   r   r   r
   r0   rD   r   r   rN   )rc   r`   r[   r\   rI   ZtorqueZtorque2r   r   r   test_apply_torque   s    &

rd   c                  C   s   t d} td}t  td}W d    n1 s40    Y  | |j }||| |j||fgkshJ |  |jg ks~J d S )Nr2   rT   rJ   )r   r	   r   r   rA   r3   r   Zclear_loads)r2   rT   rJ   ra   r   r   r   test_clear_load  s    &
re   c                  C   s   t d} t d}t  td}W d    n1 s40    Y  |j}|j}|||  ||| |j| |f||fgks|J || |j| |fgksJ ||	| |j| |f|j
|	|fgksJ |  |j| |fgksJ d S )NrY   rZ   rJ   )r	   r   r   r0   rN   r3   r   Zremove_loadrD   Zcrossr   )rY   rZ   rJ   rV   rW   r   r   r   test_remove_load  s    &
"rf   c                  C   s  t  0 td} td}td}td}W d   n1 s<0    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s8J |j|j|j| | j f|j||	|  | j
 fgksrJ |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WrJ   rT   rU   Nzq1 q2zk c g kTzF Tr]   )r   r   r   r   r3   r0   diffr   rN   rD   rA   r   r   r   )rg   rJ   rT   rU   Zq1Zq2kcr_   ZkTFTr   r   r   9test_apply_loads_on_multi_degree_freedom_holonomic_system  s&    &2:8rm   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	t " td|| ||d}
W d    n1 s|0    Y  |
	|	}t| |||d   |||d   |||d |d    | | | d}||ksJ t d	}|
| | jd
 t|
	|	|| |tddks&J td}|d|| j || j  }	t   td||| d}W d    n1 sx0    Y  |	|	| }t| ||d  ||d  ||d |d   | | | d}|jrJ ||ksJ d S )NrI   zm, I_x, I_y, I_z, a, bopR)r   r   r   r.   r;   )r   rH   r"      rT   )r   r   r   )r
   r   r   r	   r/   r0   rN   r   r   Zparallel_axisrQ   rA   r   Z	to_matrixr   r%   )rI   mZIxZIyZIzr2   rU   ZIorn   ro   rp   ZIpZIp_expectedrH   rT   r   r   r   test_parallel_axis9  s:    0
 "
0*rs   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   rC   rF   rM   rO   rR   rS   rX   rb   rd   re   rf   rm   rs   r   r   r   r   <module>   s(   $&!

"