U
    L?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|t‚ˆ ||jd¡ ˆ ˆ d¡ˆ| ˆ gks¦t‚ˆ ˆd¡ˆ|ˆgksÀt‚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    úW/var/www/html/venv/lib/python3.8/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   ÚAssertionErrorr   Ú
ValueError)r   r   r    )r   r   r   r   r!   Útest_dict_list   s    r)   c               
      sÚ  t dƒ‰ tdˆ dƒˆ d ks t‚tdˆ dƒˆ d ks8t‚tdˆ dƒˆ d ksPt‚tt‡ fdd„ƒ tdƒ} tddƒ}tˆ d tƒržtˆ d tƒržtˆ d tƒs¢t‚ˆ  ˆ ¡ˆ d ˆ d ˆ d ˆ d ˆ d ˆ d iksØt‚ˆ d jˆ ksêt‚ˆ  	d	d
| ˆ j
g¡}| ˆ ¡|d ˆ d |d ˆ d  t| ƒ ˆ d t| ƒ  |d ˆ d t| ƒ ˆ d t| ƒ  iksht‚ˆ  |¡ˆ d |d t| ƒ |d t| ƒ  ˆ d |d t| ƒ |d t| ƒ  ˆ d |d iksÐt‚t|d ˆ ƒˆ d  t| ƒ | ˆ d t| ƒ |  kst‚t|d ˆ ƒˆ d  t| ƒ | ˆ d t| ƒ |  ksLt‚t|d ˆ ƒdksdt‚t|d ˆ ddˆ d t| ƒ ˆ d t| ƒ  ksœt‚t|d ˆ ddˆ d  t| ƒ ˆ d t| ƒ  ksÖt‚t|d ˆ ddˆ d ksöt‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j
  |ƒˆ d | ˆ j ˆ d | ˆ j  ksPt‚t|d |j |d |j  |d |j
  ˆ ƒ|d  | |j |d | |j  ks¬t‚t|d |d  |d  ˆ ddˆ d ˆ d  t| ƒ ˆ d t| ƒ   ˆ d t| ƒ ˆ d t| ƒ   kst‚t|d |d  |d  ˆ ƒˆ d ˆ d d  td|  ƒ dˆ d  ˆ d  td|  ƒ  ˆ d d td|  ƒ   |   ¡ dks¨t‚t|d |j |d |j  |d |j
  ˆ ƒ|d t| ƒ |d t| ƒ  ˆ j |d t| ƒ |d t| ƒ  ˆ j  |d ˆ j
  ks8t‚t|d |j |d |j  |d |j
  ˆ dd ¡ ˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j
  ks t‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j
  |ƒˆ d t| ƒ ˆ d t| ƒ  |j ˆ d  t| ƒ ˆ d t| ƒ  |j  ˆ d |j
  ks2t‚tˆ d ˆ j ˆ d ˆ j  ˆ d ˆ j
  |dd ¡ |d |j |d |j  |d |j
  ksšt‚| 	dd
