o
    oh=@                     @   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sJ 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-   x/var/www/html/construction_image-detection-poc/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sJ 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sJ 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sJ |ddt   tddddksJ |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sJ 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sJ | d t td td  td  td  dt t dt t dt t ksJ 
d	t td
dtddtddtd	dksJ 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sJ 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sJ 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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sjJ t tttt tksxJ t dddd t ddddksJ t tttt t dtttksJ  t ddtd d dtd d dtd d ksJ  
dt ddddksJ   dksJ |  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 s2J t ddd!d  du s@J t tdd!d  d u sNJ ! dt"td ks]J | ! dt#ttd td  td  t kswJ t $t ddd"ddu sJ t $t dd	d#d$du sJ t $t ddd%d&du sJ t $t dddddu sJ t $t tdd"ddu sJ t $t ddddd u sJ t $t ttttd u sJ 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 sBJ 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 sJ t dddd&t tttd"d u sJ ttfd)d t dddd't dd	dddu sJ t dddd't dddd"d u sJ t dddd't tttd"d u sJ 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s8J t dddd) ttdksJJ * du sSJ * d u s\J t tddd* d u sjJ 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
 
 

&&&
(,
*&&@"
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sJ 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sJ | ddt|d tt|d  fdtt|d  fksJ 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_homogeneous2  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_iss15939  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sJ tddd}t t|t	|dd}t tddd }|| || ksJ || || ksJ 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_multiplicationI  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   `  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_16318]  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>r  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_eulerl  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_iss24504z  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     

!v&
	