a
    kº”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               
      s:  t ttttƒ} | |  t dt dt dt dt ƒks8J ‚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sŠJ ‚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,   úR/var/www/auris/lib/python3.9/site-packages/sympy/algebras/tests/test_quaternion.pyÚ<lambda>&   ó    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@ÿr9   c                  C   s|   t tdƒŽ } t ttttƒ}t| |  ¡ d |  ¡ d | ¡ d   ƒdksNJ ‚t ttttdd}| |  ¡ |  ¡ ksxJ ‚d S )Núa:dr"   r   r%   )Únorm)r   r   r)   r*   r1   r+   r   r;   )Úq1r6   r8   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 sLJ ‚d S )Nr%   r   ©r%   r%   r%   r#   é   g»½×Ùß|Û=)r   r2   Úmathr   Úinverser;   )Úpr5   ZqiÚtestr,   r,   r.   Útest_issue_252543   s
    rD   c                  C   sT   t ttttƒ} t  |  ¡ ¡}t  |  d¡¡}| |  ¡ s<J ‚|  ¡ |  ¡ sPJ ‚d S )NT)	r   r)   r*   r1   r+   Zfrom_MatrixÚ	to_MatrixÚis_zero_quaternionÚvector_part)r5   Zq_fullZq_vectr,   r,   r.   Útest_to_and_from_Matrix=   s
    rH   c                  C   sœ   t ttttƒ} t tdƒŽ }| |  ¡ | j| ¡  ks8J ‚| |  ¡ |j|  ¡  ksVJ ‚| j| jj	 dd …dd …f }t
|  ¡ |  ¡ d  ƒ}||ks˜J ‚d S )Nr:   r%   r"   )r   r)   r*   r1   r+   r   rE   Zproduct_matrix_leftZproduct_matrix_rightÚTr   Úto_rotation_matrixr;   )r<   r6   ÚR1ÚR2r,   r,   r.   Útest_product_matricesE   s    rM   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 ‚qôd S )N)©r%   r   r   r   )r%   r   r   r   rN   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   )Z	test_dataÚaxisÚangleÚexpectedr,   r,   r.   Útest_quaternion_axis_angleP   s    $$$(>÷rT   c                  C   s¤   t  dtdƒ¡} | jttdƒd ƒks*J ‚| jtdƒttdƒd ƒ d ksPJ ‚| jtdƒttdƒd ƒ d ksvJ ‚| j	dtdƒ ttdƒd ƒ d ks J ‚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
    &&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szJ ‚tˆ ttd	t ttƒks˜J ‚|ˆ 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s0J ‚td	dddƒ}tddddƒ}|| |ks^J ‚|| |kspJ ‚|| |ks‚J ‚d S )NrW   T©ÚcomplexrX   ©ÚrealrY   Fr'   r%   c                      s   ˆ ˆ S r(   r,   r,   ©rY   r5   r,   r.   r/   t   r0   z7test_quaternion_complex_real_addition.<locals>.<lambda>c                      s   ˆˆ  S r(   r,   r,   ra   r,   r.   r/   u   r0   c                      s   ˆ ˆ S r(   r,   r,   ra   r,   r.   r/   v   r0   r#   r?   r"   é   r   rV   é   ©Z
