U
    —9%e+  ã                   @   sN  d dl mZ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mZmZmZmZmZmZmZmZmZ d dlmZmZmZ d dlmZ edƒ\ZZ Z!Z"Z#edƒZ$e$ %dd	ee$j&g¡Z'e' %d
d	e e'j(g¡Z)e) %dd	e!e)j*g¡Z+dd„ Z,dd„ Z-dd„ Z.dd„ Z/dd„ Z0dd„ Z1dd„ Z2dd„ Z3dd„ Z4dd„ Z5d d!„ Z6d"d#„ Z7d$S )%é    )ÚsinÚcosÚtanÚpiÚsymbolsÚMatrixÚSÚFunction)ÚParticleÚPointÚReferenceFrameÚ	RigidBody)Úangular_momentumÚdynamicsymbolsÚinertiaÚinertia_of_point_massÚkinetic_energyÚlinear_momentumÚouterÚpotential_energyÚmsubsÚfind_dynamicsymbolsÚ
Lagrangian)ÚgravityÚcenter_of_massÚ_validate_coordinates)Úraiseszq1 q2 q3 q4 q5ÚNÚAZAxisÚBÚCc                  C   sD  t dƒ} tdƒ\}}}tdƒ\}}}t| |||ƒ|| j| jB  || j| jB   || j| jB   ksft‚t| dddƒd| j| jB  ksˆt‚ttdd„ ƒ t| ||||||ƒ|| j| jB  || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   ks@t‚d S )Nr   zixx iyy izzzixy iyz izxr   c                   S   s   t ddddƒS )Nr   )r   © r!   r!   úk/var/www/html/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_functions.pyÚ<lambda>   ó    ztest_inertia.<locals>.<lambda>)	r   r   r   ÚxÚyÚzÚAssertionErrorr   Ú	TypeError)r   ZixxZiyyZizzZixyZiyzZizxr!   r!   r"   Útest_inertia   s>    
ÿÿ
" ÿÿÿ
ÿÿþþÿþýr*   c            
      C   s  t dƒ\} }}}tdƒ}| |j }t|||ƒ}||| d  |j|jB  || d  |j|jB   ksft‚||j }t|||ƒ}|||d  |j|jB  ||d  |j|jB   ks´t‚||j }t|||ƒ}|||d  |j|jB  ||d  |j|jB   kst‚|| | }	t||	|ƒ}|||d |d   |j|jB  ||  | |j|jB   ||  | |j|jB   ||  | |j|jB   || d |d   |j|jB   || | |j|jB   ||  | |j|jB   || | |j|jB   || d |d   |j|jB   kst‚d S )Nzr s t mr   é   )r   r   r%   r   r&   r'   r(   )
ÚrÚsÚtÚmr   ZpxÚIÚpyZpzÚpr!   r!   r"   Útest_inertia_of_point_mass$   s<    
8
8
: ÿþýüûúùør3   c                     s¼   t dƒ‰tdƒ} |  ˆdˆj ¡ tˆjˆjƒ}td| ˆd|| fƒ‰ tdƒ}td|dƒ‰ˆj ˆd	ˆj ¡ t	t
‡ ‡fd
d„ƒ t	t
‡‡fdd„ƒ tˆˆ ˆƒd	ˆj dˆj  ks¸t‚d S )Nr   ÚAcé   r   é   ÚPÚPaé   é
   c                      s   t ˆ ˆ ˆƒS ©N©r   r!   ©r   r8   r!   r"   r#   J   r$   z&test_linear_momentum.<locals>.<lambda>c                      s   t ˆ ˆ ˆƒS r;   r<   r!   ©r   r8   r!   r"   r#   K   r$   iô  )r   r   Úset_velr&   r   r%   r   r
   Úpointr   r)   r   r(   )r4   r0   r7   r!   ©r   r   r8   r"   Útest_linear_momentumA   s    rB   c            	   	      s†  t dƒ\} }}}tdƒ}tdƒ‰tdƒ}tdƒ‰ˆ d|ˆj ¡}| d|ˆj ¡}ˆ ˆdˆj ¡ | ˆ|ˆj ¡ | 	ˆˆ|¡ | 	ˆˆ|¡ t
