o
    GZŽh‹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J ‚d| j d| j  }|j|jŽ |ks%J ‚| jj| jjŽ | jks2J ‚| j| j | j| j  | j| j  }|j|jŽ |ksNJ ‚| j	j| j	jŽ | j	ks[J ‚d S )NÚAé   é   )
r   ÚxÚfuncÚargsÚyÚiÚjÚzÚkÚorigin)r   ÚexprÚv© r*   úS/var/www/auris/lib/python3.10/site-packages/sympy/vector/tests/test_coordsysrect.pyÚtest_func_args   s   $r,   c                  C   s4   t dƒ} t dƒ}|| ksJ ‚t dƒ}| |ksJ ‚d S )Nr   ÚBr   )r   ÚA1r-   r*   r*   r+   Útest_coordsys3d_equivalence$   s
   r/   c               	   C   sf  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g d¢gƒks>J ‚| ¡ 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s¹J ‚| ¡ 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s4J ‚| ¡ 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±J ‚d S )Nr   Ú123r   )r   r   é   é   éþÿÿÿ)r   r   Úar&   r   ÚbÚcr   r   Úq1Úq2Úq3Úq4Úrotation_matrixÚMatrixr   r   )r   Zaxis_orienterZbody_orienterZspace_orienterZ
q_orienterr*   r*   r+   Útest_orienters,   sV   
ý4&ÿ8&ÿÿû&&4ÿ6ÿÿü4ÿ4ÿ4ÿûr=   c               
   C   s´  t dƒ} td| ddƒ| jksJ ‚td| ddƒ| jksJ ‚td| d	d
ƒ| jks(J ‚td| ddƒ ¡ | j ¡ ks8J ‚t| jtƒrJt| jtƒrJt| jtƒsLJ ‚| j| j | j| j ksZJ ‚|  | ¡| j| j| j| j| j| jiksoJ ‚| jj| kswJ ‚| j 	| j¡dks‚J ‚|  
dt| j¡}| | ¡|j| j|j| j ttƒ | jttƒ  |j| jttƒ | jttƒ  iks¶J ‚|  |¡| j|jttƒ |jttƒ  | j|jttƒ |jttƒ  | j|jiksáJ ‚t|j| dd| jttƒ | jttƒ  ksùJ ‚t|j| dd| j ttƒ | jttƒ  ksJ ‚t|j| dd| jks!J ‚tt|j|j |j | ddƒt| j| j ttƒ | jttƒ   | jttƒ | jttƒ   ƒksVJ ‚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–J ‚tt|j|j |j|j  |j|j  | ddƒ| j| j | j| j  | j| j  ksÄJ ‚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J ‚tt| j| j | j| j  | j| j  |ddƒ|j|j |j|j  |j|j  ks3J ‚| 
dt |j¡}| | ¡|j| j|j| j|j| jiksRJ ‚|  
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  ¡sJ ‚|| j  |jdtttd  ƒ d  d |jdttƒ d  d  |jdtttd  ƒ d  d  ¡sÕJ ‚|| j  |jdtttd  ƒ d  d |jdtttd  ƒ d  d  |jdttƒ d  d  ¡sJ ‚|  dt| j t| j  t| j  ¡}| | ¡|j| jt |j| jt |j| jt iks<J ‚|  
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„J ‚| | ¡|j| jt ttƒ | jt ttƒ  |j| j t ttƒ | jt ttƒ  |j| jt iks»J ‚|  dtj¡}|  |¡| j|j| j|j| j|jiksØJ ‚dS )zo
    Tests the coordinate variables functionality with respect to
    reorientation of coordinate systems.
    r   r   ZA_xz\mathbf{{x}_{A}}r1   ZA_yz\mathbf{{y}_{A}}r2   ZA_zz\mathbf{{z}_{A}}r-   T)Ú	variablesÚNÚCr   r3   é   ÚDÚEÚFN)r   r   r   r"   r%   Ú__hash__Ú
