o
    æð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*   úX/var/www/vscode/kcb/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   Úaxis_orienterÚbody_orienterÚspace_orienterÚ
q_orienterr*   r*   r+   Útest_orienters,   sV   
ý4&ÿ8&ÿÿû&&4ÿ6ÿÿü4ÿ4ÿ4ÿûrA   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   ÚA_xz\mathbf{{x}_{A}}r1   ÚA_yz\mathbf{{y}_{A}}r2   ÚA_zz\mathbf{{z}_{A}}r-   T)Ú	variablesÚNÚCr   r3   é   ÚDÚEÚFN)r   r   r   r"   r%   Ú__hash__Ú
isinstanceÚ
scalar_mapÚsystemÚdiffÚorient_new_axisÚqr&   r   r   r   r   r#   r$   r   Úequalsr   Ú
locate_newr4   r5   r6   r   Úzero)r   r-   rF   rG   ÚmappingrI   rJ   rK   r*   r*   r+   Útest_coordinate_varsL   s®    
ÿ
þ*0
ÿ&&
ÿ04B
ÿ(,ÿÿ
ÿ
ÿ(ÿ"
þ( "ÿ
ÿ
ÿ(ÿ"
þ
ÿ
" ÿ þ*ÿ þ* ÿþ&8,""þ.(þ0rW   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 )NrF   r   r-   rG   rI   rJ   r0   rK   ÚGr   r2   r3   )r   rQ   r7   r&   r8   r#   r9   r$   r:   Úorient_new_spaceÚorient_new_quaternionÚorient_new_bodyr;   r<   r   r   r   r   )	rF   r   r-   rG   rI   rJ   rK   rX   Útest_matr*   r*   r+   Útest_rotation_matrix   sÈ   
0ÿ&ÿ&*ÿÿ&û
,ÿÿÿ(ÿþ,ÿ&ÿ*ÿÿÿÿÿÿÿÿýúÿ
&&4ÿ&ÿÿÿÿýÿ
,þ4ÿþü
4&þ8&þ$ÿür]   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.
    rF   r   r-   rG   r   r1   N)r   rQ   r7   r&   r8   r#   r9   r$   r4   r5   r6   Ú	to_matrixr<   r   r   ÚdotÚcrossr   rU   r   Útrigsimp)rF   r   r-   rG   Úv1r*   r*   r+   Útest_vector_with_orientationÁ   sf   "þ*ÿ$&&$""2$"0""62"2$"ÿ20rc   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 )NrF   r0   rI   rJ   rK   rX   )r   r   r:   r$   r   r7   r8   r9   r   r   Ú
orient_newrQ   rY   rZ   r[   )	rF   Ú	orienter1Ú	orienter2Ú	orienter3Ú	orienter4rI   rJ   rK   rX   r*   r*   r+   Útest_orient_new_methods  s   ri   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   rG   Úpr3   Úp1r2   )r   r   r   Úp2r1   N)r   rM   r'   r   r4   r#   r5   r$   r6   r&   rT   Úposition_wrtÚexpress_coordinatesr   rU   )r   r)   rG   rj   rk   rl   r*   r*   r+   Útest_locatenew_point  s<   
ÿþþ
ÿþþ ÿ,ro   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   Ú
create_newÚ_parentÚtransformation_to_parentÚrr   Úthetar   ÚphiÚtransformation_from_parentr   r   r"   r%   r   r   )r4   r6   r*   r*   r+   Útest_create_new-  s   @ÿXÿrz   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&   ÚnÚevalfr{   )r   r)   r*   r*   r+   Ú
test_evalf7  s   (r~   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   rp   r1   ©r1   r1   r1   Ú	cartesianÚcylindrical)r   Úlame_coefficientsrv   r   rw   ©r4   r*   r*   r+   Útest_lame_coefficients>  s   
$

r„   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   rp   ©rv   rw   rx   )rr   Úvariable_namesc                      ó   ˆ j S ©N)r   r*   rƒ   r*   r+   Ú<lambda>U  ó    z/test_transformation_equations.<locals>.<lambda>c                      r‡   rˆ   )r"   r*   rƒ   r*   r+   r‰   V  rŠ   c                      r‡   rˆ   )r%   r*   rƒ   r*   r+   r‰   W  rŠ   r1   r2   r   ©rv   rw   r%   r€   r   z	r theta z)r†   c                 S   s
   | ||fS rˆ   r*   ©r   r"   r%   r*   r*   r+   r‰     s   
 c                 S   s2   | t |ƒ t|ƒ | t |ƒ t |ƒ | t|ƒ fS rˆ   )r   r   r…   r*   r*   r+   r‰   —  s   2 c                 S   s   | t |ƒ | t|ƒ |fS rˆ   )r   r   r‹   r*   r*   r+   r‰   ¤  s   c                
      s@   t dˆ ˆ tˆƒ tˆƒ ˆˆ tˆƒ tˆƒ ˆˆ tˆƒ idS ©Nr4   rq   ©r   r   r   r*   rŒ   r*   r+   r‰   °  s    4ÿ)r	   r   Úbase_scalarsrv   rw   rx   r   ÚAttributeErrorru   r   r   r‚   Ú#transformation_from_parent_functionr   r   r   r   r"   r%   Ú_calculate_inv_trans_equationsÚx1Úx2Úx3Ú	TypeErrorr…   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   rq   Tc                      s   t dˆ ˆˆfˆ ˆ ˆffdS r   r   r*   rŒ   r*   r+   r‰   ¾  s    z*test_check_orthogonality.<locals>.<lambda>c                      sH   t dˆ ˆˆfˆ tˆd ƒ tˆƒ ˆ tˆƒ tˆƒ ˆ tˆƒ ffdS )Nr4   r2   rq   rŽ   r*   rŒ   r*   r+   r‰   ¿  s    <ÿ)
r	   r   r   r   Ú_check_orthogonalityÚ_transformationr
   r   r   Ú
ValueError)Úur)   r4   r*   rŒ   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	   Ú_rotation_trans_equationsÚ_parent_rotation_matrixr   r   r"   r%   Ú_inverse_rotation_matrixrQ   r&   r   r   )r4   r	   r   r5   r6   r*   r*   r+   Útest_rotation_trans_equationsÃ  s   &(&(<ÿ<ÿr¢   N)AÚsympy.testing.pytestr   Úsympy.vector.coordsysrectr   Úsympy.vector.scalarr   Úsympy.core.functionr   Úsympy.core.numbersr   rž   r	   Ú%sympy.functions.elementary.hyperbolicr
   r   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   r   r   r   Úsympy.matrices.denser   Úsympy.matrices.immutabler   r<   Úsympy.simplify.simplifyr   Úsympy.vector.functionsr   Úsympy.vector.pointr   Úsympy.vector.vectorr   Úsympy.vector.orientersr   r   r   r   r   r"   r%   r4   r5   r6   rR   r7   r8   r9   r:   r,   r/   rA   rW   r]   rc   ri   ro   rz   r~   r„   r—   rœ   r¢   r*   r*   r*   r+   Ú<module>   sB     C2A
k