d	|| ƒ‰td
||||tˆjˆjƒ |fƒ‰ d|  | | ˆj || | ˆj  }tˆˆ ˆƒ|ksþt‚tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ tt‡‡‡fdd„ƒ |||d   d|  |d   | ˆj }tˆˆˆ ˆƒ|ks‚t‚dS )a   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.z
m, M, l, IÚomegar   ÚaÚOr4   r7   r   r8   r   r+   c                      s   t ˆˆˆ ˆƒS r;   ©r   r!   rA   r!   r"   r#   c   r$   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>c                      s   t ˆˆˆ ˆƒS r;   rF   r!   )r   rE   r8   r!   r"   r#   d   r$   c                      s   t ˆˆ ˆˆƒS r;   rF   r!   )r   rE   r8   r!   r"   r#   e   r$   é   N)r   r   r   r   Ú	locatenewr%   r?   Úset_ang_velr'   Úv2pt_theoryr
   r   r   r&   r   r(   r   r)   r   )	r/   ÚMÚlr0   rC   rD   r4   r7   Úexpectedr!   )r   r   rE   r8   r"   Ú)test_angular_momentum_and_linear_momentumO   s(    "(*rN   c            	         sB  t dƒ\} }}tdƒ}tdƒ‰tdƒ}| ˆdˆj ¡ | d|ˆj ¡}| d|ˆj ¡}tdƒ}| ˆ|ˆj ¡ | 	|ˆ|¡ | 	|ˆ|¡ t
d	|| ƒ‰tˆjˆjƒ}td
|||||fƒ‰ tt‡ ‡fdd„ƒ tt‡ ‡fdd„ƒ dtˆˆˆ ƒ||d  |d  d d|d  |  |d   |d d    ¡ ks>t‚d S )Nzm M l1rC   r   rE   r   r4   r7   rD   r8   r   c                      s   t ˆˆˆ ƒS r;   ©r   r!   r=   r!   r"   r#   y   r$   z%test_kinetic_energy.<locals>.<lambda>c                      s   t ˆˆˆ ƒS r;   rO   r!   )r   r   r!   r"   r#   z   r$   r+   )r   r   r   r   r?   r%   rH   rI   r'   rJ   r
   r   r   r   r)   r   Úexpandr(   )	r/   rK   Úl1rC   rE   r4   r7   rD   r0   r!   rA   r"   Útest_kinetic_energyj   s*    "ÿ
ÿrR   c                  C   s  t dƒ\} }}}}}tdƒ}tdƒ}tdƒ}| |d|j ¡ | d||j ¡}	|	 d||j ¡}
tdƒ}| |||j ¡ |	 	|||¡ |
 	|||¡ t
d	|
| ƒ}t|j|jƒ}td
|	||||	fƒ}| | | |_|| | |_t||ƒ| | | || |  ks
t‚d S )Nzm M l1 g h HrC   r   rE   r   r4   r7   rD   r8   r   )r   r   r   r   r?   r%   rH   rI   r'   rJ   r
   r   r   r   r(   )r/   rK   rQ   ÚgÚhÚHrC   r   rE   r4   r7   rD   r8   r0   r   r!   r!   r"   Útest_potential_energy   s"    rV   c            	         s
  t dƒ\} }}}tdƒ‰tdƒ}| ˆdˆj ¡ | ddˆj ¡}| ˆdˆj ¡ td|dƒ‰| d	d
