U
    Þ@·f‹L  ã                   @   s~  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mZ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  d dl!m"Z" d dl#m$Z$m%Z%m&Z&m'Z' edƒ\Z(Z)Z*edƒ\Z+Z,Z-Z.edƒ\Z/Z0Z1Z2d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@d0S )1é    )Úraises©Ú
CoordSys3D)Ú
BaseScalar)Úexpand)Úpi©Úsymbols)ÚcoshÚsinh)Úsqrt)ÚacosÚatan2ÚcosÚsin)Úzeros)ÚImmutableDenseMatrix)Úsimplify)Úexpress)ÚPoint)ÚVector)ÚAxisOrienterÚBodyOrienterÚSpaceOrienterÚQuaternionOrienterúx y zza b c qzq1 q2 q3 q4c                  C   sº   t dƒ} | jj| jjŽ | jks"t‚d| j d| j  }|j|jŽ |ksJt‚| jj| jjŽ | jksdt‚| j| j | j| j  | j| j	  }|j|jŽ |ksœt‚| j
j| j
jŽ | j
ks¶t‚d S )NÚAé   é   )r   ÚxÚfuncÚargsÚAssertionErrorÚyÚiÚjÚzÚkÚorigin)r   ÚexprÚv© r+   úH/tmp/pip-unpacked-wheel-6uje5nh9/sympy/vector/tests/test_coordsysrect.pyÚtest_func_args   s    $r-   c                  C   s4   t dƒ} t dƒ}|| kst‚t dƒ}| |ks0t‚d S )Nr   ÚB)r   r"   )r   ÚA1r.   r+   r+   r,   Útest_coordsys3d_equivalence$   s
    r0   c               	   C   sj  t dƒ} tt| jƒ}ttttdƒ}ttttdƒ}tt	t
ttƒ}| | ¡tttƒttƒdgttƒ ttƒdgdddggƒks~t‚| ¡ tttƒttƒ ttƒttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒttƒ ttƒ  gttƒ ttƒ ttƒ ttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒttƒ ttƒ  gttƒttƒ ttƒ ttƒttƒ ggƒksvt‚| ¡ tttƒttƒ ttƒttƒ ttƒ gttƒttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒ ttƒttƒ  ttƒttƒ gttƒttƒ ttƒttƒ ttƒ  ttƒ ttƒ ttƒttƒ ttƒ  ttƒttƒ ggƒkslt‚| ¡ tt	d t
d  td  td  dt	 t dt
 t  dt	 t dt
 t  gdt	 t dt
 t  t	d t
d  td  td  dt	 t
 dt t  gdt	 t dt
 t  dt	 t
 dt t  t	d t
d  td  td  ggƒksft‚d S )Nr   Ú123r   é   é   éþÿÿÿ)r   r   Úar'   r   ÚbÚcr   r   Úq1Úq2Úq3Úq4Úrotation_matrixÚMatrixr   r   r"   )r   Zaxis_orienterZbody_orienterZspace_orienterZ
q_orienterr+   r+   r,   Útest_orienters,   sZ    
ý4&ÿ8&ÿÿû&&& ÿ6ÿÿü4ÿ4ÿ ÿûr>   c               
   C   s¼  t dƒ} td| ddƒ| jks t‚td| ddƒ| jks8t‚td| d	d
