a
    {d+                     @   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J | jg ks$J td}|| jd | j}| j}||||ks^J | jt	dkspJ 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   nameloadsr	   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ZixxZiyyZizzixyZiyzZizx r   p/var/www/html/stable-diffusion-webui/venv/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d| |||}|j}|j}| |d ||| |ksfJ |	|| 	|ks~J |j
|ksJ |j|| fksJ |jsJ t|dsJ t|dsJ t|d	sJ 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   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sVJ ||| |ksnJ |j	|ks|J t
|drJ t
|dsJ t
|dsJ t
|d	sJ |jtd|jfksJ |jtdksJ |jrJ t|d
d
d
|_|jt|d
d
d
ksJ |jsJ td|d}|jr6J |jd|j}|t|jdd
d
 }||f|_|j||fksxJ |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   central_inertiar$   	locatenewx)r)   r*   r+   r,   r   r   r   Zpoint_inertiar   r   r   test_particle_body1   s<    

r0   c            	      C   s   t d} td}td}td| ||}td}||jj }|||j t|j	dksZJ |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/   apply_forcer   lenr   r.   _namer   r   r#   )	r)   r*   r+   r,   r1   force_vectorr   force_pointr   r   r   r   test_particle_body_add_forceU   s&    
r7   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sJ jd d }j}||||ksJ ||||ksJ jd d  ksJ   tjd
ksJ jd d  ks$J 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   r2   r   r5   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/   r   zr2   r3   r   r   r#   r   	TypeError)	r   r   r    r&   r8   r9   r   r6   r   r   r=   r   test_body_add_forcem   s6    

rB   c                     sZ   t d  jj}  |  t jdks,J  jd  j| fksDJ 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/   rC   r3   r   r   rA   )Ztorque_vectorr   rD   r   test_body_add_torque   s    
rE   c                  C   sX   t d} td}t d|d}| j||j | ||jks@J | ||jksTJ d S NANBr   )r   r
   r   r   r@   Zmasscenter_velrG   rH   rI   r   r   r   test_body_masscenter_vel   s    rL   c                  C   sn   t d} td}t d|d}| j||j | ||jks@J || |j ksVJ | ||jksjJ d S rF   )r   r
   r   Zset_ang_velyZ
ang_vel_inrK   r   r   r   test_body_ang_vel   s    rN   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g dgksdJ | |jttdtddgtd tddgg dgksJ d S )NrG   rI   
   r   )r   r   r!   )r   r   orient_axisr@   Zdcmr   r   r   )rG   rI   r   r   r   test_body_dcm   s
    >rQ   c                  C   sH   t d} td| d}|j| jks$J |j| jks4J |j| jksDJ d S )NrH   rI   rJ   )r
   r   r/   rM   r@   )rH   rI   r   r   r   test_body_axis   s
    rR   c                  C   sx   t d\} }td}td}| |j }||j }||| |j||fgksPJ ||| |j||| fgkstJ d S )Nza bPrI   )r   r	   r   r/   rM   r2   r   )r1   brS   rI   f1f2r   r   r   #test_apply_force_multiple_one_point   s    

rW   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sJ |	j
|| fgksJ |j| |
j }|	j| |
j }|	| |		| |j
||f|j|fgksJ |	j
|| f|	j|fgks&J ||
j }|j	||	d |j
||f|j|| fgks`J |	j
|| f|	j| | fgksJ d S )	Nzf gz	q x v1 v2P1P2B1B2rH   Zreaction_body)r   r   r	   r   r
   r   r   r/   r@   r2   r   r   rM   r   )fgqr/   Zv1Zv2rX   rY   rZ   r[   rH   forceZg1Zg2Zforce2r   r   r   test_apply_force   s.    

 
"ra   c                  C   s   t d} td}td}td}td}| | |j }||| |j|j|fgksXJ |j|j| fgkspJ | |j }|| |j|j|| fgksJ d S )Ntr_   rZ   r[   rH   )	r   r   r   r
   r/   rC   r   r   rM   )rb   r_   rZ   r[   rH   ZtorqueZtorque2r   r   r   test_apply_torque   s    

rc   c                  C   s\   t d} td}td}| |j }||| |j||fgksBJ |  |jg ksXJ d S )Nr1   rS   rI   )r   r	   r   r@   r2   r   Zclear_loads)r1   rS   rI   r`   r   r   r   test_clear_load   s    
rd   c                  C   s   t d} t d}td}|j}|j}|||  ||| |j| |f||fgksVJ || |j| |fgkstJ ||| |j| |f|j	||fgksJ |  |j| |fgksJ d S )NrX   rY   rI   )
r	   r   r/   rM   r2   r   Zremove_loadrC   Zcrossr   )rX   rY   rI   rU   rV   r   r   r   test_remove_load   s    
"re   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J |j
|j|j| | j f|j||	|  | j	 fgksLJ |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WrI   rS   rT   zq1 q2zk c g kTzF Tr\   N)r   r   r   r2   r/   diffr   rM   rC   r@   r   r   r   )rf   rI   rS   rT   Zq1Zq2kcr^   ZkTFTr   r   r   9test_apply_loads_on_multi_degree_freedom_holonomic_system  s$    2:8rl   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| ||d}
|
|	}t| |||d   |||d   |||d |d    | | | d}||ksJ t d	}|	| | j
d
 t|
|	|| |tddksJ td}|d|| j || j  }	td||| d}||	| }t| ||d  ||d  ||d |d   | | | d}|jrJ ||ksJ d S )NrH   zm, I_x, I_y, I_z, a, bopR)r   r   r   r-   r:   )r   rG   r!      rS   )r   r   r   )r
   r   r   r	   r.   r/   rM   r   Zparallel_axisrP   r@   r   Z	to_matrixr   r$   )rH   mZIxZIyZIzr1   rT   ZIorm   rn   ro   ZIpZIp_expectedrG   rS   r   r   r   test_parallel_axis&  s6    
 "*rr   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(   r0   r7   rB   rE   rL   rN   rQ   rR   rW   ra   rc   rd   re   rl   rr   r   r   r   r   <module>   s(   $$ 		!