ˆj ¡}| ˆdˆj ¡ tdƒ}| ˆdˆj	 ¡ t
ˆj	ˆj	ƒ}td||d||fƒ‰ || | ˆ_| | | ˆ _tt‡ ‡fdd„ƒ tt‡‡fdd„ƒ d S )NzM m g hr   rE   r   r7   r9   r:   r8   r4   r+   é   rD   r   r6   c                      s   t ˆ ˆ ˆƒS r;   ©r   r!   r=   r!   r"   r#   £   r$   z!test_Lagrangian.<locals>.<lambda>c                      s   t ˆ ˆ ˆƒS r;   rX   r!   r>   r!   r"   r#   ¤   r$   )r   r   r   r?   r%   rH   r
   r&   rI   r'   r   r   r   r   r)   )	rK   r/   rS   rT   rE   r7   r4   rD   r0   r!   rA   r"   Útest_Lagrangian“   s"    rY   c                  C   sˆ  t dƒ\} }tdƒ\}}}t| | | || ¡  | g| ¡  ¡ |t| ¡ ƒ ggƒ}t| | |g| ¡  ¡ dggƒ}|d|d| ¡ d| ¡ di}t||ƒ|ks¤t‚t|| ƒt|| ƒ || ¡   }|d|t	d | ¡ di}t||dd|d ksút‚t
dƒ}||j ||j  }	||j|jB  ||j|jB   }
d|j }d|j|jB  }|d|di}t|	|ƒ|kspt‚t|
|ƒ|ks„t‚d S )	Núa, búx, y, zr9   r   r+   T)Zsmartr   )r   r   r   Údiffr   r   r(   r   r   r   r   r%   r&   )rD   Úbr%   r&   r'   ÚexprÚsolÚsdr   ÚvÚdZv_solZd_solr!   r!   r"   Ú
test_msubs§   s*    ÿÿ$ 
rc   c                     s(  t dƒ\} }tdƒ\}}}t| | | || ¡  | g| ¡  ¡ |t| ¡ ƒ ggƒ}|| ¡ || ¡  ¡ || ¡ h}t|ƒ|ks†t‚|||g}| ¡ | ¡  ¡ | ¡ h}t||d|ks¾t‚tdƒ\}}	}
tdƒ}||j |	|j	  |
|j
  ‰ ||	|
h}tˆ |d|kst‚tt‡ fdd„ƒ d S )	NrZ   r[   )Úexcludezd, e, fr   )Zreference_framec                      s   t ˆ ƒS r;   )r   r!   ©ra   r!   r"   r#   Ò   r$   z*test_find_dynamicsymbols.<locals>.<lambda>)r   r   r   r\   r   r   r(   r   r%   r&   r'   r   Ú
ValueError)rD   r]   r%   r&   r'   r^   r_   Zexclude_listrb   ÚeÚfr   r!   re   r"   Útest_find_dynamicsymbols¿   s     ÿ 

ri   c                  C   s  t dƒ} tdƒ\}}}tdƒ\}}tdƒ}td||ƒ}t dƒ}tdƒ}	t|j|jƒ}
td|	|||
|	fƒ}||f|	|fg}| t	|| j
 ||ƒ¡ ||f|	|f||| | j
 f|	|| | j
 fg}tt|ƒƒD ]6}tt|| ƒƒD ] }|| | || | ksàt‚qàqÌd S )	Nr   zm M gzF1 F2ÚpoÚpar   r7   r   )r   r   r   r   r
   r   r%   r   Úextendr   r&   ÚrangeÚlenr(   )r   r/   rK   rS   ZF1ZF2rj   rk   r   r7   r0   r   Z	forceListrL   ÚiÚjr!   r!   r"   Útest_gravityÕ   s    0rq   c               
   C   sv  t dƒ} tddd}tdtdƒtjƒ}tdtdƒtd	ƒƒ}td
tdƒtdƒƒ}tdtdƒ|ƒ}t dƒ}tdƒ}tdƒ}td|||t|j|jƒ|fƒ}	|j	 
|j	| j¡ |j	 
|j	| j| j ¡ |j	 
|j	| j¡ |	j 
|j	| j| j ¡ tdƒ}
|
 
|j	t|j	|||||	ƒ¡ d|| d  | j || d || d  | j  ||| d  | j  }|
 |j	¡| dksrt‚d S )NrD   r/   T)ÚrealÚp1Zp1_ptÚp2Zp2_ptr+   Úp3Zp3_pté   Úp4Zp4_ptÚb_fÚb_cmÚmbr]   ÚorW   é   r   )r   r   r
   r   r   ZOner   r   r%   r@   Zset_posr&   Z
