Ë
    7^(hU0  ã                   ó´   — d dl mZmZmZ d dlmZmZ d dlZd„ Zd„ Z	d„ Z
d„ Zd„ Zd	„ Zd
„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zd„ Zy)é    )ÚdynamicsymbolsÚPointÚReferenceFrame)ÚraisesÚignore_warningsNc                  óø  — t        d«      \  } }t        dd«      \  }}t        dd«      \  }}t        d«      }t        d«      }|j                  |||j                  z  «       t	        d«      }|j                  d|j                  «      }	|	j                  |d«       |j                  |d«       |	j                  |||«      ||j                  z  k(  sJ ‚|j                  ||j                  «       |	j                  |||«      |j                  ||j                  z  z   k(  sJ ‚|	j                  ||j                  «       |	j                  |||«      |j                  |j                  z   ||j                  z  z   k(  sJ ‚y ©	Núq q2é   é   ÚNÚBÚOÚPr   )
r   r   Úset_ang_velÚzr   Ú	locatenewÚxÚset_velÚv1pt_theoryÚy©
ÚqÚq2ÚqdÚq2dÚqddÚq2ddr   r   r   r   s
             úc/var/www/skyplay_api_hub/venv/lib/python3.12/site-packages/sympy/physics/vector/tests/test_point.pyÚtest_point_v1pt_theorysr       s7  € Ü˜6Ó"�E€A€rÜ˜V QÓ'�G€BˆÜ˜v qÓ)�I€CˆÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�R˜!Ÿ#™#‘XÔÜˆc‹
€AØ	�‰�C˜Ÿ™Ó€AØ‡I�Iˆa�„OØ‡I�Iˆa�„OØ�=‰=˜˜A˜qÓ! R¨!¯#©#¡XÒ-Ð-Ð-Ø‡I�Iˆa�—‘ÔØ�=‰=˜˜A˜qÓ! Q§S¡S¨2°·±©8¡^Ò3Ð3Ð3Ø‡I�Iˆa�—‘ÔØ�=‰=˜˜A˜qÓ! Q§S¡S¨1¯3©3¡Y°°a·c±c±Ñ%9Ò9Ð9Ñ9ó    c                  ó~  — t        d«      \  } }t        dd«      \  }}t        dd«      \  }}t        d«      }t        d«      }|j                  |||j                  z  «       t	        d«      }|j                  d|j                  «      }	|	j                  |d«       |j                  |d«       |	j                  |||«      |dz   |j                  z  ||j                  z  z   k(  sJ ‚|	j                  |||j                  z  «       |	j                  |||«      |dz   |j                  z  ||j                  z  z   ||j                  z  z   k(  sJ ‚|j                  |||j                  z  «       |	j                  |||«      ||dz  z
  |j                  z  ||z  |z   |j                  z  z   ||j                  z  z   k(  sJ ‚y r	   )
r   r   r   r   r   r   r   r   Úa1pt_theoryr   r   s
             r   Útest_point_a1pt_theorysr$      s‘  € Ü˜6Ó"�E€A€rÜ˜V QÓ'�G€BˆÜ˜v qÓ)�I€CˆÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�R˜!Ÿ#™#‘XÔÜˆc‹
€AØ	�‰�C˜Ÿ™Ó€AØ‡I�Iˆa�„OØ‡I�Iˆa�„OØ�=‰=˜˜A˜qÓ! r¨1¡u X°·±¡^°c¸A¿C¹C±iÑ%?Ò?Ð?Ð?Ø‡I�Iˆa��q—s‘s‘ÔØ�=‰=˜˜A˜qÓ! r¨1¡u X°·±¡^°c¸A¿C¹C±iÑ%?À$ÈÏÉÁ*Ñ%LÒLÐLÐLØ‡I�Iˆa��q—s‘s‘ÔØ�=‰=˜˜A˜qÓ! t¨b°!©e¡|°q·s±sÑ&:¸cÀB¹hÈ¹nÐPQ×PSÑPSÑ=SÑ&SØ# a§c¡c™zñ'*ò +ð +ñ +r!   c                  óL  — t        d«      } t        dd«      }t        d«      }|j                  dd| |j                  g«      }t	        d«      }|j                  dd«      }|j                  |d«       |j                  |||«      dk(  sJ ‚|j                  d|j                  «      }|j                  |||«      ||j                  z  |j                  z  k(  sJ ‚|j                  ||j                  «       |j                  |||«      |j                  ||j                  z  z   k(  sJ ‚y )	Nr   r   r   r   ÚAxisr   r   r   )
r   r   Ú	orientnewr   r   r   r   Úv2pt_theoryr   r   )r   r   r   r   r   r   s         r   Útest_point_v2pt_theorysr)   *   sù   € Ü�sÓ€AÜ	˜˜QÓ	€BÜ�sÓ€AØ	�‰�C˜ ! Q§S¡S Ó*€AÜˆc‹
€AØ	�‰�C˜Ó€AØ‡I�Iˆa�„OØ�=‰=˜˜A˜qÓ! QÒ&Ð&Ð&Ø	�‰�C˜Ÿ™Ó€AØ�=‰=˜˜A˜qÓ! b¨1¯3©3¡h°·±¡nÒ5Ð5Ð5Ø‡I�Iˆa�—‘ÔØ�=‰=˜˜A˜qÓ! Q§S¡S¨2°·±©8¡^Ò3Ð3Ñ3r!   c                  óÖ  — t        d«      } t        dd«      }t        dd«      }t        d«      }|j                  dd| |j                  g«      }t	        d«      }|j                  dd	«      }|j                  |d	«       |j                  |||«      d	k(  sJ ‚|j                  ||j                  «       |j                  |||«      |dz   |j                  z  ||j                  z  z   k(  sJ ‚y )
Nr   r   r   r   r   r&   r   r   r   )r   r   r'   r   r   r   r   Úa2pt_theoryÚset_posr   r   )r   r   r   r   r   r   r   s          r   Útest_point_a2pt_theorysr-   9   sÑ   € Ü�sÓ€AÜ	˜˜QÓ	€BÜ
˜˜aÓ
 €CÜ�sÓ€AØ	�‰�C˜ ! Q§S¡S Ó*€AÜˆc‹