|  |j
g¡}dd„ | ˆ ¡ ¡ D ƒ|d ˆ d |d ˆ d |d ˆ d iksöt‚ˆ  	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²t‚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sDt‚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sÖt‚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(   r   Ú
isinstanceZvariable_mapÚframeÚ	orientnewÚzr   r   r   r   r&   Úyr   r   Úitemsr   )r/   Zqdr   r3   r   Úmappingr    r.   r!   Útest_coordinate_vars,   sØ    
ÿþ6: ÿ,   ÿ>>8: Z\ Hÿ
ÿþþÿý
.2ÿÿÿÿ
,ÿ(ÿ
.$&ÿÿÿ
,ÿ(ÿ
&ÿ
 
0ÿ
þþ
ý,
ÿÿ
ÿÿ
ÿ8ÿ
ÿÿ
ÿrA   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s t‚|
 |¡||
j ||	j  ksÂt‚| |¡||j ||
j  ||	j  ksît‚| d
d||jg¡}| |¡dkst‚| |	¡| |j ks2t‚| |
¡| |	j ||
j  ksXt‚| |¡| |	j ||
j  ||
j  ksˆt‚| |¡| |j ks¤t‚|	 |¡||j ks¾t‚|	 |	¡dksÒt‚|	 |
¡| |
j ksît‚|	 |¡| |
j ||
j  kst‚|	 |¡||j ||j  ks8t‚|
 |¡||	j ||	j  ks\t‚|
 |	¡||	j ksvt‚|
 |
¡dksŠt‚|
 |¡| |
j ks¦t‚|
 |¡||	j ||	j  ||j  ksÔt‚| |¡||	j ||	j  ||
j  kst‚| |	¡||	j ||j  ks&t‚| |
¡||
j ks@t‚| |¡dksTt‚| |¡||	j ||	j  ||
j  ||j  ksŒt‚| |¡||j ks¦t‚| |	¡||j ||j  ksÊt‚| |
¡||j ||	j  ||	j  ksøt‚| |¡||j ||	j  ||	j  ||
j  ks0t‚| |¡dksDt‚| |||j ||j  ||j  ¡ | |¡||j ||j  ||j  ks˜t‚| |¡| |j | |j  | |j  ksÌt‚| |¡||j ||j  ||j  | |j  kst‚| |¡| |j | |j  | |j  ||j  ksDt‚t dƒ}t ddƒ}| dd|| ||f¡}| |¡d|| ||  ||  ||    |j d|| ||   ||  ||   |j  d|| ||  ||   ||   |j  ksút‚| dd| ||fd¡}| |¡t	|ƒt	|ƒ | t
|ƒ|  |j t	|ƒt
|ƒ | t	|ƒ|  |j  t
|ƒ| | |j  ks‚t‚| dd| |j|j f¡}| |¡||j|j  ¡  ksÀt‚| |¡| |j|j  ¡  ksæt‚d S )Núq1 q2 q3 q4r+   r3   r   r0   r   r   r   zu1 u2 u3ÚA2r   Úq0r   Ú
Quaternionr,   r   ÚBodyi9  ÚG)r   r   r<   r=   r&   r>   Ú
ang_vel_inr'   Úset_ang_velr   r   Ú	normalize)Úq1Úq2Úq3Úq4Zq1dZq2dZq3dZq4dr3   r   r   r   r   Úu1Úu2Úu3rC   rD   Zq0dr   r   rG   r    r    r!   Útest_ang_velf   sz    ",&0&$$..$8$.8&.4:>
((ÿ(þÿ
,$ÿÿ$rR   c                  C   sh  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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	|ƒ    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slt
‚|	 |¡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sdt
‚d S )NrB   r3   r   r0   r   r   r   r   ÚSpaceÚ123r-   )r   r   r<   r=   r&   r>   ÚdcmÚMatrixr   r   r'   Úexpandr   )rK   rL   rM   rN   r3   r   r   r   r   r   Ztest_matr    r    r!   Útest_dcm¨   s¤    
0ÿ&ÿÿÿ ÿÿÿÿü
,ÿÿÿ$ÿÿþÿ ÿÿ.ÿÿÿ.ÿþûÿ

&<ÿÿÿ(þþÿrX   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„t‚d S )
Nr   r   zC11 C12 C13zC21 C22 C23zC31 C32 C33r+   )ÚlevelÚDCMr   )r   r   rV   Úorientr&   r   r>   r=   rI   r   ÚdtrH   r'   )r   r   Zc11Zc12Zc13Zc21Zc22Zc23Zc31Zc32Zc33Zc11dZc12dZc13dZc21dZc22dZc23dZc31dZc32dZc33drZ   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rvt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|n t‚| |¡| |¡  |¡ ¡ d	ks¤t‚d S )
Núq1:4r3   r   Zaxisr   r   r   rZ   r   )r   r   r<   r&   r>   r=   rU   ÚTrV   r   r   r'   rH   r   r   )	rK   rL   rM   r3   r   r   r   rZ   r   r    r    r!   Útest_w_diff_dcm2î   s,    $ÿÿÿ(&þ&úra   c                  C   s>   G dd„ dt ƒ} | dƒ}| ddd|jg¡}t|| ƒs:t‚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   re   r   r   r0   r   )r   r<   r&   r:   r'   )re   r   r   r    r    r!   Ú$test_orientnew_respects_parent_class  s    rf   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\t‚|j|ksjt‚d S )	Nr3   rK   Úar0   c                 S   s   g | ]}|d  ‘qS ©Ú1r    ©r4   r&   r    r    r!   Ú
<listcomp>  s     z9test_orientnew_respects_input_indices.<locals>.<listcomp>Úb©Úindices)r   r   r<   r=   rn   r'   )r3   rK   r   Zmindsr   r    r    r!   Ú%test_orientnew_respects_input_indices  s    ro   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sÞt‚|j|ksìt‚|j|ksút‚d S )Nr3   rK   rg   r0   z\mathbf{\hat{%s}_%s}r   r+   r,   rl   c                 S   s   g | ]}|d  ‘qS rh   r    rj   r    r    r!   rk   $  s     z8test_orientnew_respects_input_latexs.<locals>.<listcomp>z\mathbf{\hat{%s}_{%s}})Zlatexs)	r   r   r<   r=   ÚnameÚlowerrn   Z
latex_vecsr'   )r3   rK   r   Zdef_latex_vecsrp   rn   Znew_latex_vecsr   r    r    r!   Ú$test_orientnew_respects_input_latexs  s:    
ÿÿ
ÿýÿÿÿýrr   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sZt‚qZt|jƒD ]\}}|j|| ksŒt‚qŒd S )
Nr3   rK   rg   r0   rl   c                 S   s   g | ]}d | d ‘qS )Znotb_ri   r    rj   r    r    r!   rk   8  s     z;test_orientnew_respects_input_variables.<locals>.<listcomp>r1   Ú_)	r   r   r<   r=   rn   Ú	enumerateZvarlistrp   r'   )r3   rK   r   rp   Znew_variablesr   ÚjÚvarr    r    r!   Ú'test_orientnew_respects_input_variables1  s     rw   c                  C   s$   t dƒ} tdƒ}| dd| d¡ d S )Nzu:3ÚIr   ÚspaceZXYZ)r   r   r<   )Úurx   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   r0   é#   r   éF   )r   r<   r>   r[   r=   )r   r   r    r    r!   Útest_issue_11503G  s    r~   c                  C   sÒ   t dƒ} t dƒ}tdƒ\}}| | ||j || j  ¡ |  ||¡|j ksPt‚|  |||¡|j | j fksrt‚| | |¡|jksˆt‚| | ||¡|j| jfks¦t‚|  | |¡dksºt‚| ||¡dksÎt‚d S )Nr3   r   zu1, u2r   )r   r   rI   r&   r>   Zpartial_velocityr'   )r3   r   rO   rP   r    r    r!   Útest_partial_velocityN  s    "r   c               
   C   s&  t dƒ} t dƒ}|  |dtdƒ¡ |  |¡tdddgdddgdddggƒksPt‚| | ¡tdddgdddgdddggƒks~t‚|  |dtdddgdddgdddggƒ¡ | | ¡tdddgdddgdddggƒksÖt‚|  |¡tdddgdddgdddggƒkst‚| | ¡j|  |¡ks"t‚d S )Nr   r   rZ   r-   r+   r   éÿÿÿÿ)r   r[   r   rU   rV   r'   r`   ©r   r   r    r    r!   Útest_issue_11498a  s    ..*.0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s¬t‚ˆd tdˆdƒksÄt‚ˆd tdˆdƒksÜt‚t t‡fdd„ƒ tddddgƒ‰ˆd ˆjkst‚ˆd ˆjks&t‚ˆd ˆjks:t‚t t‡fdd„ƒ t	ˆƒdks^t‚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s¨t‚ˆ ˆd¡ ˆ ˆ¡tdƒksÌt‚d S )&Nc                   S   s   t dƒS r$   ©r   r    r    r    r!   r"   t  r#   z&test_reference_frame.<locals>.<lambda>c                   S   s
   t ddƒS )Nr3   r   rƒ   r    r    r    r!   r"   u  r#   c                   S   s   t dddgƒS )Nr3   r   r+   rƒ   r    r    r    r!   r"   v  r#   c                   S   s   t ddddgƒS )Nr3   r   r+   r,   rƒ   r    r    r    r!   r"   w  r#   c                   S   s   t ddddgdƒS ©Nr3   rg   rl   Úcr   rƒ   r    r    r    r!   r"   x  r#   c                   S   s   t ddddgddgƒS ©Nr3   rg   rl   r…   r   r+   rƒ   r    r    r    r!   r"   y  r#   c                   S   s   t ddddgdddgƒS ©Nr3   rg   rl   r…   r   r+   r,   rƒ   r    r    r    r!   r"   z  r#   c                   S   s   t ddddgdddgdƒS r„   rƒ   r    r    r    r!   r"   {  s    ÿc                   S   s   t ddddgdddgddgƒS r†   rƒ   r    r    r    r!   r"   }  s    ÿc                   S   s    t ddddgdddgdddgƒS r‡   rƒ   r    r    r    r!   r"     s    ÿr3   r   ZN_xr+   ZN_yr,   ZN_zc                      s   ˆ d S )Nr-   r    r    ©r3   r    r!   r"   …  r#   rg   rl   r…   c                      s   ˆ d S )NÚdr    r    rˆ   r    r!   r"   Š  r#   r   r   zq0 q1 q2 q3c                      s   ˆ   ˆdd¡S )NrZ   r   ©r[   r    r   r    r!   r"     r#   c                      s   ˆ   ˆdˆˆˆgd¡S )NrS   Ú222rŠ   r    )r   r3   rK   rL   rM   r    r!   r"   ‘  r#   c                      s    ˆ   ˆdˆˆjdˆj  gd¡S )Nr0   r,   r‹   )r[   r&   r>   r    ©r   r3   rK   r    r!   r"   ’  r#   c                      s   ˆ   ˆdˆ¡S ©Nr0   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 )NrE   r‹   rŠ   r    )r   r3   rD   rK   rL   rM   r    r!   r"   •  r#   c                      s   ˆ   ˆdˆ¡S ©NrE   rŠ   r    )r   r3   rD   r    r!   r"   –  r#   c                      s   ˆ   ˆdˆˆˆg¡S rŽ   rŠ   r    ©r   r3   rD   rK   rL   r    r!   r"   —  r#   c                      s   ˆ   ˆdˆˆˆg¡S )NZFoorŠ   r    r   r    r!   r"   ˜  r#   c                      s   ˆ   ˆdˆˆgd¡S )NrF   Ú232rŠ   r    ©r   r3   rK   rL   r    r!   r"   ™  r#   c                      s   ˆ   ˆdˆˆgd¡S )NrS   r   rŠ   r    r‘   r    r!   r"   š  r#   )r   Ú	TypeErrorr(   r   r   r'   r&   r>   r=   Ústrr   Ú