masscenterr'   r   Zpos_fromr(   )rD   r/   rs   rt   ru   rw   rx   ry   rz   r]   Zpoint_or^   r!   r!   r"   Útest_center_of_massê   s$    Jr}   c                      s  t dƒ\‰‰‰‰
‰‰tdƒ\‰‰‰tˆˆˆgˆ
ˆˆgƒ tˆˆgƒ tˆˆgˆ
gƒ tˆ
ˆgd tˆˆˆgˆ
ˆˆgdd tt‡‡‡
‡‡fdd„ƒ tˆˆˆgˆ
ˆˆgdd tt‡‡‡‡
‡fdd„ƒ tt‡‡‡‡‡fd	d„ƒ tˆˆ ˆgdd
 tt‡‡‡fdd„ƒ tˆˆˆgdˆ
ˆgdd
 tt‡‡‡‡
‡fdd„ƒ tˆˆ ˆ ˆgdˆ
gdd
 tt‡‡‡‡‡
fdd„ƒ t j‰	tdƒ‰ tdtd\‰‰tˆˆ ƒˆˆ ƒgdd
 tt‡ ‡‡fdd„ƒ tt‡ ‡‡fdd„ƒ ˆ t _tˆˆ ƒˆˆ ƒgƒ tt‡‡‡	fdd„ƒ ˆ	t _d S )Nz	q1:4 u1:4zs1:4©ZspeedsF©Zcheck_duplicatesc                      s   t ˆ ˆˆgˆˆˆgƒS r;   ©r   r!   )Úq1Úq2Úu1Úu2Úu3r!   r"   r#   
  s    ÿz+test_validate_coordinates.<locals>.<lambda>c                      s   t ˆ ˆˆgˆˆˆgddS ©NTr   r€   r!   )r   r‚   Úq3rƒ   r„   r!   r"   r#     s     ÿc                      s   t ˆ ˆˆgˆ ˆˆgddS r†   r€   r!   )r   r‚   r‡   r„   r…   r!   r"   r#     s     ÿ©Zis_dynamicsymbolsc                      s   t ˆ ˆ ˆgƒS r;   r€   r!   )r   r‚   r‡   r!   r"   r#     r$   r   c                      s   t ˆˆ ˆgdˆˆgddS ©Nr   Trˆ   r€   r!   )r   r‚   Ús1rƒ   r„   r!   r"   r#     s     ÿc                      s   t ˆˆ ˆ ˆ gdˆgddS r‰   r€   r!   )r   rŠ   Ús2Ús3rƒ   r!   r"   r#     s     ÿrD   zf1:3)Úclsc                      s   t ˆˆ ƒˆˆ ƒgƒS r;   r€   r!   ©rD   Úf1Úf2r!   r"   r#     r$   c                      s   t ˆˆ ƒˆˆ ƒgdS )Nr~   r€   r!   rŽ   r!   r"   r#      r$   c                      s   t ˆ ˆƒˆˆƒgƒS r;   r€   r!   )r   r   r.   r!   r"   r#   #  r$   )r   r   r   r   rf   Z_tr	   r!   r!   )rD   r   r   r   r‚   r‡   rŠ   r‹   rŒ   r.   rƒ   r„   r…   r"   Útest_validate_coordinatesÿ   s6    r‘   N)8Zsympy.core.backendr   r   r   r   r   r   r   r	   Zsympy.physics.mechanicsr
   r   r   r   r   r   r   r   r   r   r   r   r   r   r   Z!sympy.physics.mechanics.functionsr   r   r   Zsympy.testing.pytestr   r   r‚   r‡   Zq4Zq5r   Z	orientnewr'   r   r%   r   r&   r    r*   r3   rB   rN   rR   rV   rY   rc   ri   rq   r}   r‘   r!   r!   r!   r"   Ú<module>   s*   (4