€AØ	�‰�C˜Ó€AØ‡I�Iˆa�„OØ�=‰=˜˜A˜qÓ! QÒ&Ð&Ð&Ø‡I�Iˆa�—‘ÔØ�=‰=˜˜A˜qÓ! r¨1¡u f°·±¡^°s¸a¿c¹c±kÑ%AÒAÐAÑAr!   c                  ó¦  — t        d«      \  } }t        dd«      \  }}t        dd«      \  }}t        d«      }t        d«      }|j                  |d|j                  z  «       t	        d«      }|j                  d| |j                  z  ||j                  z  z   «      }	|	j                  |«      | |j                  z  ||j                  z  z   k(  sJ ‚|	j                  |||j                  z  ||j                  z  z   «       |	j                  |«      ||j                  z  ||j                  z  z   k(  sJ ‚|j                  |d	«       |j                  |«      d	k(  sJ ‚|	j                  |||«      d
| z  |z   |j                  z  ||j                  z  z   d|z  |j                  z  z   k(  sJ ‚|j                  dd| |j                  g«      }t	        d«      }|j                  dd|j                  z  «      }	|j                  |d|j                  z  «       |j                  |«      d|j                  z  k(  sJ ‚|	j                  |||«      d|dz  z  |j                  z  d|z  |j                  z  z   k(  sJ ‚|j                  |d|j                  z  «       t	        d«      }|j                  d| |j                  z  ||j                  z  z   «      }	|	j                  |||j                  z  ||j                  z  z   «       |j                  |d	«       |	j                  |||«      ||j                  z  ||j                  z  z   d| z  |j                  z  z
  k(  sJ ‚y )Nr
   r   r   r   r   é   r   r   r   içÿÿÿéöÿÿÿr&   é
   )r   r   r   r   r   r   r   Úpos_fromr   Úvelr#   r   r'   r+   r   r   s
             r   Útest_point_funcsr4   G   sÚ  € Ü˜6Ó"�E€A€rÜ˜V QÓ'�G€BˆÜ˜v qÓ)�I€CˆÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�Q˜Ÿ™‘WÔÜˆc‹
