
    xf                        d dl Z d dlZd dlZd dlmZmZ d dlmZ d dlm	Z	m
Z
 d dlmZ d dlmZ d dlZd dl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 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/ Z9d0 Z:d1 Z;d2 Z<d3 Z=d4 Z>d5 Z?d6 Z@d7 ZAd8 ZBd9 ZCd: ZDd; ZEd< ZFd= ZGd> ZHd? ZId@ ZJdA ZKdB ZLdC ZMdD ZNdE ZOdF ZPdG ZQdH ZRdI ZSdJ ZTdK ZUdL ZVdM ZWdN ZXdO ZYdP ZZdQ Z[dR Z\dS Z]dT Z^dU Z_dV Z`dW ZadX ZbdY ZcdZ Zdd[ Zed\ Zfd] Zgd^ Zhd_ Zid` Zjda Zkdb Zldc Zmdd Znde Zodf Zpdg Zqdh Zrdi Zsdj Ztdk Zudl Zvdm Zwdn Zxy)o    N)assert_equalassert_array_almost_equal)assert_allclose)RotationSlerp)special_ortho_group)permutationsc                      t        j                  g dg dg      } t        j                  |       }| t        j                  dgdgg      z  }t	        |j                         |       y )N      r   r   )      r   r   r      nparrayr   	from_quatr   as_quatxrexpected_quats      M/usr/lib/python3/dist-packages/scipy/spatial/transform/tests/test_rotation.pytest_generic_quat_matrixr      sP    
,./A1A1#t--Maiik=9    c                      t        j                  g d      } t        j                  |       }| dz  }t	        |j                         |       y Nr   r   r   r   s      r   test_from_single_1d_quaternionr      s9    
A1AEMaiik=9r   c                      t        j                  g dg      } t        j                  |       }| dz  }t	        |j                         |       y r   r   r   s      r   test_from_single_2d_quaternionr!      s;    
, A1AEMaiik=9r   c            	          t        j                  g dg dg dg dg dg dg      } t        j                  |       }| t        j                  dgdgd	gd
gd	gd
gg      z  }t	        |j                         |       y )N)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   s      r   test_from_square_quat_matrixr*   #   su    

 	A 	1A1#taS1#sQC!@AAMaiik=9r   c                  *   t        j                  g dg dg dg dg dg      } t        j                  |       }t        j                  |       t         j
                  j                  | d      d d d f   z  }t        |j                  d	      |       y )
N)r&   r   r   r   )r   r&   r   r   )r   r   r&   r   r'   r(   r%   axisT)	canonical)	r   r   r   r   abslinalgnormr   r   r   s      r   *test_quat_double_to_canonical_single_coverr2   2   sw    

 	A 	1AFF1I		qq 9!T' BBMaii$i7Gr   c                      t        j                  t              5  t        j                  t        j                  g d             d d d        y # 1 sw Y   y xY w)Nr%   r)   r   pytestraises
ValueErrorr   r   r   r    r   r   test_malformed_1d_from_quatr:   ?   s:    	z	" 0288I./0 0 0s   +AAc                      t        j                  t              5  t        j                  t        j                  g dg dg             d d d        y # 1 sw Y   y xY w)N)r%   r)   r   r   r   )r   r            r5   r9   r   r   test_malformed_2d_from_quatr?   D   sG    	z	" 288%  	  s   /AAc                      t        j                  g dg dg dg      } t        j                  t              5  t        j                  |        d d d        y # 1 sw Y   y xY w)Nr   )r   r   r   r   r#   )r   r   r6   r7   r8   r   r   )r   s    r   test_zero_norms_from_quatrA   L   sO    
 	A
 
z	" 1  s   AA c                      g d} t        j                  |       j                         }t        |t	        j
                  d             y )Nr$   r   )r   r   	as_matrixr   r   eye)quatmats     r   #test_as_matrix_single_1d_quaternionrG   V   s2    D


T
"
,
,
.Cc266!9-r   c                      g dg} t        j                  |       j                         }t        |j                  d       t        j                  g dg dg dg      }t        |d   |       y )Nr   r   r%   r%   r%   r   r   r   r&   r   r%   r   r   r   r   r%   r   r   r   rC   r   shaper   r   r   )rE   rF   expected_mats      r   #test_as_matrix_single_2d_quaternionrQ   ]   s\    >D


T
"
,
,
.CI&88
 L
 c!fl3r   c                     g dg dg dg dg} t        j                  |       j                         }t        |j                  d       t        j                  g dg dg dg      }t        |d	   |       t        j                  g dg d
g dg      }t        |d   |       t        |d   t        j                  d             t        |d   t        j                  d             y )NrI   r   r%   r   r%   r$   r'   )r   r   r   rK   rL   rM   r   r   r%   r   r&   r   r   r%   r)   r   )	r   r   rC   r   rO   r   r   r   rD   )quatsrF   	expected0	expected1s       r    test_as_matrix_from_square_inputrY   i   s    	E 

U
#
-
-
/CI&
 I
 c!fi0
 I
 c!fi0c!fbffQi0c!fbffQi0r   c                     g dg dg dg} t        j                  |       j                         }t        |j                  d       t        j                  g dg dg dg      }t        |d   |       t        j                  g dg d	g d
g      }t        |d   |       t        j                  g dg dg dg      dz  }t        |d   |       y )NrI   rS   r%   r)   r   r   )r   r   r   rK   rL   rM   r   rT   rU   r%   )皙?g@)gffffff@r%   r\   )r&   r)   r)   r   r)   rN   )rV   rF   rW   rX   	expected2s        r   !test_as_matrix_from_generic_inputr_      s    E
 

U
#
-
-
/CI&
 I
 c!fi0
 I
 c!fi0
  	I
 c!fi0r   c                  |    g dg dg dg} g d}t        t        j                  |       j                         |       y )NrM   rL   rT         ?rb   rb   rb   )r   r   from_matrixr   rF   r   s     r   test_from_single_2d_matrixre      s;    C
 )M  %--/r   c                     t        j                  g dg dg dg      j                  d      } t        j                  g d      j                  d      }t        t	        j
                  |       j                         |       y )NrM   rL   rT   rJ   ra   r%   r   )r   r   reshaper   r   rc   r   rd   s     r   test_from_single_3d_matrixri      sk    
((
  79	 
 HH12::6BM  %--/r   c                     t        j                  g d      t        j                  d      z  } t        j                  g dg dg dg      }t        t	        j
                  |      j                         |        t        t	        j
                  |j                  d            j                         | j                  d             y )N)r%   r%   r<   r%   '   )3~˷g6iпnzo?)rm   rl   6i?)rn   rm   g3~˷?rJ   rg   )r   r   sqrtr   r   rc   r   rh   )r   rF   s     r   test_from_matrix_calculationrp      s    HH\*RWWR[8M
((/.- C
   %--/   Y!78@@B!!&)+r   c                      t        j                  ddd      } t        t        j                  |       j                         |        y )Nr   
   r   )sizerandom_state)r   rvsr   r   rc   rC   )rF   s    r    test_matrix_calculation_pipelinerv      s4    