ƒ| jksPt‚td| ddƒ ¡ | j ¡ kspt‚t| jtƒr”t| jtƒr”t| jtƒs˜t‚| j| j | j| j ks´t‚|  | ¡| j| j| j| j| j| jiksÞt‚| jj	| ksît‚| j 
| j¡dkst‚|  dt| j¡}| | ¡|j| j|j| j ttƒ | jttƒ  |j| jttƒ | jttƒ  ikspt‚|  |¡| j|jttƒ |jttƒ  | j|jttƒ |jttƒ  | j|jiksÈt‚t|j| dd| jttƒ | jttƒ  ksút‚t|j| dd| j ttƒ | jttƒ  ks.t‚t|j| dd| jksJt‚tt|j|j |j | ddƒt| j| j ttƒ | jttƒ   | jttƒ | jttƒ   ƒks´t‚t|j|j |j|j  |j|j  | ƒ|jttƒ |jttƒ  | j |jttƒ |jttƒ  | j  |j| j  ks4t‚tt|j|j |j|j  |j|j  | ddƒ| j| j | j| j  | j| j  kst‚t| j| j | j| j  | j| j  |ƒ| jttƒ | jttƒ  |j | j ttƒ | jttƒ  |j  | j|j  kst‚tt| j| j | j| j  | j| j  |ddƒ|j|j |j|j  |j|j  ksnt‚| dt |j¡}| | ¡|j| j|j| j|j| jiks¬t‚|  dt| j| j | j ¡}|  |¡}|| j  |jdttƒ d  d |jdtttd  ƒ d  d  |jdtttd  ƒ d  d  ¡sBt‚|| j  |jdtttd  ƒ d  d |jdttƒ d  d  |jdtttd  ƒ d  d  ¡s²t‚|| j  |jdtttd  ƒ d  d |jdtttd  ƒ d  d  |jdttƒ d  d  ¡s"t‚|  dt| j t| j  t| j  ¡}| | ¡|j| jt |j| jt |j| jt iks€t‚|  dt| jt| j t| j  t| j  ¡}|  |¡| j|jt | j|jttƒ |jttƒ  t | j|jttƒ |jttƒ  t ikst‚| | ¡|j| jt ttƒ | jt ttƒ  |j| j t ttƒ | jt ttƒ  |j| jt iks~t‚|  dtj¡}|  |¡| j|j| j|j| j|jiks¸t‚dS )zo
    Tests the coordinate variables functionality with respect to
    reorientation of coordinate systems.
    r   r   ZA_xz\mathbf{{x}_{A}}r2   ZA_yz\mathbf{{y}_{A}}r3   ZA_zz\mathbf{{z}_{A}}r.   T)Ú	variablesÚNÚCr   r4   é   ÚDÚEÚFN)r   r   r   r"   r#   r&   Ú__hash__Ú