€AØ	�‰�C˜˜QŸS™S™ 2¨¯©¡8Ñ+Ó,€AØ�:‰:�a‹=˜A §¡™G b¨1¯3©3¡hÑ.Ò.Ð.Ð.Ø‡I�Iˆa��a—c‘c‘˜C !§#¡#™IÑ%Ô&Ø�5‰5�‹8�r˜AŸC™C‘x #¨¯©¡)Ñ+Ò+Ð+Ð+Ø‡I�Iˆa�„OØ�5‰5�‹8�qŠ=Ðˆ=Ø�=‰=˜˜A˜qÓ! s¨Q¡w°¡}¸¿¹Ñ&;¸tÀqÇsÁs¹lÑ&JØ # b¡¨A¯C©CÑ/ñ'0ò 1ð 1ð 1ð 	
�‰�C˜ ! Q§S¡S Ó*€AÜˆc‹
€AØ	�‰�C˜˜aŸc™c™Ó"€AØ‡I�Iˆa��Q—S‘S‘ÔØ�5‰5�‹8�q˜1Ÿ3™3‘wÒÐÐØ�=‰=˜˜A˜qÓ! c¨B°©E¡k°Q·S±SÑ%8¸BÀ¹HÈÏÉÑ;KÑ%KÒKÐKÐKà‡M�M�!�Q˜Ÿ™‘WÔÜˆc‹
€AØ	�‰�C˜˜QŸS™S™ 2¨¯©¡8Ñ+Ó,€AØ‡I�Iˆa��a—c‘c‘˜C !§#¡#™IÑ%Ô&Ø‡I�Iˆa�„OØ�=‰=˜˜A˜qÓ! R¨!¯#©#¡X°°a·c±c±	Ñ%9¸AÀ¹EÀAÇCÁC¹KÑ%GÒGÐGÑGr!   c                  óX  — t        d«      } t        d«      }|j                  dd| |j                  g«      }t	        d«      }|j                  dd|j                  z  d|j                  z  z   «      }|j                  |«      d|j                  z  d|j                  z  z   k(  sJ ‚|j                  d	d|j                  z  d|j                  z  z   «      }|j                  |«      d|j                  z  d|j                  z  z   k(  sJ ‚|j                  |«      d|j                  z  d|j                  z  z   d|j                  z  z   d|j                  z  z   k(  sJ ‚|j                  |«      d
|j                  z  d|j                  z  z
  d|j                  z  z
  d|j                  z  z
  k(  sJ ‚y )Nr   r   r   r&   r   r   r1   r/   ÚQr0   )	r   r   r'   r   r   r   r   r2   r   )r   r   r   r   r   r6   s         r   Útest_point_posr7   g   sc  € Ü�sÓ€AÜ�sÓ€AØ	�‰�C˜ ! Q§S¡S Ó*€AÜˆc‹
€AØ	�‰�C˜˜aŸc™c™ A¨¯©¡GÑ+Ó,€AØ�:‰:�a‹=˜B §¡™H q¨1¯3©3¡wÑ.Ò.Ð.Ð.Ø	�‰�C˜˜aŸc™c™ A¨¯©¡GÑ+Ó,€AØ�:‰:�a‹=˜B §¡™H q¨1¯3©3¡wÑ.Ò.Ð.Ð.Ø�:‰:�a‹=˜B §¡™H r¨A¯C©C¡xÑ/°!°a·c±c±'Ñ9¸AÀÇÁ¹GÑCÒCÐCÐCØ�:‰:�a‹=˜C !§#¡#™I¨¨Q¯S©S©Ñ0°1°q·s±s±7Ñ:¸QÀÇÁ¹WÑDÒDÐDÑDr!   c                  óŽ  ‡‡‡— t        d«      } t        d«      Št        d«      Št        d«      \  Š}‰j                  | ‰‰j                  z  || j
                  z  z   «       ‰j                  | ‰«      ‰j                  k(  sJ ‚‰j                  | ‰|«      ‰j                  | j
                  fk(  sJ ‚t        t        ˆˆˆfd„«       y )Nr   ÚAÚpzu1, u2c                  ó(   •— ‰j                  ‰ ‰«      S ©N)Úpartial_velocity)r9   r:   Úu1s   €€€r   ú<lambda>z-test_point_partial_velocity.<locals>.<lambda>€   s   ø€ ˜q×1Ñ1°!°RÓ8€ r!   )	r   r   r   r   r   r   r=   r   Ú
ValueError)r   Úu2r9   r:   r>   s     @@@r   Útest_point_partial_velocityrB   s   s¤   ú€ ä�sÓ€AÜ�sÓ€Aäˆc‹
€Aä˜HÓ%�F€Bˆà‡I�Iˆa��a—c‘c‘˜B §¡™HÑ$Ô%à×Ñ˜a Ó$¨¯©Ò+Ð+Ð+Ø×Ñ˜a  RÓ(¨Q¯S©S°!·#±#¨JÒ6Ð6Ð6Ü
Œ:Õ8Õ9r!   c                  ó’  ‡‡‡‡— t        d«      \  } }t        d«      Št        d«      Št        d«      Št        d«      Š‰j                  ‰| ‰j                  z  «       t        t        ˆˆfd„«       ‰j                  ‰|‰j                  z  «       ‰j                  ‰«      |‰j                  z  k(  sJ ‚t        t        ˆˆfd„«       y )Núq1 q2r   r   r6   r   c                  ó&   •— ‰j                  ‰ «      S r<   ©r3   )r   r6   s   €€r   r?   z test_point_vel.<locals>.<lambda>‰   s   ø€  §¡ a£€ r!   c                  ó&   •— ‰j                  ‰ «      S r<   rF   )r   r   s   €€r   r?   z test_point_vel.<locals>.<lambda>Œ   s   ø€  §¡ q£€ r!   )
r   r   r   r,   r   r   r@   r   r   r3   )Úq1r   r   r   r   r6   s     @@@@r   Útest_point_velrI   ‚   s—   û€ Ü˜GÓ$�F€BˆÜ�sÓ€AÜ�sÓ€AÜˆc‹
€AÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔÜ
Œ:Ô(Ô)Ø‡I�Iˆa��a—c‘c‘ÔØ�5‰5�‹8�r˜AŸC™C‘xÒÐÐÜ
Œ:Ô)Õ*r!   c                  ó–  ‡‡‡— t         j                  } t        d«      \  }}t        d«      Št        d«      Št        d«      }t        d«      }|j	                  ||‰j
                  z  «       |j                  ‰|‰j                  z  «       |j                  ‰«      |j                  | «      ‰j
                  z  |‰j                  z  z   k(  sJ ‚t        d«      }|j	                  ||‰j
                  z  «       t        d«      Š‰j	                  ||‰j                  z  «       t        t        ˆˆfd„«       t        t        ˆˆfd	„«       y )