IndexErrorÚNotImplementedErrorZset_ang_accZ
ang_acc_inr   rI   rH   r    r    )r   r   r3   rD   rK   rL   rM   r!   Útest_reference_frames  sN    r–   c                   C   s   t tdd„ ƒ d S )Nc                   S   s   t dƒS 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   sP  t dƒ\} }}t| ƒ}t| ƒ}t|ƒ}t|ƒ}t|ƒ}t|ƒ}t|| || | ||  || | ||  g|| || | ||  || | ||  g| || || ggƒ}	tdƒ}
tdƒ}| |
d|	¡ | |
¡}|| |  ¡  || ¡   }|| | ¡  || ¡   }t| 	|
j
¡| ƒdks.t‚t| 	|j
¡| ƒdksLt‚d S )Nr_   r   r   rZ   r   )r   r   r   rV   r   r[   rH   Údiffr   r   r>   r'   )rK   rL   rM   Ús1Úc1Ús2Úc2Zs3Úc3rU   r   r   ZAwBZalpha2Zbeta2r    r    r!   Útest_dcm_diff_16824¦  s&    .,þ
rž   c                  C   s2  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st‚| 	|¡|kst‚| 	|¡| ks.t‚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   rV   r&   r>   r=   Zorient_explicitrU   r'   rH   )Ú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s”t‚d S )NrŸ   r    r¡   r   r   )r   rV   r   Z
orient_dcmrU   r'   )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snt‚||kszt‚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   r’   )ÚA1rC   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sˆt‚d S )Nr   r   )r+   r+   r   ZXYXr+   r,   r   )r   Úorient_body_fixedrU   rV   r   r   r'   r   r    r    r!   Útest_orient_bodyõ  s    r±   c               
   C   sx  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sLt‚|
 	|	¡ 
|
¡tt|ƒ t|ƒ | t|ƒ|  gt|ƒ| | gt|ƒ| t|ƒt|ƒ |  ggƒks¼t‚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sÞt‚|
 	|	¡ 
|
¡tt|ƒ| | gt|ƒ t|ƒ | gt|ƒt|ƒ | ggƒks6t‚tdƒtdƒ }	}
|
 |	|||fd¡ |
 	|	¡td	ƒkstt‚d S )