!
!!"1
=Ch2237AACSIr   c                  l   t         j                  j                  d      } | j                  d      }t	        j
                  |      j                         }t        j                  d||j                  d            }t        j                  d      }t        d      D ]  }d|d d ||f<    t        ||       y )Nr   )d   r   r   ...ij,...jk->...ik)r   r)   r%   r         ?)r   randomRandomStaterandom_sampler   rc   rC   einsum	transposezerosranger   )rndrF   	ortho_matmult_resulteye3dis         r   test_from_matrix_ortho_outputr      s    
))


"C


K
(C$$S)335I))0)%//	:<K HH[!E1X aAg k51r   c                      g d} t        j                  g d      }t        j                  |       }t	        |j                         |       y NrL   )gJ?r   r   g<(?r   r   r   from_rotvecr   r   rotvecr   results      r   test_from_1d_single_rotvecr      s7    FHH9:M!!&)Ffnn.>r   c                      g dg} t        j                  g dg      }t        j                  |       }t	        |j                         |       y r   r   r   s      r   test_from_2d_single_rotvecr      s<    [FHH:;<M!!&)Ffnn.>r   c                      g dg dg dg} t        j                  g dg dg dg      }t        t        j                  |       j                         |       y )Nr%   r)   r)   r%   r&   rb   r   r   r   )g.pG?cU(G?r   gQH?)gNZ]O?gNZ]OݿgNZ]O?g0i?r$   )r   r   r   r   r   r   )r   r   s     r   test_from_generic_rotvecr      sT    F
 HH45
 M
   (002r   c                     t        j                  dt        j                  d      z  dt        j                  d      z  dt        j                  d      z  gg dg dg      } t        j                  |       j                         }t        |d   d       t        |dd df   | d   d	z         t        |d
   d       t        |dd df   t        j                  g d             t        |d   t        j                  g d             y )NgMb@?r   gMb@)皙?g333333?r\   r   )r   r   r%   r   rb   r%   r   g@w?)g>}J?g.e?g>}J?r)   r$   )r   r   ro   r   r   r   r   r   )r   rE   s     r   test_from_rotvec_small_angler      s    XX	
	EBGGAJ.rwwqz0AB
 F '//1DDJ"DBQBKS1DJ	*BQBKHH   a"((<01r   c                  l   dt        j                  d      z  dt        j                  d      z  dt        j                  d      z  g} t        j                  | d      }|j	                         }t        j
                  |       }t        j                  |      }|j	                         }t        ||       y )Nrz   r   Tdegrees)r   cbrtr   r   r   deg2radr   )rotvec1rot1quat1rotvec2rot2quat2s         r   test_degrees_from_rotvecr     s    RWWQZrwwqz!133CDG6DLLNEjj!G(DLLNEE5!r   c                      t        j                  t        d      5  t        j                  ddg       d d d        y # 1 sw Y   y xY w)N Expected `rot_vec` to have shapematchr%   r)   r6   r7   r8   r   r   r9   r   r   test_malformed_1d_from_rotvecr   "  s8    	z)K	L %aV$% % %	   =Ac                      t        j                  t        d      5  t        j                  g dg dg       d d d        y # 1 sw Y   y xY w)Nr   r   r[   r   r<   r=   r>   r   r9   r   r   test_malformed_2d_from_rotvecr   '  s?    	z)K	L  	     AA
c            	         t        j                  g dg dg dg      } | t         j                  j                  | d      d d d f   z  } t	        j
                  |       j                         }t         j                  j                  |d      }t        | d d df   t        j                  |dz               t        t        j                  || d d d df         t        j                  d             y )	Nr%   r)   r&   rb   r%   r&   r%   ga2U0*3?r$   r%   r,   r   r)   )r   r   )r   r   r0   r1   r   r   	as_rotvecr   coscrossr   )rE   r   angles      r   test_as_generic_rotvecr   /  s    88 D
 	BIINN4aN(D11D%//1FIINN6N*EDAJuQw0BHHVT!RaR%[1288F3CDr   c                      t        j                  g d      } t        j                  g d      }t        j                  |       j	                         }t        |j                  d       t        ||       y )Nr%   r)   r)   gj*0	x?g$x?g:2(r   r   r   r   r   r   r   rO   r   rE   expected_rotvecactual_rotvecs      r   test_as_rotvec_single_1d_inputr   >  sR    88M"DhhABO&&t,668M$$d+M?3r   c                      t        j                  g dg      } t        j                  g dg      }t        j                  |       j	                         }t        |j                  d       t        ||       y )Nr   r   r   r   r   s      r   test_as_rotvec_single_2d_inputr   H  sW    88]O$Dhh BCDO&&t,668M$$f-M?3r   c                     g dg dg dg} t        j                  |       }|j                  d      }t        j                  j                  |      }t        |d       t        |d   |d          t        |d   |d	          y )
NrM   rL   rT   Tr   g      ^@r   r%   r)   )r   rc   r   r   r0   r1   r   )rF   rotr   r   s       r   test_as_rotvec_degreesr   R  sn    i
+C


s
#C]]4](FIINN6"EE5!F1Ivay)F1Ivay)r   c                      t        j                  g dg dg dg      } t        t        j                  |       j                         |        t        t        j                  | d      j                  d      |        y )Nr   r%   r&   r)   )ga2U0*3gǺ6?ga2U0*?Tr   )r   r   r   r   r   r   )r   s    r   test_rotvec_calc_pipeliner   ]  se    XX
 F
 H((0::<fEH((>HHQUHVX^_r   c                      g d} t        j                  g d      }t        j                  |       }t	        |j                         |       y N)r   r   rz   )r   r   r%   r   r   r   r   from_mrpr   r   mrpr   r   s      r   test_from_1d_single_mrpr   h  s6    
