o
    *Î®c)Y  ã                   @   sz  d dl mZ d dlmZ d dlmZmZ d dl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mZ d dlmZ d d	lmZ d d
lmZ d dlZde_dd„ Zdd„ Z dd„ Z!dd„ Z"dd„ Z#dd„ Z$dd„ Z%dd„ Z&dd„ Z'dd „ Z(d!d"„ Z)d#d$„ Z*d%d&„ Z+d'd(„ Z,d)d*„ Z-d+d,„ Z.d-d.„ Z/d/d0„ Z0d1d2„ Z1d3d4„ Z2d5d6„ Z3d7d8„ Z4d9d:„ Z5d;d<„ Z6d=d>„ Z7d?d@„ Z8dS )Aé    )Úpi)Úsymbols)ÚcosÚsin)ÚeyeÚzeros)ÚImmutableDenseMatrix)Úsimplify)ÚReferenceFrameÚVectorÚCoordinateSymÚdynamicsymbolsÚtime_derivativeÚexpressÚdot©Ú_check_frame)ÚVectorTypeError)ÚraisesNTc                     sì   t dƒ‰ t dƒ} t dƒ‰t dƒ}t dƒ‰t dƒ‰|  ˆ ˆ jd¡ ˆ | | jd¡ | ˆˆjd¡ | ˆ d¡|ˆ| ˆ gks>J ‚ˆ ||jd¡ ˆ ˆ d¡ˆ| ˆ gksSJ ‚ˆ ˆd¡ˆ|ˆgks`J ‚tt‡‡fd	d
„ƒ tt‡ ‡fdd
„ƒ d S )NÚAÚBÚCÚDÚEÚFg      ð?r   c                      s   ˆ   ˆd¡S )Né   ©Ú
_dict_list© )r   r   r   úL/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/vector/tests/test_frame.pyÚ<lambda>'   ó    z test_dict_list.<locals>.<lambda>c                      s   ˆ  ˆ d¡S ©Nr   r   r   )r   r   r   r   r    )   r!   )r
   Úorient_axisÚxr   r   Ú
ValueError)r   r   r   )r   r   r   r   r   Útest_dict_list   s   r&   c               
      s´  t dƒ‰ tdˆ dƒˆ d ksJ ‚tdˆ dƒˆ d ksJ ‚tdˆ dƒˆ d ks(J ‚tt‡ fdd„ƒ tdƒ} tddƒ}tˆ d tƒrOtˆ d tƒrOtˆ d tƒsQJ ‚ˆ  ˆ ¡ˆ d ˆ d ˆ d ˆ d ˆ d ˆ d ikslJ ‚ˆ d jˆ ksuJ ‚ˆ  d	d
