U
    —9%er  ã                   @   s@   d dl mZmZmZmZ d dlmZmZmZm	Z	m
Z
 dd„ ZdS )é    )ÚcosÚsinÚMatrixÚsymbols)ÚdynamicsymbolsÚReferenceFrameÚPointÚKanesMethodÚParticlec            "      C   s¶  t dƒ\} }t ddd\}}t dƒ\}}tdƒ\}}tdƒ}| dd| |jf¡}	|	 d	d||jf¡}
td
ƒ}| |d¡ | d||	j ¡}| 	|||	¡ | d||
j ¡}| 	|||
¡ t
d||ƒ}t
d||ƒ}tdƒ\}}|| ||  |j }|	|f|
| fg}|| || g}|| |||  g}t|| |g||g|d}| ||g|¡\}}t|| |g||g|d}| ||g|¡\}}t||d  t|ƒ |d  ||  |||   g||d  t|ƒ |d   ||  |||   ggƒ}td| |d  ||d  t|ƒ g||d  t|ƒ ||d  ggƒ}|j ¡ | ¡ ksJt‚|j ¡ | ¡ ksbt‚td|| ||   gƒ}| ¡ | ¡ ks’t‚t|| |||   | | |||   gƒ}| ¡ | ¡ ksÖt‚|| ||  |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}| ¡ | ¡ ks@t‚|| |||   |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}| ¡ | ¡ ks®t‚|| ||d   |j }|	|f|
| fg}t|| |g||g|d}| ||g|¡\}}td|| ||d    gƒ} | ¡ |  ¡ ks:t‚t|| |g||g|d}| ||g|¡\}}t|| ||| d   | | ||| d   gƒ}!| ¡ |! ¡ ks²t‚d S )Nzq1, q2é   )Úlevelzu1, u2zl, mÚNÚAZAxisÚBÚOr   ÚPÚQÚApÚBpzsigma, delta)Zkd_eqsé   é   )r   r   r   Z	orientnewÚzr   Zset_velZ	locatenewÚxZv2pt_theoryr
   r	   Zkanes_equationsr   r   r   Zmass_matrixÚexpandÚAssertionErrorZforcing)"Úq1Úq2Zqd1Zqd2Úu1Úu2ÚlÚmr   r   r   r   r   r   r   r   ÚsigÚdeltaZTaZforcesZkde1Zkde2ZKM1Zfr1Zfstar1ZKM2Zfr2Zfstar2Zforcing_matrix_expectedZmass_matrix_expectedZfr1_expectedZfr2_expectedZfr1_cubic_expectedZfr2_cubic_expected© r#   úg/var/www/html/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/tests/test_kane4.pyÚtest_replace_qdots_in_force   s€    $
ÿ$
ÿþ$ÿÿÿr%   N)Zsympy.core.backendr   r   r   r   Zsympy.physics.mechanicsr   r   r   r	   r
   r%   r#   r#   r#   r$   Ú<module>   s   