CHH\*Ms#Ffnn.>r   c                      g dg} t        j                  g dg      }t        j                  |       }t	        |j                         |       y r   r   r   s      r   test_from_2d_single_mrpr   o  s:    -CHHl^,Ms#Ffnn.>r   c                      t        j                  g dg dg dg      } t        j                  g dg dg dg      }t        t        j                  |       j                         |       y )Nr   r   r   )r   r\   r\   g)g}+;?g}+;g}+;?gߨӉؿr$   )r   r   r   r   r   r   )r   r   s     r   test_from_generic_mrpr   v  s\    
(( C HH: M h//4<<>Nr   c                      t        j                  t        d      5  t        j                  ddg       d d d        y # 1 sw Y   y xY w)NExpected `mrp` to have shaper   r%   r)   r6   r7   r8   r   r   r9   r   r   test_malformed_1d_from_mrpr     s8    	z)G	H "1a&!" " "r   c                      t        j                  t        d      5  t        j                  g dg dg       d d d        y # 1 sw Y   y xY w)Nr   r   r[   r   r   r9   r   r   test_malformed_2d_from_mrpr     s?    	z)G	H  	  r   c                  ,   t        j                  g dg dg dg      } | t         j                  j                  | d      d d d f   z  } t        j                  g dg dg dg      }t	        t        j                  |       j                         |       y )	Nr   r   r$   r%   r,   )gQUU?gvWUU?QUUտ)uFx?guFxr   r   )r   r   r0   r1   r   r   r   as_mrp)rE   expected_mrps     r   test_as_generic_mrpr     s    88 D 	BIINN4aN(D11D88-- L h006==?Nr   c                      t        j                  t        j                  t         j                  dz  dz         ddg      } t	        t        j                  dg dd      j                         |        y )	Nr)   r   g        r   xyz)i  r   r   Tr   )r   r   tanpir   r   
from_eulerr   )r   s    r   test_past_180_degree_rotationr     sT     88bffRUU1WQY//a89Lh11%dSZZ\^jkr   c                      t        j                  g d      } t        j                  g d      }t        j                  |       j	                         }t        |j                  d       t        ||       y )Nr   g?g?gO޿r   r   r   r   r   r   r   rO   r   rE   r   
actual_mrps      r   test_as_mrp_single_1d_inputr     sR    88M"D88ABL##D)002J!!4(J-r   c                      t        j                  g dg      } t        j                  g dg      }t        j                  |       j	                         }t        |j                  d       t        ||       y )Nr   r   r   r   r   s      r   test_as_mrp_single_2d_inputr     sW    88]O$D88BCDL##D)002J!!6*J-r   c                      t        j                  g dg dg dg dg      } t        j                  g dg dg dg dg      }t        t        j                  |       j                         |       y )Nr   r   )gzpy?r   r   )皙?r   r   )gK}\UUſgK}\UU?r   )r   r   r   r   r   r   )r   r   s     r   test_mrp_calc_pipeliner     sb    	 J
 88.	 L
 H%%j188:LIr   c                      t        j                  ddd      j                         } t        j                  g d      t        j
                  d      z  }t        | |       y )NzZ   Tr   rI   r)   )r   r   r   r   r   ro   r   )rE   r   s     r   test_from_euler_single_rotationr     sD    sB5==?DHH\*RWWQZ7MD-(r   c                      t        j                  ddd      j                         } t        j                  ddd      j                         }t        | |       y )Nr   r   Tr   Z)r   r   rC   r   )	extrinsic	intrinsics     r   (test_single_intrinsic_extrinsic_rotationr     sF    ##CT:DDFI##CT:DDFIIy)r   c                  2   t         j                  j                  d      } | j                  ddd      }|d d d d df   }t	        j
                  d|d      j                         }t	        j
                  d	|d      j                         }t        ||       y )
Nr      )r<   r   lowhighrs   r&   r   Tr   ZYX)r   r{   r|   randintr   r   r   r   )r   abr   ys        r   test_from_euler_rotation_orderr    s    
))


"C&1A	!TrT'
AE1d3;;=AE1d3;;=AAqr   c                      t        j                  dddgd      j                         } t        j                  g dg dg dg      }t        | |       y )Nzxr   Tr   rK   r   r   r&   rL   )r   r   rC   r   r   r   )rF   rP   s     r   -test_from_euler_elementary_extrinsic_rotationr    sK    


dRHd
;
E
E
GC88 L
 c<0r   c            	      h   g dg dg dg} t        j                  d| d      j                         }t        |d   t	        j
                  g dg d	g d
g             t        |d   t	        j
                  g dg dg dg             t        |d   t	        j
                  g dg dg dg             y )N   <   -   r  r  r  r  r  r  ZXYTr   r   )g# ?      пgΙ6d?)g$+H?+Pz?Ppƿ)gPpֿ+Pz?gPp?r%   )g+a?r  (s=?)r  r  g&(ٿ)r  r  r  r)   )ga}@?gF6g^@?)g0	8?gF6?g4?)      rb   r  r   r   rC   r   r   r   anglesrF   s     r   &test_from_euler_intrinsic_rotation_312r    s    
F
 

eVT
:
D
D
FCc!fbhh***0 '  c!fbhh***0 '  c!fbhh*)*0 ' r   c            	      h   g dg dg dg} t        j                  d| d      j                         }t        |d   t	        j
                  g dg d	g d
g             t        |d   t	        j
                  g dg dg dg             t        |d   t	        j
                  g dg dg dg             y )Nr  r  r  ZXZTr   r   )>d?g @r  )l>?^|@r  )X?r$  rb   r%   g      ?g ,r  g ,?g      ?r  r        ?rb   r)   )r  g4&d'e?)cH?Xӿg'eֿ)r        ?r  r  r  s     r   &test_from_euler_intrinsic_rotation_313r-    s    
F
 

eVT
:
D
D
FCc!fbhh,,*0 '  c!fbhh+*'0 '  c!fbhh-.*0 ' r   c            	      h   g dg dg dg} t        j                  d| d      j                         }t        |d   t	        j
                  g dg d	g d
g             t        |d   t	        j
                  g dg d	g dg             t        |d   t	        j
                  g dg dg dg             y )Nr  r  r  zxyTr   r   )4&d?gPp?r)  )r,  r  g+Pz)r+  r*  r)  r%   )g
?\2׳r,  )r1  g ,?r  r)   )r"  r#  r(  )r$  r$  g      )g>dۿ @?r  r  r  s     r   &test_from_euler_extrinsic_rotation_312r3    s    
F
 

eVT
:
D
D
FCc!fbhh++-0 '  c!fbhh+++0 '  c!fbhh,,,0 ' r   c            	      h   g dg dg dg} t        j                  d| d      j                         }t        |d   t	        j
                  g dg d	g d
g             t        |d   t	        j
                  g dg dg d
g             t        |d   t	        j
                  g dg dg dg             y )Nr  r  r  zxzTr   r   )r!  gl>r$  )r2  r#  gXr'  r%   r%  r&  r)   )r  gcHr  )r0  r+  r  )r)  r)  r  r  r  s     r   &test_from_euler_extrinsic_rotation_313r6  7  s    
F
 

eVT
:
D
D
FCc!fbhh-.)0 '  c!fbhh,+)0 '  c!fbhh,,+0 ' r   c                     d } t         j                  j                  d      }d}t        j                  |df      }|j	                  t         j
                   t         j
                  |f      |d d df<   |j	                  t         j
                   dz  t         j
                  dz  |f      |d d df<   |j	                  t         j
                   t         j
                  |f      |d d df<   t        d      D ]  }d	j                  |      }t        j                  ||      }|j                  |      }|j                  |      }t        ||dd
       t        ||dd
        | ||z
  dd        | ||z
  dd       |j                         }t        j                  ||      }|j                  |      }|j                  |      }t        ||dd
       t        ||dd
        | ||z
  dd        | ||z
  dd        y )Nc                    t        j                  | d      }t        j                  | d      }t        j                  ||      }t        j                  t        j
                  |      |k        sJ t        j                  ||k        sJ y Nr   r,   r   meanstdhypotallr/   errormean_maxrms_maxr;  r<  rmss         r   