| ˆ j	g¡}| ˆ ¡|d ˆ d |d ˆ d  t
| ƒ ˆ d t| ƒ  |d ˆ d t| ƒ ˆ d t
| ƒ  iks³J ‚ˆ  |¡ˆ d |d t| ƒ |d t
| ƒ  ˆ d |d t
| ƒ |d t| ƒ  ˆ d |d iksæJ ‚t|d ˆ ƒˆ d  t
| ƒ | ˆ d t| ƒ |  ksJ ‚t|d ˆ ƒˆ d  t| ƒ | ˆ d t
| ƒ |  ks$J ‚t|d ˆ ƒdks0J ‚t|d ˆ ddˆ d t| ƒ ˆ d t
| ƒ  ksLJ ‚t|d ˆ ddˆ d  t
| ƒ ˆ d t| ƒ  ksiJ ‚t|d ˆ ddˆ d ksyJ ‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j	  |ƒˆ d | ˆ j ˆ d | ˆ j  ks¦J ‚t|d |j |d |j  |d |j	  ˆ ƒ|d  | |j |d | |j  ksÔJ ‚t|d |d  |d  ˆ ddˆ d ˆ d  t
| ƒ ˆ d t| ƒ   ˆ d t| ƒ ˆ d t
| ƒ   ksJ ‚t|d |d  |d  ˆ ƒˆ d ˆ d d  td|  ƒ dˆ d  ˆ d  t
d|  ƒ  ˆ d d td|  ƒ   |   ¡ dksRJ ‚t|d |j |d |j  |d |j	  ˆ ƒ|d t| ƒ |d t
| ƒ  ˆ j |d t
| ƒ |d t| ƒ  ˆ j  |d ˆ j	  ksšJ ‚t|d |j |d |j  |d |j	  ˆ ddˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j	  ksÌJ ‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j	  |ƒˆ d t| ƒ ˆ d t
| ƒ  |j ˆ d  t
| ƒ ˆ d t| ƒ  |j  ˆ d |j	  ksJ ‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j	  |dd|d |j |d |j  |d |j	  ksGJ ‚| dd
|  |j	g¡}| ˆ ¡|d ˆ d |d ˆ d |d ˆ d iksnJ ‚ˆ  dd
| ˆ jˆ j ˆ j	 g¡}ˆ  |¡}|ˆ d  d|d  t| ƒ d |d d  d|d  t
| td  ƒ d  |d d  d|d  t| td  ƒ d  |d d  ksÊJ ‚|ˆ d  d|d  t| td  ƒ d |d d  d|d  t| ƒ d  |d d  d|d  t
| td  ƒ d  |d d  ksJ ‚|ˆ d  d|d  t
| td  ƒ d |d d  d|d  t| td  ƒ d  |d d  d|d  t| ƒ d  |d d  ksXJ ‚dS )z,Tests the coordinate variables functionalityr   ÚAxr   é   é   c                      s   t dˆ dƒS )Nr'   é   )r   r   ©r   r   r   r    2   r!   z&test_coordinate_vars.<locals>.<lambda>Úqr   ÚAxisT©Ú	variablesÚNr   r*   é   éþÿÿÿN)r
   r   r   r%   r   Ú
isinstanceÚvariable_mapÚframeÚ	orientnewÚzr   r   r   r   r$   ÚyÚtrigsimpr   )r,   Úqdr   r0   r   Úmappingr   r+   r   Útest_coordinate_vars,   sÀ   
ÿþ6:$
ÿ,0
ÿ>>8: Z\ H
ÿÿþþÿ
ý.2ÿÿÿ
ÿ2(
ÿ.$&ÿÿ
ÿ2(
ÿ8 
L
ÿÿ
ÿ(
ÿÿ
ÿÿ
ÿ4ÿ
ÿÿ
ÿr<   c                  C   sÚ  t dƒ\} }}}t ddƒ\}}}}tdƒ}| dd| |jg¡}	|	 dd||	jg¡}
|
 dd||
jg¡}| dd||jg¡}t d	ƒ\}}}|	 |¡||	j ksPJ ‚|
 |¡||
j ||	j  ksaJ ‚| |¡||j ||
j  ||	j  kswJ ‚| d
d||jg¡}| |¡dksŠJ ‚| |	¡| |j ks—J ‚| |
¡| |	j ||
j  ks©J ‚| |¡| |	j ||
j  ||
j  ksÀJ ‚| |¡| |j ksÍJ ‚|	 |¡||j ksÙJ ‚|	 |	¡dksâJ ‚|	 |
¡| |
j ksïJ ‚|	 |¡| |
j ||
j  ksJ ‚|	 |¡||j ||j  ksJ ‚|
 |¡||	j ||	j  ks&J ‚|
 |	¡||	j ks3J ‚|
 |
¡dks=J ‚|
 |¡| |
j ksKJ ‚|
 |¡||	j ||	j  ||j  ksbJ ‚| |¡||	j ||	j  ||
j  ksyJ ‚| |	¡||	j ||j  ks‹J ‚| |
¡||
j ks˜J ‚| |¡dks¢J ‚| |¡||	j ||	j  ||
j  ||j  ks¾J ‚| |¡||j ksËJ ‚| |	¡||j ||j  ksÝJ ‚| |
¡||j ||	j  ||	j  ksôJ ‚| |¡||j ||	j  ||	j  ||
j  ksJ ‚| |¡dksJ ‚| |||j ||j  ||j  ¡ | |¡||j ||j  ||j  ksDJ ‚| |¡| |j | |j  | |j  ks^J ‚| |¡||j ||j  ||j  | |j  ks{J ‚| |¡| |j | |j  | |j  ||j  ksšJ ‚t dƒ}t ddƒ}| dd|| ||f¡}| |¡d|| ||  ||  ||    |j d|| ||   ||  ||   |j  d|| ||  ||   ||   |j  ksõJ ‚| dd| ||fd¡}| |¡t|ƒt|ƒ | t	|ƒ|  |j t|ƒt	|ƒ | t|ƒ|  |j  t	|ƒ| | |j  ks9J ‚| dd| |j|j f¡}| |¡||j|j  
¡  ksXJ ‚| |¡| |j|j  
¡  kskJ ‚d S )Núq1 q2 q3 q4r(   r0   r   r-   r   r   r   zu1 u2 u3ÚA2r   Úq0r   Ú
Quaternionr)   r   ÚBodyi9  ÚG)r   r
   r6   r7   r$   r8   Ú