NrD   r   r   r   r6   ÚP1ÚP2c                  ó&   •— ‰j                  ‰ «      S r<   rF   )r   rL   s   €€r   r?   z%test_auto_point_vel.<locals>.<lambda>œ   ó   ø€  §¡ q£	€ r!   c                  ó&   •— ‰j                  ‰ «      S r<   rF   ©r   rL   s   €€r   r?   z%test_auto_point_vel.<locals>.<lambda>ž   s   ø€ ˜rŸv™v a›y€ r!   )r   Ú_tr   r   r,   r   r   r   r3   Údiffr   r   r@   )	ÚtrH   r   r   r6   rK   r   r   rL   s	         @@@r   Útest_auto_point_velrT   Ž   sö   ú€ Ü×Ñ€AÜ˜GÓ$�F€BˆÜ�sÓ€AÜ�sÓ€AÜˆc‹
€AÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔØ‡I�Iˆa��a—c‘c‘ÔØ�5‰5�‹8�r—w‘w˜q“z A§C¡CÑ'¨"¨q¯s©s©(Ñ2Ò2Ð2Ð2Ü	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔÜ	ˆt‹€BØ‡J�Jˆr�2˜Ÿ™‘8ÔÜ
Œ:Ô)Ô*ä
Œ:Ô(Õ)r!   c                  ó˜  — t         j                  } t        d«      \  }}t        d«      }t        d«      }|j	                  |||j
                  z  «       t        d«      }|j                  |||j                  z  «       |j	                  |||j                  z  «       t        d«      }|j                  |||j                  z  «       t        d«      }|j                  |d|z  |j                  z  «       |j                  |«      d|j                  | «      z  |j                  z  ||j                  | «      z   |j                  z  z   k(  sJ ‚y )NrD   r   r   rK   rL   ÚP3r1   )r   rQ   r   r   r   r   r,   r   r   r3   rR   )rS   rH   r   r   r   rK   rL   rV   s           r   Ú'test_auto_point_vel_multiple_point_pathrW       s   € Ü×Ñ€AÜ˜GÓ$�F€BˆÜ�sÓ€AÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ‡J�Jˆq�"�q—s‘s‘(ÔÜ	ˆt‹€BØ‡J�Jˆr�2˜Ÿ™‘8ÔÜ	ˆt‹€BØ‡J�Jˆr�2˜‘7˜QŸS™S‘=Ô!Ø�6‰6�!‹9˜˜RŸW™W Q›Z™¨!¯#©#Ñ-°°b·g±g¸a³j±ÀAÇCÁCÑ0GÑGÒGÐGÑGr!   c                  óN  — t         j                  } t        d«      \  }}}t        d«      }t        d«      }|j	                  |||j
                  z  «       t        d«      }|j                  |||j                  z  «       |j                  |«      |j                  | «      |j                  z  ||j
                  z  z   k(  sJ ‚|j                  |«      ||j
                  z  k(  sJ ‚|j	                  |||j                  z  «       |j                  |«      ||j                  z  k(  sJ ‚y )Nz
q1, q2, u1r   rK   )r   rQ   r   r   r   r   r,   r   r3   rR   r   )rS   rH   r   r>   r   r   rK   s          r   Útest_auto_vel_dont_overwriterY   ¯   së   € Ü×Ñ€AÜ Ó-�J€BˆˆBÜ�sÓ€AÜˆd‹€AØ‡I�Iˆa��a—c‘c‘ÔÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ�6‰6�!‹9˜Ÿ™ ›
 Q§S¡SÑ(¨2°·±©8Ñ3Ò3Ð3Ð3Ø�5‰5�‹8�r˜AŸC™C‘xÒÐÐØ‡J�Jˆq�"�q—s‘s‘(ÔØ�6‰6�!‹9˜˜QŸS™S™Ò Ð Ñ r!   c                  óD  ‡‡‡— t        d«      \  } }t        d«      Št        d«      Št        d«      }|j                  ‰| ‰j                  z  «       t        d«      Š‰j                  |‰j                  «       t        t        ˆˆfd„«       t        t        ˆˆfd„«       y )NrD   r   ÚSr   rK   c                  ó&   •— ‰j                  ‰ «      S r<   rF   )r   rK   s   €€r   r?   zRtest_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector.<locals>.<lambda>Ä   rN   r!   c                  ó&   •— ‰ j                  ‰«      S r<   rF   )rK   r[   s   €€r   r?   zRtest_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector.<locals>.<lambda>Å   rN   r!   )	r   r   r   r   r   r,   r   r   r@   )rH   r   r   r   rK   r[   s      @@@r   Ú@test_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vectorr^   ¼   su   ú€ Ü˜GÓ$�F€BˆÜ�sÓ€AÜ�sÓ€AÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔÜ	ˆt‹€BØ‡J�Jˆq�!—#‘#ÔÜ
Œ:Ô)Ô*Ü
Œ:Ô)Õ*r!   c                  óÖ  — t         j                  } t        d«      \  }}}}t        d«      }t        d«      }|j	                  |||j
                  z  «       t        d«      }|j                  |||j                  z  «       |j	                  |||j                  z  «       t        d«      }|j                  |||j                  z  «       t        d«      }	|	j                  |d|z  |j                  z  «       t        d«      }
