o
    *ήcd$                     @   s   d dl mZmZmZmZmZ d dl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%S )&    )SymbolsymbolssincosMatrix)PointReferenceFramedynamicsymbols)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s8J t
d\}}}t
d\}}}	| jt| j||||||	| jfksYJ d S )Nbodybody_masscenterr   	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   ixxiyyizzixyiyzizx r   N/tmp/pip-target-vg8gfxp4/lib/python/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s3J |	|| 	|ks?J |j
|ksFJ |j|| fksOJ |jsTJ t|ds[J t|dsbJ t|d	siJ t|d
spJ 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s+J ||| |ks7J |j	|ks>J t
|drEJ t
|dsLJ t
|dsSJ t
|d	sZJ |jr_J 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/   r0   r   r   r   r   r    test_particle_body/   s"   r2   c            	      C   s   t d} td}td}td| ||}td}||jj }|||j t|j	dks-J |j
|jd d}||jd |j	d d }|j}||||ksTJ ||||ks`J |j	d d |kskJ d S )	Nr-   r.   r/   r0   ar%   _point0r   )r   r   r   r   r   xapply_forcer   lenr   	locatenew_namer   r   r'   )	r-   r.   r/   r0   r3   force_vectorr   force_pointr   r   r   r    test_particle_body_add_forceE   s&   
r<   c            	         sJ  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sJJ jd d }j}||||ks`J ||||kslJ jd d  kswJ   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   r6   r   r:   r+   r   r    <lambda>z   s    z%test_body_add_force.<locals>.<lambda>c                      
     dS rA   rB   r   )r+   r   r    rD   {      
 )r   r   r   r
   r   r   r8   r   r5   r   zr6   r7   r   r   r'   r   	TypeError)	r"   r#   r$   r*   r=   r>   r   r;   r   r   rC   r    test_body_add_force]   s6   

rI   c                     sZ   t d  jj}  |  t jdksJ  jd  j| fks"J tt fdd d S )Nr   r%   r   c                      rE   rA   )apply_torquer   r   r   r    rD      rF   z&test_body_add_torque.<locals>.<lambda>)r   r   r5   rJ   r7   r   r   rH   )torque_vectorr   rK   r    test_body_add_torque}   s   
rM   c                  C   sX   t d} td}t d|d}| j||j | ||jks J | ||jks*J d S NANBr   )r   r   r   r   rG   masscenter_velrO   rP   rQ   r   r   r    test_body_masscenter_vel   s   rU   c                  C   sn   t d} td}t d|d}| j||j | ||jks J || |j ks+J | ||jks5J d S rN   )r   r   r   set_ang_vely
ang_vel_inrT   r   r   r    test_body_ang_vel   s   rY   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s2J | |jttdtddgtd tddgg dgksRJ d S )NrO   rQ   
   r   )r   r   r%   )r   r   orient_axisrG   dcmr   r   r   )rO   rQ   r   r   r    test_body_dcm   s
   >Dr]   c                  C   sH   t d} td| d}|j| jksJ |j| jksJ |j| jks"J d S )NrP   rQ   rR   )r   r   r5   rW   rG   )rP   rQ   r   r   r    test_body_axis   s
   r^   c                  C   sx   t d\} }td}td}| |j }||j }||| |j||fgks(J ||| |j||| fgks:J d S )Nza bPrQ   )r   r   r   r5   rW   r6   r   )r3   br_   rQ   f1f2r   r   r    #test_apply_force_multiple_one_point   s   

rc   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sOJ |	j
|| fgksZJ |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B2rP   reaction_body)r   r	   r   r   r   r   r   r5   rG   r6   r   r   rW   r   )fgqr5   v1v2rd   re   rf   rg   rP   forceg1g2force2r   r   r    test_apply_force   s.   


 (rs   c                  C   s   t d} td}td}td}td}| | |j }||| |j|j|fgks,J |j|j| fgks8J | |j }|| |j|j|| fgksOJ d S )Ntrl   rf   rg   rP   )	r   r	   r   r   r5   rJ   r   r   rW   )rt   rl   rf   rg   rP   torquetorque2r   r   r    test_apply_torque   s   

rw   c                  C   s\   t d} td}td}| |j }||| |j||fgks!J |  |jg ks,J d S )Nr3   r_   rQ   )r   r   r   rG   r6   r   clear_loads)r3   r_   rQ   ro   r   r   r    test_clear_load   s   
ry   c                  C   s   t d} t d}td}|j}|j}|||  ||| |j| |f||fgks+J || |j| |fgks:J ||| |j| |f|j	||fgksSJ |  |j| |fgksaJ d S )Nrd   re   rQ   )
r   r   r5   rW   r6   r   remove_loadrJ   crossr   )rd   re   rQ   ra   rb   r   r   r    test_remove_load   s   
"r|   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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WrQ   r_   r`   zq1 q2zk c g kTzF Trh   N)r   r	   r   r6   r5   diffr   rW   rJ   rG   r   r   r   )r}   rQ   r_   r`   q1q2kcrk   kTFTr   r   r    9test_apply_loads_on_multi_degree_freedom_holonomic_system   s$   0860r   N)sympy.core.backendr   r   r   r   r   sympy.physics.vectorr   r   r	   sympy.physics.mechanicsr
   r   sympy.testing.pytestr   r!   r,   r2   r<   rI   rM   rU   rY   r]   r^   rc   rs   rw   ry   r|   r   r   r   r   r    <module>   s(     		!