isinstanceZ
scalar_mapÚsystemZdiffÚorient_new_axisÚqr'   r   r   r   r   r$   r%   r   Úequalsr   Ú
locate_newr5   r6   r7   r   Úzero)r   r.   r@   rA   ÚmappingrC   rD   rE   r+   r+   r,   Útest_coordinate_varsL   s¾     
ÿ
þ*0 ÿ&   ÿ24Bÿ
(,ÿÿ
ÿÿ
(ÿ"þ
( "ÿ
ÿÿ
(ÿ"þ
ÿ

" ÿ þ*ÿ þ* ÿþ&8,  þ. $ þrO   c            	      C   s^  t dƒ} |  dt| j¡}| dt|j¡}| dt|j¡}|  dt| j¡}|  	dtttd¡}|  
dtttt¡}|  d	tttd¡}|  |¡tttƒ ttƒ ttƒ ttƒttƒ  ttƒ ttƒ ttƒttƒ ttƒ ttƒttƒ  gttƒttƒ ttƒttƒ ttƒ  ttƒttƒ ttƒttƒ ttƒttƒ ttƒ  gttƒ ttƒ ttƒttƒttƒ ggƒksxt‚| |¡tttƒttƒ ttƒ ttƒttƒ ttƒ ttƒttƒ ttƒ    ttƒ ttƒ ttƒttƒ ttƒ  ttƒttƒ ttƒ ttƒttƒ ttƒ ttƒttƒ ttƒ    gttƒttƒ ttƒttƒ ttƒ  ttƒttƒ ttƒttƒ ttƒttƒ ttƒ  gttƒttƒ ttƒ ttƒttƒttƒ ttƒttƒ ttƒ    ttƒttƒ ttƒttƒ ttƒ  ttƒttƒ ttƒ ttƒttƒttƒ ttƒttƒ ttƒ    ggƒ }| ¡ td
d
ƒkslt‚| | ¡tttƒttƒ ttƒttƒ ttƒ gttƒttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒ ttƒttƒ  ttƒttƒ gttƒttƒ ttƒttƒ ttƒ  ttƒ ttƒ ttƒttƒ ttƒ  ttƒttƒ ggƒksdt‚| | ¡ttd td  td  td  dt t dt t  dt t dt t  gdt t dt t  td td  td  td  dt t dt t  gdt t dt t  dt t dt t  td td  td  td  ggƒks`t‚| | ¡tttƒttƒ ttƒttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒttƒ ttƒ  gttƒ ttƒ ttƒ ttƒ ttƒ ttƒttƒ  ttƒttƒ ttƒttƒ ttƒ  gttƒttƒ ttƒ ttƒttƒ ggƒksZt‚d S )Nr@   r   r.   rA   rC   rD   r1   rE   ÚGr   r3   r4   )r   rI   r8   r'   r9   r$   r:   r%   r;   Úorient_new_spaceÚorient_new_quaternionÚorient_new_bodyr<   r=   r   r   r"   r   r   )	r@   r   r.   rA   rC   rD   rE   rP   Ztest_matr+   r+   r,   Útest_rotation_matrix   sØ    
0ÿ&ÿ& ÿÿ&û
,ÿÿÿ(ÿþ,ÿ&ÿ*ÿÿÿÿÿÿÿÿýúÿ
&&& ÿ&ÿÿÿÿýÿ
 þ ÿþü
 &&þ (&þ  ÿürT   c                  C   sô  t dƒ} |  dt| j¡}| dt|j¡}| dt|j¡}t| j t	| j  t
| j  }| |¡ttttƒ t	ttƒ  gt ttƒ t	ttƒ  gt
ggƒks¢t‚| j |j¡ttƒks¼t‚| j |j¡ttƒ ksØt‚| j |j¡dksît‚| j |j¡ttƒks
t‚| j |j¡ttƒks&t‚| j |j¡dks>t‚| j |j¡dksVt‚| j |j¡dksnt‚| j |j¡dks†t‚| j |j|j ¡ttƒ ttƒ   krÌ|j|j  | j¡ksÒn t‚|j |j¡ttƒksît‚|j |j¡dkst‚|j |j¡ttƒks"t‚|j |j¡ttƒttƒ ksFt‚|j |j¡ttƒksbt‚|j |j¡ttƒ ttƒ ksˆt‚|j |j¡ttƒ ttƒ ks®t‚|j |j¡ttƒksÊt‚|j |j¡ttƒttƒ ksît‚| j |j¡ttƒ|j kst‚| j |j¡ttƒ|j ks2t‚| j |j¡ttƒ |j ttƒ|j  ksdt‚| j |j¡ttƒ |j ksˆt‚| j |j¡ttƒ|j ksªt‚| j |j¡ttƒ|j ttƒ|j  ksÚt‚| j |j¡|jksôt‚| j |j¡|j kst‚| j |j¡tjks*t‚| j |j¡ttƒ|j ksLt‚| j |j¡ttƒ|j ksnt‚| j |j|j ¡ttƒ|j ttƒ|j  ks¤t‚|j|j  | j¡ttƒ ttƒ | j ksÖt‚|j |j¡ttƒ|j ksøt‚|j |j¡ttƒ |j ttƒ|j  ks*t‚|j |j¡ttƒ |j ksNt‚|j |j¡ttƒ ttƒ |j ttƒ ttƒ |j  ks’t‚|j |j¡ttƒ|j ttƒ |j  ksÄt‚t|j |j¡|ƒ ¡ ttƒ|j ksðt‚dS )z`
    Tests the effects of orientation of coordinate systems on
    basic vector operations.
    r@   r   r.   rA   r   r2   N)r   rI   r8   r'   r9   r$   r:   r%   r5   r6   r7   Z	to_matrixr=   r   r   r"   ÚdotZcrossr   rM   r   Ztrigsimp)r@   r   r.   rA   Zv1r+   r+   r,   Útest_vector_with_orientationÁ   sf    "þ,ÿ$&&$""2$"0""62"2$"ÿ2rV   c            	      C   sà   t dƒ} tt| jƒ}ttttdƒ}tttttƒ}t	tttdƒ}|  