ang_vel_inÚset_ang_velr   r   Ú	normalize)Úq1Úq2Úq3Úq4Úq1dÚq2dÚq3dÚq4dr0   r   r   r   r   Úu1Úu2Úu3r>   r?   Úq0dr   r   rB   r   r   r   Útest_ang_velc   sz   ",$.&$$..$8$.8&.4:>
((ÿ(þ
ÿ,$ÿÿ$*rR   c                  C   sf  t dƒ\} }}}tdƒ}| dd| |jg¡}| dd||jg¡}| dd||jg¡}| dd||jg¡}| dd	| ||gd
¡}	| |¡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	|ƒ  g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	|ƒ    g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|ƒ    ggƒ }
|
 
¡ tddƒksµJ ‚|	 |¡tt	|ƒt	|ƒ t|ƒt	|ƒ t|ƒ g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	|ƒ ggƒks1J ‚d S )Nr=   r0   r   r-   r   r   r   r   ÚSpaceÚ123r*   )r   r
   r6   r7   r$   r8   ÚdcmÚMatrixr   r   Úexpandr   )rF   rG   rH   rI   r0   r   r   r   r   r   Útest_matr   r   r   Útest_dcm¥   s    
0ÿ&ÿÿÿÿÿÿÿü
,ÿÿÿ$ÿÿþÿÿÿ.ÿÿÿ.ÿþûÿ

&<ÿÿÿ(þþÿrY   c               	   C   s†  t dƒ} t dƒ}tdƒ\}}}tdƒ\}}}tdƒ\}}	}
tddd\}}}tddd\}}}tddd\}}}t|||g|||g||	|
ggƒ}| | d|¡ |j | ¡}|j | ¡}|j | ¡}| | |jt	| 
| ¡|jƒ |jt	| 
| ¡|jƒ  |jt	| 
| ¡|jƒ  ¡ || ||  |	|  |j || ||  |
|  |j  || ||  ||  |j  }| | ¡| d	ksÁJ ‚d S )
Nr   r   zC11 C12 C13zC21 C22 C23zC31 C32 C33r(   )ÚlevelÚDCMr   )r
   r   rV   Úorientr$   r   r8   r7   rD   r   ÚdtrC   )r   r   Úc11Úc12Úc13Úc21Úc22Úc23Úc31Úc32Úc33Úc11dÚc12dÚc13dÚc21dÚc22dÚc23dÚc31dÚc32dÚc33dr[   Úb1aÚb2aÚb3aÚexprr   r   r   Útest_w_diff_dcm1Æ   s8   ýÿþÿþrt   c            	   	   C   s¤  t dƒ\} }}tdƒ}| dd| |jg¡}| dd||jg¡}| dd||jg¡}| |¡j}| dd|¡}| |¡| |¡  krº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	| ƒ  gt
|ƒt
| ƒ t	|ƒ t	| ƒt	|ƒ ggƒks½J ‚ J ‚| |¡| |¡  |¡ ¡ d	ksÐJ ‚d S )
Núq1:4r0   r   Úaxisr   r   r   r[   r   )r   r
   r6   r$   r8   r7   rU   ÚTrV   r   r   rC   r   r	   )	rF   rG   rH   r0   r   r   r   r[   r   r   r   r   Útest_w_diff_dcm2ë   s,   $ÿÿÿ(&þ&ú*rx   c                  C   s>   G dd„ dt ƒ} | dƒ}| ddd|jg¡}t|| ƒsJ ‚d S )Nc                   @   s   e Zd ZdS )z>test_orientnew_respects_parent_class.<locals>.MyReferenceFrameN)Ú__name__Ú