real_fieldiöÿÿÿé   é&   éûÿÿÿ)r   r   r)   r*   r1   r+   r   r   r   r   r4   )rW   rX   r<   r6   Úq0r,   ra   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   r2   r!   Úsubs©r5   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sˆJ ‚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sJJ ‚|  ¡ ttd td  td  td  ƒkszJ ‚|  ¡ 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stJ ‚| 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sBJ ‚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sÐJ ‚ˆ ¡ 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sHJ ‚ˆ 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sZJ ‚t dttd td ƒ t¡t ttd d td d td d ƒks¢J ‚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sfJ ‚ˆ ¡ t ddtdƒ d dtdƒ d dtdƒ d ƒks¦J ‚ˆ ¡  
d¡t ddddƒksÈJ ‚ˆ ¡  ¡ dksÞJ ‚|  ¡ 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sNJ ‚ˆ  ¡ du s`J ‚ˆ  ¡ d u srJ ‚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 s8J ‚t  $ˆt dd	d#d$ƒ¡du sXJ ‚t  $ˆt ddd%d&ƒ¡du sxJ ‚t  $ˆt ddddƒ¡du s˜J ‚t  $ˆt tdd"dƒ¡du s¸J ‚t  $ˆt ddddƒ¡d u sØJ ‚t  $ˆt ttttƒ¡d u søJ ‚tt‡‡fd'd„ƒ t  %t ddd%d&ƒt ddd"dƒt ddddƒ¡du 	sBJ ‚t  %t ddddƒt ddd"dƒt ddddƒ¡du 	sxJ ‚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 
sHJ ‚t ddddƒ &t tttd"ƒ¡d u 
spJ ‚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 
süJ ‚tt‡‡fd*d„ƒ ˆ (¡ t ddtd+ƒ d dtd+ƒ d dtd+ƒ d ƒksPJ ‚t ddd,dƒ (¡ t ddd,dƒksvJ ‚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(   ©rA   r,   ©rh   r,   r.   r/       r0   z+test_quaternion_functions.<locals>.<lambda>éþÿÿÿiùÿÿÿéá   é–   ç      à¿c                      s   ˆ d S )Nru   r,   r,   )r<   r,   r.   r/   ¨   r0   é   é   é<   éZ   éx   r>   rb   rV   Únc                      s   ˆˆ  S r(   r,   r,   ©r{   r<   r,   r.   r/   Ë   r0   T)Úintegerc                      s   ˆˆ  S r(   r,   r,   r|   r,   r.   r/   Í   r0   é   é   é7   rc   Fé
   é   éýÿÿÿéüÿÿÿé   é   c                      s   t  ˆˆ ¡S r(   )r   Úarc_coplanarr,   ©rh   r<   r,   r.   r/   í   r0   c                      s   t  ˆ t ddddƒˆ¡S )Nr   r?   r‚   rc   )r   Úvector_coplanarr,   rˆ   r,   r.   r/   û   s    c                      s
   ˆ   ˆ¡S r(   )Úparallelr,   rˆ   r,   r.   r/     r0   c                      s
   ˆ   ˆ¡S r(   )Ú
orthogonalr,   rˆ   r,   r.   r/     r0   if  é	   éz   )+r   r)   r*   r1   r+   r   r;   r   Ú	normalizerA   Úpowr   r4   r   ÚNotImplementedÚ	TypeErrorÚexpr   r   r   r   r   Zpow_cos_sinr   r   Úrotate_pointr	   ÚOner
   Zscalar_partrG   rQ   Úis_purerR   r   r   r‡   r‰   rŠ   r‹   Zindex_vectorZmensorrF   rk   r,   )r{   rh   r<   r.   Útest_quaternion_functions–   s    0>@RP
 ÿ ÿýÿ
ýÿ
&&&ýÿ
(,ÿ
$ÿ
*0ÿ
(Dÿ
*&&@"
ý4       ýý
$ÿÿ
$ÿÿ
ýý
((((((ý&$$r–   c               
   C   sn  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sJJ ‚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sjJ ‚d S )Nr%   r"   r#   r?   rv   rw   rr   é   re   ro   rU   r>   rb   ÚthetaTr_   r   r$   rO   rP   )r   Úto_axis_angler   r   rJ   r   r   r	   ZZeror”   r   r   r   r   r   )r<   r˜   r6   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sNJ ‚d S )NT)Zhomogeneousr"   F)r   r)   r*   r1   r+   rJ   r;   r   )r5   rK   rL   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   rN   N)r   r   r!   r   r   rJ   r   rk   r,   r,   r.   Ú test_quaternion_rotation_iss1593B  s    	ýrœ   c                  C   sh  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s`J ‚t  tdƒ| ¡t ddt  ddt  dddt  ƒksšJ ‚| d¡t ddddƒks¶J ‚| |¡t dt d dt d ddt  td ƒksîJ ‚| |¡|| ksJ ‚tddd}t t|ƒt	|ƒddƒ}t tdddŽ }|| || ksNJ ‚|| || ksdJ ‚d S )Nr#   r?   r"   rb   r   rV   rc   Frd   r%   r…   r†   é   é   é    r‚   r   rg   rŒ   r+   Tr]   zq:4r_   )
r   r   r1   Z_generic_mulr	   r”   Úmulr   r   r   )r<   r6   r8   r+   Zz_quatr5   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sfJ ‚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  r0   z"test_issue_16318.<locals>.<lambda>r%   r"   r#   r?   r>   rb   rV   ro   )	r   r   r4   r™   r“   r	   r”   r   r   )r5   rQ   rR   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s4J ‚q4d S )N)ZzxyÚzyxÚzyzZzxzc                 S   s   g | ]}|  ¡ ‘qS r,   )Úupper)Ú.0Úseqr,   r,   r.   Ú
<listcomp>{  r0   z!test_to_euler.<locals>.<listcomp>)	r   r)   r*   r1   r+   rŽ   Úto_eulerr   Ú
from_euler)r5   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s2J ‚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§   r5   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 ]v}t |ƒD ]h}dD ]^}|rJd |d |d |d g¡‰ n
d |¡‰ tg d¢d	d
D ]}t|Ž }| ¡ sd| |ƒ qdq$qqd S )Nc                    sb   t |  ˆ dd¡ƒ}t |  ˆ dd¡ƒ}t|| ƒ ¡ }|D ]$}|t dt  t }|dk s8J ‚q8d S )NTFr"   gíµ ÷Æ°>)r   r©   r   rm   r   )r5   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   rF   )r­   r°   Z	seq_tupleZ	symmetricÚelementsr5   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*   r1   r+   r!   r9   r=   rD   rH   rM   rT   r\   ri   rl   rn   r–   rš   r›   rœ   r¡   r¢   r«   r¬   r®   rµ   r,   r,   r,   r.   Ú<module>   sR    

!&
	