d|f¡}|  
d|f¡}|  
d|f¡}|  
d|f¡}||  dt| j¡ksŽt‚||  dtttd¡ks¨t‚||  dtttt¡ksÂt‚||  dtttd¡ksÜt‚d S )Nr@   r1   rC   rD   rE   rP   )r   r   r;   r%   r   r8   r9   r:   r   r   Z
orient_newrI   r"   rQ   rR   rS   )	r@   Z	orienter1Z	orienter2Z	orienter3Z	orienter4rC   rD   rE   rP   r+   r+   r,   Útest_orient_new_methods  s    rW   c                  C   sÒ  t dƒ} t| jtƒst‚t| j t| j  t	| j
  }|  d|¡}|j | ¡| | ¡  krx|j | j¡  krx|ks~n t‚| j |¡|  |¡  kr¶| j |j¡  kr¶| ks¼n t‚| j |¡t t t	 fksÜt‚| j d| ¡}| | ¡t t t	 fkst‚| |j¡| |¡  kr6d| ks<n t‚| dd| ¡}| |j¡tjksdt‚| |¡dksxt‚| d| j¡}| |¡d| | j ks¤t‚| |¡dt d	 dt dt	 fksÎt‚d
S )zA
    Tests Point class, and locate_new method in CoordSys3D.
    r   rA   Úpr4   Úp1r3   )r   r   r   Úp2r2   N)r   rG   r(   r   r"   r5   r$   r6   r%   r7   r'   rL   Zposition_wrtZexpress_coordinatesr   rM   )r   r*   rA   rX   rY   rZ   r+   r+   r,   Útest_locatenew_point  s<    
ÿþþ
ÿþþ  ÿr[   c                  C   sÜ   t dƒ} | jddd}|j| ks$t‚| ¡ |jt|jƒ t|j	ƒ |jt|jƒ t|j	ƒ |jt|jƒ fksrt‚| 
¡ t| jd | jd  | jd  ƒt| jt| jd | jd  | jd  ƒ ƒt| j| jƒfksØt‚d S )Nr5   r7   Ú	spherical©Útransformationr3   )r   Z
create_newÚ_parentr"   Útransformation_to_parentÚrr   Úthetar   ÚphiZtransformation_from_parentr   r   r#   r&   r   r   )r5   r7   r+   r+   r,   Útest_create_new-  s    @ÿXÿrd   c                  C   sb   t dƒ} d| j d| j  t| j  }| ¡ | ¡ ks:t‚|jtdid| td¡ ¡ ks^t‚d S )Nr   r   r   r2   )Úsubs)	r   r$   r%   r5   r'   ÚnZevalfr"   re   )r   r*   r+   r+   r,   Ú
test_evalf7  s    rg   c                  C   s†   t ddƒ} |  ¡ d| jt| jƒ| j fks.t‚t dƒ} |  ¡ dksFt‚t ddƒ} |  ¡ dks`t‚t ddƒ} |  ¡ d| jdfks‚t‚d S )Nr5   r\   r2   ©r2   r2   r2   Ú	cartesianÚcylindrical)r   Úlame_coefficientsra   r   rb   r"   ©r5   r+   r+   r,   Útest_lame_coefficients>  s    
$

rm   c                     s  t dƒ\‰‰‰tdddddgd‰ ˆ  ¡ \} }}| ˆ jks>t‚|ˆ jksLt‚|ˆ jksZt‚tt‡ fdd	„ƒ tt‡ fd
d	„ƒ tt‡ fdd	„ƒ ˆ  	¡ | t
|ƒ t|ƒ | t
|ƒ t
|ƒ | t|ƒ fksÎt‚ˆ  ¡ d| | t
|ƒ fksìt‚ˆ  ¡ ˆˆˆƒtˆd ˆd  ˆd  ƒtˆtˆd ˆd  ˆd  ƒ ƒtˆˆƒfksJt‚tdddddgd‰ ˆ  ¡ \} }‰ˆ  	¡ | t|ƒ | t
|ƒ ˆfks”t‚ˆ  ¡ dˆ jdfks®t‚ˆ  ¡ ˆˆˆƒtˆd ˆd  ƒtˆˆƒˆfksät‚tddƒ‰ ˆ  	¡ ˆ jˆ jˆ jfkst‚ˆ  ¡ dkst‚ˆ  ¡ ˆˆˆƒˆˆˆfks>t‚t dƒ\‰‰‰tdˆˆˆfˆˆˆffƒ‰ ˆ  ¡  ˆ  	¡ ˆ jˆ jˆ jfksŒt‚ˆ  ¡ dksžt‚ˆ  ¡ ˆˆˆƒˆˆˆfks¾t‚t dƒ\} }‰td| |ˆf| t|ƒ | t
|ƒ ˆfgdddgd‰ ˆ  ¡ \} }‰ˆ  	¡ | t|ƒ | t
|ƒ ˆfks6t‚ˆ  ¡ tt
|ƒd t|ƒd  ƒt| d t
|ƒd  | d t|ƒd   ƒdfksŽt‚tddd	„ ƒ‰ ˆ  	¡ ˆ jˆ jˆ jfksºt‚ˆ  ¡ dksÌt‚ˆ  ¡  ˆ  ¡ ˆˆˆƒˆˆˆfksôt‚tddd	„ dddgd‰ ˆ  ¡ \} }}ˆ  	¡ | t
|ƒ t|ƒ | t
|ƒ t
|ƒ | t|ƒ fksZt‚ˆ  ¡ tt
|ƒd t
|ƒd  t
|ƒd t|ƒd   t|ƒd  ƒt| d t
|ƒd  t|ƒd  | d t
|ƒd   | d t|ƒd  t|ƒd   ƒt| d t
|ƒd  t
|ƒd  | d t
|ƒd  t|ƒd   ƒfksBt‚tddd	„ dddgd‰ ˆ  ¡ \} }‰ˆ  	¡ | t|ƒ | t
|ƒ ˆfkst‚ˆ  ¡ tt
|ƒd t|ƒd  ƒt| d t
|ƒd  | d t|ƒd   ƒdfksèt‚tt‡‡‡fdd	„ƒ d S )Nr   r5   r\   ra   rb   rc   )r^   Úvariable_namesc                      s   ˆ j S ©N)r   r+   rl   r+   r,   Ú<lambda>U  ó    z/test_transformation_equations.<locals>.<lambda>c                      s   ˆ j S ro   )r#   r+   rl   r+   r,   rp   V  rq   c                      s   ˆ j S ro   )r&   r+   rl   r+   r,   rp   W  rq   r2   r3   rj   r&   ri   rh   z	r theta z)rn   c                 S   s
   | ||fS ro   r+   ©r   r#   r&   r+   r+   r,   rp     rq   c                 S   s2   | t |ƒ t|ƒ | t |ƒ t |ƒ | t|ƒ fS ro   )r   r   ©ra   rb   rc   r+   r+   r,   rp   —  rq   c                 S   s   | t |ƒ | t|ƒ |fS ro   )r   r   )ra   rb   r&   r+   r+   r,   rp   ¤  s    c                
      s@   t dˆ ˆ tˆƒ tˆƒ ˆˆ tˆƒ tˆƒ ˆˆ tˆƒ idS ©Nr5   r]   ©r   r   r   r+   rr   r+   r,   rp   °  s        
ÿ)r	   r   Úbase_scalarsra   r"   rb   rc   r   ÚAttributeErrorr`   r   r   rk   Z#transformation_from_parent_functionr   r   r   r   r#   r&   Z_calculate_inv_trans_equationsÚx1Zx2Zx3Ú	TypeErrorrs   r+   )r5   r   r#   r&   r,   Útest_transformation_equationsI  s¶    ÿ
ý
"ýÿ