test_statsz1test_as_euler_asymmetric_axes.<locals>.test_statsT  g    wwu1%ffU#hhtS!vvbffTlX-...vvcGm$$$r   r     r   r   r)   r%   r    -q=atolrtolV瞯<+=r   r{   r|   emptyuniformr   r	   joinr   r   as_euler_as_euler_from_matrixr   upper	rD  r   nr  	seq_tupleseqrotationangles_quat
angles_mats	            r   test_as_euler_asymmetric_axesr\  R  s   % ))


"CAXXq!fF;;BEE6QD;AF1a4L;;BEE6A:BEEAIQD;IF1a4L;;BEE6QD;AF1a4L!%( 6	ggi &&sF3'',33C8
!%@
?;'6:&u5 iik&&sF3'',33C8
!%@
?;'6:&u5'6r   c                     d } t         j                  j                  d      }d}t        j                  |df      }|j	                  t         j
                   t         j
                  |f      |d d df<   |j	                  dt         j
                  |f      |d d df<   |j	                  t         j
                   t         j
                  |f      |d d df<   t        d      D ]  }d	j                  |d   |d   |d   g      }t        j                  ||      }|j                  |      }|j                  |      }t        ||dd
       t        ||dd        | ||z
  dd        | ||z
  dd
       |j                         }t        j                  ||      }|j                  |      }|j                  |      }t        ||dd
       t        ||dd        | ||z
  dd        | ||z
  dd
        y )Nc                    t        j                  | d      }t        j                  | d      }t        j                  ||      }t        j                  t        j
                  |      |k        sJ t        j                  ||k        sJ y r9  r:  r?  s         r   rD  z0test_as_euler_symmetric_axes.<locals>.test_statsz  rE  r   r   rF  r   r   r%   r)   r   rG  gvIh%<=rI  g&.>缉ؗҜ<rM  rL  rN  rU  s	            r   test_as_euler_symmetric_axesr`  x  s   % ))


"CAXXq!fF;;BEE6QD;AF1a4L;;1255t;<F1a4L;;BEE6QD;AF1a4L!%( 6	ggy|Yq\9Q<@A&&sF3'',33C8
!%@
>;'6:&u5 iik&&sF3'',33C8
!%@
>;'6:&u5'6r   c                     t        j                  g dg dg dg dg      } t        j                  t        d      5  t        d      D ]  }dj                  |      }t        j                  || d	
      }|j                         }|j                  |d	
      }t        j                  ||d	
      j                         }t        ||       |j                         }t        j                  || d	
      }|j                         }|j                  |d	
      }t        j                  ||d	
      j                         }t        ||        	 d d d        y # 1 sw Y   y xY w)Nr  r   #   rc     rc  r      rh  re     Gimbal lockr   r   rG  Tr   r   r   r6   warnsUserWarningr	   rQ  r   r   rC   rR  r   rT  r  rW  rX  rY  mat_expectedangle_estimatesmat_estimateds          r   (test_as_euler_degenerate_asymmetric_axesrs    sE    XX	 F 
k	7 C%e, 	CI'')$C**3EH#--/L&//T/BO$//_d)+  &lMB ))+C**3EH#--/L&//T/BO$//_d)+  &lMB/	CC C Cs   DEEc                     t        j                  g dg dg dg dg      } t        j                  t        d      5  t        d      D ]
  }dj                  |d	   |d
   |d	   g      }t        j                  || d      }|j                         }|j                  |d      }t        j                  ||d      j                         }t        ||       |j                         }t        j                  || d      }|j                         }|j                  |d      }t        j                  ||d      j                         }t        ||        	 d d d        y # 1 sw Y   y xY w)Nrj  r   r  rc  r   K   r  r   rc  rj  iLrh  rk  r   r   rG  r   r%   Tr   rl  ro  s          r   'test_as_euler_degenerate_symmetric_axesrz    sX    XX	 F 
k	7 C%e, 	CI''9Q<1y|DEC**3EH#--/L&//T/BO$//_d)+  &lMB ))+C**3EH#--/L&//T/BO$//_d)+  &lMB/	CC C Cs   DE!!E*c            	         t        j                  g dg dg dg dg      } t        j                  t        d      5  t        d      D ]  }dj                  |      }t        j                  || d	
      }|j                  |d	
      }|j                  |d	
      }t        |d d ddgf   |d d ddgf   dd       t        |d d df   |d d df   dd       |j                         }t        j                  || d	
      }|j                  |d	
      }|j                  |d	
      }t        |d d ddgf   |d d ddgf   dd       t        |d d df   |d d df   dd        	 d d d        t        j                  g dg dg dg dg      } | d d df   dk(  }t        j                  t        d      5  t        d      D ]T  }dj                  |d   |d   |d   g      }t        j                  || d	
      }|j                  |d	
      }|j                  |d	
      }t        |d d ddgf   |d d ddgf   dd       t        || df   || df   dd       t        ||df   ||df   d       |j                         }t        j                  || d	
      }|j                  |d	
      }|j                  |d	
      }t        |d d ddgf   |d d ddgf   dd       t        || df   || df   dd       t        ||df   ||df   d       W 	 d d d        y # 1 sw Y   xY w# 1 sw Y   y xY w)Nrb  rd  rg  ri  rk  r   r   rG  Tr   r   r)   rH  rI  r%   Hz>ru  rv  rx  ry  ư>rJ  )r   r   r6   rm  rn  r	   rQ  r   r   rS  rR  r   rT  )r  rW  rX  r   estimates_matrixestimates_quatidxs          r   +test_as_euler_degenerate_compare_algorithmsr    s   
 XX	 F 
k	7 /%e, 	/I'')$C%%c64@C"88d8K \\#t\<N,QAY7*1q!f95AEK,QT2N1a44H!"/ ))+C%%c64@C"88d8K \\#t\<N,QAY7*1q!f95AEK,QT2N1a44H!"/%	//4 XX	 F A,!
C	k	7 '%e, 	'I''9Q<1y|DEC%%c64@C"88d8K \\#t\<N,QAY7*1q!f95AEK ,cT1W5~sdAg7N!"/ ,S!V4nS!V6L!%' ))+C%%c64@C"88d8K \\#t\<N,QAY7*1q!f95AEK ,cT1W5~sdAg7N!"/ ,S!V4nS!V6L!%'5	'' 'E/ /D' 's   D,L(:E$L5(L25L>c                     t         j                  j                  d      } d}t        j                  ||       }|j	                         }|j                         }|j                         }t        j                  d||      }t        j                  d||      }t        j                  |ddf      }t        j                  d      |d d  t        ||       t        ||       y )Nr   rr   numrt   ry   r   )
r   r{   r|   r   invrC   r~   rO  rD   r   )	r   rV  pqp_matq_matresult1result2r   s	            r   test_invr  2  s    
))


"C
AAC0A	AKKMEKKMEii,eU;Gii,eU;GHHaAYEvvayE!Hgu-gu-r   c                     t         j                  j                  d      } t        j                  |       }|j	                         }|j                         }|j                         }t        j                  ||      }t        j                  ||      }t        j                  d      }t        ||       t        ||       t        j                  d|       }|j	                         }	|j                         }
|	j                         }t        j                  d|
|      }t        j                  d||
      }t        j                  d      }t        j                  d      |d d  t        ||       t        ||       y )Nr   rt   r   r%   r  ry   rJ   )r   r{   r|   r   r  rC   dotrD   r   r~   rO  )r   r  r  r  r  res1res2rD   r   r  x_matrixy_matrixr  r  r   s                  r   test_inv_single_rotationr  D  s   
))


"CS)A	AKKMEKKME66%D66%D
&&)CdC(dC(AC0A	A{{}H{{}Hii,hAGii,hAGHHYEvvayE!Hgu-gu-r   c                      d} t        t        j                  |       j                         d       t        t        j                  |       j	                         j                         d       y )Nrr   r   )r   r   identity	magnituder  )rV  s    r   test_identity_magnituder  b  sL    
AH%%a(224a8H%%a(,,.88:A>r   c                      t        j                         j                         dk(  sJ t        j                         j                         j                         dk(  sJ y )Nr   )r   r  r  r  r9   r   r   test_single_identity_magnituder  h  sK    ((*a///""$..0A555r   c                  6   d} t        j                  | d      }|t        j                  |       z  }t        |j	                         |j	                                ||j                         z  }t        |j                         t        j                  |              y Nrr   r   r  	r   r{   r  r   r   r  r  r   r   rV  r  r   s      r   test_identity_invariancer  m  sn    
A*A""1%%Faiik6>>+;<aeegFf..0"((1+>r   c                  4   d} t        j                  | d      }|t        j                         z  }t        |j	                         |j	                                ||j                         z  }t        |j                         t        j                  |              y r  r  r  s      r   test_single_identity_invariancer  x  sl    
A*A""$$Faiik6>>+;<aeegFf..0"((1+>r   c                     t        j                  t        j                  d            } | j	                         }t        |t        j                  t        j                  t        j                  dg       t        j                  t        j                  d             } | j	                         }t        |t        j                  t        j                  t        j                  dg       y )Nr   r   )r   r   r   rD   r  r   r   )r   r   s     r   test_magnituder    s    266!9%A[[]FfruubeeRUUA&>?BFF1I:&A[[]FfruubeeRUUA&>?r   c                      t        j                  t        j                  d            } | d   j	                         }t        |t        j                         | d   j	                         }t        |d       y )Nr   r   r   )r   r   r   rD   r  r   r   )r   r  r  s      r   test_magnitude_single_rotationr    sR    266!9%AdnnGGRUU#dnnGGQr   c                  b   t        j                  t        j                  d       t        j                  d      f      } t        j                  dt         j                  dz  d      }|D ]D  }t        j                  || z        }t        |j                         j                         dd       F y )Nr   r   r)   rx   绽|=r~  )
r   concatenaterD   linspacer   r   r   r   r;  r  )axesthetastr   s       r   	test_meanr    s    >>BFF1I:rvvay12D[[BEEAIs+F =  T***,ae<=r   c                     t        j                  g dg dg dg      } t        j                  dt         j                  dz  d      }|D ]  }t	        j
                  || d d z        }|j                  ddg      }t	        j
                  || z        }|j                         }t        ||j                         z  j                         dd	        y )
Nr   rL   r   r)   rx   r%   weightsr  r~  )
r   r   r  r   r   r   r;  r   r  r  )r  r  r  rwmwr   ms          r   test_weighted_meanr    s    88Y	956D[[BEEAIs+F C!!!d2Ah,/WWaVW$  T*FFHRVVX002AEBCr   c                      t        j                  t        d      5  t        j                  t        j                  d            } | j                  t        j                  d              d d d        y # 1 sw Y   y xY w)Nznon-negativer   r   r  )	r6   r7   r8   r   r   r   rD   r;  onesr   s    r   test_mean_invalid_weightsr    sU    	z	8 $rvvay)	
{#$ $ $s   AA44A=c                  p    t        j                         j                  d      } t        | t               sJ y )NFreturn_indices)r   r  reduce
isinstance)r   s    r   test_reduction_no_indicesr    s.     ''u'=Ffh'''r   c                      t        j                         j                  d      } t        |       t        k(  sJ t        |       dk(  sJ | \  }}}|J |J y )NTr  r   )r   r  r  typetuplelen)r   reduced	left_best
right_bests       r   test_reduction_none_indicesr    sf     ''t'<F<5   v;!%+"GY
r   c                  R   t         j                  j                  d      } t        j                  d|       }t        j                  d|       }t        j                  d|       }|j	                  ||d      \  }}}t        j
                  t        |      t        |      t        |      f      }t        |      D ]]  \  }}	t        |      D ]J  \  }
}t        |      D ]7  \  }}t        j                  |	|z  |z  j                         d         |||
|f<   9 L _ t        j                  t        j                  |d	d      |j                  d	   d
f      }t        j                  t        j                  |t        |      d
f      d	      }|t        |      z  }|t        |      z  }||k(  j                         sJ ||k(  j                         sJ ||   |z  ||   z  }|j                         |z  j!                         }t#        |t        j
                  t        |                   y )Nr   r   r  rr   r=   Tr  r   r%   r&   r,   )r   r{   r|   r   r  r   r  	enumerater/   r   rh   moveaxisrO   argmaxr>  r  r  r   )rnglr   r  r  r  r  scalarsr   lijpjkrkmax_indleft_best_checkright_best_checkreduced_checkmags                      r   !test_reduction_scalar_calculationr    s   
))


"C,A-A,A%&XXa4X%H"GY
 hhAAA/0G1 G2q\ 	GEAr"1 G2#%6627R<*@*@*B1*E#F1a G	GG jjWa3gmmA6F5KLGii

7SVRL9BGQ'OQ'(--///**//111o&*Q/?-@@M;;==(
3
3
5Cc288CF#34r   c                     t        j                  g dg dg dg      } t        j                  |       }t        j                  t        j                  | d            }t        j                  g d      }t        j                  |d      }t        j                  g d      }t        j                  |d      }t        |j                  |      |       t        |j                  |      |       t        |j                  |      |       t        |j                  |      |       t        j                  g d      }t        j                  |d      }t        |j                  |d	
      |       t        |j                  |d	
      |       t        |j                  |d	
      |       t        |j                  |d	
      |       y )NrK   rL   rM   r   r,   r4   r]   r%   r   r)   r&   r   Tinverser   r   r   rc   expand_dimsr   apply)	rF   r_1dr_2dv_1dv_2dv1d_rotatedv2d_rotatedv1d_inversev2d_inverses	            r   'test_apply_single_rotation_single_pointr    sM   
(( C
 $Ds ;<D88ID>>$Q'D((:&K..15KDJJt$k2DJJt$k2DJJt$k2DJJt$k2((:&K..15KDJJtTJ2K@DJJtTJ2K@DJJtTJ2K@DJJtTJ2K@r   c                  @   t        j                  g dg dg dg      } t        j                  |       }t        j                  t        j                  | d            }t        j                  g dg dg      }t        j                  g dg d	g      }t        |j                  |      |       t        |j                  |      |       t        j                  g d
g dg      }t        |j                  |d      |       t        |j                  |d      |       y )NrK   rL   rM   r   r,   r4   r   r   r<   r  )r   r<   r  )r   r<   Tr  r  )rF   r1r2v	v_rotated	v_inverses         r   *test_apply_single_rotation_multiple_pointsr    s    
(( C
 
		c	"B			bnnSq9	:B
)Y'(A*j12IBHHQK+BHHQK+*j12IBHHQH-y9BHHQH-y9r   c                     t        j                  d      } t        j                  g dg dg dg      | d<   t        j                  g dg dg dg      | d<   t        j                  |       }t        j                  g d	      }t        j
                  |d
      }t        j                  g dg dg      }t        |j                  |      |       t        |j                  |      |       t        j                  g dg dg      }t        |j                  |d      |       t        |j                  |d      |       y )Nr)   r   r   rK   rL   rM   r   r  rT   r%   r4   r,   r  )r%   r   r)   r  )r%   r   r]   Tr  )r   rO  r   r   rc   r  r   r  )rF   r   v1v2r  r  s         r   *test_apply_multiple_rotations_single_pointr    s    
((9
CXX CF
 XX CF
 	S!A	)	B		#B*j12IAGGBK+AGGBK+*j12IAGGBG-y9AGGBG-y9r   c                     t        j                  d      } t        j                  g dg dg dg      | d<   t        j                  g dg dg dg      | d<   t        j                  |       }t        j                  g d	g d
g      }t        j                  g dg dg      }t        |j                  |      |       t        j                  g dg dg      }t        |j                  |d      |       y )Nr  rK   rL   rM   r   r  rT   r%   r4   r  r  )r   ir   r  )r   r<   r  Tr  )r   rO  r   r   rc   r   r  )rF   r   r  r  r  s        r   -test_apply_multiple_rotations_multiple_pointsr  '  s    
((9
CXX CF
 XX CF
 	S!A
)Y'(A*j12IAGGAJ	**j12IAGGAtG,i8r   c                     t        j                  d      } t        j                  g dg dg dg      | d<   t        j                  g dg dg dg      | d<   t        j                  |       }t        |d   j                         | d   d	
       t        |d   j                         | d   d	
       t        |d d j                         t        j                  | d   d      d	
       y )Nr  rK   rL   rM   r   r  rT   r%   rL  r~  r&   r,   )r   rO  r   r   rc   r   rC   r  rF   r   s     r   test_getitemr  =  s    
((9
CXX CF
 XX CF
 	S!AAaDNN$c!f59AaDNN$c!f59AcrF$$&s1vA(FUSr   c                      t        j                  t        d      5  t        j                         d    d d d        y # 1 sw Y   y xY wNznot subscriptabler   r   )r6   r7   	TypeErrorr   r  r9   r   r   test_getitem_singler  P  s7    	y(;	< A  r   c                      t        j                         } t        j                  t        d      5  t        j                         | d<   d d d        y # 1 sw Y   y xY wr   )r   r  r6   r7   r  r  s    r   test_setitem_singler  U  sF    A	y(;	< #  "!# # #s   AAc                      t         j                  j                  d      } t        j                  d|       }t        j                  d|       }||dd t	        |dd j                         |j                                y )Nr   seedrr   r  r   r%   r<   r   r{   r|   r   r   r   r  r  r  s      r   test_setitem_slicer
  [  sa    
))

Q

'C	#	.B		-BBqGAa"BJJL1r   c                      t         j                  j                  d      } t        j                  d|       }t        j                  |       }||d<   t	        |d   j                         |j                                y )Nr   r  rr   r  r%   r  r	  s      r   test_setitem_integerr  c  sZ    
))

Q

'C	#	.B	c	*BBqEA"**,/r   c                      t        j                  dd      } t        j                  t        d      5  d| d<   d d d        y # 1 sw Y   y xY w)Nrr   r   r  zRotation objectr   r%   )r   r{   r6   r7   r  r  s    r   test_setitem_wrong_typer  k  s?    +A	y(9	: !  s   AAc                  6   t        j                  d      } t        j                  g dg dg dg      | d<   t        j                  g dg dg dg      | d<   t        j                  |       }t        t        |      d	       t        t        |d d
       d       y )Nr  rK   rL   rM   r   r  rT   r%   r)   r&   )r   rO  r   r   rc   r   r  r  s     r   test_n_rotationsr  q  s    
((9
CXX CF
 XX CF
 	S!AQQsVa r   c                     t        j                  g dg dg      } | j                         }t        j                  | |      \  }}t        |j                         t        j                  d             t        |dd       y )Nr4   r  r   r   r}  r~  )	r   r   copyr   align_vectorsr   rC   rD   r   )r   r  r   rmsds       r   test_align_vectors_no_rotationr    sZ    
)Y'(A	A$$Q*GAtakkmRVVAY7D!$'r   c                  R   t         j                  j                  d      } t        j                  |       }| j	                  d      }|j                  |      }t        j                  ||      \  }}t        |j                         |j                                t        |dd       y )Nr   r  r   r   rs   r|  r~  )	r   r{   r|   r   normalr  r  r   r   )r   cr  r  estr  s         r   test_align_vectors_no_noiser    sy    
))


"CS)A


A	
A&&q!,ICAIIK/D!$'r   c                      t        j                  g dg dg      } t        j                  g dg dg      }t        j                  | |      \  }}t	        | |j                  |      d       t	        |dd       y )	N)gq?gH*fܿgHlB?)gMs^E?g+Obp޿gn^w)g=2)p?g[[VKg?gw7?)gѿgGvgpr}  r~  r   r|  )r   r   r   r  r   r  )r   r  r  r  s       r   $test_align_vectors_improper_rotationr    sl    
679 	:A
68: 	;A &&q!,ICAsyy|$/D!$'r   c                  "   t         j                  j                  d      } t        j                  |       }| j	                  d      }|j                  |      }t        j                  ||t        j                  d      d      \  }}}t        j                  ||dt        j                  d      z  d      \  }}}	t        |j                         |j                                t        t        j                  d      |z  |       t        ||	       y )Nr   r  r  r  r   Tr)   )r   r{   r|   r   r  r  r  r  r   rC   ro   )
r  r  r  r  est1rmsd1cov1est2rmsd2cov2s
             r   !test_align_vectors_scaled_weightsr&    s    
))


"CS)A


A	
A ..q!RWWQZFD% ..q!Q^TJD%DNN$dnn&67BGGAJ&.D$r   c            	          t         j                  j                  d      } d}t        j                  |       }| j	                  |df      }|j                  |      }t        j                  d      }d|z  }t        j                  | j	                  |df|            }|j                  |      }t        j                  ||d	
      \  }	}
}||	j                         z  j                         }t        |d   d|       t        |d   d|       t        |d   d|       ||z  }t        |d   d|       t        |d   d|       t        |d   d|       t        |
t        j                  ||	j                  |      z
  dz        dz         y )Nr   rx   r  r   r  r%         ?)rs   scaleT)return_sensitivityr~  r)   r   r   )r%   r%   )r)   r)   rb   )r   r{   r|   r   r  r  r   r   r  r  r   r   sum)r   	n_vectorsr   vectorsr   sigma	tolerancenoisenoisy_resultr  r  coverror_vectors                r   test_align_vectors_noiser5    sl   
))


"CI
//s
+Cjjy!nj-GYYwF JJqMEeI  

Q 	 	
E ;;v&L++L'?CENCs #'')O..0LLOQY7LOQY7LOQY7 5LCCIqy1CIqy1CIqy1D"&&,71C"Ca!GH#MNr   c                     t        j                         j                  d      rd} nd} t        j                  t
        d      5  t        j                  g dgg dg      \  }}t        |d| 	       d d d        y # 1 sw Y   y xY w)
Ni686g+i)+`>r_  zOptimal rotation is notr   )r%   r&   r%   )r%   r%   r&   r   r~  )	platformmachine
startswithr6   rm  rn  r   r  r   )eps
r_estimater  s      r    test_align_vectors_single_vectorr=    sn    $$V,	k)B	C +#11:,M
Dac*+ + +s   .A;;Bc                     t        j                  t        d      5  t        j                  g dg dg       d d d        t        j                  t        d      5  t        j                  g dgg d       d d d        t        j                  t        d      5  t        j                  g dg dgg dg       d d d        t        j                  t        d      5  t        j                  g dgg dgdgg	       d d d        t        j                  t        d
      5  t        j                  g dgg dgddg	       d d d        t        j                  t        d      5  t        j                  g dgg dgdg	       d d d        y # 1 sw Y   XxY w# 1 sw Y   &xY w# 1 sw Y   xY w# 1 sw Y   xY w# 1 sw Y   xY w# 1 sw Y   y xY w)Nz Expected input `a` to have shaper   r4   z Expected input `b` to have shapez/Expected inputs `a` and `b` to have same shapesr  z&Expected `weights` to be 1 dimensionalr%   r  z+Expected `weights` to have number of valuesr)   z)`weights` may not contain negative valuesr&   )r6   r7   r8   r   r  r9   r   r    test_align_vectors_invalid_inputr?    s   	z)K	L 7y9+67 
