o
    ¦ƒðj©A  ã                   @   sÐ  d dl mZ d dlmZ d dlmZ d dlmZmZmZm	Z	 d dl
mZ d dlmZmZ d dlmZmZmZmZmZ d dlmZ d d	lmZ d d
lmZmZmZmZmZmZ d dl m!Z! d dl"m#Z# d dl$m%Z% d dl&m'Z' d dl(m)Z) d dl m*Z* d dl+Z+d dl,m-Z-m.Z. edƒ\Z/Z0Z1Z2edƒZ3dd„ Z4dd„ Z5dd„ Z6dd„ Z7dd„ Z8dd „ Z9d!d"„ Z:d#d$„ Z;d%d&„ Z<d'd(„ Z=d)d*„ Z>d+d,„ Z?d-d.„ Z@d/d0„ ZAd1d2„ ZBd3d4„ ZCed5d6„ ƒZDd7d8„ ZEd9d:„ ZFed;d<„ ƒZGdS )=é    )Úslow)Údiff)Úexpand)ÚEÚIÚRationalÚpi)ÚS)ÚSymbolÚsymbols)ÚAbsÚ	conjugateÚimÚreÚsign)Úlog)Úsqrt)ÚacosÚasinÚcosÚsinÚatan2Úatan)Ú	integrate)ÚMatrix)Úsimplify)Útrigsimp)Ú
Quaternion)ÚraisesN)ÚpermutationsÚproductzw:zÚphic               
      s8  t ttttƒ} | |  t dt dt dt dt ƒksJ ‚t  tdƒd tdƒd tdƒd fttddƒ ¡}|t t	j
t	j
t	j
t	j
ƒksEJ ‚tttƒttƒ dgttƒttƒdgg d¢gƒ}tt  |¡ƒ}|t tdƒtttƒd ƒ d ddtddttƒ  ƒtttƒƒ d ƒks‹J ‚tddd�‰ tt‡ fd	d
„ƒ d S )Né   é   r   ©r   r   é   r%   ÚncF©Úcommutativec                      s   t ttˆ tƒS ©N)r   ÚwÚxÚz© ©r&   r-   ú¦/tmp/claude-1000/-var-www-html-academy/3f42efd1-c2b7-4d08-9b05-30d86bb35479/scratchpad/piper/venv/lib/python3.10/site-packages/sympy/algebras/tests/test_quaternion.pyÚ<lambda>&   s    z.test_quaternion_construction.<locals>.<lambda>)r   r*   r+   Úyr,   Úfrom_axis_angler   r   r   r	   ÚHalfr   r   r!   r   r   Úfrom_rotation_matrixr   r
   r   Ú
ValueError)ÚqÚq2ÚMÚq3r-   r.   r/   Útest_quaternion_construction   s   *$ÿ
ÿ0@
ÿr:   c                  C   s|   t tdƒŽ } t ttttƒ}t| |  ¡ d |  ¡ d | ¡ d   ƒdks'J ‚t ttttdd�}| |  ¡ |  ¡ ks<J ‚d S )Núa:dr"   r   r%   )Únorm)r   r   r*   r+   r1   r,   r   r<   )Úq1r7   r9   r-   r-   r/   Ú!test_quaternion_construction_norm)   s
   4r>   c                  C   sP   t ddddƒ} t  ddtj d ¡}| ¡ }||  | }||   ¡ dk s&J ‚d S )Nr%   r   ©r%   r%   r%   r#   é   g»½×Ùß|Û=)r   r2   Úmathr   Úinverser<   )Úpr6   ÚqiÚtestr-   r-   r/   Útest_issue_252543   s
   rF   c                  C   sT   t ttttƒ} t  |  ¡ ¡}t  |  d¡¡}| |  ¡ sJ ‚|  ¡ |  ¡ s(J ‚d S )NT)	r   r*   r+   r1   r,   Úfrom_MatrixÚ	to_MatrixÚis_zero_quaternionÚvector_part)r6   Úq_fullÚq_vectr-   r-   r/   Útest_to_and_from_Matrix=   s
   rM   c                  C   sœ   t ttttƒ} t tdƒŽ }| |  ¡ | j| ¡  ksJ ‚| |  ¡ |j|  ¡  ks+J ‚| j| jj	 dd …dd …f }t