Nr_   úc1:4r+   r   r   Zzxyéƒ   é{   r   )r   r   r   r°   rU   rV   r   r   r'   rH   Ú	to_matrixr   ©rK   rL   rM   rš   rœ   r   rO   rP   rQ   r   r   r    r    r!   Útest_orient_body_advancedü  sL    
8&ÿ4&ÿ&û" ý
&6(ÿ4(ÿüýr·   c               
   C   sx  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sLt‚|
 	|	¡ 
|
¡tt|ƒ t|ƒ | t|ƒ|  gt|ƒ| | gt|ƒ| t|ƒt|ƒ |  ggƒks¼t‚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sÞt‚|
 	|	¡ 
|
¡tt|ƒ| | gt|ƒ t|ƒ | gt|ƒt|ƒ | ggƒks6t‚tdƒtdƒ }	}
|
 |	|||fd¡ |
 	|	¡td	ƒkstt‚d S )
Nr_   r²   r+   r   r   Zyxzr³   r´   r   )r   r   r   Úorient_space_fixedrU   rV   r   r   r'   rH   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sXt‚|jd d d t|ƒt|ƒ |  |¡ t	|ƒ| |¡  ksšt‚|jd d d t|ƒt	|ƒ |  |¡ t|ƒ| |¡  ksÜt‚|jd d d t	|ƒ|  |¡ | |¡ kst‚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°   rH   Úargsr'   r   r˜   r   )ÚpsiÚthetaÚphiÚtr   r   ZA_w_Br    r    r!   Útest_orient_body_simple_ang_velK  s    
(ÿ
(ÿ
r¿   c                  C   sP   t dƒ} t dƒ}| | dd¡ | | ¡tdddgdddgdddggƒksLt‚d S )Nr   r   )r   r   r   rT   r+   r   )r   r¸   rU   rV   r'   r   r    r    r!   Útest_orient_space]  s    rÀ   c                  C   sN   t dƒ} t dƒ}| | d¡ | | ¡tdddgdddgdddggƒksJt‚d S )Nr   r   )r   r   r   r   r   )r   Zorient_quaternionrU   rV   r'   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J}t d¡ |  ||j|¡ t|d jt	ƒs‚t
‚d	t|d jƒks˜t
‚W 5 Q R X 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'   r“   Úmessage)r   r   r   rg   rl   r…   Úwr    r    r!   Útest_looped_frame_warningi  s    
ÿrÍ   c                  C   sØ  t dƒ} t dƒ}t dƒ}tdƒ\}}}| | | j|¡ | j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiksvt‚|j| tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒiks¶t‚|ji ksÄt‚| ||j|¡ | j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒikst‚|j| tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒ|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒiksŠt‚|j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiksÌt‚|  ||j|¡ |j|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒ| tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiksPt‚| j|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒiks’t‚|j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiksÔt‚d S )Nr   r   r   rÂ   r+   r   )	r   r   r%   r&   Ú	_dcm_dictrV   r   r   r'   )r   r   r   rg   rl   r…   r    r    r!   Útest_frame_dicty  s,    @@B6 0ÿB6 0ÿBrÏ   c                  C   s@  t dƒ} t dƒ}t dƒ}t dƒ}tdƒ\}}}| | | j|¡ | ||j|¡ | ||j|¡ |j|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒiksžt‚|j|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒ|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒikst‚|j| tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒ|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiks†t‚| j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiksÈt‚|j|j	ksÚt‚| 
| ¡ t| j	 ¡ ƒ| ||gkst‚t|j	 ¡ ƒ|| gkst‚t| j ¡ ƒ|gks8t‚t|j ¡ ƒ|gksRt‚| j| j	ksdt‚|  ||j|¡ | j|tdddgdt|ƒt|ƒgdt|ƒ t|ƒggƒiks¶t‚| j| j	ksÈt‚|j|tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒ| tdddgdt|ƒt|ƒ gdt|ƒt|ƒggƒiks<t‚d S )Nr   r   r   r   rÂ   r+   r   )r   r   r%   r&   rÎ   rV   r   r   r'   Z
_dcm_cacherU   ÚlistÚkeys)r   r   r   r   rg   rl   r…   r    r    r!   Útest_dcm_cache_dict“  s@    @6 0ÿ6 0ÿB
B6 0ÿrÒ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S ©Nr3   r   ri   Ú2Ú3rm   )r   Úxxr   Úouterr&   r'   ©r3   r   r    r    r!   Útest_xx_dyadµ  s    rÙ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Zxyr   r×   r&   r>   r'   rØ   r    r    r!   Útest_xy_dyad»  s    rÚ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Úxzr   r×   r&   r=   r'   rØ   r    r    r!   Útest_xz_dyadÁ  s    rÜ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Zyxr   r×   r>   r&   r'   rØ   r    r    r!   Útest_yx_dyadÇ  s    rÝ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Úyyr   r×   r>   r'   rØ   r    r    r!   Útest_yy_dyadÍ  s    rß   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Zyzr   r×   r>   r=   r'   rØ   r    r    r!   Útest_yz_dyadÓ  s    rà   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Zzxr   r×   r=   r&   r'   rØ   r    r    r!   Útest_zx_dyadÙ  s    rá   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Zzyr   r×   r=   r>   r'   rØ   r    r    r!   Útest_zy_dyadß  s    râ   c                  C   sR   t dƒ} t ddddgd}| jt | j| j¡ks4t‚|jt |j|j¡ksNt‚d S rÓ   )r   Úzzr   r×   r=   r'   rØ   r    r    r!   Útest_zz_dyadå  s    rä   c                  C   sV   t dƒ} t ddddgd}| j| j| j | j ks6t‚|j|j|j |j ksRt‚d S rÓ   )r   rz   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^t
‚d S )Nr3   r   r+   r   )r   r%   r&   rU   ÚpickleÚloadsÚdumpsÚtuplerÎ   rÑ   r'   )r3   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	   rV   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)   rA   rR   rX   r^   ra   rf   ro   rr   rw   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>   sh   $:B!%/ '("