z)K	L 7	{I67 
z *? 
@ C	)4ykBC 
zE
G H	{YK1#GH 
zJ
L I	{YK!QHI 
zH
J G	{YK"FG G%7 77 7C CH HI IG GsG   FF F(!F4!!G % GFF%(F14F= G	Gc                     t         j                  j                  d      } t        t	        j                  |       j                         j                  d       t        t	        j                  d |       j                         j                  d       t        t	        j                  d|       j                         j                  d       t        t	        j                  d|       j                         j                  d       y )Nr   r  )r   r%   rg   r   r   r   )r   r{   r|   r   r   r   rO   )r   s    r   test_random_rotation_shaperB    s    
))


"Cc2::<BBDIC8@@BHH$O5==?EEvN5==?EEvNr   c                     t         j                  j                  d      } t        j                  | j                  d            }|j                         }g d}t        ||      }g d} ||      }|j                         }||d d df   dk  xx   dz  cc<   ||d d df   dk  xx   dz  cc<   t        |d   |d          t        |d   |d          t        |d	   |d
          t        |d   |d          t        |d   |d          t        j                  |d   |d
   z        }t        j                  |d
   |d   z        }	t        ||	       t        j                  |d   |d   z        }
t        j                  |d   |d	   z        }t        |
|       t        j                  |d   |d   z        }t        |d
|d
z  z  dz
         t        t        |      t        |             y )Nr   rA  r  )r   r%   r)   r   r   )r   rb   r,  r%   r(  r)   g      @r   g      
