a
    ¬<b¨$  ã                   @   s4  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 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$ edƒ\Z%Z&Z'Z(edƒZ)dd„ Z*dd„ Z+dd„ Z,dd„ Z-dd„ Z.dd„ Z/dd„ Z0dd„ Z1d d!„ Z2d"d#„ Z3d$S )%é    )Údiff)ÚEÚIÚRationalÚpi)ÚS)ÚSymbolÚsymbols)ÚAbsÚ	conjugateÚimÚreÚsign)Úlog)Úsqrt)ÚacosÚasinÚcosÚsin)Ú	integrate)ÚMatrix)Útrigsimp)Ú
Quaternion)Úraisesz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%   út/Users/vegardjervell/Documents/master/model/venv/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Pr2   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   r3   r   r   )r   r   r   r   )r   r   r   r   ©r   r   r   r   ©r   r   r   r   ©r   r   r   r   r   )r   r   r   r,   r   r+   )Z	test_dataÚaxisÚangleÚexpectedr%   r%   r'   Útest_quaternion_axis_angle#   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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+   r   Úar   Úbr   r   ÚcÚd)Úresultr%   r%   r'   Ú)test_quaternion_axis_angle_simplification5   s
    &&rC   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 )Nr>   T©Úcomplexr?   ©Úrealr@   Fr    r   c                      s   ˆ ˆ S r!   r%   r%   ©r@   r.   r%   r'   r(   G   r)   z7test_quaternion_complex_real_addition.<locals>.<lambda>c                      s   ˆˆ  S r!   r%   r%   rH   r%   r'   r(   H   r)   c                      s   ˆ ˆ S r!   r%   r%   rH   r%   r'   r(   I   r)   r   r;   r   é   r   r=   é   ©Z
real_fieldiöÿÿÿé   é&   éûÿÿÿ)r	   r   r"   r#   r*   r$   r   r   r   r   r-   )r>   r?   Úq1r/   Úq0r%   rH   r'   Ú%test_quaternion_complex_real_addition<   s2    *"*<$Hÿ
$rQ   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   Zevalfr%   r%   r%   r'   Útest_quaternion_evalf]   s    <rR   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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„ƒ d S )Nr   r   r   r;   r   éÿÿÿÿc                      s   ˆ   ¡ S r!   )Úinverser%   ©rP   r%   r'   r(   l   r)   z+test_quaternion_functions.<locals>.<lambda>éþÿÿÿiùÿÿÿéá   é–   ç      à¿c                      s   ˆ d S )NrY   r%   r%   )rO   r%   r'   r(   r   r)   é   é   é<   éZ   éx   r6   rI   r=   Únc                      s   ˆˆ  S r!   r%   r%   ©r_   rO   r%   r'   r(   Œ   r)   T)Úintegerc                      s   ˆˆ  S r!   r%   r%   r`   r%   r'   r(   Ž   r)   )r   r"   r#   r*   r$   r   Znormr   Ú	normalizerT   Úpowr   r-   r   ÚNotImplementedÚ	TypeErrorÚexpr   r   r   Z_lnr   r   Zpow_cos_sinr   r   Úrotate_pointr   ÚOner   ©r.   r%   )r_   rP   rO   r'   Útest_quaternion_functionsb   sZ     0>@RP64ýÿ
ýÿ
&&&ýÿ
(,ÿ
*rj   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;   rZ   r[   rV   é   rL   rS   r<   r6   rI   ÚthetaTrF   r   r   r4   r5   )r   Úto_axis_angler   r   Úto_rotation_matrixr   r   r   ZZerorh   r	   r   r   r   r
   )rO   rl   r/   r%   r%   r'   Útest_quaternion_conversions‘   s@    þý
"þ,"ýý&ÿ$$üro   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   r3   N)r   r   r   r   r   rn   r   ri   r%   r%   r'   Ú test_quaternion_rotation_iss1593´   s    	ýrp   c                  C   s\  t ddt  ddt  dddt  dd	} t d
dddƒ}t d
d
d
tƒ}t  dd
¡dksZJ ‚t  d| ¡t ddt  ddt  dddt  ƒksJ ‚| 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sBJ ‚|| || ksXJ ‚d S )Nr   r;   r   rI   r   r=   rJ   FrK   r   é   é   é   é   é    é   é
   rN   é	   r$   TrD   zq:4rF   )r   r   r*   Z_generic_mulÚmulr	   r   r   )rO   r/   r1   r$   Zz_quatr.   r%   r%   r'   Útest_quaternion_multiplicationÄ   s    *68rz   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%   rU   r%   r'   r(   Ú   r)   z"test_issue_16318.<locals>.<lambda>r   r   r   r;   r6   rI   r=   rS   )	r   r   r-   rm   rg   r   rh   r   r   )r.   r7   r8   r%   rU   r'   Útest_issue_16318×   s    ,(r{   N)4Zsympy.core.functionr   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   Zsympy.integrals.integralsr   Zsympy.matrices.denser   Zsympy.simplify.trigsimpr   Zsympy.algebras.quaternionr   Zsympy.testing.pytestr   r"   r#   r*   r$   r   r2   r:   rC   rQ   rR   rj   ro   rp   rz   r{   r%   r%   r%   r'   Ú<module>   s0   !/#