__module__Ú__qualname__r   r   r   r   ÚMyReferenceFrame  s    r|   r   r   r-   r   )r
   r6   r$   r3   )r|   r   r   r   r   r   Ú$test_orientnew_respects_parent_class  s   r}   c                  C   sn   t dƒ} tdƒ}|  dd|| jg¡}dd„ | jD ƒ}| jdd|| jg|d}| j|jks.J ‚|j|ks5J ‚d S )	Nr0   rF   Úar-   c                 S   ó   g | ]}|d  ‘qS ©Ú1r   ©Ú.0r$   r   r   r   Ú
<listcomp>  ó    z9test_orientnew_respects_input_indices.<locals>.<listcomp>Úb)Úindices)r
   r   r6   r7   r‡   )r0   rF   r   Úmindsr   r   r   r   Ú%test_orientnew_respects_input_indices	  s   r‰   c                  C   sþ   t dƒ} tdƒ}|  dd|| jg¡}d|j ¡ |jd f d|j ¡ |jd f d|j ¡ |jd f g}d	}d
d„ | jD ƒ}d| ¡ |d f d| ¡ |d f d| ¡ |d f g}| j|d|| jg|d}|j|ksoJ ‚|j|ksvJ ‚|j|ks}J ‚d S )Nr0   rF   r~   r-   z\mathbf{\hat{%s}_%s}r   r(   r)   r†   c                 S   r   r€   r   r‚   r   r   r   r„   !  r…   z8test_orientnew_respects_input_latexs.<locals>.<listcomp>z\mathbf{\hat{%s}_{%s}})Úlatexs)r
   r   r6   r7   ÚnameÚlowerr‡   Ú
latex_vecs)r0   rF   r   Údef_latex_vecsr‹   r‡   Únew_latex_vecsr   r   r   r   Ú$test_orientnew_respects_input_latexs  s:   
ÿÿ
ÿýÿÿÿýr   c                  C   s¬   t dƒ} tdƒ}|  dd|| jg¡}d}dd„ | jD ƒ}| j|d|| jg|d}t|jƒD ]\}}|j|jd	 |j|  ks@J ‚q-t|jƒD ]\}}|j|| ksSJ ‚qFd S )
Nr0   rF   r~   r-   r†   c                 S   s   g | ]}d | d ‘qS )Únotb_r   r   r‚   r   r   r   r„   5  ó    z;test_orientnew_respects_input_variables.<locals>.<listcomp>r.   Ú_)r
   r   r6   r7   r‡   Ú	enumerateÚvarlistr‹   )r0   rF   r   r‹   Únew_variablesr   ÚjÚvarr   r   r   Ú'test_orientnew_respects_input_variables.  s    ÿr™   c                  C   s$   t dƒ} tdƒ}| dd| d¡ d S )Nzu:3ÚIr   ÚspaceÚXYZ)r   r
   r6   )Úurš   r   r   r   Útest_issue_10348>  s   rž   c                  C   s<   t dƒ} |  ddd| jg¡ t dƒ}|  |dd|jg¡ d S )Nr   r   r-   é#   r   éF   )r
   r6   r8   r\   r7   )r   r   r   r   r   Útest_issue_11503D  s   r¡   c                  C   sÒ   t dƒ} t dƒ}tdƒ\}}| | ||j || j  ¡ |  ||¡|j ks(J ‚|  |||¡|j | j fks9J ‚| | |¡|jksDJ ‚| | ||¡|j| jfksSJ ‚|  | |¡dks]J ‚| ||¡dksgJ ‚d S )Nr0   r   zu1, u2r   )r
   r   rD   r$   r8   Úpartial_velocity)r0   r   rN   rO   r   r   r   Útest_partial_velocityK  s   "r£   c               	   C   s  t dƒ} t dƒ}|  |dtdƒ¡ |  |¡tg d¢g d¢g d¢gƒks%J ‚| | ¡tg d¢g d¢g d¢gƒks9J ‚|  |dtg d¢g d¢g d	¢gƒ¡ | | ¡tg d¢g d¢g d	¢gƒks_J ‚|  |¡tg d¢g d¢g d