@g@r   r&   r   r%   r   r)   r=   rr   r   )r   r{   r|   r   r   rP  r   r   r   r,  r   r  )r   key_rots	key_quats	key_timesinterpolatortimesinterp_rotsinterp_quats
cos_theta1
cos_theta2
cos_theta4
cos_theta5
cos_theta3s                r   
test_slerprP    s   
))


"C!!#++6+":;H  "IIH-L=Eu%K&&(L ae$q()R/)i2"#r)# LOYq\2LOYq\2LOYq\2LOYq\2L$il3 Q,q/9:JQ,q/9:JJ
+Q,q/9:JQ,q/9:JJ
+ Q,q/9:JJZ] 3a 78 [!3u:.r   c                      t        j                  t        d      5  t        j                  g dg dg      } t        j                  ddg      }t        ||        d d d        y # 1 sw Y   y xY w)Nzmust be a `Rotation` instancer   r[   r$   r   r%   )r6   r7   r  r   r   r   )r   r  s     r   test_slerp_rot_is_rotationrR  0  sY    	y(G	H HHi! "HHaVa	  s   ?A$$A-c                      d} t        j                  t        |       5  t        j                  g d      }t        dg|       d d d        y # 1 sw Y   y xY wNz*must be a sequence of at least 2 rotationsr   r[   r%   r6   r7   r8   r   r   r   msgr   s     r   test_slerp_single_rotrX  8  sE    
6C	z	- |,qc1  s   %AAc                      d} t        j                  t        |       5  t        j                  g dg      }t        dg|       d d d        y # 1 sw Y   y xY wrT  rU  rV  s     r   test_slerp_rot_len1rZ  ?  sG    
6C	z	- ~.qc1  s   &AAc                  6   t        j                  t        d      5  t        j                  j                  d      } t        j                  | j                  d            }t        j                  dgdgg      }t        ||       d d d        y # 1 sw Y   y xY w)Nz.times to be specified in a 1 dimensional arrayr   r   )r)   r   r  r%   r)   )r6   r7   r8   r   r{   r|   r   r   rP  r   r   r   r   r  s      r   test_slerp_time_dim_mismatchr]  F  s}    	zM
O ii##A&s{{{78HHqcc a  s   A*BBc                  .   t        j                  t        d      5  t        j                  j                  d      } t        j                  | j                  d            }t        j                  d      }t        ||       d d d        y # 1 sw Y   y xY w)Nz7number of rotations to be equal to number of timestampsr   r   rA  r  r=   )r6   r7   r8   r   r{   r|   r   r   rP  aranger   r\  s      r   !test_slerp_num_rotations_mismatchr`  P  sq    	z *@ 
A ii##A&s{{{78IIaLa  s   A&BBc                     t        j                  t        d      5  t        j                  j                  d      } t        j                  | j                  d            }g d}t        ||       d d d        y # 1 sw Y   y xY w)Nstrictly increasing orderr   r   rA  r  )r   r%   r)   r)   r   
r6   r7   r8   r   r{   r|   r   r   rP  r   r\  s      r   test_slerp_equal_timesrd  Y  c    	z)D	E ii##A&s{{{78a	     AA::Bc                     t        j                  t        d      5  t        j                  j                  d      } t        j                  | j                  d            }g d}t        ||       d d d        y # 1 sw Y   y xY w)Nrb  r   r   rA  r  )r   r%   r   r)   r   rc  r\  s      r   test_slerp_decreasing_timesrh  a  re  rf  c                  p   t         j                  j                  d      } t        j                  | j                  d            }t        j                  d      }t        ||      }t        j                  t        d      5  t        j                  dgdgg      } ||       d d d        y # 1 sw Y   y xY w)	Nr   rA  r  r   z&`times` must be at most 1-dimensional.r   g      @g@)r   r{   r|   r   r   rP  r_  r   r6   r7   r8   r   )r   r   r  sinterp_timess        r   !test_slerp_call_time_dim_mismatchrl  i  s    
))