|
j                  |	||j
                  z  «       t        d	«      }|j	                  |||j                  z  «       t        d
«      }|j                  |||j                  z  «       |
j                  |||j
                  z  ||j                  z  z   «       t        j                  «       5  t        j                  d«       t        t        «      5  |
j                  |«      |j                  | «      |j
                  z  ||j                  z  z   d|j                  | «      z  |j                  z  z   k(  sJ ‚	 d d d «       d d d «       y # 1 sw Y   ŒxY w# 1 sw Y   y xY w)Nzq1 q2 u1 u2r   r   rK   rL   rV   r1   ÚP4r   ÚO1Úerrorr   )r   rQ   r   r   r   r   r,   r   r   ÚwarningsÚcatch_warningsÚsimplefilterr   ÚUserWarningr3   rR   )rS   rH   r   r>   rA   r   r   rK   rL   rV   r`   r   ra   s                r   Ú!test_auto_point_vel_shortest_pathrg   Ç   sç  € Ü×Ñ€AÜ# MÓ2�N€BˆˆB�Ü�sÓ€AÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ‡J�Jˆq�"�q—s‘s‘(ÔÜ	ˆt‹€BØ‡J�Jˆr�2˜Ÿ™‘8ÔÜ	ˆt‹€BØ‡J�Jˆr�2˜‘7˜QŸS™S‘=Ô!Ü	ˆt‹€BØ‡J�Jˆr�2˜Ÿ™‘8ÔÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘ÔÜ	ˆt‹€BØ‡J�Jˆq�"�q—s‘s‘(ÔØ‡J�Jˆr�2˜Ÿ™‘8˜b 1§3¡3™hÑ&Ô'Ü	×	 Ñ	 Ó	"ñ SÜ×Ñ˜gÔ&Üœ[Ó)ñ 	SØ—6‘6˜!“9 §¡¨£
¨Q¯S©SÑ 0°2¸¿¹±8Ñ ;¸aÀ"Ç'Á'È!Ã*¹nÈqÏsÉsÑ>RÑ RÒRÐRÑR÷	S÷Sð S÷	Sð 	Sú÷Sð Sús%   Æ7%IÇA%IÉIÉI	ÉIÉI(c                  óŽ  ‡‡— t         j                  } t        d«      \  }}}}t        d«      Št        d«      }t        d«      }|j	                  ‰|‰j
                  z  «       t        d«      Š‰j                  ||‰j
                  z  ||j                  z  z   «       t        t        ˆˆfd„«       ‰j                  |d||j
                  f«       ‰j                  ‰«      ||j                  | «      z   ‰j
                  z  |j                  | «      |j                  z  z   ||j                  | «      z  |j                  z  z
  k(  sJ ‚y )Nz	q q1 q2 ur   r   r   r   c                  ó&   •— ‰j                  ‰ «      S r<   rF   )r   r   s   €€r   r?   z6test_auto_point_vel_connected_frames.<locals>.<lambda>é   s   ø€ ˜qŸu™u Q›x€ r!   r&   )r   rQ   r   r   r   r   r,   r   r   r@   Úorientr3   rR   r   )	rS   r   rH   r   Úur   r   r   r   s	          @@r   Ú$test_auto_point_vel_connected_framesrl   à   s   ù€ Ü×Ñ€AÜ! +Ó.�L€A€rˆ2ˆqÜ�sÓ€AÜ�sÓ€AÜˆc‹
