o
    FZŽh©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©Zcommutativec                      s   t ttˆ tƒS ©N)r   ÚwÚxÚz© ©r&   r,   úS/var/www/auris/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   Zfrom_rotation_matrixr   r
   r   Ú
ValueError)ÚqÚq2ÚMÚq3r,   r-   r.   Útest_quaternion_construction   s   *$ÿ
ÿ0@
ÿr8   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*   r0   r+   r   r:   )Úq1r5   r7   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   r1   Úmathr   Úinverser:   )Úpr4   ZqiÚtestr,   r,   r.   Útest_issue_252543   s
   rC   c                  C   sT   t ttttƒ} t  |  ¡ ¡}t  |  d¡¡}| |  ¡ sJ ‚|  ¡ |  ¡ s(J ‚d S )NT)	r   r)   r*   r0   r+   Zfrom_MatrixÚ	to_MatrixÚis_zero_quaternionÚvector_part)r4   Zq_fullZq_vectr,   r,   r.   Útest_to_and_from_Matrix=   s
   rG   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 )Nr9   r%   r"   )r   r)   r*   r0   r+   r   rD   Zproduct_matrix_leftZproduct_matrix_rightÚTr   Úto_rotation_matrixr:   )r;   r5   ÚR1ÚR2r,   r,   r.   Útest_product_matricesE   s   rL   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   rM   r"   r   )r   r%   r   r$   )r   r%   r   r   ©r   r   r%   r   ©r   r   r   r%   r=   r%   r#   )r   r   r	   r2   r   r1   )Z	test_dataÚaxisÚangleÚexpectedr,   r,   r.   Útest_quaternion_axis_angleP   s   $$$(>÷ÿrS   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   r1   r   Úar   Úbr   r   ÚcÚd)Úresultr,   r,   r.   Ú)test_quaternion_axis_angle_simplificationb   s
   &&.r[   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 )NrV   T©ÚcomplexrW   ©ÚrealrX   Fr'   r%   c                      s   ˆ ˆ S r(   r,   r,   ©rX   r4   r,   r.   r/   t   ó    z7test_quaternion_complex_real_addition.<locals>.<lambda>c                      s   ˆˆ  S r(   r,   r,   r`   r,   r.   r/   u   ra   c                      s   ˆ ˆ S r(   r,   r,   r`   r,   r.   r/   v   ra   r#   r>   r"   é   r   rU   é   ©Z
