
    MZde                     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e_        d Zd Z d Z!d Z"d Z#d Z$d Z%d Z&d Z'd Z(d Z)d Z*d Z+d Z,d Z-d Z.d Z/d Z0d Z1d  Z2d! Z3d" Z4d# Z5d$ Z6d% Z7d& Z8d' Z9d( Z:y))    )pi)symbols)cossin)eyezeros)ImmutableDenseMatrix)simplify)ReferenceFrameVectorCoordinateSymdynamicsymbolstime_derivativeexpressdot_check_frame)VectorTypeError)raisesNTc                  f   t        d      t        d      } t        d      t        d      }t        d      t        d      | j                  j                  d       j                  | | j                  d       |j                  j                  d       |j                  d      || gk(  sJ j                  ||j                  d       j                  d      | gk(  sJ j                  d      |gk(  sJ t	        t
        fd	       t	        t
        fd
       y )NABCDEFg      ?r   c                  (     j                  d      S )N   
_dict_list)r   r   s   G/usr/lib/python3/dist-packages/sympy/physics/vector/tests/test_frame.py<lambda>z test_dict_list.<locals>.<lambda>'       q||Aq1     c                  (    j                   d      S Nr   r   )r   r   s   r!   r"   z test_dict_list.<locals>.<lambda>)   r#   r$   )r   orient_axisxr    r   
ValueError)r   r   r   r   r   r   s     @@@@r!   test_dict_listr*      s   sAsAsAsAsAsAMM!QSS#MM!QSS#MM!QSS#<<1!Q1---MM!QSS#<<1!Q***<<1!Q*** :12
:12r$   c                     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       t	        d      } t	        dd      }t        d   t              r&t        d   t              rt        d   t              sJ j                        d   d   d   d   d   d   ik(  sJ d   j                  k(  sJ j                  dd	| j                  g      }|j                        |d   d   |d   d    t        |       z  d   t        |       z  z   |d   d   t        |       z  d   t        |       z  z   ik(  sJ j                  |      d   |d   t        |       z  |d   t        |       z  z
  d   |d   t        |       z  |d   t        |       z  z   d   |d   ik(  sJ t        |d         d    t        |       z  |z  d   t        |       z  |z  z   k(  sJ t        |d         d    t        |       z  |z  d   t        |       z  |z  z
  k(  sJ t        |d         dk(  sJ t        |d   d
      d   t        |       z  d   t        |       z  z   k(  sJ t        |d   d
      d    t        |       z  d   t        |       z  z   k(  sJ t        |d   d
      d   k(  sJ t        d   j                  z  d   j                  z  z   d   j                  z  z   |      d   |z  j                  z  d   |z  j                  z  z
  k(  sJ t        |d   |j                  z  |d   |j                  z  z   |d   |j                  z  z         |d    |z  |j                  z  |d   |z  |j                  z  z   k(  sJ t        |d   |d   z  |d   z  d
      d   d    t        |       z  d   t        |       z  z   z  d   t        |       z  d   t        |       z  z   z  k(  sJ t        |d   |d   z  |d   z        d   d   dz   t        d| z        z  dd   z  d   z  t        d| z        z  z
  d   dz  t        d| z        z  z   z  |z  z
  j!                         dk(  sJ t        |d   |j                  z  |d   |j                  z  z   |d   |j                  z  z         |d   t        |       z  |d   t        |       z  z
  j                  z  |d   t        |       z  |d   t        |       z  z   j                  z  z   |d   j                  z  z   k(  sJ t        |d   |j                  z  |d   |j                  z  z   |d   |j                  z  z   d
      d   j                  z  d   j                  z  z   d   j                  z  z   k(  sJ t        d   j                  z  d   j                  z  z   d   j                  z  z   |      d   t        |       z  d   t        |       z  z   |j                  z  d    t        |       z  d   t        |       z  z   |j                  z  z   d   |j                  z  z   k(  sJ t        d   j                  z  d   j                  z  z   d   j                  z  z   |d
      |d   |j                  z  |d   |j                  z  z   |d   |j                  z  z   k(  sJ |j                  dd	|  |j                  g      }|j                        |d   d   |d   d   |d   d   ik(  sJ j                  dd	| j                  j                  z   j                  z   g      }j                  |      }|d      d|d   z  t        |       z  dz  |d   dz  z   d|d   z  t        | t"        dz  z         z  dz  z
  |d   dz  z   d|d   z  t        | t"        dz  z         z  dz  z
  |d   dz  z   k(  sJ |d      d|d   z  t        | t"        dz  z         z  dz  |d   dz  z   d|d   z  t        |       z  dz  z   |d   dz  z   d|d   z  t        | t"        dz  z         z  dz  z
  |d   dz  z   k(  sJ |d      d|d   z  t        | t"        dz  z         z  dz  |d   dz  z   d|d   z  t        | t"        dz  z         z  dz  z
  |d   dz  z   d|d   z  t        |       z  dz  z   |d   dz  z   k(  sJ y)z,Tests the coordinate variables functionalityr   Axr         c                      t        d d      S )Nr,      )r   )r   s   r!   r"   z&test_coordinate_vars.<locals>.<lambda>2   s    }T1a8 r$   qr   AxisT	variablesNr   r0      N)r   r   r   r)   r   
isinstancevariable_mapframe	orientnewzr   r   r   r   r(   ytrigsimpr   )r1   qdr   r5   r   mappingr   s         @r!   test_coordinate_varsrA   ,   sF
   sAq!$!,,,q!$!,,,q!$!,,,
:89sA	Q	BadM*adM*adM*+ + >>!1adAaD1qtAaD AAAAQ4::??	C!QSS*A>>!1qtQqTAaD5Q<!A$s1v+3M!"1qtCF{QqT#a&['@!B B B B>>!1qtCF{QqT#a&['@!"1qtCF{QqT#a&['@!A$!!N N N N1Q4#!uSV|B1c!fR'GGGG1Q4#!uSV|B1c!fR'GGGG1Q4#q(((1Q4d+qtCF{QqT#a&[/HHHH1Q4d+!uSV|ad3q6k/IIII1Q4d+qt3331Q48ad133h.1acc91=1bqQRtTVwWXWZWZ{AZZZZ1Q48ad133h.1acc91=1Q4133QRSTQUVXQXYZY\Y\Q\A\\\\1Q4!9QqT>15Q4!A$s1v!SV+,ad3q6kAaDQK.GHI I IAaD1IadNA.qTAaD!G8C!H$qtVAaD[QqS!"qT1WS1X !"# %-HJ!4 4 4 1Q48ad133h&1acc115aDQK!A$s1v+%qss*ad3q6kQ4A;/. d133h'' ' ' 1Q48ad133h&1acc11EQ48ad133h1acc)* * *1Q48ad133h&1acc115aDQK!A$s1v+%qss*qTE#a&L1Q4A;&+,./d133h77 7 7 1Q48ad133h&1acc11EQ48ad133h1acc)* * *	C1"acc+A>>!1qtQqT1Q41qt DDDD	C!QSS133Y_!56AnnQG1Q4=AadF3q6M!Oad1f4q1vc!bd(m7KA7MMQ46adF3q2a4x=(*+-.qT!V4 4 4 41Q4=BqtGCBqDM1!3Q46adF3q6M!O$&'d1f-/01vc!bd(m/CA/EFHI!QO O O O1Q4=BqtGCBqDM1!3ad1f<QqT6#a"Q$h-!"$%aDF+-.qtVCF]1_=?@tAvF F F Fr$   c                     t        d      \  } }}}t        dd      \  }}}}t        d      }|j                  dd| |j                  g      }	|	j                  dd||	j                  g      }
|
j                  dd||
j
                  g      }|j                  dd||j
                  g      }t        d	      \  }}}|	j                  |      ||	j                  z  k(  sJ |
j                  |      ||
j                  z  ||	j                  z  z   k(  sJ |j                  |      ||j
                  z  ||
j                  z  z   ||	j                  z  z   k(  sJ |j                  d
d||j
                  g      }|j                  |      dk(  sJ |j                  |	      | |j                  z  k(  sJ |j                  |
      | |	j                  z  ||
j                  z  z
  k(  sJ |j                  |      | |	j                  z  ||
j                  z  z
  ||
j
                  z  z
  k(  sJ |j                  |      | |j
                  z  k(  sJ |	j                  |      ||j                  z  k(  sJ |	j                  |	      dk(  sJ |	j                  |
      | |
j                  z  k(  sJ |	j                  |      | |
j                  z  ||
j
                  z  z
  k(  sJ |	j                  |      ||j                  z  ||j
                  z  z
  k(  sJ |
j                  |      ||	j                  z  ||	j                  z  z   k(  sJ |
j                  |	      ||	j                  z  k(  sJ |
j                  |
      dk(  sJ |
j                  |      | |
j
                  z  k(  sJ |
j                  |      ||	j                  z  ||	j                  z  z   ||j
                  z  z
  k(  sJ |j                  |      ||	j                  z  ||	j                  z  z   ||
j
                  z  z   k(  sJ |j                  |	      ||	j                  z  ||j
                  z  z   k(  sJ |j                  |
      ||
j
                  z  k(  sJ |j                  |      dk(  sJ |j                  |      ||	j                  z  ||	j                  z  z   ||
j
                  z  z   ||j
                  z  z
  k(  sJ |j                  |      ||j
                  z  k(  sJ |j                  |	      ||j
                  z  ||j                  z  z
  k(  sJ |j                  |
      ||j
                  z  ||	j                  z  z
  ||	j                  z  z
  k(  sJ |j                  |      ||j
                  z  ||	j                  z  z
  ||	j                  z  z
  ||
j
                  z  z
  k(  sJ |j                  |      dk(  sJ |j                  |||j                  z  ||j
                  z  z   ||j                  z  z          |j                  |      ||j                  z  ||j
                  z  z   ||j                  z  z   k(  sJ |j                  |      | |j                  z  | |j
                  z  z   | |j                  z  z   k(  sJ |j                  |      ||j                  z  ||j
                  z  z   ||j                  z  z   | |j
                  z  z   k(  sJ |j                  |      | |j                  z  | |j
                  z  z   | |j                  z  z   ||j
                  z  z   k(  sJ t        d      }t        dd      }|j                  dd|| ||f      }|j                  |      d||z  ||z  z   ||z  z
  || z  z
  z  |j                  z  d||z  || z  z   ||z  z
  ||z  z
  z  |j
                  z  z   d||z  ||z  z   || z  z
  ||z  z
  z  |j                  z  z   k(  sJ |j                  dd| ||fd      }|j                  |      t        |      t        |      z  |z  t        |      |z  z   |j                  z  t        |      t        |      z  |z  t        |      |z  z
  |j
                  z  z   t        |      |z  |z   |j                  z  z   k(  sJ |j                  dd| |j                  |j
                  z   f      }|j                  |      ||j                  |j
                  z   j                         z  k(  sJ |j                  |      | |j                  |j
                  z   j                         z  k(  sJ y )Nq1 q2 q3 q4r-   r5   r   r2   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q4q1dq2dq3dq4dr5   r   r   r   r   u1u2u3rD   rE   q0dr   r   rH   s                         r!   test_ang_velrX   c   sw   #M2NBB'q9Cc3sA	C"acc+A	C"acc+A	C"acc+A	C"acc+A
+JBB<<?sACCi'''<<?sACCi3)3333<<?sACCi3)3sACCi????	
T6B9	-B<<?a<<?sd133h&&&<<?sd133hQSS0000<<?sd133hQSS03qss7::::<<tACCx'''<<?c!##g%%%<<?a<<?eACCi'''<<?eACCi#acc'1111<<s133wQSS0000<<?c!##gACC////<<?c!##g%%%<<?a<<?sd133h&&&<<s133wQSS03qss7::::<<?c!##gACC/#acc'9999<<?c!##gACC////<<?c!##g%%%<<?a<<s133wQSS03qss7:SWDDDD==s244x'''==s244x#acc'1111==s133wQSS03qss7::::==s133wQSS03qss7:SWDDDD==!!!MM!RVbf_r!##v-.<<?r133h"acc1RH<<<<<<?sACCiB3)3sACCi????<<?r133h"acc1RH<acczIIII<<?sACCiB3)3sACCi?3)KKKK		B
q
!C	CBB'78A<<?	S2Xb 38+cBh67!##=	S2Xb 38+cBh67!##=	>	S2Xb 38+cBh67!##=	>? ? ?
 	
C"b"s3A<<?BB 3c"gck A133F	RR	s2ws{	*ACC/ 036r73;3Dacc2I J K K K	C"accACCi1A<<?cQSS133Y$9$9$;;;;;<<?sdaccACCi%:%:%<<<<<r$   c                     t        d      \  } }}}t        d      }|j                  dd| |j                  g      }|j                  dd||j                  g      }|j                  dd||j
                  g      }|j                  dd||j
                  g      }|j                  dd	| ||gd
      }	|j                  |      t        t        |        t        |      z  t        |      z  t        |       t        |      z  z   t        |        t        |      z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   gt        |       t        |      z  t        |      t        |      z  t        |       z  z   t        |       t        |      z  t        |       t        |      z  t        |      t        |       z  t        |      z  z
  gt        |       t        |      z  t        |      t        |      t        |      z  gg      k(  sJ |j                  |      t        t        |       t        |      z  t        |      z  t        |      t        |       t        |      z  t        |       t        |      z  t        |      z  z   z  z
  t        |       t        |      z  t        |       t        |      z  t        |      z  z
  t        |      t        |       z  t        |      z  t        |      t        |       t        |      z  t        |       t        |      z  t        |      z  z   z  z   gt        |       t        |      z  t        |      t        |      z  t        |       z  z   t        |       t        |      z  t        |       t        |      z  t        |      t        |       z  t        |      z  z
  gt        |      t        |       z  t        |      z  t        |      t        |      t        |      z  t        |       t        |      z  t        |      z  z   z  z
  t        |      t        |      z  t        |       t        |      z  t        |      z  z
  t        |      t        |      z  t        |       z  t        |      t        |      t        |      z  t        |       t        |      z  t        |      z  z   z  z   gg      z
  }
|
j                         t        dd      k(  sJ |	j                  |      t        t        |      t        |      z  t        |      t        |      z  t        |       gt        |       t        |      z  t        |      z  t        |      t        |       z  z
  t        |       t        |      z  t        |      z  t        |       t        |      z  z   t        |       t        |      z  gt        |       t        |      z  t        |      t        |       z  t        |      z  z   t        |        t        |      z  t        |      t        |      z  t        |       z  z   t        |       t        |      z  gg      k(  sJ y )NrC   r5   r   r2   r   r   r   r   Space123r0   )r   r   r;   r<   r(   r=   dcmMatrixr   r   expandr   )rL   rM   rN   rO   r5   r   r   r   r   r   test_mats              r!   test_dcmr`      s   #M2NBBsA	C"acc+A	C"acc+A	C"acc+A	C"acc+A	C2r2,6A558vr7SW	s2w	&R3r7):	:c"gIB=R3r7"SW,s2wR/@@	BDGGBEb'CG#c"g-E./22wR/@#b'GC"gB'#b'1C2D37:2wYR5H#b'B#b'5	     uuQx&
b'CG
c"g
%BSW9s2w3FB#b'CG#4$ )% %(+Bi#b'&9CGG="g= '"2wR03r7:SW#b'	BIb'CG#c"g-I. >/  /
0 36b'CG2CB#b'CG#3$%(Ws2w%6B#b'8IGc"gB'9(2)+.r7SW+<s2w+FB3r7SW$s2wR'83r7'BBC,DEHWBFb'CG+c"g5F67:2wR7H3r77RB3r7SW,s2wR/@3r7/JJK8L+M	N	O 	OH ??a+++558v
b'#b'/3r73r7?SWH	5	RRR	 3r73r7?	2CGCGOCG4KBB5RR	*,/GCGOBBB- #&r73r7!2SWSW_SW5L!L	RR,	   r$   c            
          t        d      } t        d      }t        d      \  }}}t        d      \  }}}t        d      \  }}	}
t        dd      \  }}}t        dd      \  }}}t        dd      \  }}}t        |||g|||g||	|
gg      }|j                  | d|       |j                  j                  |       }|j                  j                  |       }|j                  j                  |       }|j                  | |j                  t        |j                  |       |j                        z  |j                  t        |j                  |       |j                        z  z   |j                  t        |j                  |       |j                        z  z          ||z  ||z  z   |	|z  z   |j                  z  ||z  ||z  z   |
|z  z   |j                  z  z   ||z  ||z  z   ||z  z   |j                  z  z   }|j                  |       |z
  d	k(  sJ y )
Nr   r   zC11 C12 C13zC21 C22 C23zC31 C32 C33r-   )levelDCMr   )r   r   r]   orientr(   r   r=   r<   rJ   r   dtrI   )r   r   c11c12c13c21c22c23c31c32c33c11dc12dc13dc21dc22dc23dc31dc32dc33drc   b1ab2ab3aexprs                            r!   test_w_diff_dcm1r|      s    	sAsA"=1MCc"=1MCc"=1MCc%m1=D$%m1=D$%m1=D$
	c3	c3	c3 C HHQs33--
C33--
C33--
C MM!QSS#shhqk133/0SS#shhqk133/01SS#shhqk133/01 2
 4x#d("SX-qss24x#d("SX-qss234x#d("SX-qss23D <<?T!Q&&&r$   c                     t        d      \  } }}t        d      }|j                  dd| |j                  g      }|j                  dd||j                  g      }|j                  dd||j
                  g      }|j                  |      j                  }|j                  dd|      }|j                  |      |j                  |      cxk(  r_t        t        |      t        |      z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   t        |       t        |      z  t        |      t        |       z  t        |      z  z
  gt        |       t        |      z  t        |        t        |      z  t        |      z  t        |       t        |      z  z   t        |       t        |      z  t        |      t        |      z  t        |       z  z   gt        |      t        |        t        |      z  t        |       t        |      z  gg      k(  sJ  J |j                  |      |j                  |      z
  j                  |      j                         d	k(  sJ y )
Nq1:4r5   r   axisr   r   r   rc   r   )r   r   r;   r(   r=   r<   r\   Tr]   r   r   rI   r   r
   )	rL   rM   rN   r5   r   r   r   rc   r   s	            r!   test_w_diff_dcm2r      s	   'JBBsA	C"acc+A	C"acc+A	C"acc+A
%%(**C	C$A
 558quuQx 76	RR#b'#b'/#b'1BBRRBBB 	!$'G8CG#3	RRR 3r73r7?2BB#b'#b'/#b'11#3 
R3r7(3r7"CGCGO4+6 $7 7 7 7 7 7 LLOall1o-66q9BBDIIIr$   c                       G d dt               }  | d      }|j                  ddd|j                  g      }t        ||       sJ y )Nc                       e Zd Zy)>test_orientnew_respects_parent_class.<locals>.MyReferenceFrameN)__name__
__module____qualname__ r$   r!   MyReferenceFramer     s    r$   r   r   r   r2   r   )r   r;   r(   r8   )r   r   r   s      r!   $test_orientnew_respects_parent_classr     sC    > A	C!QSS*Aa)***r$   c                  J   t        d      } t        d      }| j                  dd|| j                  g      }| j                  D cg c]  }|dz   	 }}| j                  dd|| j                  g|      }| j                  |j                  k(  sJ |j                  |k(  sJ y c c}w )Nr5   rL   ar2   1b)indices)r   r   r;   r<   r   )r5   rL   r   r(   mindsr   s         r!   %test_orientnew_respects_input_indicesr   	  s    sA		B	C"acc+AII&qQsU&E&	C"accE:A99		!!!99	 's   B c                     t        d      } t        d      }| j                  dd|| j                  g      }d|j                  j                         d|j                  d   dd|j                  j                         d|j                  d	   dd|j                  j                         d|j                  d
   dg}d}| j                  D cg c]  }|dz   	 }}d|j                         d|d   dd|j                         d|d	   dd|j                         d|d
   dg}| j                  |d|| j                  g|      }|j                  |k(  sJ |j                  |k(  sJ |j                  |k7  sJ y c c}w )Nr5   rL   r   r2   z\mathbf{\hat{z}_r   }r-   r.   r   r   z}_{z}})latexs)r   r   r;   r<   namelowerr   
latex_vecs)	r5   rL   r   def_latex_vecsr   r(   r   new_latex_vecsr   s	            r!   $test_orientnew_respects_input_latexsr     sQ   sA		B	C"acc+A 34&&,,.iil$vv||~qyy|(523&&,,.iil$&N Dii(qu(G(48JJLaj"zz|WQZ&148JJLaj"$N 	
D&2qss)NCA<<>)))<<>)))99 )s   E?c                     t        d      } t        d      }| j                  dd|| j                  g      }d}| j                  D cg c]
  }d|z   dz    }}| j                  |d|| j                  g|      }t        |j                        D ]3  \  }}|j                  |j                  d	z   |j                  |   z   k(  r3J  t        |j                        D ]  \  }}|j                  ||   k(  rJ  y c c}w )
Nr5   rL   r   r2   r   notb_r   r3   _)r   r   r;   r<   r   	enumeratevarlistr   )	r5   rL   r   r   r(   new_variablesr   jvars	            r!   'test_orientnew_respects_input_variablesr   .  s    sA		B	C"acc+A D,-II6qWQYs]6M6	D&2qss)}EA199% 7#xx166C<!))A,66667 199% ,#xx=++++, 7s   C5c                  X    t        d      } t        d      }|j                  dd| d       y )Nzu:3Ir   spaceXYZ)r   r   r;   )ur   s     r!   test_issue_10348r   >  s(    uAsAKKWa'r$   c                      t        d      } | j                  ddd| j                  g       t        d      }| j                  |dd|j                  g       y )Nr   r   r2   #   r   F   )r   r;   r=   rd   r<   )r   r   s     r!   test_issue_11503r   D  sF    sAKKVb!##Y'sAHHQQSS	"r$   c                  H   t        d      } t        d      }t        d      \  }}|j                  | ||j                  z  || j                  z  z          | j                  ||      |j                   k(  sJ | j                  |||      |j                   | j                   fk(  sJ |j                  | |      |j                  k(  sJ |j                  | ||      |j                  | j                  fk(  sJ | j                  | |      dk(  sJ |j                  ||      dk(  sJ y )Nr5   r   zu1, u2r   )r   r   rJ   r(   r=   partial_velocity)r5   r   rT   rU   s       r!   test_partial_velocityr   K  s   sAsAH%FBMM!R!##XQSS()a$,,,aR(accTACC4L888a$+++aR(QSS!##J666a$)))a$)))r$   c            
      P   t        d      } t        d      }| j                  |dt        d             | j                  |      t	        g dg dg dg      k(  sJ |j                  |       t	        g dg dg dg      k(  sJ | j                  |dt	        g dg dg d	g             |j                  |       t	        g dg dg d	g      k(  sJ | j                  |      t	        g dg dg d
g      k(  sJ |j                  |       j
                  | j                  |      k(  sJ y )Nr   r   rc   r0   r-   r   r   r   r-   r   r   r   r-   )r   r   )r   r   r   )r   r   r   )r   rd   r   r\   r]   r   r   r   s     r!   test_issue_11498r   ^  s    sAsA HHQs1v558vy)Y?@@@@558vy)Y?@@@@
 HHQvy*jABC558vy*jABBBB558vz9jABBBB558::q!!!r$   c            	          t        t        d        t        t        d        t        t        d        t        t        d        t        t        d        t        t        d        t        t        d        t        t        d        t        t        d	        t        t        d
        t        d      d   t	        dd      k(  sJ d   t	        dd      k(  sJ d   t	        dd      k(  sJ t        t        fd       t        dg d      d   j
                  k(  sJ d   j                  k(  sJ d   j                  k(  sJ t        t        fd       t              dk(  sJ t        d       t        d      t        d      \  t        t         fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd       t        t        fd        t        t        fd!       t        t        fd"       t        t        fd#       t        t        fd$       t        t        fd%       j                  d       j                        t        d      k(  sJ j                  d       j!                        t        d      k(  sJ y )&Nc                      t        d      S r&   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>q  s    nQ/ r$   c                      t        dd      S )Nr5   r   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>r  s    nS!4 r$   c                      t        dddg      S )Nr5   r   r-   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>s  s    ~cAq6: r$   c                      t        dg d      S )Nr5   r   r-   r.   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>t  s    nS)< r$   c                       t        dg dd      S Nr5   r   r   cr   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>u  s    nS/1E r$   c                  $    t        dg dddg      S Nr5   r   r   r-   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>v  s    ~c?QFK r$   c                  $    t        dg dg d      S Nr5   r   r   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>w  s    nS/9M r$   c                  &    t        dg dg dd      S r   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>x  s    nS/1@!E r$   c                  *    t        dg dg dddg      S r   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>z  s    ~c?.=1v G r$   c                  *    t        dg dg dg d      S r   r   r   r$   r!   r"   z&test_reference_frame.<locals>.<lambda>|  s    nS/-<iI r$   r5   r   N_xr-   N_yr.   N_zc                       d   S )Nr0   r   r5   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    qt r$   r   r   r   r   c                       d   S )Ndr   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    qv r$   r   r   zq0 q1 q2 q3c                  *     j                  dd      S )Nrc   r   rd   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq%3 r$   c                  2     j                  dgd      S )NrZ   222r   )r   r5   rL   rM   rN   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq'BB<G r$   c                  d     j                  dj                  dj                  z  z   gd      S )Nr2   r.   r   )rd   r(   r=   r   r5   rL   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s+    ahhq&2qssQW}2EuM r$   c                  *     j                  d      S Nr2   r   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq&"5 r$   c                  ,     j                  dg      S r   r   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    qxx6B48 r$   c                  4     j                  dgd      S )NrF   r   r   )r   r5   rE   rL   rM   rN   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq,RR8H%P r$   c                  *     j                  d      S NrF   r   )r   r5   rE   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq,; r$   c                  0     j                  dg      S r   r   r   r5   rE   rL   rM   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq,RE r$   c                  0     j                  dg      S )NFoor   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    EBB<(H r$   c                  0     j                  dgd      S )NrG   232r   r   r5   rL   rM   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq&2r(EB r$   c                  0     j                  dgd      S )NrZ   r   r   r   s   r!   r"   z&test_reference_frame.<locals>.<lambda>  s    ahhq'B8UC r$   )r   	TypeErrorr)   r   r   r(   r=   r<   strr   
IndexErrorNotImplementedErrorset_ang_acc
ang_acc_inr   rJ   rI   )r   r   r5   rE   rL   rM   rN   s   @@@@@@@r!   test_reference_framer   p  s=   
9/0
945
::;
9<=
9EF
:KL
9MN
9 E F
: G H
9 I JsAQ4=1----Q4=1----Q4=1----
:|$sO,AS6QSS==S6QSS==S6QSS==
:~&q6S==sAsA]+NBB
934
9GH
9MN
956
:89
9PPQ
9;<
9EF
 HI
9BC
9CDMM!Q<<?fQi'''MM!Q<<?fQi'''r$   c                  &    t        t        d        y )Nc                      t        d      S r&   r   r   r$   r!   r"   z"test_check_frame.<locals>.<lambda>  s
    LO r$   )r   r   r   r$   r!   test_check_framer     s    
?34r$   c                     t        d      \  } }}t        |       }t        |       }t        |      }t        |      }t        |      }t        |      }t        ||z  ||z  |z  ||z  z
  ||z  |z  ||z  z   g||z  ||z  |z  ||z  z   ||z  |z  ||z  z
  g| ||z  ||z  gg      }	t	        d      }
t	        d      }|j                  |
d|	       |j                  |
      }||z  | j                         z  ||j                         z  z   }||z  |j                         z  ||j                         z  z   }t        |j                  |
j                        |z
        dk(  sJ t        |j                  |j                        |z
        dk(  sJ y )Nr~   r   r   rc   r   )r   r   r   r]   r   rd   rI   diffr
   r   r=   )rL   rM   rN   s1c1s2c2s3c3r\   r   r   AwBalpha2beta2s                  r!   test_dcm_diff_16824r     s     'JBB	RB	RB	RB	RB	RB	RB
2b5"R%(RU*BrE"Hr"u,<=b5"R%(RU*BrE"Hr"u,<=3"R%BrE24 5C 	sAsAHHQs
,,q/CU2779_r"'')|+FrE"'')Obl*ECGGACCL6)*a///CGGACCL5()Q...r$   c                      t        d      } t        d      }| j                  |t        d             | j                  |      t	        g dg dg dg      k(  sJ y )Nr   r   r0   r   r   r   )r   orient_explicitr   r\   r]   r   s     r!   test_orient_explicitr     sL    sAsAaQ 558vy)Y?@@@@r$   c                     t        d      t        d      j                  j                   d       j                        } j                  j                  d       j                        }j                  dj                          j                        }| |k(  sJ ||k(  sJ t	        t
        fd       y )Nr   r   r-   r   c                  *     j                  dd      S )Nr-   )r'   r   s   r!   r"   z"test_orient_axis.<locals>.<lambda>  s    ammAq!4 r$   )r   r'   r(   r\   r   r   )A1rD   A3r   r   s      @@r!   test_orient_axisr    s    sAsAMM!QSSD!	
qBMM!QSS"	
qBMM!Q	
qB8O88O8
945r$   c            
      |   t        d      } t        d      }|j                  | dd       |j                  |       t        t	        d      t        d      dz  t        d       t	        d      z  gdt	        d      t        d      gt        d      t        d       t	        d      z  t	        d      dz  gg      k(  sJ y )Nr   r   )r-   r-   r   XYXr-   r.   r   )r   orient_body_fixedr\   r]   r   r   r   s     r!   test_orient_bodyr    s    sAsA7E*558vAA	CF73q6>BQAPSTUPVDWZ]^_Z`cfghcibijmnojpbpruvwrxz{r{Y|}~~~~r$   c                  p	   t        d      \  } }}t        d      \  }}}t        dd      \  }}}t        d      t        d      }
}	|
j                  |	| ||fd       |	j	                  |
      t        t        |        t        |      z  t        |      z  t        |       t        |      z  z   t        |        t        |      z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   gt        |       t        |      z  t        |      t        |      z  t        |       z  z   t        |       t        |      z  t        |       t        |      z  t        |      t        |       z  t        |      z  z
  gt        |       t        |      z  t        |      t        |      t        |      z  gg      k(  sJ |
j                  |	      j                  |
      t        t        |       t        |      z  |z  t        |      |z  z   gt        |      |z  |z   gt        |      |z  t        |      t        |      z  |z  z   gg      k(  sJ t        d      t        d      }
}	|
j                  |	| ||fd       |	j	                  |
      t        t        |      t        |       t        |      z  t        |      t        |      z  gt        |      t        |       z  t        |        t        |      z  t        |      t        |       z  t        |      z  z   t        |        t        |      z  t        |      t        |      z  t        |       z  z
  gt        |      t        |       z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   t        |        t        |      z  t        |      z  t        |       t        |      z  z   gg      k(  sJ |
j                  |	      j                  |
      t        t        |      |z  |z   gt        |       t        |      z  |z  gt        |      t        |      z  |z  gg      k(  sJ t        d      t        d      }
}	|
j                  |	|||fd       |
j                  |	      t        d	      k(  sJ y )
Nr~   c1:4r-   r   r   zxy   {   r   )r   r   r   r  r\   r]   r   r   rI   	to_matrixr   rL   rM   rN   r   r   r   rT   rU   rV   r   r   s              r!   test_orient_body_advancedr    s0   'JBBJBB*JBB #s 3qABB</558v
b'CG	c"g	%B#b'(9	9CG8c"g;M	R3r7	SW	$s2wR'8	8	:	R3r7	SWs2w.R8	8#b'CG:K	R3r7	SWs2w.R8	8	:
b'CG	SWc"gB&78: ; ; ; ; <<?$$Q'6
b'CG	b	 3r7R<	/0	R2		R2B#b')B.	./31 ,2 2 2 2 #s 3qABB<-558v	R3r7(SW$c"gB&78	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"gB/#b'9	9	;	R3r7	CGc"g-B7#b'CG:KK
b'CG	c"g	%B#b'(9	9	;	< = = = = <<?$$Q'6	R2	
b'CG	b	 !	R3r7	R	 3" ,# # # # #s 3qABB<-<<?fQi'''r$   c                  p	   t        d      \  } }}t        d      \  }}}t        dd      \  }}}t        d      t        d      }
}	|
j                  |	||| fd       |	j	                  |
      t        t        |        t        |      z  t        |      z  t        |       t        |      z  z   t        |        t        |      z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   gt        |       t        |      z  t        |      t        |      z  t        |       z  z   t        |       t        |      z  t        |       t        |      z  t        |      t        |       z  t        |      z  z
  gt        |       t        |      z  t        |      t        |      t        |      z  gg      k(  sJ |
j                  |	      j                  |
      t        t        |       t        |      z  |z  t        |      |z  z   gt        |      |z  |z   gt        |      |z  t        |      t        |      z  |z  z   gg      k(  sJ t        d      t        d      }
}	|
j                  |	||| fd       |	j	                  |
      t        t        |      t        |       t        |      z  t        |      t        |      z  gt        |      t        |       z  t        |        t        |      z  t        |      t        |       z  t        |      z  z   t        |        t        |      z  t        |      t        |      z  t        |       z  z
  gt        |      t        |       z  t        |       t        |      z  t        |      z  t        |      t        |       z  z   t        |        t        |      z  t        |      z  t        |       t        |      z  z   gg      k(  sJ |
j                  |	      j                  |
      t        t        |      |z  |z   gt        |       t        |      z  |z  gt        |      t        |      z  |z  gg      k(  sJ t        d      t        d      }
}	|
j                  |	|||fd       |
j                  |	      t        d	      k(  sJ y )
Nr~   r  r-   r   r   yxzr
  r  r   )r   r   r   orient_space_fixedr\   r]   r   r   rI   r  r   r  s              r!   test_orient_space_advancedr    s0   'JBBJBB*JBB #s 3qARRL%0558v
b'CG	c"g	%B#b'(9	9CG8c"g;M	R3r7	SW	$s2wR'8	8	:	R3r7	SWs2w.R8	8#b'CG:K	R3r7	SWs2w.R8	8	:
b'CG	SWc"gB&78: ; ; ; ; <<?$$Q'6
b'CG	b	 3r7R<	/0	R2		R2B#b')B.	./31 ,2 2 2 2 #s 3qARRL#.558v	R3r7(SW$c"gB&78	R3r7	SWHs2w.R3r71BSW1LL
b'CG	c"gB/#b'9	9	;	R3r7	CGc"g-B7#b'CG:KK
b'CG	c"g	%B#b'(9	9	;	< = = = = <<?$$Q'6	R2	
b'CG	b	 !	R3r7	R	 3" ,# # # # #s 3qARRL#.<<?fQi'''r$   c                     t        d      \  } }}t         j                  }t        d      }t        d      }|j                  || ||fd       |j	                  |      }|j
                  d   d   |k(  sJ |j
                  d   d   d   t        |      t        |      z  | j                  |      z  t        |      |j                  |      z  z   k(  sJ |j
                  d   d   d   t        |      t        |      z  | j                  |      z  t        |      |j                  |      z  z
  k(  sJ |j
                  d   d   d   t        |      | j                  |      z  |j                  |      z   k(  sJ y)	zThis 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  rI   argsr   r   r   )psithetaphitr   r   A_w_Bs          r!   test_orient_body_simple_ang_velr  ,  se    %%9:OCAsAsAC,e4LLOE::a=q   ::a=A3u:c#h#6sxx{#B#&s8EJJqM#9$: ; ; ;::a=A3u:c#h#6sxx{#B#&s8EJJqM#9$: ; ; ;::a=A#e*SXXa["8388A;"FFFFr$   c                      t        d      } t        d      }|j                  | dd       |j                  |       t        g dg dg dg      k(  sJ y )Nr   r   r   r   r   r[   r   r   r   )r   r  r\   r]   r   s     r!   test_orient_spacer  >  sJ    sAsAGU+558vy)Y?@@@@r$   c                      t        d      } t        d      }|j                  | d       |j                  |       t        g dg dg dg      k(  sJ y )Nr   r   )r   r   r   r   r  )r   orient_quaternionr\   r]   r   s     r!   test_orient_quaternionr"  D  sH    sAsA9%558vy)Y?@@@@r$   c                  
   t        d      } t        d      }t        d      }t        d      \  }}}|j                  | | j                  |       |j                  ||j                  |       t	        j
                  d      5 }t	        j                  d       | j                  ||j                  |       t        |d   j                  t              sJ d	t        |d   j                        v sJ 	 d d d        y # 1 sw Y   y xY w)
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   ws          r!   test_looped_frame_warningr/  J  s    sAsAsAgGAq!MM!QSS!MM!QSS!		 	 $	/ j1h'	aa !B%..+666TWZ[\]_[`[h[hWij 	j j	j j js   A.C99Dc                     t        d      } t        d      }t        d      }t        d      \  }}}|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      ik(  sJ |j                  i k(  sJ |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                  |       |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 y )Nr   r   r   r$  r   r   )r   r   r'   r(   	_dcm_dictr]   r   r   )r   r   r   r   r   r   s         r!   test_frame_dictr2  Z  s(   sAsAsAgGAq!MM!QSS!;;1fiCFSVG0DaQRUVWRXEY%Z[\\\\;;1fiSVSV0Da#a&RUVWRXEY%Z[\\\\;;"MM!QSS!;;1fiCFSVG0DaQRUVWRXEY%Z[\\\\;;1fiSVSV0Da#a&RUVWRXEY%Z[	69a#a&#a&11s1vgs1v2FGHJ J J J;;1fiCFSVG0DaQRUVWRXEY%Z[\\\\MM!QSS!;;1fiSVSV0Da#a&RUVWRXEY%Z[	69aQ#a&11c!fs1v2FGHJ J J J;;1fiSVSV0Da#a&RUVWRXEY%Z[\\\\;;1fiCFSVG0DaQRUVWRXEY%Z[\\\\r$   c                  f   t        d      } t        d      }t        d      }t        d      }t        d      \  }}}|j                  | | j                  |       |j                  ||j                  |       |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      |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 |j                  |        t        | j                  j                               | ||gk(  sJ t        |j                  j                               || gk(  sJ t        | j                  j                               |gk(  sJ t        |j                  j                               |gk(  sJ | j                  | j                  k7  sJ | j                  ||j                  |       | j                  |t        g ddt        |      t        |      gdt        |       t        |      gg      ik(  sJ | j                  | j                  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 y )Nr   r   r   r   r$  r   r   )r   r   r'   r(   r1  r]   r   r   
_dcm_cacher\   listkeys)r   r   r   r   r   r   r   s          r!   test_dcm_cache_dictr7  t  s   sAsAsAsAgGAq!MM!QSS!MM!QSS!MM!QSS!;;1fiSVSV0Da#a&RUVWRXEY%Z[\\\\;;1fiSVSV0Da#a&RUVWRXEY%Z[	69aQ#a&11c!fs1v2FGHJ J J J;;1fiSVSV0Da#a&RUVWRXEY%Z[	69aQ#a&11c!fs1v2FGHJ J J J;;1fiCFSVG0DaQRUVWRXEY%Z[\\\\;;!,,&&&EE!H!!#$Aq	111!!#$A...  "#s***  "#s***;;!,,&&&MM!QSS!;;1fiSVSV0Da#a&RUVWRXEY%Z[\\\\;;!,,&&&;;1fiCFSVG0DaQRUVWRXEY%Z[	69aQ#a&11c!fs1v2FGHJ J J Jr$   );sympy.core.numbersr   sympy.core.symbolr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   r   sympy.matrices.immutabler	   r]   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*   rA   rX   r`   r|   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r  r"  r/  r2  r7  r   r$   r!   <module>rC     s    ! % ? - C ,' ' ' 4 7 ' 344Fn?=DB#'JJ,+	 4, (#*&"$,(^5/@A6$(N%(PG$AAj ]4 Jr$   