¢gƒkssJ ‚| | ¡j|  |¡ks€J ‚d S )Nr   r   r[   r*   ©r(   r   r   ©r   r(   r   ©r   r   r(   )r   r   éÿÿÿÿ)r§   r   r   )r   r§   r   )r
   r\   r   rU   rV   rw   ©r   r   r   r   r   Útest_issue_11498^  s   (($((r©   c                      sÆ  t tdd„ ƒ t tdd„ ƒ t tdd„ ƒ t tdd„ ƒ t tdd„ ƒ t tdd„ ƒ t tdd„ ƒ t td	d„ ƒ t td
d„ ƒ t tdd„ ƒ tdƒ‰ˆd tdˆdƒksVJ ‚ˆd tdˆdƒksbJ ‚ˆd tdˆdƒksnJ ‚t t‡fdd„ƒ tdg d¢ƒ‰ˆd ˆjks‡J ‚ˆd ˆjksJ ‚ˆd ˆjks™J ‚t t‡fdd„ƒ tˆƒdksªJ ‚tdƒ‰ tdƒ‰t	dƒ\‰‰‰‰t t‡ ‡fdd„ƒ t t‡‡‡‡‡fdd„ƒ t t‡‡‡fdd„ƒ t t‡‡‡fdd„ƒ t t
‡‡‡fd d„ƒ t t‡‡‡‡‡‡fd!d„ƒ t t‡‡‡fd"d„ƒ t t‡‡‡‡‡fd#d„ƒ t t‡‡‡‡‡fd$d„ƒ t t‡‡‡‡fd%d„ƒ t t‡‡‡‡fd&d„ƒ ˆ ˆd¡ ˆ ˆ¡tdƒksOJ ‚ˆ ˆd¡ ˆ ˆ¡tdƒksaJ ‚d S )'Nc                   S   ó   t dƒS r"   ©r
   r   r   r   r   r    q  ó    z&test_reference_frame.<locals>.<lambda>c                   S   s
   t ddƒS )Nr0   r   r«   r   r   r   r   r    r  s   
 c                   S   s   t dddgƒS )Nr0   r   r(   r«   r   r   r   r   r    s  ó    c                   S   s   t dg d¢ƒS )Nr0   ©r   r(   r)   r«   r   r   r   r   r    t  r­   c                   S   s   t dg d¢dƒS ©Nr0   ©r~   r†   Úcr   r«   r   r   r   r   r    u  ó    c                   S   s   t dg d¢ddgƒS ©Nr0   r°   r   r(   r«   r   r   r   r   r    v  r…   c                   S   s   t dg d¢g d¢ƒS ©Nr0   r°   r®   r«   r   r   r   r   r    w  r…   c                   S   s   t dg d¢g d¢dƒS r¯   r«   r   r   r   r   r    x  s   
 ÿc                   S   s   t dg d¢g d¢ddgƒS r³   r«   r   r   r   r   r    z  ó   
 ÿc                   S   s   t dg d¢g d¢g d¢ƒS r´   r«   r   r   r   r   r    |  rµ   r0   r   ÚN_xr(   ÚN_yr)   ÚN_zc                      ó   ˆ d S )Nr*   r   r   ©r0   r   r   r    ‚  r¬   r°   r~   r†   r±   c                      r¹   )NÚdr   r   rº   r   r   r    ‡  r¬   r   r   zq0 q1 q2 q3c                      s   ˆ   ˆdd¡S )Nr[   r   ©r\   r   r¨   r   r   r      r­   c                      s   ˆ   ˆdˆˆˆgd¡S )NrS   Ú222r¼   r   )r   r0   rF   rG   rH   r   r   r    Ž  s    c                      s    ˆ   ˆdˆˆjdˆj  gd¡S )Nr-   r)   r½   )r\   r$   r8   r   ©r   r0   rF   r   r   r      s     c                      ó   ˆ   ˆdˆ¡S ©Nr-   r¼   r   r¾   r   r   r      r­   c                      s   ˆ   ˆdˆg¡S rÀ   r¼   r   r¾   r   r   r    ‘  r²   c                      s   ˆ   ˆdˆˆˆˆgd¡S )Nr@   r½   r¼   r   )r   r0   r?   rF   rG   rH   r   r   r    ’  r’   c                      r¿   ©Nr@   r¼   r   )r   r0   r?   r   r   r    “  r­   c                      ó   ˆ   ˆdˆˆˆg¡S rÁ   r¼   r   ©r   r0   r?   rF   rG   r   r   r    ”  r…   c                      rÂ   )NÚFoor¼   r   rÃ   r   r   r    •  r…   c                      ó   ˆ   ˆdˆˆgd¡S )NrA   Ú232r¼   r   ©r   r0   rF   rG   r   r   r    –  r…   c                      rÅ   )NrS   rÆ   r¼   r   rÇ   r   r   r    —  r…   )r   Ú	TypeErrorr%   r
   r   r$   r8   r7   Ústrr   Ú