isinstanceZ
scalar_mapÚsystemÚdiffÚorient_new_axisÚqr&   r   r   r   r   r#   r$   r   Úequalsr   Ú
locate_newr4   r5   r6   r   Úzero)r   r-   r?   r@   ÚmappingrB   rC   rD   r*   r*   r+   Útest_coordinate_varsL   s®    
ÿ
þ*0
ÿ&&
ÿ04B
ÿ(,ÿÿ
ÿ
ÿ(ÿ"
þ( "ÿ
ÿ
ÿ(ÿ"
þ
ÿ
" ÿ þ*ÿ þ* ÿþ&8,""þ.(þ0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s»J ‚| |¡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sµJ ‚| | ¡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s1J ‚| | ¡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¯J ‚| | ¡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s,J ‚d S )Nr?   r   r-   r@   rB   rC   r0   rD   ÚGr   r2   r3   )r   rI   r7   r&   r8   r#   r9   r$   r:   Úorient_new_spaceÚorient_new_quaternionÚorient_new_bodyr;   r<   r   r   r   r   )	r?   r   r-   r@   rB   rC   rD   rP   Ztest_matr*   r*   r+   Útest_rotation_matrix   sÈ   
0ÿ&ÿ&*ÿÿ&û
,ÿÿÿ(ÿþ,ÿ&ÿ*ÿÿÿÿÿÿÿÿýúÿ
&&4ÿ&ÿÿÿÿýÿ
,þ4ÿþü
4&þ8&þ$ÿü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sQJ ‚| j |j¡ttƒks^J ‚| j |j¡ttƒ kslJ ‚| j |j¡dkswJ ‚| j |j¡ttƒks„J ‚| j |j¡ttƒks‘J ‚| j |j¡dksœJ ‚| j |j¡dks§J ‚| j |j¡dks²J ‚| j |j¡dks½J ‚| j |j|j ¡ttƒ ttƒ   krß|j|j  | j¡ksâJ ‚ J ‚|j |j¡ttƒksïJ ‚|j |j¡dksúJ ‚|j |j¡ttƒksJ ‚|j |j¡ttƒttƒ ksJ ‚|j |j¡ttƒks(J ‚|j |j¡ttƒ ttƒ ks;J ‚|j |j¡ttƒ ttƒ ksNJ ‚|j |j¡ttƒks\J ‚|j |j¡ttƒttƒ ksnJ ‚| j |j¡ttƒ|j ksJ ‚| j |j¡ttƒ|j ksJ ‚| j |j¡ttƒ |j ttƒ|j  ks©J ‚| j |j¡ttƒ |j ks»J ‚| j |j¡ttƒ|j ksÌJ ‚| j |j¡ttƒ|j ttƒ|j  ksäJ ‚| j |j¡|jksñJ ‚| j |j¡|j ksÿJ ‚| j |j¡tjksJ ‚| j |j¡ttƒ|j ksJ ‚| j |j¡ttƒ|j ks.J ‚| j |j|j ¡ttƒ|j ttƒ|j  ksIJ ‚|j|j  | j¡ttƒ ttƒ | j ksbJ ‚|j |j¡ttƒ|j kssJ ‚|j |j¡ttƒ |j ttƒ|j  ksŒJ ‚|j |j¡ttƒ |j ksžJ ‚|j |j¡ttƒ ttƒ |j ttƒ ttƒ |j  ksÀJ ‚|j |j¡ttƒ|j ttƒ |j  ksÙJ ‚t|j |j¡|ƒ ¡ ttƒ|j ksïJ ‚dS )z`
    Tests the effects of orientation of coordinate systems on
    basic vector operations.
    r?   r   r-   r@   r   r1   N)r   rI   r7   r&   r8   r#   r9   r$   r4   r5   r6   Z	to_matrixr<   r   r   ÚdotÚcrossr   rM   r   Ztrigsimp)r?   r   r-   r@   Úv1r*   r*   r+   Útest_vector_with_orientationÁ   sf   "þ*ÿ$&&$""2$"0""62"2$"ÿ20rX   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sGJ ‚||  dtttd¡ksTJ ‚||  dtttt¡ksaJ ‚||  dtttd¡ksnJ ‚d S )Nr?   r0   rB   rC   rD   rP   )r   r   r:   r$   r   r7   r8   r9   r   r   Z
orient_newrI   rQ   rR   rS   )	r?   Z	orienter1Z	orienter2Z	orienter3Z	orienter4rB   rC   rD   rP   r*   r*   r+   Útest_orient_new_methods  s   rY   c                  C   sÊ  t dƒ} t| jtƒsJ ‚t| j t| j  t| j	  }|  
d|¡}|j | ¡| | ¡  kr=|j | j¡  kr=|ks@J ‚ J ‚| j |¡|  |¡  kr]| j |j¡  kr]| ks`J ‚ J ‚| j |¡t t t fkspJ ‚| j 
d| ¡}| | ¡t t t fks‡J ‚| |j¡| |¡  kr›d| ksžJ ‚ J ‚| 
dd| ¡}| |j¡tjks±J ‚| |¡dksºJ ‚| 
d| j¡}| |¡d| | j ksÏJ ‚| |¡dt d	 dt dt fksãJ ‚d
S )zA
    Tests Point class, and locate_new method in CoordSys3D.
    r   r@   Úpr3   Úp1r2   )r   r   r   Úp2r1   N)r   rF   r'   r   r4   r#   r5   r$   r6   r&   rL   Zposition_wrtZexpress_coordinatesr   rM   )r   r)   r@   rZ   r[   r\   r*   r*   r+   Útest_locatenew_point  s<   