"C3;;F;34A
		!AaA	zE
G xx#"%!( )	,	  s   "B,,B5c                     t         j                  j                  d      } t        j                  | j                  d            }t        j                  d      dz   }t        ||      }t        j                  t        d      5   |g d       d d d        t        j                  t        d      5   |g d	       d d d        y # 1 sw Y   8xY w# 1 sw Y   y xY w)
Nr   rA  r  r   r%   ztimes must be within the ranger   )r   r%   r)   )r%   r)   r<   )r   r{   r|   r   r   rP  r_  r   r6   r7   r8   )r   r   r  rj  s       r   !test_slerp_call_time_out_of_rangern  v  s    
))


"C3;;F;34A
		!qAaA	z)I	J 	)	z)I	J 	)   s   C2CCCc                      t        j                  dddgd      } t        ddg|       } |d      }t        j                  ddd      }||j                         z  }t	        |j                         dd	
       y )NXr   P   Tr   r%   r,  rf  r_  r~  )r   r   r   r  r   r  )r   rj  r_interpolatedr_interpolated_expecteddeltas        r   test_slerp_call_scalar_timeru    so    C!R$7Aq!faAtWN&11#r4H488::EEOO%qu5r   c                      t        j                  dd      } t        j                  dd      }| D ]C  }|||z  z  }t        t        j                  j                  |j                         d      d       E y )N2   r   r  rF  r%   r,   )r   r{   r   r   r0   r1   r   )qsrsr  s      r   test_multiplication_stabilityrz    sa    	!	,B	A	.B A
