o
    GZŽhós  ã                   @   sø  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
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 dl 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,„ Z0d-d.„ Z1d/d0„ Z2d1d2„ Z3d3d4„ Z4d5d6„ Z5d7d8„ Z6d9d:„ Z7d;d<„ Z8d=d>„ Z9d?d@„ Z:dAdB„ Z;dCdD„ Z<dEdF„ Z=dGdH„ Z>dIdJ„ Z?dKdL„ Z@dMdN„ ZAdOdP„ ZBdQdR„ ZCdSdT„ ZDdUdV„ ZEdWdX„ ZFdYdZ„ ZGd[d\„ ZHdS )]é    )Úpi)Úsymbols)Útrigsimp)ÚcosÚsin)ÚeyeÚzeros)ÚImmutableDenseMatrix©Úsimplify)ÚReferenceFrameÚVectorÚCoordinateSymÚdynamicsymbolsÚtime_derivativeÚexpressÚdot©Ú_check_frame)ÚVectorTypeError)ÚraisesNc                     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    úT/var/www/auris/lib/python3.10/site-packages/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sKJ ‚| dd
|  |j	g¡}dd„ | ˆ ¡ ¡ D ƒ|d ˆ d |d ˆ d |d ˆ d iksyJ ‚ˆ  dd
| ˆ jˆ j ˆ j	 g¡}ˆ  |¡}t|ˆ 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 ‚t|ˆ 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 ‚t|ˆ d  ƒd|d  t
| td  ƒ d |d d  d|d  t| td  ƒ d  |d d  d|d  t| ƒ d  |d d  ksiJ ‚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ÚNc                 S   s   i | ]	\}}||  ¡ “qS r    r
   )Ú.0ÚkÚvr    r    r!   Ú
<dictcomp>X   s    z(test_coordinate_vars.<locals>.<dictcomp>r   r,   é   éþÿÿÿN)r   r   r   r'   r   Ú
isinstanceZvariable_mapÚframeÚ	orientnewÚzr   r   r   r   r&   Úyr   r   Úitemsr   )r.   Zqdr   r2   r   Úmappingr    r-   r!   Útest_coordinate_vars,   sÐ   
ÿþ6:$
ÿ,0
ÿ>>8: Z\ H
ÿÿþþÿ
ý.2ÿÿÿ
ÿ,ÿ,
ÿ.$&ÿÿ
ÿ,ÿ,
ÿ&
ÿ 
0ÿ
þþ
ý,
ÿÿ
ÿÿ
ÿ8ÿ
ÿÿ
ÿ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*   r2   r   r/   r   r   r   zu1 u2 u3ÚA2r   Úq0r   Ú
Quaternionr+   r   ÚBodyi9  ÚG)r   r   r;   r<   r&   r=   Ú
ang_vel_inÚset_ang_velr   r   Ú	normalize)Úq1Úq2Úq3Úq4Zq1dZq2dZq3dZq4dr2   r   r   r   r   Úu1Úu2Úu3rB   rC   Zq0dr   r   rF   r    r    r!   Útest_ang_velf   sz   ",$.&$$..$8$.8&.4:>
((ÿ(þ
ÿ,$ÿÿ$*rQ   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 )NrA   r2   r   r/   r   r   r   r   ÚSpaceÚ123r,   )r   r   r;   r<   r&   r=   ÚdcmÚMatrixr   r   Úexpandr   )rJ   rK   rL   rM   r2   r   r   r   r   r   Ztest_matr    r    r!   Útest_dcm¨   s    
0ÿ&ÿÿÿÿÿÿÿü
,ÿÿÿ$ÿÿþÿÿÿ.ÿÿÿ.ÿþûÿ