€AØ‡I�Iˆa��Q—S‘S‘ÔÜˆc‹
€AØ‡I�Iˆa��a—c‘c‘˜B §¡™HÑ$Ô%Ü
Œ:Ô'Ô(Ø‡H�HˆQ�˜˜AŸC™C˜Ô!Ø�5‰5�‹8˜˜BŸG™G A›J™¨!¯#©#Ñ-°·±¸³
¸Q¿S¹SÑ0@Ñ@À2ÈÏÉÈqË	Á>ÐTU×TWÑTWÑCWÑWÒWÐWÑWr!   c                  óª  ‡‡— t        d«      \  } }t        d«      Št        d«      Št        d«      }t        d«      }t        d«      }|j                  ‰|‰j                  z  «       |j                  ‰|‰j
                  z  «       |j                  ‰|‰j                  z  «       ‰j                  || ‰j                  z  «       ‰j                  || ‰j
                  z  «       ‰j                  || ‰j                  z  «       t        j                  «       5  t        j                  d«       t        t        ˆˆfd„«       d d d «       y # 1 sw Y   y xY w)	Nzq ur   r   r   r6   ÚRrb   c                  ó&   •— ‰j                  ‰ «      S r<   rF   ©r   r   s   €€r   r?   zCtest_auto_point_vel_multiple_paths_warning_arises.<locals>.<lambda>ü   s   ø€  A§E¡E¨!£H€ r!   )r   r   r   r   r   r   r   r,   rc   rd   re   r   rf   )r   rk   r   r6   rn   r   r   s        @@r   Ú1test_auto_point_vel_multiple_paths_warning_arisesrq   í   sý   ù€ Ü˜%Ó �D€A€qÜ�sÓ€AÜˆc‹
€AÜˆc‹
€AÜˆc‹
€AÜˆc‹
€AØ‡I�Iˆa��Q—S‘S‘ÔØ‡I�Iˆa��A—C‘C‘ÔØ‡I�Iˆa��Q—S‘S‘ÔØ‡I�Iˆa��Q—S‘S‘ÔØ‡I�Iˆa��Q—S‘S‘ÔØ‡I�Iˆa��Q—S‘S‘ÔÜ	×	 Ñ	 Ó	"ñ .Ü×Ñ˜gÔ&ÜŒ{Ô,Ô-÷.÷ .ñ .ús   Ä*E	Å	Ec                  óL  ‡‡— t        d«      } t        d«      }t        d«      Št        d«      }t        d«      Š| j                  ‰‰j                  «       |j	                  | ‰j                  «       ‰j	                  |‰j
                  «       |j	                  ‰‰j                  «       |j	                  |‰j                  ‰j
                  z   «       t        j                  «       5  t        j                  d«       t        t        ˆˆfd„«       d d d «       y # 1 sw Y   y xY w)Nr   rK   rL   rV   r   rb   c                  ó&   •— ‰j                  ‰ «      S r<   rF   rP   s   €€r   r?   z5test_auto_vel_cyclic_warning_arises.<locals>.<lambda>  s   ø€  B§F¡F¨1£I€ r!   )r   r   r   r   r,   r   r   rc   rd   re   r   rf   )r   rK   rV   r   rL   s      @@r   Ú#test_auto_vel_cyclic_warning_arisesrt   þ   sÍ   ù€ Üˆc‹
€AÜ	ˆt‹€BÜ	ˆt‹€BÜ	ˆt‹€BÜ�sÓ€AØ‡I�Iˆa�—‘ÔØ‡J�Jˆq�!—#‘#ÔØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆr�1—3‘3˜Ÿ™‘9ÔÜ	×	 Ñ	 Ó	"ñ /Ü×Ñ˜gÔ&ÜŒ{Ô-Ô.÷/÷ /ñ /ús   Ã'*DÄD#c                  óâ  — t        d«      } t        d«      }t        d«      }t        d«      }t        d«      }| j                  ||j                  «       |j	                  | |j                  «       |j	                  ||j
                  «       |j	                  ||j                  «       |j	                  ||j                  |j
                  z   «       t        j                  d¬«      5 }t        j                  d«       |j                  |«       t        |d	   j                  «      j                  d