a"f		rzz|!<a@Ar   c                  F   t        j                  d      } t        j                  dd      }t        j                  |       }t	        |j
                  d       t        j                  |      }t	        |j
                  d       t        |d   j                         |d   j                                t        |d   j                         |d   j                                t        j                  | g      }t	        |j
                  d       t	        |d   |        t        j                  |g      }t	        |j
                  d       t        |d	   j                         |d   j                                t        |d
   j                         |d   j                                t        j                  | |gt              }t	        |j
                  d       t	        |d   |        t	        |d   |       t        j                  |||g      }t	        |j
                  d       y )Nr   r  r)   r%   r9   )r)   )r%   )r%   r)   r+  )r   r%   )dtype)r   r)   )	r   r{   r   r   r   rO   r   rC   object)singlemultipler   s      r    test_rotation_within_numpy_arrayr    s   __!,Fqq1HHHVEb!HHXEd#E!H&&((1+*?*?*ABE!H&&((1+*?*?*ABHHfXEd#q6"HHhZ Ef%E$K))+Xa[-B-B-DEE$K))+Xa[-B-B-DEHHfh'v6Ed#q6"q8$HHh(34Ef%r   c            	      n   t        j                  ddt        j                  t        j                  dz        t        j
                  t        j                  dz        g      } t        j                  |       }t        j                  |      }t        | j                         |j                         d       y Nr   r   rL  r~  )r   r   r   sinr   r   pickledumpsloadsr   rC   )r   pkl	unpickleds      r   test_picklingr    sq    Aq"&&q/266"%%'?CDA
,,q/CS!IAKKM9#6#6#8uEr   c            	      D   t        j                  ddt        j                  t        j                  dz        t        j
                  t        j                  dz        g      } t        j                  |       }t        | j                         |j                         d       y r  )
r   r   r   r  r   r   r  deepcopyr   rC   )r   r  s     r   test_deepcopyr    sa    Aq"&&q/266"%%'?CDA	q	BAKKM2<<>>r   c                  :   t        j                  g d      } | j                  d      }| j                  d      }|j                  d   du sJ |j                  d   du sJ t	        d |j
                  D              sJ t	        d |j
                  D              sJ y )Nr$   r   XYZC_CONTIGUOUSTc              3   &   K   | ]	  }|d k\    ywr   Nr9   .0r   s     r   	<genexpr>z+test_as_euler_contiguous.<locals>.<genexpr>       *!qAv*   c              3   &   K   | ]	  }|d k\    ywr  r9   r  s     r   r  z+test_as_euler_contiguous.<locals>.<genexpr>  r  r  )r   r   rR  flagsr>  strides)r   e1e2s      r   test_as_euler_contiguousr    s    <(A	
E	B	
E	B88N#t+++88N#t+++*rzz*****rzz****r   c                  L   t        j                  dd      } g d}dgt        t        j                  |            z   }t        ||      D cg c]  \  }}| |||z     }}}t        j                  |      }t        | j                         |j                                y c c}}w )Nrr   r   r  )r%   r)   r   r%   r   )	r   r{   listr   cumsumzipr  r   r   )rY  sizesstartsr   rV  splitr   s          r   test_concatenater    s    r2HES4		%())F+.vu+=>41aXaA>E>!!%(F!!#V^^%56 ?s   B c                      t        j                  t        d      5  t        j                  t        j
                         dd g       d d d        y # 1 sw Y   y xY w)NzRotation objects onlyr   r%   )r6   r7   r  r   r  r  r9   r   r   test_concatenate_wrong_typer    sD    	y(?	@ =h//11d;<= = =s   +AAc                     t        t        j                  d            } t        g dg      }t        g dg dg      }t        g d      }t        |       dk(  sJ t        |      dk(  sJ t        |      dk(  sJ t	        j
                  t        d      5  t        |       d d d        | sJ |sJ |sJ |sJ y # 1 sw Y   xY w)N)r   r   r$   r   r%   r)   zSingle rotation has no len().r   )r   r   rO  r  r6   r7   r  )rotation_multi_emptyrotation_multi_onerotation_multirotation_singles       r   test_len_and_boolr    s    #BHHV$45!<.1|\:;N|,O#$)))!"a'''~!###	y(G	H O  >? s   B;;C)yr6   r8  numpyr   numpy.testingr   r   r   scipy.spatial.transformr   r   scipy.statsr   	itertoolsr	   r  r  r   r   r!   r*   r2   r:   r?   rA   rG   rQ   rY   r_   re   ri   rp   rv   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-  r3  r6  r\  r`  rs  rz  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&  r5  r=  r?  rB  rP  rR  rX  rZ  r]  r`  rd  rh  rl  rn  ru  rz  r  r  r  r  r  r  r  r9   r   r   <module>r     sF      A ) 3 + "  ::::
H0
.	4181>		+J
2?? 22	"%
E44*`??	O"
Ol..J)*16666#6L#6L!CH!CHI'X.$.<?6
??@ =
C$(
54A8:*:89,T&
#20!$((	( $ON+G0O)/X
		6A&<F?+7=r   