ý  ÿ
  &ÿ
 
 ÿ*ý	 
ÿ  
ÿ>VBý
þ(*ýrz   c                     s*  t dƒ\‰ ‰‰t dƒ\} }tdˆ ˆˆfˆ tˆƒ tˆƒ ˆ tˆƒ tˆƒ ˆ tˆƒ ffd}| |j¡dksrt‚tdˆ ˆˆfˆ tˆƒ ˆ tˆƒ ˆffd}| |j¡dks²t‚td| |ˆft| ƒt|ƒ t| ƒt|ƒ ˆffd}| |j¡dksút‚t	t
‡ ‡‡fdd„ƒ t	t
‡ ‡‡fdd„ƒ d S )	Nr   zu, vr5   r]   Tc                      s   t dˆ ˆˆfˆ ˆ ˆffdS rt   r   r+   rr   r+   r,   rp   ¾  rq   z*test_check_orthogonality.<locals>.<lambda>c                      sH   t dˆ ˆˆfˆ tˆd ƒ tˆƒ ˆ tˆƒ tˆƒ ˆ tˆƒ ffdS )Nr5   r3   r]   ru   r+   rr   r+   r,   rp   ¿  s    4ÿ)r	   r   r   r   Z_check_orthogonalityZ_transformationr"   r
   r   r   Ú