d«      }t        |d	   j                  t         «      sJ ‚d|v sJ ‚	 d d d «       y # 1 sw Y   y xY w)Nr   rK   rL   rV   r   T©ÚrecordÚalwayséÿÿÿÿú
ú z€Kinematic loops are defined among the positions of points. This is likely not desired and may cause errors in your calculations.)r   r   r   r   r,   r   r   rc   rd   re   r3   ÚstrÚmessageÚreplaceÚ
issubclassÚcategoryrf   )r   rK   rL   rV   r   ÚwÚmsgs          r   Ú test_auto_vel_cyclic_warning_msgrƒ     s*  € Üˆc‹
€AÜ	ˆt‹€BÜ	ˆt‹€BÜ	ˆt‹€BÜ�sÓ€AØ‡I�Iˆa�—‘ÔØ‡J�Jˆq�!—#‘#ÔØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆr�1—3‘3˜Ÿ™‘9ÔÜ	×	 Ñ	 ¨$Ô	/ð Y°1Ü×Ñ˜hÔ'Ø
�‰ˆqŒ	Ü�!�B‘%—-‘-Ó ×(Ñ(¨¨sÓ3ˆÜ˜!˜B™%Ÿ.™.¬+Ô6Ð6Ð6ð Rð  VYñ  Yð  	Yñ  Y÷Y÷ Yñ Yús   Ã'A4E%Å%E.c                  ó’  — t        d«      } t        d«      }t        d«      }t        d«      }|j                  | | j                  «       |j                  | | j                  «       |j                  || j                  «       |j                  || j                  «       t        j                  d¬«      5 }t        j                  d«       |j                  | «       t        |d   j                  «      j                  d	d
«      }t        |d   j                  t         «      sJ ‚d|v sJ ‚d|v sJ ‚d|v sJ ‚	 d d d «       y # 1 sw Y   y xY w)Nr   r   r   r6   Trv   rx   ry   rz   r{   ÚVelocityz'automatically calculated based on pointzfVelocities from these points are not necessarily the same. This may cause errors in your calculations.)r   r   r   r   r   r,   r   rc   rd   re   r3   r|   r}   r~   r   r€   rf   )r   r   r   r6   r�   r‚   s         r   Ú'test_auto_vel_multiple_path_warning_msgr†     s  € Ü�sÓ€AÜˆc‹
€AÜˆc‹
€AÜˆc‹
€AØ‡I�Iˆa�—‘ÔØ‡I�Iˆa�—‘ÔØ‡I�Iˆa�—‘ÔØ‡I�Iˆa�—‘ÔÜ	×	 Ñ	 ¨$Ô	/ð °1Ü×Ñ˜hÔ'Ø	�‰ˆaŒÜ�!�B‘%—-‘-Ó ×(Ñ(¨¨sÓ3ˆÜ˜!˜B™%Ÿ.™.¬+Ô6Ð6Ð6Ø˜SÑ Ð Ð Ø8¸CÑ?Ð?Ð?ØwÐ{~Ñ~Ð~Ñ~÷÷ ñ ús   Â3B D=Ä=Ec                  óò  — t        d«      \  } }t        dd«      \  }}t        d«      }t        d«      }t        d«      }|j                  ||j                  | «       |j	                  |||j                  z  «       |j                  ||j                  |«       |j	                  |||j                  z  «       t        d«      }|j                  |d«       t        d	«      }|j                  ||j                  «       |j                  |d«       |j                  |d«       t        d
«      }	|	j                  ||j                  «       |	j                  |d«       |	j                  j                  «       }
|	j                  |«      ||j                  z  ||z   |j                  z  z   k(  sJ ‚|
|	_	        |	j                  |||«       |	j                  |«      ||j                  z  ||z   |j                  z  z   k(  sJ ‚y )Núq1:3zu1:3r   r9   r   ÚCÚAmr   ÚBmÚCm)r   r   Úorient_axisr   r   r   r   r,   r   Ú	_vel_dictÚcopyr3   r   r(   )rH   r   r>   rA   r9   r   r‰   rŠ   r‹   rŒ   Útemps              r   Útest_auto_vel_derivativer‘   1  s�  € Ü˜FÓ#�F€BˆÜ˜F AÓ&�F€BˆÜ�sÓ€AÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�Q—S‘S˜"ÔØ‡M�M�!�R˜!Ÿ#™#‘XÔØ‡M�M�!�Q—S‘S˜"ÔØ‡M�M�!�R˜!Ÿ#™#‘XÔä	ˆt‹€BØ‡J�Jˆq�!ÔÜ	ˆt‹€BØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆq�!ÔØ‡J�Jˆq�!ÔÜ	ˆt‹€BØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆq�!ÔØ�<‰<×ÑÓ€DØ�6‰6�!‹9˜˜aŸc™c™ R¨"¡W°·±¡OÑ3Ò4Ð4Ð4Ø€B„LØ‡N�N�2�q˜!ÔØ�6‰6�!‹9˜˜aŸc™c™ R¨"¡W°·±¡OÑ3Ò4Ð4Ñ4r!   c                  óš   — t        d«      } t        d«      }|j                  | d«       |j                  | «      d| j                  z  k(  sJ ‚y )Nr   r   r   )r   r   r   Úaccr   rp   s     r   Útest_auto_point_acc_zero_velr”   K  s@   € Ü�sÓ€AÜˆc‹
€AØ‡I�Iˆa�„OØ�5‰5�‹8�q˜1Ÿ3™3‘wÒÐÑr!   c                  óÔ  — t         j                  } t        d«      }t        d«      }t        d«      }|j                  ||j                  |«       t        d«      }|j                  |d«       t        d«      }|j                  ||j                  «       |j                  |«      |j                  | «      dz   |j                  z  |j                  | d«      |j                  z  z   k(  sJ ‚y )NrH   r   r9   r   r   r   r   )r   rQ   r   r�   r   r   r   r,   r   r“   rR   r   )rS   rH   r   r9   r   r   s         r   Útest_auto_point_acc_compute_velr–   Q  s¶   € Ü×Ñ€AÜ	˜Ó	€BÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�Q—S‘S˜"Ôäˆc‹
€AØ‡I�Iˆa�„OÜˆc‹
€AØ‡I�Iˆa�—‘ÔØ�5‰5�‹8˜Ÿ™ ›
 a™Ð'¨!¯#©#Ñ-°·±¸¸1³ÀÇÁÑ0CÑCÒCÐCÑCr!   c                  óÆ  — t        d«      \  } }t        dd«      \  }}t        dd«      \  }}t        d«      }t        d«      }t        d«      }|j                  ||j                  | «       |j                  ||j                  |«       t	        d«      }	|	j                  |d«       t	        d	«      }