ÿþþ
ÿþþ ÿ,r]   c                  C   sÜ   t dƒ} | jddd}|j| ksJ ‚| ¡ |jt|jƒ t|jƒ |jt|jƒ t|jƒ |jt|jƒ fks9J ‚| 	¡ t
| jd | jd  | jd  ƒt| jt
| jd | jd  | jd  ƒ ƒt| j| jƒfkslJ ‚d S )Nr4   r6   Ú	spherical©Útransformationr2   )r   Z
create_newÚ_parentÚtransformation_to_parentÚrr   Úthetar   ÚphiZtransformation_from_parentr   r   r"   r%   r   r   )r4   r6   r*   r*   r+   Útest_create_new-  s   @ÿXÿrf   c                  C   sb   t dƒ} d| j d| j  t| j  }| ¡ | ¡ ksJ ‚|jtdid| td¡ ¡ ks/J ‚d S )Nr   r   r   r1   )Úsubs)r   r#   r$   r4   r&   ÚnZevalfrg   )r   r)   r*   r*   r+   Ú
test_evalf7  s   (ri   c                  C   s†   t ddƒ} |  ¡ d| jt| jƒ| j fksJ ‚t dƒ} |  ¡ dks#J ‚t ddƒ} |  ¡ dks0J ‚t ddƒ} |  ¡ d| jdfksAJ ‚d S )Nr4   r^   r1   ©r1   r1   r1   Ú	cartesianÚcylindrical)r   Úlame_coefficientsrc   r   rd   ©r4   r*   r*   r+   Útest_lame_coefficients>  s   
$