&<ÿÿÿ(þþÿrW   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   rU   Úorientr&   r   r=   r<   rH   r   ÚdtrG   )r   r   Zc11Zc12Zc13Zc21Zc22Zc23Zc31Zc32Zc33Zc11dZc12dZc13dZc21dZc22dZc23dZc31dZc32dZc33drY   Zb1aZb2aZb3aÚexprr    r    r!   Útest_w_diff_dcm1É   s8   ýÿþÿþr]   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:4r2   r   Zaxisr   r   r   rY   r   )r   r   r;   r&   r=   r<   rT   ÚTrU   r   r   rG   r   r   )	rJ   rK   rL   r2   r   r   r   rY   r   r    r    r!   Útest_w_diff_dcm2î   s,   $ÿÿÿ(&þ&ú*r`   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    rd   r   r   r/   r   )r   r;   r&   r9   )rd   r   r   r    r    r!   Ú$test_orientnew_respects_parent_class  s   re   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 )	Nr2   rJ   Úar/   c                 S   ó   g | ]}|d  ‘qS ©Ú1r    ©r3   r&   r    r    r!   Ú
<listcomp>  ó    z9test_orientnew_respects_input_indices.<locals>.<listcomp>Úb©Úindices)r   r   r;   r<   ro   )r2   rJ   r   Zmindsr   r    r    r!   Ú%test_orientnew_respects_input_indices  s   rp   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 )Nr2   rJ   rf   r/   z\mathbf{\hat{%s}_%s}r   r*   r+   rm   c                 S   rg   rh   r    rj   r    r    r!   rk   $  rl   z8test_orientnew_respects_input_latexs.<locals>.<listcomp>z\mathbf{\hat{%s}_{%s}})Zlatexs)r   r   r;   r<   ÚnameÚlowerro   Z
latex_vecs)r2   rJ   r   Zdef_latex_vecsrq   ro   Znew_latex_vecsr   r    r    r!   Ú$test_orientnew_respects_input_latexs  s:   
ÿÿ
ÿýÿÿÿýrs   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 )
Nr2   rJ   rf   r/   rm   c                 S   s   g | ]}d | d ‘qS )Znotb_ri   r    rj   r    r    r!   rk   8  ó    z;test_orientnew_respects_input_variables.<locals>.<listcomp>r0   Ú_)r   r   r;   r<   ro   Ú	enumerateZvarlistrq   )r2   rJ   r   rq   Znew_variablesr   ÚjÚvarr    r    r!   Ú'test_orientnew_respects_input_variables1  s    ÿry   c                  C   s$   t dƒ} tdƒ}| dd| d¡ d S )Nzu:3ÚIr   ÚspaceZXYZ)r   r   r;   )Úurz   r    r    r!   Útest_issue_10348A  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   r;   r=   rZ   r<   )r   r   r    r    r!   Útest_issue_11503G  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 )Nr2   r   zu1, u2r   )r   r   rH   r&   r=   Zpartial_velocity)r2   r   rN   rO   r    r    r!   Útest_partial_velocityN  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   rY   r,   ©r*   r   r   ©r   r*   r   ©r   r   r*   )r   r   éÿÿÿÿ)r…   r   r   )r   r…   r   )r   rZ   r   rT   rU   r_   ©r   r   r    r    r!   Útest_issue_11498a  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"   t  ó    z&test_reference_frame.<locals>.<lambda>c                   S   s
   t ddƒS )Nr2   r   r‰   r    r    r    r!   r"   u  s   
 c                   S   s   t dddgƒS )Nr2   r   r*   r‰   r    r    r    r!   r"   v  ó    c                   S   s   t dg d¢ƒS )Nr2   ©r   r*   r+   r‰   r    r    r    r!   r"   w  r‹   c                   S   s   t dg d¢dƒS ©Nr2   ©rf   rm   Úcr   r‰   r    r    r    r!   r"   x  ó    c                   S   s   t dg d¢ddgƒS ©Nr2   rŽ   r   r*   r‰   r    r    r    r!   r"   y  rl   c                   S   s   t dg d¢g d¢ƒS ©Nr2   rŽ   rŒ   r‰   r    r    r    r!   r"   z  rl   c                   S   s   t dg d¢g d¢dƒS r   r‰   r    r    r    r!   r"   {  s   
 ÿc                   S   s   t dg d¢g d¢ddgƒS r‘   r‰   r    r    r    r!   r"   }  ó   
 ÿc                   S   s   t dg d¢g d¢g d¢ƒS r’   r‰   r    r    r    r!   r"     r“   r2   r   ZN_xr*   ZN_yr+   ZN_zc                      ó   ˆ d S )Nr,   r    r    ©r2   r    r!   r"   …  rŠ   rŽ   rf   rm   r   c                      r”   )NÚdr    r    r•   r    r!   r"   Š  rŠ   r   r   zq0 q1 q2 q3c                      s   ˆ   ˆdd¡S )NrY   r   ©rZ   r    r†   r    r!   r"     r‹   c                      s   ˆ   ˆdˆˆˆgd¡S )NrR   Ú222r—   r    )r   r2   rJ   rK   rL   r    r!   r"   ‘  s    c                      s    ˆ   ˆdˆˆjdˆj  gd¡S )Nr/   r+   r˜   )rZ   r&   r=   r    ©r   r2   rJ   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 )NrD   r˜   r—   r    )r   r2   rC   rJ   rK   rL   r    r!   r"   •  rt   c                      rš   ©NrD   r—   r    )r   r2   rC   r    r!   r"   –  r‹   c                      ó   ˆ   ˆdˆˆˆg¡S rœ   r—   r    ©r   r2   rC   rJ   rK   r    r!   r"   —  rl   c                      r   )NZFoor—   r    rž   r    r!   r"   ˜  rl   c                      ó   ˆ   ˆdˆˆgd¡S )NrE   Ú232r—   r    ©r   r2   rJ   rK   r    r!   r"   ™  rl   c                      rŸ   )NrR   r    r—   r    r¡   r    r!   r"   š  rl   )r   Ú	TypeErrorr'   r   r   r&   r=   r<   Ústrr   Ú
IndexErrorÚNotImplementedErrorZset_ang_accZ
ang_acc_inr   rH   rG   r    r    )r   r   r2   rC   rJ   rK   rL   r!   Útest_reference_frames  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 )Nr^   r   r   rY   r   )r   r   r   rU   r   rZ   rG   Údiffr   r   r=   )rJ   rK   rL   Ús1Úc1Ús2Úc2Zs3Úc3rT   r   r   ZAwBZalpha2Zbeta2r    r    r!   Útest_dcm_diff_16824¦  s&   .,þ
 r®   c                  C   s,  t dƒ\} }}t dƒ\}}}t dƒ\}}}t ddƒ\}	}
}t ddƒ\}}}t ddƒ\}}}tdƒ}tdƒ}t| ||g|||g|||ggƒ}|| ||  ||  |j ||	 ||  ||  |j  | | ||
  ||  |j  }| ||¡ | |¡|ksJ ‚| |¡|ksŠJ ‚| |¡| ks”J ‚d S )Núc_{xx}, c_{yy}, c_{zz}úc_{xy}, c_{xz}, c_{yx}úc_{yz}, c_{zx}, c_{zy}r*   r   r   )	r   r   rU   r&   r=   r<   Zorient_explicitrT   rG   )ÚcxxÚcyyÚczzÚcxyÚcxzÚcyxÚcyzÚczxÚczyZdcxxZdcyyZdczzZdcxyZdcxzZdcyxZdcyzZdczxZdczyr   r   ÚB_C_AZB_w_Ar    r    r!   Útest_orient_explicitÆ  s*   
þÿþr¼   c                  C   s˜   t dƒ\} }}t dƒ\}}}t dƒ\}}}t| ||g|||g|||ggƒ}	tdƒ}
tdƒ}| |
|	¡ | |
¡t| ||g|||g|||ggƒksJJ ‚d S )Nr¯   r°   r±   r   r   )r   rU   r   Z
orient_dcmrT   )r²   r³   r´   rµ   r¶   r·   r¸   r¹   rº   r»   r   r   r    r    r!   Útest_orient_dcmÚ  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&   rT   r   r¢   )ÚA1rB   Z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   ZXYXr*   r+   r   )r   Úorient_body_fixedrT   rU   r   r   r†   r    r    r!   Útest_orient_bodyõ  s   nrÁ   c               
   C   st  t dƒ\} }}tdƒ\}}}t ddƒ\}}}tdƒtdƒ}	}
|
 |	| ||f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|ƒ|  gt|ƒ| | gt|ƒ| t|ƒt|ƒ |  ggƒksÜJ ‚tdƒtdƒ}	}
|
 |	| ||fd¡ |	 |
¡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smJ ‚|
 |	¡ 	|
¡tt|ƒ| | gt|ƒ t|ƒ | gt|ƒt|ƒ | ggƒks™J ‚tdƒtdƒ}	}
|
 |	|||fd¡ |
 |	¡t
d	ƒks¸J ‚d S )
Nr^   úc1:4r*   r   r   Zzxyéƒ   é{   r   )r   r   r   rÀ   rT   rU   r   r   rG   Ú	to_matrixr   ©rJ   rK   rL   rª   r¬   r­   rN   rO   rP   r   r   r    r    r!   Útest_orient_body_advancedü  sL   
8&ÿ4&ÿ&û" ý
&6(ÿ4(ÿüýrÇ   c               
   C   st  t dƒ\} }}tdƒ\}}}t ddƒ\}}}tdƒtdƒ}	}
|
 |	||| f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|ƒ|  gt|ƒ| | gt|ƒ| t|ƒt|ƒ |  ggƒksÜJ ‚tdƒtdƒ}	}
|
 |	||| fd¡ |	 |
¡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smJ ‚|
 |	¡ 	|
¡tt|ƒ| | gt|ƒ t|ƒ | gt|ƒt|ƒ | ggƒks™J ‚tdƒtdƒ}	}
|
 |	|||fd¡ |
 |	¡t
d	ƒks¸J ‚d S )
Nr^   rÂ   r*   r   r   ZyxzrÃ   rÄ   r   )r   r   r   Úorient_space_fixedrT   rU   r   r   rG   rÅ   r   rÆ   r    r    r!   Útest_orient_space_advanced#  sL   
8&ÿ4&ÿ&û" ý
&6(ÿ4(ÿüý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 )	zƒ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   ZZXZr   r*   r+   N)	r   Z_tr   rÀ   rG   Úargsr   r¨   r   )ÚpsiÚthetaÚphiÚtr   r   ZA_w_Br    r    r!   Útest_orient_body_simple_ang_velK  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   rS   r‚   rƒ   r„   )r   rÈ   rT   rU   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   Zorient_quaternionrT   rU   r†   r    r    r!   Útest_orient_quaternionc  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   rf   rm   r   Úwr    r    r!   Útest_looped_frame_warningi  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_dictrU   r   r   )r   r   r   rf   rm   r   r    r    r!   Útest_frame_dicty  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ß   rU   r   r   Z
_dcm_cacherT   ÚlistÚkeys)r   r   r   r   rf   rm   r   r    r    r!   Útest_dcm_cache_dict“  s:   >40
ÿ40
ÿ>
@40ÿrã   c                  C   óP   t dƒ} t dg d¢d}| jt | j| j¡ksJ ‚|jt |j|j¡ks&J ‚d S ©Nr2   r   )ri   Ú2Ú3rn   )r   Úxxr   Úouterr&   ©r2   r   r    r    r!   Útest_xx_dyadµ  ó   rë   c                  C   óP   t dƒ} t dg d¢d}| jt | j| j¡ksJ ‚|jt |j|j¡ks&J ‚d S rå   )r   Zxyr   ré   r&   r=   rê   r    r    r!   Útest_xy_dyad»  rì   rî   c                  C   rí   rå   )r   Úxzr   ré   r&   r<   rê   r    r    r!   Útest_xz_dyadÁ  rì   rð   c                  C   rí   rå   )r   Zyxr   ré   r=   r&   rê   r    r    r!   Útest_yx_dyadÇ  rì   rñ   c                  C   rä   rå   )r   Úyyr   ré   r=   rê   r    r    r!   Útest_yy_dyadÍ  rì   ró   c                  C   rí   rå   )r   Zyzr   ré   r=   r<   rê   r    r    r!   Útest_yz_dyadÓ  rì   rô   c                  C   rí   rå   )r   Zzxr   ré   r<   r&   rê   r    r    r!   Útest_zx_dyadÙ  rì   rõ   c                  C   rí   rå   )r   Zzyr   ré   r<   r=   rê   r    r    r!   Útest_zy_dyadß  rì   rö   c                  C   rä   rå   )r   Úzzr   ré   r<   rê   r    r    r!   Útest_zz_dyadå  rì   rø   c                  C   sT   t dƒ} t dg d¢d}| j| j| j | j ksJ ‚|j|j|j |j ks(J ‚d S rå   )r   r|   rè   rò   r÷   rê   r    r    r!   Útest_unit_dyadicë  s    rù   c                  C   sb   t dƒ} t dƒ}| | | jd¡ | | ¡}t t | ¡¡}t|j 	¡ ƒd }| |¡|ks/J ‚d S )Nr2   r   r*   r   )
r   r%   r&   rT   ÚpickleÚloadsÚdumpsÚtuplerß   râ   )r2   r   ZA_C_NZN1r¾   r    r    r!   Útest_pickle_frameò  s   
rþ   )IZsympy.core.numbersr   Zsympy.core.symbolr   Zsympy.simplifyr   Z(sympy.functions.elementary.trigonometricr   r   Zsympy.matrices.denser   r   Zsympy.matrices.immutabler	   rU   Zsympy.simplify.simplifyr   Zsympy.physics.vectorr   r   r   r   r   r   r   Zsympy.physics.vector.framer   Zsympy.physics.vector.vectorr   Zsympy.testing.pytestr   rÖ   rú   r(   r@   rQ   rW   r]   r`   re   rp   rs   ry   r}   r€   r   r‡   r¦   r§   r®   r¼   r½   r¿   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>   sj    $:B!%/ '("