|  ¡ |  ¡ d  ƒ}||ksLJ ‚d S )Nr;   r%   r"   )r   r*   r+   r1   r,   r   rH   Úproduct_matrix_leftÚproduct_matrix_rightÚTr   Úto_rotation_matrixr<   )r=   r7   ÚR1ÚR2r-   r-   r/   Útest_product_matricesE   s   rT   c                  C   s  ddt d tdƒd tdƒd ddffdt d tdƒd dtdƒd dffdt d tdƒd ddtdƒd ffdt dfdt dfdt d	fd
t ddtdƒ dtdƒ dtdƒ fftdƒd tdƒd tdƒd ft d d tjtjtjtjffg	} | D ]\}}}t ||¡t|Ž ks‹J ‚qzd S )N)©r%   r   r   r   )r%   r   r   r   rU   r"   r   )r   r%   r   r$   )r   r%   r   r   ©r   r   r%   r   ©r   r   r   r%   r?   r%   r#   )r   r   r	   r3   r   r2   )Ú	test_dataÚaxisÚangleÚexpectedr-   r-   r/   Útest_quaternion_axis_angleP   s   $$$(>÷ÿr\   c                  C   s¤   t  dtdƒ¡} | jttdƒd ƒksJ ‚| jtdƒttdƒd ƒ d ks(J ‚| jtdƒttdƒd ƒ d ks;J ‚| j	dtdƒ ttdƒd ƒ d ksPJ ‚d S )N)r%   r"   r#   r@   r"   é   é   r#   )
r   r2   r   Úar   Úbr   r   ÚcÚd)Úresultr-   r-   r/   Ú)test_quaternion_axis_angle_simplificationb   s
   &&.rd   c               	      s€  t ddd�} t ddd�}t ddd�‰ tttttƒ‰| ˆ ttt| ƒ tt| ƒ ttƒks.J ‚d	ˆ td	t tttƒks=J ‚tˆ ttd	t ttƒksLJ ‚|ˆ tt| tttƒks[J ‚t	t
‡ ‡fd
d„ƒ t	t
‡ ‡fdd„ƒ t	t
‡ ‡fdd„ƒ ˆ tt t t t ƒks‰J ‚tddt  ddt  dddt  dd�}td	dddƒ}|ddt   tddt  ddt  dddt  ƒksÂJ ‚|ddt   tddddƒksÓJ ‚|ddt   tddt  ddt   ddt  ddt   dddt  ddt   ƒk�sJ ‚|ddt   tddddƒk�sJ ‚td	dddƒ}tddddƒ}|| |k�s,J ‚|| |k�s5J ‚|| |k�s>J ‚d S )Nr_   T©Úcomplexr`   ©Úrealra   Fr'   r%   c                      s   ˆ ˆ S r)   r-   r-   ©ra   r6   r-   r/   r0   t   ó    z7test_quaternion_complex_real_addition.<locals>.<lambda>c                      s   ˆˆ  S r)   r-   r-   ri   r-   r/   r0   u   rj   c                      s   ˆ ˆ S r)   r-   r-   ri   r-   r/   r0   v   rj   r#   r@   r"   é   r   r^   é   ©Ú
real_fieldiöÿÿÿé   é&   éûÿÿÿ)r   r   r*   r+   r1   r,   r   r   r   r   r5   )r_   r`   r=   r7   Úq0r-   ri   r/   Ú%test_quaternion_complex_real_additioni   s2   * *:"H
ÿ$rs   c                  C   s.   t  dt¡} |  td¡t ddddƒksJ ‚d S )Nr$   r   r%   )r   r2   r!   Úsubs©r6   r-   r-   r/   Útest_quaternion_subsŠ   s   "rv   c                   C   sŒ   t tdƒddtdƒƒ ¡ t tdƒ ¡ ddtdƒ ¡ ƒksJ ‚t dtdƒ dddtdƒ ƒ ¡ t dtdƒ  ¡ dddtdƒ  ¡ ƒksDJ ‚d S )Nr"   r   r#   r%   )r   r   Úevalfr-   r-   r-   r/   Útest_quaternion_evalf�   s   ÿ $ÿrx   c                     sú  t ttttƒ} t ddddƒ‰t ddddƒ‰t| ƒt tt t t ƒks%J ‚|  ¡ ttd td  td  td  ƒks=J ‚|  ¡ t ttttƒttd td  td  td  ƒ ks\J ‚|  	¡ t tt t t ƒtd td  td  td   ks|J ‚|  	¡ |  
d¡ks‡J ‚tt‡fdd„ƒ |  
d¡t td td  td  td  dt t dt t dt t ƒks¸J ‚| d t td td  td  td  dt t dt t dt t ƒksßJ ‚ˆ 
d	¡t td
dƒtddƒtddƒtd	dƒƒksùJ ‚ˆd	 t td
dƒtddƒtddƒtd	dƒƒk�sJ ‚ˆ 
d¡tk�sJ ‚tt‡fdd„ƒ ˆ ¡ t tttdƒƒ dtdƒ t ttdƒƒ d dtdƒ t ttdƒƒ d dtdƒ t ttdƒƒ d ƒk�sdJ ‚ˆ ¡ t ttdƒƒdtdƒ ttdƒd ƒ d dtdƒ ttdƒd ƒ d dtdƒ ttdƒd ƒ d ƒk�s J ‚ˆ d¡t dtdttdƒd ƒ ƒ dtdƒ tdttdƒd ƒ ƒ d dtdƒ tdttdƒd ƒ ƒ d dtdƒ tdttdƒd ƒ ƒ d ƒk�sñJ ‚tt ttttƒtƒt ddddƒk�sJ ‚tt ttttƒtƒt td d td d td d td d ƒk�s)J ‚t dttd td ƒ t¡t ttd d td d td d ƒk�sMJ ‚t ttƒttƒtdt ƒtdt ƒƒ t¡t ttƒ ttƒtdt ƒ d tdt ƒd ƒk�sJ ‚t td td td tt t ƒ tt¡t td t d ttd  d tt td  td td  t d ƒk�sºJ ‚t  dˆ¡tjd dtdƒd fk�sÏJ ‚tdƒ‰ tt‡ ‡fdd„ƒ tddd�‰ tt‡ ‡fdd„ƒ t ddddƒ ¡ dk�sûJ ‚t ttttƒ ¡ tk�s	J ‚t ddddƒ ¡ t ddddƒk�sJ ‚t ttttƒ ¡ t dtttƒk�s/J ‚ˆ ¡ t ddtdƒ d dtdƒ d dtdƒ d ƒk�sOJ ‚ˆ ¡  
d¡t ddddƒk�s`J ‚ˆ ¡  ¡ dk�skJ ‚|  ¡ t dtttd td  td  ƒ tttd td  td  ƒ tttd td  td  ƒ ƒk�s£J ‚ˆ  ¡ du �s¬J ‚ˆ  ¡ d u �sµJ ‚t ddddƒ  ¡ du �sÃJ ‚t ddd!dƒ  ¡ du �sÑJ ‚t tdd!dƒ  ¡ d u �sßJ ‚ˆ !¡ dt"tdƒƒ k�sîJ ‚|  !¡ dt#ttd td  td  ƒtƒ k�sJ ‚t  $ˆt ddd"dƒ¡du �sJ ‚t  $ˆt dd	d#d$ƒ¡du �s(J ‚t  $ˆt ddd%d&ƒ¡du �s8J ‚t  $ˆt ddddƒ¡du �sHJ ‚t  $ˆt tdd"dƒ¡du �sXJ ‚t  $ˆt ddddƒ¡d u �shJ ‚t  $ˆt ttttƒ¡d u �sxJ ‚tt‡‡fd'd„ƒ t  %t ddd%d&ƒt ddd"dƒt ddddƒ¡du �s�J ‚t  %t ddddƒt ddd"dƒt ddddƒ¡du �s¸J ‚t  %t dddd"ƒt ddd"d"ƒt ddddƒ¡d u �sÓJ ‚t  %t ddddƒt ddtd"ƒt dd"ddƒ¡d u �sîJ ‚tt‡‡fd(d„ƒ t ddddƒ &t dddd"ƒ¡du �sJ ‚t ddddƒ &t dddd"ƒ¡d u �s J ‚t ddddƒ &t tttd"ƒ¡d u �s4J ‚tt‡‡fd)d„ƒ t ddddƒ 't dd	ddƒ¡du �sRJ ‚t ddddƒ 't dddd"ƒ¡d u �sfJ ‚t ddddƒ 't tttd"ƒ¡d u �szJ ‚tt‡‡fd*d„ƒ ˆ (¡ t ddtd+ƒ d dtd+ƒ d dtd+ƒ d ƒk�s¤J ‚t ddd,dƒ (¡ t ddd,dƒk�s·J ‚t ddd,dƒ )¡ ttd-ƒƒk�sÉJ ‚t ddddƒ )¡ ttdƒƒk�sÛJ ‚ˆ *¡ du �säJ ‚ˆ *¡ d u �síJ ‚t tdddƒ *¡ d u �sûJ ‚d S ).Nr%   r"   r#   r@   r   éÿÿÿÿc                      s   ˆ   ¡ S r)   ©rB   r-   ©rr   r-   r/   r0       rj   z+test_quaternion_functions.<locals>.<lambda>éþÿÿÿiùÿÿÿéá   é–   ç      à¿c                      s   ˆ d S )Nr   r-   r-   )r=   r-   r/   r0   ¨   rj   é   é   é<   éZ   éx   r?   rk   r^   Únc                      ó   ˆˆ  S r)   r-   r-   ©r…   r=   r-   r/   r0   Ë   rj   T)Úintegerc                      r†   r)   r-   r-   r‡   r-   r/   r0   Í   rj   é   é   é7   rl   Fé
   é   éýÿÿÿéüÿÿÿé   é   c                      s   t  ˆˆ ¡S r)   )r   Úarc_coplanarr-   ©rr   r=   r-   r/   r0   í   s    c                      s   t  ˆ t ddddƒˆ¡S )Nr   r@   r�   rl   )r   Úvector_coplanarr-   r“   r-   r/   r0   û   s   c                      ó
   ˆ   ˆ¡S r)   )Úparallelr-   r“   r-   r/   r0     ó   
 c                      r•   r)   )Ú
orthogonalr-   r“   r-   r/   r0     r—   if  é	   éz   )+r   r*   r+   r1   r,   r   r<   r   Ú	normalizerB   Úpowr   r5   r   ÚNotImplementedÚ	TypeErrorÚexpr   r   r   r   r   Úpow_cos_sinr   r   Úrotate_pointr	   ÚOner
   Úscalar_partrJ   rY   Úis_purerZ   r   r   r’   r”   r–   r˜   Úindex_vectorÚmensorrI   ru   r-   )r…   rr   r=   r/   Útest_quaternion_functions–   s   0>@PN
 
ÿ ÿý
ÿý
ÿ&&&ý
ÿ(,
ÿ$
ÿ*0
ÿ(D
ÿ*&&@"
ý4       ý
ý$ÿ
ÿ$ÿ
ÿý
ý((((((ý&$$ r§   c               
   C   sh  t ddddƒ} |  ¡ dtdƒ d dtdƒ d dtdƒ d fdttdƒd ƒ fks.J ‚|  ¡ ttddƒtddƒtd	dƒgtddƒtd
dƒtddƒgtddƒtddƒtddƒggƒks_J ‚|  d¡ttddƒtddƒtd	dƒtddƒgtddƒtd
dƒtddƒtjgtddƒtddƒtddƒtddƒgtjtjtjtj	ggƒks¤J ‚t
ddd�}t t|d ƒddt|d ƒƒ}t| ¡ ƒtt|ƒt|ƒ dgt|ƒt|ƒdgg d¢gƒksÙJ ‚| ¡ ddt|d ƒtt|d ƒƒ fdtt|d ƒƒ fksúJ ‚t| d¡ƒtt|ƒt|ƒ dt|ƒt|ƒ d gt|ƒt|ƒdt|ƒ t|ƒ d gg d¢g d¢gƒk�s2J ‚d S )Nr%   r"   r#   r@   r€   r�   r|   é   ro   ry   r]   r?   rk   ÚthetaTrg   r   r$   rV   rW   )r   Úto_axis_angler   r   rQ   r   r   r	   ÚZeror¢   r   r   r   r   r   )r=   r©   r7   r-   r-   r/   Útest_quaternion_conversions  sJ   þ
ýþÿ""üÿý&
ÿ$$ür¬   c                  C   sR   t ttttƒ} | jdd�|  ¡ d  }t| jdd�|  ¡ d  ƒ}||ks'J ‚d S )NT)Úhomogeneousr"   F)r   r*   r+   r1   r,   rQ   r<   r   )r6   rR   rS   r-   r-   r/   Ú test_rotation_matrix_homogeneous;  s   r®   c                  C   sb   t ttd ƒttd ƒddƒ} t|  ¡ ƒtg d¢dttƒttƒ gdttƒttƒggƒks/J ‚dS )a3  
    There was a sign mistake in the definition,
    of the rotation matrix. This tests that particular sign mistake.
    See issue 1593 for reference.
    See wikipedia
    https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
    for the correct definition
    r"   r   rU   N)r   r   r!   r   r   rQ   r   ru   r-   r-   r/   Ú test_quaternion_rotation_iss1593B  s   	ýr¯   c                  C   sb  t ddt  ddt  dddt  dd	�} t d
dddƒ}t d
d
d
tƒ}t  tdƒtj¡dks0J ‚t  tdƒ| ¡t ddt  ddt  dddt  ƒksMJ ‚| d¡t ddddƒks[J ‚| |¡t dt d dt d ddt  td ƒkswJ ‚| |¡|| ks‚J ‚tddd�}t t|ƒt	|ƒddƒ}t tddd�Ž }|| || ks¥J ‚|| || ks¯J ‚d S )Nr#   r@   r"   rk   r   r^   rl   Frm   r%   r�   r‘   é   é   é    r�   rŒ   rq   r™   r,   Tre   zq:4rg   )
r   r   r1   Ú_generic_mulr	   r¢   Úmulr   r   r   )r=   r7   r9   r,   Úz_quatr6   r-   r-   r/   Útest_quaternion_multiplicationR  s   *$ÿ8r¶   c                     sÀ   t ddddƒ‰ tt‡ fdd„ƒ t ddddƒ} |  ¡ \}}t  d||f¡tjd	 dtd
ƒd	 fks3J ‚t ddddƒ} tdƒ d tdƒ d tdƒ d f}dt d }||f|  ¡ ks^J ‚d S )Nr   c                      s   dˆ  S )Nr%   r-   r-   r{   r-   r/   r0   i  rj   z"test_issue_16318.<locals>.<lambda>r%   r"   r#   r@   r?   rk   r^   ry   )	r   r   r5   rª   r¡   r	   r¢   r   r   )r6   rY   rZ   r-   r{   r/   Útest_issue_16318f  s   ,(r·   c                  C   sd   t ttttƒ} |  ¡ }g d¢}|dd„ |D ƒ7 }|D ]}|  |¡}tt  ||¡ƒ}||ks/J ‚qd S )N)ÚzxyÚzyxÚzyzÚzxzc                 S   s   g | ]}|  ¡ ‘qS r-   )Úupper)Ú.0Úseqr-   r-   r/   Ú
<listcomp>{  s    z!test_to_euler.<locals>.<listcomp>)	r   r*   r+   r1   r,   r›   Úto_eulerr   Ú
from_euler)r6   Úq_normalizedÚseqsr¾   Úeuler_from_qÚq_backr-   r-   r/   Útest_to_euleru  s   
ýrÆ   c                  C   s6   t  tddfd¡} t|  d¡dd�tddfksJ ‚dS )z_
    There was a mistake in the degenerate case testing
    See issue 24504 for reference.
    r   rº   Trz   N)r   rÁ   r!   r   rÀ   ru   r-   r-   r/   Útest_to_euler_iss24504ƒ  s   $rÇ   c                  C   sÀ   dd„ } | t d ddfdƒ | t d ddfdƒ | t d t dfdƒ | t d t dfdƒ | t d t d dfdƒ | t d t  d dfdƒ | t d t d dfdƒ | t d t  d dfdƒ d S )	Nc                 S   s"   t  | |¡}| |¡| ksJ ‚d S r)   )r   rÁ   rÀ   )Úanglesr¾   r6   r-   r-   r/   Útest_one_caseŽ  s   z<test_to_euler_numerical_singilarities.<locals>.test_one_caser"   r   rº   ÚZYZr¹   ÚZYX)r   )rÉ   r-   r-   r/   Ú%test_to_euler_numerical_singilaritiesŒ  s   rÌ   c               	      sŒ   ‡ fdd„} dD ];}t |ƒD ]4}dD ]/}|r%d |d |d |d g¡‰ nd |¡‰ tg d¢d	d
�D ]}t|Ž }| ¡ s@| |ƒ q2qqqd S )Nc                    sb   t |  ˆ dd¡ƒ}t |  ˆ dd¡ƒ}t|| ƒ ¡ }|D ]}|t dt  t }|dk s.J ‚qd S )NTFr"   g�íµ ÷Æ°>)r   rÀ   r   rw   r   )r6   Úangles1Úangles2Úangle_errorsÚangle_error©r¾   r-   r/   rÉ   ¡  s   ýz,test_to_euler_options.<locals>.test_one_case)ÚxyzÚXYZ)TFÚ r   r%   )ry   r   r%   r@   )Úrepeat)r   Újoinr    r   rI   )rÉ   rÒ   Ú	seq_tupleÚ	symmetricÚelementsr6   r-   rÑ   r/   Útest_to_euler_optionsŸ  s    	
€ýúÿÿrÚ   )HÚsympy.testing.pytestr   Úsympy.core.functionr   r   Úsympy.core.numbersr   r   r   r   Úsympy.core.singletonr	   Úsympy.core.symbolr
   r   Ú$sympy.functions.elementary.complexesr   r   r   r   r   Ú&sympy.functions.elementary.exponentialr   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   r   r   r   r   r   Úsympy.integrals.integralsr   Úsympy.matrices.denser   Úsympy.simplifyr   Úsympy.simplify.trigsimpr   Úsympy.algebras.quaternionr   r   rA   Ú	itertoolsr   r    r*   r+   r1   r,   r!   r:   r>   rF   rM   rT   r\   rd   rs   rv   rx   r§   r¬   r®   r¯   r¶   r·   rÆ   rÇ   rÌ   rÚ   r-   r-   r-   r/   Ú<module>   sT     

!&
	