U
    9%e+                     @   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 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_matrix)PointReferenceFramedynamicsymbolsDyadic)inertiaBody)raisesc            
   	   C   s   t d} | jdkst| jg ks$ttd}|| jd | j}| j}||||ks^t| j	t
dkspt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   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/Darija-Ai-API/env/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d| |||}|j}|j}| |d ||| |ksft	|
|| 
|ks~t	|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   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d| ||}|j}|j}| |d ||| |ksVt|	|| 	|ksnt|j
|ks|t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sttd|d}|jr6t|jd|j}|t|jdd
d
 }||f|_|j||fksxt|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   central_inertiar%   	locatenewx)r*   r+   r,   r-   r   r   r   Zpoint_inertiar   r   r   test_particle_body1   s<    

r1   c            	      C   s   t d} td}td}td| ||}td}||jj }|||j t|j	dksZt
|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   r0   apply_forcer   lenr   r   r/   _namer   r   r$   )	r*   r+   r,   r-   r2   force_vectorr   force_pointr   r   r   r   test_particle_body_add_forceU   s(     r8   c            	         sN  t d} td}td}t|ddd}td| |||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st  tjd
kstjd d  ks$t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   r0   r   zr3   r4   r   r   r   r$   r   	TypeError)	r   r    r!   r'   r9   r:   r   r7   r   r   r>   r   test_body_add_forcem   s8     

rC   c                     sZ   t d  jj}  |  t jdks,t jd  j| fksDt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   r0   rD   r4   r   r   r   rB   )Ztorque_vectorr   rE   r   test_body_add_torque   s    
rF   c                  C   sX   t d} td}t d|d}| j||j | ||jks@t| ||jksTtd S NANBr   )r   r
   r   r   rA   Zmasscenter_velr   rH   rI   rJ   r   r   r   test_body_masscenter_vel   s    rM   c                  C   sn   t d} td}t d|d}| j||j | ||jks@t|| |j ksVt| ||jksjtd S rG   )r   r
   r   Zset_ang_velyZ
ang_vel_inr   rL   r   r   r   test_body_ang_vel   s    rO   c                  C   s   t d} t d}| j|j|jjd | |ttdtddgtd tddgdddggksft| |jttdtddgtd tddgdddggkstd S )NrH   rJ   
   r   r"   )	r   r   orient_axisrA   Zdcmr   r   r   r   )rH   rJ   r   r   r   test_body_dcm   s
    @rR   c                  C   sH   t d} td| d}|j| jks$t|j| jks4t|j| jksDtd S )NrI   rJ   rK   )r
   r   r0   r   rN   rA   )rI   rJ   r   r   r   test_body_axis   s
    rS   c                  C   sx   t d\} }td}td}| |j }||j }||| |j||fgksPt||| |j||| fgksttd S )Nza bPrJ   )r   r	   r   r0   rN   r3   r   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d}td}	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s&t||
j }|j	||	d |j
||f|j|| fgks`t|	j
|| f|	j| | fgkstd S )	Nzf gz	q x v1 v2P1P2B1B2rI   Zreaction_body)r   r   r	   r   r
   r   r   r0   rA   r3   r   r   r   rN   r   )fgqr0   Zv1Zv2rY   rZ   r[   r\   rI   forceg1g2Zforce2r   r   r   test_apply_force   s.    

 
"rd   c                  C   s   t d} td}td}td}td}| | |j }||| |j|j|fgksXt|j|j| fgkspt| |j	 }|| |j|j|| fgkstd S )Ntr`   r[   r\   rI   )
r   r   r   r
   r0   rD   r   r   r   rN   )re   r`   r[   r\   rI   ZtorqueZtorque2r   r   r   test_apply_torque   s    

rf   c                  C   s\   t d} td}td}| |j }||| |j||fgksBt|  |jg ksXtd S )Nr2   rT   rJ   )r   r	   r   rA   r3   r   r   Zclear_loads)r2   rT   rJ   ra   r   r   r   test_clear_load   s    
rg   c                  C   s   t d} t d}td}|j}|j}|||  ||| |j| |f||fgksVt|| |j| |fgkstt||	| |j| |f|j
|	|fgkst|  |j| |fgkstd S )NrY   rZ   rJ   )r	   r   r0   rN   r3   r   r   Zremove_loadrD   crossr   )rY   rZ   rJ   rV   rW   r   r   r   test_remove_load   s    
"ri   c                  C   s  t d} t d}t d}t d}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sLt|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WrJ   rT   rU   zq1 q2zk c g kTzF Tr]   N)r   r   r   r3   r0   diffr   rN   rD   rA   r   r   r   r   )rj   rJ   rT   rU   q1q2kcr_   ZkTFTr   r   r   9test_apply_loads_on_multi_degree_freedom_holonomic_system  s$    2:8rr   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| ||d}
|
|	}t| |||d   |||d   |||d |d    | | | d}||kst	t d	}|
| | jd
 t|
|	|| |tddkst	td}|d|| j || j  }	td||| d}||	| }t| ||d  ||d  ||d |d   | | | d}|jrt	||kst	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   Zparallel_axisr   rQ   rA   r   Z	to_matrixr   r%   )rI   mZIxZIyZIzr2   rU   Iors   rt   ru   ZIpZIp_expectedrH   rT   r   r   r   test_parallel_axis&  s8    
  *ry   N)#Zsympy.core.backendr   r   r   r   r   r   r   Zsympy.physics.vectorr	   r
   r   r   Zsympy.physics.mechanicsr   r   Zsympy.testing.pytestr   r   r)   r1   r8   rC   rF   rM   rO   rR   rS   rX   rd   rf   rg   ri   rr   ry   r   r   r   r   <module>   s(   $$ 		!