|
j                  |	|j                  «       |
j                  |d«       |
j                  |d«       t	        d
«      }|j                  |
|j                  «       |j                  |d«       |
j                  j                  «       |j                  j                  «       }}|
j                  j                  «       |j                  j                  «       }}|dz   |j                  z  ||j                  z  z   ||z   dz  |j                  z  z
  ||z   |j                  z  z   }|j                  |«      |k(  sJ ‚||c|
_        |_        ||c|
_
        |_
        |
j                  |	||«       |j                  |
||«       |
j                  |	||«       |j                  |
||«      |k(  sJ ‚y )Nrˆ   r   r   r9   r   r‰   rŠ   r   r‹   rŒ   )r   r   r�   r   r   r   r,   r   rŽ   r�   Ú	_acc_dictr   r“   r(   r+   )rH   r   r>   rA   Úv1Úv2r9   r   r‰   rŠ   r‹   rŒ   ÚBm_vel_dictÚCm_vel_dictÚBm_acc_dictÚCm_acc_dictÚchecks                    r   Útest_auto_acc_derivativer    ^  sÿ  € ô ˜FÓ#�F€BˆÜ˜F AÓ&�F€BˆÜ˜F AÓ&�F€BˆÜ�sÓ€AÜ�sÓ€AÜ�sÓ€AØ‡M�M�!�Q—S‘S˜"ÔØ‡M�M�!�Q—S‘S˜"Ôä	ˆt‹€BØ‡J�Jˆq�!ÔÜ	ˆt‹€BØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆq�!ÔØ‡J�Jˆq�!ÔÜ	ˆt‹€BØ‡J�Jˆr�1—3‘3ÔØ‡J�Jˆq�!Ôð  "Ÿ|™|×0Ñ0Ó2°B·L±L×4EÑ4EÓ4G�€KØ!Ÿ|™|×0Ñ0Ó2°B·L±L×4EÑ4EÓ4G�€KØ�1‰WˆH�q—s‘s‰N˜R !§#¡#™XÑ%¨¨b©°Q©¸¿¹Ñ(<Ñ<ÀÀRÁÈ1Ï3É3¹ÑN€EØ�6‰6�!‹9˜ÒÐÐØ!,¨kÐ€B„L�"”,Ø!,¨kÐ€B„L�"”,Ø‡N�N�2�q˜!ÔØ‡N�N�2�q˜!ÔØ‡N�N�2�q˜!ÔØ�>‰>˜"˜a Ó# uÒ,Ð,Ñ,r!   )Úsympy.physics.vectorr   r   r   Úsympy.testing.pytestr   r   rc   r    r$   r)   r-   r4   r7   rB   rI   rT   rW   rY   r^   rg   rl   rq   rt   rƒ   r†   r‘   r”   r–   r    © r!   r   ú<module>r¤      s‰   ðß FÑ Fß 8Û ò:ò$+ò&4òBòHò@
Eò:ò
+ò*ò$Hò!ò	+òSò2Xò.ò"/òYò$ò$5ò4òDó -r!   