IndexErrorÚNotImplementedErrorÚset_ang_accÚ
ang_acc_inr   rD   rC   r   r   )r   r   r0   r?   rF   rG   rH   r   Útest_reference_framep  sN   rÎ   c                   C   s   t tdd„ ƒ d S )Nc                   S   rª   r"   r   r   r   r   r   r       r¬   z"test_check_frame.<locals>.<lambda>)r   r   r   r   r   r   Útest_check_frameŸ  s   rÏ   c                  C   sL  t dƒ\} }}t| ƒ}t| ƒ}t|ƒ}t|ƒ}t|ƒ}t|ƒ}t|| || | ||  || | ||  g|| || | ||  || | ||  g| || || ggƒ}	tdƒ}
tdƒ}| |
d|	¡ | |
¡}|| |  ¡  || ¡   }|| | ¡  || ¡   }t| 	|
j
¡| ƒdks–J ‚t| 	|j
¡| ƒdks¤J ‚d S )Nru   r   r   r[   r   )r   r   r   rV   r
   r\   rC   Údiffr	   r   r8   )rF   rG   rH   Ús1Úc1Ús2Úc2Ús3Úc3rU   r   r   ÚAwBÚalpha2Úbeta2r   r   r   Útest_dcm_diff_16824£  s&   .,þ
 rÚ   c                  C   sL   t dƒ} t dƒ}|  |tdƒ¡ |  |¡tg d¢g d¢g d¢gƒks$J ‚d S )Nr   r   r*   r¤   r¥   r¦   )r
   Úorient_explicitr   rU   rV   r¨   r   r   r   Útest_orient_explicitÃ  s   ,rÜ   c                     s’   t dƒ‰ t dƒ‰ˆ  ˆˆj d¡ ˆ  ˆ¡} ˆ  ˆˆjd¡ ˆ  ˆ¡}ˆ  ˆdˆj ¡ ˆ  ˆ¡}| |ks7J ‚||ks=J ‚tt‡ ‡fdd„ƒ d S )Nr   r   r(   r§   c                      s   ˆ   ˆdd¡S )Nr(   )r#   r   r¨   r   r   r    Ô  r­   z"test_orient_axis.<locals>.<lambda>)r
   r#   r$   rU   r   rÈ   )ÚA1r>   ÚA3r   r¨   r   Útest_orient_axisÉ  s   


rß   c                  C   sŒ   t dƒ} t dƒ}| | dd¡ | | ¡ttdƒtdƒd tdƒ tdƒ gdtdƒtdƒgtdƒtdƒ tdƒ tdƒd ggƒksDJ ‚d S )Nr   r   )r(   r(   r   ÚXYXr(   r)   r   )r
   Úorient_body_fixedrU   rV   r   r   r¨   r   r   r   Útest_orient_bodyÖ  s   nrâ   c                  C   s  t dƒ\} }}t j}tdƒ}tdƒ}| || ||fd¡ | |¡}|jd d |ks,J ‚|jd d d t|ƒt|ƒ |  |¡ t|ƒ| |¡  ksMJ ‚|jd d d t|ƒt|ƒ |  |¡ t|ƒ| |¡  ksnJ ‚|jd d d t|ƒ|  |¡ | |¡ ks‡J ‚dS )	a  orient_body_fixed() uses kinematic_equations() internally and solves
    those equations for the measure numbers of the angular velocity. This test
    ensures that the simplest form of that linear system solution is returned,
    thus the == for the expression comparison.zpsi, theta, varphir   r   ÚZXZr   r(   r)   N)	r   Ú_tr
   rá   rC   Úargsr   rÐ   r   )ÚpsiÚthetaÚphiÚtr   r   ÚA_w_Br   r   r   Útest_orient_body_simple_ang_velÝ  s   