ro   c                     sî  t dƒ\‰‰‰tddg d¢d‰ ˆ  ¡ \} }}| ˆ jksJ ‚|ˆ jks%J ‚|ˆ jks,J ‚tt‡ fdd„ƒ tt‡ fdd„ƒ tt‡ fd	d„ƒ ˆ  ¡ | t	|ƒ t
|ƒ | t	|ƒ t	|ƒ | t
|ƒ fksfJ ‚ˆ  ¡ d
| | t	|ƒ fksuJ ‚ˆ  ¡ ˆˆˆƒtˆd ˆd  ˆd  ƒtˆtˆd ˆd  ˆd  ƒ ƒtˆˆƒfks£J ‚tddg d¢d‰ ˆ  ¡ \} }‰ˆ  ¡ | t
|ƒ | t	|ƒ ˆfksÆJ ‚ˆ  ¡ d
ˆ jd
fksÒJ ‚ˆ  ¡ ˆˆˆƒtˆd ˆd  ƒtˆˆƒˆfksìJ ‚tddƒ‰ ˆ  ¡ ˆ jˆ jˆ jfksÿJ ‚ˆ  ¡ dksJ ‚ˆ  ¡ ˆˆˆƒˆˆˆfksJ ‚t dƒ\‰‰‰tdˆˆˆfˆˆˆffƒ‰ ˆ  ¡  ˆ  ¡ ˆ jˆ jˆ jfks?J ‚ˆ  ¡ dksHJ ‚ˆ  ¡ ˆˆˆƒˆˆˆfksXJ ‚t dƒ\} }‰td| |ˆf| t
|ƒ | t	|ƒ ˆfgg d¢d‰ ˆ  ¡ \} }‰ˆ  ¡ | t
|ƒ | t	|ƒ ˆfks“J ‚ˆ  ¡ tt	|ƒd t
|ƒd  ƒt| d t	|ƒd  | d t
|ƒd   ƒd
fks¿J ‚tddd„ ƒ‰ ˆ  ¡ ˆ jˆ jˆ jfksÕJ ‚ˆ  ¡ dksÞJ ‚ˆ  ¡  ˆ  ¡ ˆˆˆƒˆˆˆfksòJ ‚tddd„ g d¢d‰ ˆ  ¡ \} }}ˆ  ¡ | t	|ƒ t
|ƒ | t	|ƒ t	|ƒ | t
|ƒ fks$J ‚ˆ  ¡ 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s˜J ‚tddd„ g d¢d‰ ˆ  ¡ \} }‰ˆ  ¡ | t
|ƒ | t	|ƒ ˆfks¾J ‚ˆ  ¡ tt	|ƒd t
|ƒd  ƒt| d t	|ƒd  | d t
|ƒd   ƒd
fksêJ ‚tt‡‡‡fdd„ƒ d S )Nr   r4   r^   ©rc   rd   re   )r`   Úvariable_namesc                      ó   ˆ j S ©N)r   r*   rn   r*   r+   Ú<lambda>U  ó    z/test_transformation_equations.<locals>.<lambda>c                      rr   rs   )r"   r*   rn   r*   r+   rt   V  ru   c                      rr   rs   )r%   r*   rn   r*   r+   rt   W  ru   r1   r2   rl   ©rc   rd   r%   rk   rj   z	r theta z)rq   c                 S   s
   | ||fS rs   r*   ©r   r"   r%   r*   r*   r+   rt     s   
 c                 S   s2   | t |ƒ t|ƒ | t |ƒ t |ƒ | t|ƒ fS rs   )r   r   rp   r*   r*   r+   rt   —  s   2 c                 S   s   | t |ƒ | t|ƒ |fS rs   )r   r   rv   r*   r*   r+   rt   ¤  s   c                
      s@   t dˆ ˆ tˆƒ tˆƒ ˆˆ tˆƒ tˆƒ ˆˆ tˆƒ idS ©Nr4   r_   ©r   r   r   r*   rw   r*   r+   rt   °  s    4ÿ)r	   r   Úbase_scalarsrc   rd   re   r   ÚAttributeErrorrb   r   r   rm   Z#transformation_from_parent_functionr   r   r   r   r"   r%   Z_calculate_inv_trans_equationsÚx1Zx2Zx3Ú	TypeErrorrp   r*   )r4   r   r"   r%   r+   Útest_transformation_equationsI  s¬   ÿ

ý"
ýÿ


ý 

ÿ
  &ÿÿ*ý	 
ÿ.ÿ>VBý
þ(*ýr~   c                     s*  t dƒ\‰ ‰‰t dƒ\} }tdˆ ˆˆfˆ tˆƒ tˆƒ ˆ tˆƒ tˆƒ ˆ tˆƒ ffd}| |j¡du s9J ‚tdˆ ˆˆfˆ tˆƒ ˆ tˆƒ ˆffd}| |j¡du sYJ ‚td| |ˆft| ƒt|ƒ t| ƒt|ƒ ˆffd}| |j¡du s}J ‚tt	‡ ‡‡fdd„ƒ tt	‡ ‡‡fdd„ƒ d S )	Nr   zu, vr4   r_   Tc                      s   t dˆ ˆˆfˆ ˆ ˆffdS rx   r   r*   rw   r*   r+   rt   ¾  s    z*test_check_orthogonality.<locals>.<lambda>c                      sH   t dˆ ˆˆfˆ tˆd ƒ tˆƒ ˆ tˆƒ tˆƒ ˆ tˆƒ ffdS )Nr4   r2   r_   ry   r*   rw   r*   r+   rt   ¿  s    <ÿ)
r	   r   r   r   Z_check_orthogonalityZ_transformationr
   r   r   Ú
ValueError)Úur)   r4   r*   rw   r+   Útest_check_orthogonality´  s   D,4r   c                  C   sŠ  t dƒ} ddlm} |dƒ}|  | j|  ¡ ¡| j| j| jfks!J ‚|  |  	¡ |  ¡ ¡| j| j| jfks5J ‚|  
dd| j ¡}| |j| ¡ ¡|j|j|jfksQJ ‚| | 	¡ | ¡ ¡|j|j|jfkseJ ‚|  
d|| j ¡}| |j| ¡ ¡t|ƒ |j t|ƒ|j  t|ƒ|j t|ƒ|j  |jfks˜J ‚| | 	¡ | ¡ ¡t|ƒ|j t|ƒ|j  t|ƒ |j t|ƒ|j  |jfksÃJ ‚d S )Nr4   r   r   Úq0r5   r6   )r   Úsympy.core.symbolr	   Z_rotation_trans_equationsZ_parent_rotation_matrixrz   r   r"   r%   Z_inverse_rotation_matrixrI   r&   r   r   )r4   r	   r‚   r5   r6   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%   r4   r5   r6   rJ   r7   r8   r9   r:   r,   r/   r=   rO   rT   rX   rY   r]   rf   ri   ro   r~   r   r„   r*   r*   r*   r+   Ú<module>   sB     C2A
k