real_fieldiöÿÿÿé   é&   éûÿÿÿ)r   r   r)   r*   r0   r+   r   r   r   r   r3   )rV   rW   r;   r5   Úq0r,   r`   r.   Ú%test_quaternion_complex_real_additioni   s2   * *:"H
ÿ$ri   c                  C   s.   t  dt¡} |  td¡t ddddƒksJ ‚d S )Nr$   r   r%   )r   r1   r!   Úsubs©r4   r,   r,   r.   Útest_quaternion_subsŠ   s   "rl   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   ÿ $ÿrn   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 sJ ‚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(   ©r@   r,   ©rh   r,   r.   r/       ra   z+test_quaternion_functions.<locals>.<lambda>éþÿÿÿiùÿÿÿéá   é–   ç      à¿c                      s   ˆ d S )Nru   r,   r,   )r;   r,   r.   r/   ¨   ra   é   é   é<   éZ   éx   r=   rb   rU   Únc                      ó   ˆˆ  S r(   r,   r,   ©r{   r;   r,   r.   r/   Ë   ra   T)Úintegerc                      r|   r(   r,   r,   r}   r,   r.   r/   Í   ra   é   é   é7   rc   Fé
   é   éýÿÿÿéüÿÿÿé   é   c                      s   t  ˆˆ ¡S r(   )r   Úarc_coplanarr,   ©rh   r;   r,   r.   r/   í   s    c                      s   t  ˆ t ddddƒˆ¡S )Nr   r>   rƒ   rc   )r   Úvector_coplanarr,   r‰   r,   r.   r/   û   s   c                      ó
   ˆ   ˆ¡S r(   )Úparallelr,   r‰   r,   r.   r/     ó   
 c                      r‹   r(   )Ú
orthogonalr,   r‰   r,   r.   r/     r   if  é	   éz   )+r   r)   r*   r0   r+   r   r:   r   Ú	normalizer@   Úpowr   r3   r   ÚNotImplementedÚ	TypeErrorÚexpr   r   r   r   r   Zpow_cos_sinr   r   Úrotate_pointr	   ÚOner
   Zscalar_partrF   rP   Zis_purerQ   r   r   rˆ   rŠ   rŒ   rŽ   Zindex_vectorZmensorrE   rk   r,   )r{   rh   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>   rv   rw   rr   é   re   ro   rT   r=   rb   ÚthetaTr^   r   r$   rN   rO   )r   Úto_axis_angler   r   rI   r   r   r	   ZZeror—   r   r   r   r   r   )r;   rš   r5   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)Zhomogeneousr"   F)r   r)   r*   r0   r+   rI   r:   r   )r4   rJ   rK   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   rM   N)r   r   r!   r   r   rI   r   rk   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"   rb   r   rU   rc   Frd   r%   r†   r‡   é   é   é    rƒ   r‚   rg   r   r+   Tr\   zq:4r^   )
r   r   r0   Z_generic_mulr	   r—   Úmulr   r   r   )r;   r5   r7   r+   Zz_quatr4   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,   rq   r,   r.   r/   i  ra   z"test_issue_16318.<locals>.<lambda>r%   r"   r#   r>   r=   rb   rU   ro   )	r   r   r3   r›   r–   r	   r—   r   r   )r4   rP   rQ   r,   rq   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)ZzxyÚzyxÚzyzZzxzc                 S   s   g | ]}|  ¡ ‘qS r,   )Úupper)Ú.0Úseqr,   r,   r.   Ú
<listcomp>{  s    z!test_to_euler.<locals>.<listcomp>)	r   r)   r*   r0   r+   r‘   Úto_eulerr   Ú
from_euler)r4   Zq_normalizedZseqsr©   Zeuler_from_qZ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¦   Trp   N)r   r¬   r!   r   r«   rk   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«   )Zanglesr©   r4   r,   r,   r.   Útest_one_caseŽ  s   z<test_to_euler_numerical_singilarities.<locals>.test_one_caser"   r   r¦   ZZYZr¥   Z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   rm   r   )r4   Zangles1Zangles2Zangle_errorsZangle_error©r©   r,   r.   r¯   ¡  s   ýz,test_to_euler_options.<locals>.test_one_case)ÚxyzZXYZ)TFÚ r   r%   )ro   r   r%   r>   )Úrepeat)r   Újoinr    r   rE   )r¯   r²   Z	seq_tupleZ	symmetricÚelementsr4   r,   r±   r.   Útest_to_euler_optionsŸ  s    	
€ýúÿÿr·   )HZsympy.testing.pytestr   Zsympy.core.functionr   r   Zsympy.core.numbersr   r   r   r   Zsympy.core.singletonr	   Zsympy.core.symbolr
   r   Z$sympy.functions.elementary.complexesr   r   r   r   r   Z&sympy.functions.elementary.exponentialr   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   r   r   r   r   r   Zsympy.integrals.integralsr   Zsympy.matrices.denser   Zsympy.simplifyr   Zsympy.simplify.trigsimpr   Zsympy.algebras.quaternionr   r   r?   Ú	itertoolsr   r    r)   r*   r0   r+   r!   r8   r<   rC   rG   rL   rS   r[   ri   rl   rn   r˜   rœ   r   rž   r£   r¤   r­   r®   r°   r·   r,   r,   r,   r.   Ú<module>   sT     

!&
	