(
ÿ(
ÿ6rë   c                  C   sJ   t dƒ} t dƒ}| | dd¡ | | ¡tg d¢g d¢g d¢gƒks#J ‚d S )Nr   r   ©r   r   r   rT   r¤   r¥   r¦   )r
   Úorient_space_fixedrU   rV   r¨   r   r   r   Útest_orient_spaceñ  s   ,rî   c                  C   sH   t dƒ} t dƒ}| | d¡ | | ¡tg d¢g d¢g d¢gƒks"J ‚d S )Nr   r   )r   r   r   r   rì   )r
   Úorient_quaternionrU   rV   r¨   r   r   r   Útest_orient_quaternion÷  s   ,rð   c                  C   s¼   t dƒ} t dƒ}t dƒ}tdƒ\}}}| | | j|¡ | ||j|¡ tjdd+}t d¡ |  ||j|¡ t|d jt	ƒsAJ ‚d	t
|d jƒv sLJ ‚W d   ƒ d S 1 sWw   Y  d S )
Nr   r   r   úa b cT)ÚrecordÚalwaysr§   zxLoops are defined among the orientation of frames. This is likely not desired and may cause errors in your calculations.)r
   r   r#   r$   ÚwarningsÚcatch_warningsÚsimplefilterÚ
issubclassÚcategoryÚUserWarningrÉ   Úmessage)r   r   r   r~   r†   r±   Úwr   r   r   Útest_looped_frame_warningý  s   

ÿ"ürü   c                  C   s¾  t dƒ} t dƒ}t dƒ}tdƒ\}}}| | | j|¡ | j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiks:J ‚|j| tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒiksYJ ‚|ji ks`J ‚| ||j|¡ | j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiks‡J ‚|j| tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒ|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒiks¾J ‚|j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiksÝJ ‚|  ||j|¡ |j|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒ| tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiksJ ‚| j|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒiks=J ‚|j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiks]J ‚d S )Nr   r   r   rñ   r¤   r   )r
   r   r#   r$   Ú	_dcm_dictrV   r   r   )r   r   r   r~   r†   r±   r   r   r   Útest_frame_dict  s(   >>>40
ÿ>40ÿ@Drþ   c                  C   s$  t dƒ} t dƒ}t dƒ}t dƒ}tdƒ\}}}| | | j|¡ | ||j|¡ | ||j|¡ |j|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒiksNJ ‚|j|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒ|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiks…J ‚|j| tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒ|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiks¼J ‚| j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiksÛJ ‚|j|jksãJ ‚| 	| ¡ t
| j ¡ ƒ| ||gksöJ ‚t
|j ¡ ƒ|| gksJ ‚t
| j ¡ ƒ|gksJ ‚t
|j ¡ ƒ|gksJ ‚| j| jks'J ‚|  ||j|¡ | j|tg d¢dt|ƒt|ƒgdt|ƒ t|ƒggƒiksOJ ‚| j| jksXJ ‚|j|tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒ| tg d¢dt|ƒt|ƒ gdt|ƒt|ƒggƒiksJ ‚d S )Nr   r   r   r   rñ   r¤   r   )r
   r   r#   r$   rý   rV   r   r   Ú
_dcm_cacherU   ÚlistÚkeys)r   r   r   r   r~   r†   r±   r   r   r   Útest_dcm_cache_dict'  s:   >40
ÿ40
ÿ>
@40ÿr  )9Úsympy.core.numbersr   Úsympy.core.symbolr   Ú(sympy.functions.elementary.trigonometricr   r   Úsympy.matrices.denser   r   Úsympy.matrices.immutabler   rV   Úsympy.simplify.simplifyr	   Úsympy.physics.vectorr
   r   r   r   r   r   r   Úsympy.physics.vector.framer   Úsympy.physics.vector.vectorr   Úsympy.testing.pytestr   rô   Úsimpr&   r<   rR   rY   rt   rx   r}   r‰   r   r™   rž   r¡   r£   r©   rÎ   rÏ   rÚ   rÜ   rß   râ   rë   rî   rð   rü   rþ   r  r   r   r   r   Ú<module>   sL    $7B!%/ 