a
    t@8bd$                     @   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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izzZixyZiyzZizx r   Flib/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rJ 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   r   r   r   r   test_particle_body/   s"    r)   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   xapply_forcer   lenr   	locatenew_namer   r   r   )	r%   r&   r'   r(   r*   force_vectorr   force_pointr   r   r   r   test_particle_body_add_forceE   s&    
r2   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   r,   r   r0   r#   r   r   <lambda>z       z%test_body_add_force.<locals>.<lambda>c                      s
     dS r6   r7   r   )r#   r   r   r9   {   r:   )r   r   r   r
   r   r   r.   r   r+   r   zr,   r-   r   r   r   r   	TypeError)	r   r   r   r"   r3   r4   r   r1   r   r   r8   r   test_body_add_force]   s6    

r=   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 r6   )apply_torquer   r   r   r   r9      r:   z&test_body_add_torque.<locals>.<lambda>)r   r   r+   r>   r-   r   r   r<   )Ztorque_vectorr   r?   r   test_body_add_torque}   s    
r@   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rB   rC   rD   r   r   r   test_body_masscenter_vel   s    rG   c                  C   sn   t d} td}t d|d}| j||j | ||jks@J || |j ksVJ | ||jksjJ d S rA   )r   r   r   Zset_ang_velyZ
ang_vel_inrF   r   r   r   test_body_ang_vel   s    rI   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 )NrB   rD   
   r   )r   r   r   )r   r   Zorient_axisr;   Zdcmr   r   r   )rB   rD   r   r   r   test_body_dcm   s
    >rK   c                  C   sH   t d} td| d}|j| jks$J |j| jks4J |j| jksDJ d S )NrC   rD   rE   )r   r   r+   rH   r;   )rC   rD   r   r   r   test_body_axis   s
    rL   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PrD   )r   r   r   r+   rH   r,   r   )r*   brM   rD   f1f2r   r   r   #test_apply_force_multiple_one_point   s    

rQ   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B2rC   Zreaction_body)r   r	   r   r   r   r   r   r+   r;   r,   r   r   rH   r   )fgqr+   Zv1Zv2rR   rS   rT   rU   rC   forceZg1Zg2Zforce2r   r   r   test_apply_force   s.    

 
"r[   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trY   rT   rU   rC   )	r   r	   r   r   r+   r>   r   r   rH   )r\   rY   rT   rU   rC   ZtorqueZtorque2r   r   r   test_apply_torque   s    

r]   c                  C   s\   t d} td}td}| |j }||| |j||fgksBJ |  |jg ksXJ d S )Nr*   rM   rD   )r   r   r   r;   r,   r   Zclear_loads)r*   rM   rD   rZ   r   r   r   test_clear_load   s    
r^   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 )NrR   rS   rD   )
r   r   r+   rH   r,   r   Zremove_loadr>   Zcrossr   )rR   rS   rD   rO   rP   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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WrD   rM   rN   zq1 q2zk c g kTzF TrV   N)r   r	   r   r,   r+   Zdiffr   rH   r>   r;   r   r   r   )r`   rD   rM   rN   Zq1Zq2kcrX   ZkTFTr   r   r   9test_apply_loads_on_multi_degree_freedom_holonomic_system   s$    2:8re   N)Zsympy.core.backendr   r   r   r   r   Zsympy.physics.vectorr   r   r	   Zsympy.physics.mechanicsr
   r   Zsympy.testing.pytestr   r   r$   r)   r2   r=   r@   rG   rI   rK   rL   rQ   r[   r]   r^   r_   re   r   r   r   r   <module>   s&    		!