ValueError)Úur*   r5   r+   rr   r,   Útest_check_orthogonality´  s    D,4r}   c                  C   sŽ  t dƒ} ddlm} |dƒ}|  | j|  ¡ ¡| j| j| jfksBt	‚|  |  
¡ |  ¡ ¡| j| j| jfksjt	‚|  dd| j ¡}| |j| ¡ ¡|j|j|jfks¢t	‚| | 
¡ | ¡ ¡|j|j|jfksÊt	‚|  d|| j ¡}| |j| ¡ ¡t|ƒ |j t|ƒ|j  t|ƒ|j t|ƒ|j  |jfks2t	‚| | 
¡ | ¡ ¡t|ƒ|j t|ƒ|j  t|ƒ |j t|ƒ|j  |jfksŠt	‚d S )Nr5   r   r   Úq0r6   r7   )r   Úsympy.core.symbolr	   Z_rotation_trans_equationsZ_parent_rotation_matrixrv   r   r#   r&   r"   Z_inverse_rotation_matrixrI   r'   r   r   )r5   r	   r~   r6   r7   r+   r+   r,   Útest_rotation_trans_equationsÃ  s    &(&(<ÿ
<ÿr€   N)AZsympy.testing.pytestr   Zsympy.vector.coordsysrectr   Zsympy.vector.scalarr   Zsympy.core.functionr   Zsympy.core.numbersr   r   r	   Z%sympy.functions.elementary.hyperbolicr
   r   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   r   r   r   Zsympy.matrices.denser   Zsympy.matrices.immutabler   r=   Zsympy.simplify.simplifyr   Zsympy.vector.functionsr   Zsympy.vector.pointr   Zsympy.vector.vectorr   Zsympy.vector.orientersr   r   r   r   r   r#   r&   r5   r6   r7   rJ   r8   r9   r:   r;   r-   r0   r>   rO   rT   rV   rW   r[   rd   rg   rm   rz   r}   r€   r+   r+   r+   r,   Ú<module>   s@    C2A
k