
    S_f}                        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& Z/d' Z0d( Z1d) Z2d* Z3d+ Z4d, Z5d- Z6d. Z7d/ Z8d0 Z9d1 Z:d2 Z;d3 Z<d4 Z=d5 Z>d6 Z?d7 Z@e j                  j                  d8 ed9            e j                  j                  d:d;      d<               ZCe j                  j                  d8 ed9            e j                  j                  d:d;      d=               ZDe j                  j                  d8 ed9            e j                  j                  d:d;      d>               ZEe j                  j                  d8 ed9            e j                  j                  d:d;      d?               ZFe j                  j                  d8 ed9            e j                  j                  d:d;      d@               ZGdA ZHdB ZIdC ZJdD ZKdE ZLdF ZMdG ZNdH ZOdI ZPdJ ZQdK ZRdL ZSdM ZTdN ZUdO ZVdP ZWdQ ZXdR ZYdS ZZdT Z[dU Z\dV Z]dW Z^dX Z_dY Z`dZ Zad[ Zbd\ Zcd] Zdd^ Zed_ Zfd` Zgda Zhdb Zidc Zjdd Zkde Zldf Zmdg Zndh Zodi Zpdj Zqdk Zrdl Zsdm Ztdn Zudo Zvdp Zwdq Zxdr Zyds Zzdt Z{du Z|dv Z}dw Z~dx Zdy Zdz Zd{ Zd| Zd} Zd~ Zd Zd Zd Zd Zd Zd Zy)    N)assert_equalassert_array_almost_equal)assert_allclose)RotationSlerp)special_ortho_group)permutationsc                 :    | dk(  rg dS | dk(  rg dS | dk(  rg dS y )Nx   r   r   yr   r   r   zr   r   r    axiss    Klib/python3.12/site-packages/scipy/spatial/transform/tests/test_rotation.py	basis_vecr      s.    s{		 
    c                      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r   rexpected_quats      r   test_generic_quat_matrixr'      sP    
,./A1A1#t--Maiik=9r   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   )r0   r0   r   r   r   r   r0   r0   r0   r0   r0   r   r   r      r   r$   s      r   test_from_square_quat_matrixr4   *   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)r0   r   r   r   )r   r0   r   r   )r   r   r0   r   r1   r2   r   r   T	canonical)	r    r!   r   r"   abslinalgnormr   r#   r$   s      r   *test_quat_double_to_canonical_single_coverr;   9   sw    

 	A 	1AFF1I		qq 9!T' BBMAIII-}=r   c                     t        j                  g d      } t        j                  |       }t	        | |j                  d             t        j                  g d      t        j                  d      z  } t        j                  |       }||z  |z  }t        |j                  d      t        j                  d      z  g d       t        |j                         j                  d      t        j                  d      z  g d       t        |j                  d      t        j                  d      z  g d       t        |j                         j                  d      t        j                  d      z  g d       t        ||j                         z  j                  d      g d	d
       t        ||j                         z  j                  d      g d	d
       t        ||z  j                  d      g dd
       t        |j                         |j                         z  j                  d      g dd
       y )Nr1   Fr6   )r   r   r   r   r3   )r0   r   r   r   )r   r   r   r0   )r0   r   r   r0   r/   gؗҬ<atol)	r    r!   r   r"   r   r#   sqrtr   inv)qr%   r3s      r   test_quat_double_coverrC   F   s    	A1AAIII./ 	rwwqz)A1A	
1QBAIII.rwwqz9 "AEEGOOeO4RWWQZ?!#BJJJ/
:!#BFFH$$u$5bggaj@"$ QquuwY''%'8 u.R[))E): u.QrTNNUN3!/QUUWRVVX%...?!/r   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   r3   r   pytestraises
ValueErrorr   r"   r    r!   r   r   r   test_malformed_1d_from_quatrJ   f   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   r3   r   r   r   )r   r            rF   r   r   r   test_malformed_2d_from_quatrO   k   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!   rG   rH   rI   r   r"   )r   s    r   test_zero_norms_from_quatrQ   s   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_quaternionrW   }   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   r0   r   r   r   r   r   r"   rS   r   shaper    r!   r   )rU   rV   expected_mats      r   #test_as_matrix_single_2d_quaternionr_      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 )NrY   r   r   r   r   r/   r1   )r   r   r   r[   r   r   r   r   r0   r   r   r   r3   r   )	r   r"   rS   r   r]   r    r!   r   rT   )quatsrV   	expected0	expected1s       r    test_as_matrix_from_square_inputrf      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 )NrY   ra   r   r3   r   r   )r   r   r   r[   r   r   r   r   rb   r   )皙?g@)gffffff@r   ri   )r0   r3   r3   r   r3   r\   )rc   rV   rd   re   	expected2s        r   !test_as_matrix_from_generic_inputrl      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 )Nr   r   r         ?ro   ro   ro   )r   r   from_matrixr#   rV   r&   s     r   test_from_single_2d_matrixrr      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 )Nr   r   r   rZ   rn   r   r   )r    r!   reshaper   r   rp   r#   rq   s     r   test_from_single_3d_matrixrv      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   rL   r   '   )3~˷g6iпnzo?)rz   ry   6i?)r{   rz   g3~˷?rZ   rt   )r    r!   r?   r   r   rp   r#   ru   )r&   rV   s     r   test_from_matrix_calculationr|      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   rp   rS   )rV   s    r    test_matrix_calculation_pipeliner      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   r3   r   r         ?)r    randomRandomStaterandom_sampler   rp   rS   einsum	transposezerosranger   )rndrV   	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 Nr   )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   r3   r3   r   r0   ro   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?ri   r   )r   r   r   r   ro   r   r   g@w?)g>}J?g.e?g>}J?r3   r/   )r    r!   r?   r   r   r#   r   r   )r   rU   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 )Nr   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   r3   rG   rH   rI   r   r   r   r   r   test_malformed_1d_from_rotvecr   I  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   rh   r   rL   rM   rN   r   r   r   r   test_malformed_2d_from_rotvecr   N  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   r3   r0   ro   r   r0   r   ga2U0*3?r/   r   r   r   r3   )r   r   )r    r!   r9   r:   r   r"   	as_rotvecr   coscrossr   )rU   r   angles      r   test_as_generic_rotvecr   V  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   r3   r3   gj*0	x?g$x?g:2(r   r    r!   r   r"   r   r   r]   r   rU   expected_rotvecactual_rotvecs      r   test_as_rotvec_single_1d_inputr   e  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   o  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 )
Nr   r   r   Tr   g      ^@r   r   r3   )r   rp   r   r    r9   r:   r   )rV   rotr   r   s       r   test_as_rotvec_degreesr   y  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   r0   r3   )ga2U0*3gǺ6?ga2U0*?Tr   )r    r!   r   r   r   r   )r   s    r   test_rotvec_calc_pipeliner     sg    XX
 F
 H((0::<fEH((>HHQUHVr   c                      g d} t        j                  g d      }t        j                  |       }t	        |j                         |       y N)r   r   r   )r   r   r   r   r    r!   r   from_mrpr   r#   mrpr&   r   s      r   test_from_1d_single_mrpr     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     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   ri   ri   g)g}+;?g}+;g}+;?gߨӉؿr/   )r    r!   r   r   r   r#   )r   r&   s     r   test_from_generic_mrpr     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   r3   rG   rH   rI   r   r   r   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   rh   r   r   r   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!   r9   r:   r   r   r"   as_mrp)rU   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 )	Nr3   r           r   xyz)i  r   r   Tr   )r    r!   tanpir   r   
from_eulerr   )r   s    r   test_past_180_degree_rotationr     sW     88bffRUU1WQY//a89LE;=DDFr   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   r]   r   rU   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 )Nr   Z   Tr   rY   r3   )r   r   r#   r    r!   r?   r   )rU   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   rS   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      )rL   r   lowhighr   r0   r   Tr   ZYX)r    r   r   randintr   r   r#   r   )r   abr   r   s        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   r[   r   r   r0   r   )r   r   rS   r    r!   r   )rV   r^   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"  r3   )ga}@?gF6g^@?)g0	8?gF6?g4?)      ro   r"  r   r   rS   r   r    r!   anglesrV   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?r0  ro   r   g      ?g ,r"  g ,?g      ?r&  r"        ?ro   r3   )r#  g4&d'e?)cH?Xӿg'eֿ)r"        ?r$  r'  r(  s     r   &test_from_euler_intrinsic_rotation_313r9  ,  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?r5  )r8  r"  g+Pz)r7  r6  r5  r   )g
?\2׳r8  )r=  g ,?r"  r3   )r.  r/  r4  )r0  r0        )g>dۿ @?r"  r'  r(  s     r   &test_from_euler_extrinsic_rotation_312r@  G  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>r0  )r?  r/  gXr3  r   r1  r2  r3   )r#  gcHr"  )r<  r7  r!  )r5  r5  r$  r'  r(  s     r   &test_from_euler_extrinsic_rotation_313rC  b  s    
F
 

eVT
:
D
D
FCc!fbhh-.)0 '  c!fbhh,+)0 '  c!fbhh,,+0 ' r   	seq_tupler   r	  )FTc                    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<   dj                  |       }|r|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allr8   errormean_maxrms_maxrI  rJ  rmss         r   
test_statsz1test_as_euler_asymmetric_axes.<locals>.test_stats  g    wwu1%ffU#hhtS!vvbffTlX-...vvcGm$$$r   r     r   r  r3   r    -q=r>   rtolV瞯<+=r    r   r   emptyuniformr   joinupperr   r   as_euler_as_euler_from_matrixr   
rD  r	  rR  r   nr)  seqrotationangles_quat
angles_mats
             r   test_as_euler_asymmetric_axesrh  }  sB   % ))


"CAXXq!fF;;BEE6QD;AF1a4L;;BEE6A:BEEAIQD;IF1a4L;;BEE6QD;AF1a4L
'')
C iik""3/H##C(K//4JFKae<FJQU;{V#UE2zF"E51r   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<   dj                  | d   | d   | d   g      }|r|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 rG  rH  rM  s         r   rR  z0test_as_euler_symmetric_axes.<locals>.test_stats  rS  r   r   rT  r   r  r   r3   rU  gvIh%<=rW  &.>缉ؗҜ<rZ  rY  r[  rb  s
             r   test_as_euler_symmetric_axesrm    sG   % ))


"CAXXq!fF;;BEE6QD;AF1a4L;;1255t;<F1a4L;;BEE6QD;AF1a4L ''9Q<1y|<
=Ciik""3/H##C(K//4JFKae<FJQT:{V#UE2zF"E51r   c                    t        j                  g dg dg dg dg      }dj                  |       }|r|j                         }t	        j
                  ||d      }|j                         }t        j                  t        d	      5  |j                  |d      }d d d        t	        j
                  |d      j                         }t        ||       y # 1 sw Y   <xY w)
Nr  r  #   rp     rp  r     ru  rr     rU  Tr   Gimbal lockr   r    r!   r^  r_  r   r   rS   rG   warnsUserWarningr`  r   rD  r	  r)  rd  re  mat_expectedangle_estimatesmat_estimateds           r   (test_as_euler_degenerate_asymmetric_axesr    s    
 XX	 F '')
C iik""3=H%%'L	k	7 ?"++C+>?''_dKUUWMlM:	? ?s   CC c                    t        j                  g dg dg dg dg      }dj                  | d   | d   | d   g      }|r|j                         }t	        j
                  ||d	      }|j                         }t        j                  t        d
      5  |j                  |d	      }d d d        t	        j
                  |d	      j                         }t        ||       y # 1 sw Y   <xY w)Nrw  r   r  rp  r   K   r  r  rp  rw  iLru  rU  r   r   Tr   rx  r   ry  r|  s           r   'test_as_euler_degenerate_symmetric_axesr    s    
 XX	 F ''9Q<1y|<
=C iik""3=H%%'L	k	7 ?"++C+>?''_dKUUWMlM:	? ?s   C##C,c                    t        j                  g dg dg dg dg      }dj                  |       }|r|j                         }t	        j
                  ||d      }t        j                  t        d	      5  |j                  |d      }d d d        t        j                  t        d	      5  |j                  |d      }d d 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       t        j                  g dg dg dg dg      }|d d df   d
k(  }dj                  | d
   | d   | d
   g      }|r|j                         }t	        j
                  ||d      }t        j                  t        d	      5  |j                  |d      }d d d        t        j                  t        d	      5  |j                  |d      }d d 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       y # 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)Nro  rq  rt  rv  rU  Tr   rx  r   r   r3   rV  rW  r   Hz>r  r  r  r  ư>r=   )r    r!   r^  r_  r   r   rG   rz  r{  ra  r`  r   )rD  r	  r)  rd  r   estimates_matrixestimates_quatidxs           r   +test_as_euler_degenerate_compare_algorithmsr    s}    XX	 F '')
C iik


c64
8C	k	7 H44S$4GH	k	7 9c489QF#^A1vI%>QU $QT*N1a4,@qtT XX	 F A,!
C ''9Q<1y|<
=C iik


c64
8C	k	7 H44S$4GH	k	7 9c489QF#^A1vI%>QU #q!>3$'#: a .a"8tUH H9 98H H9 9s0   8I /III& I
II#&I/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   r~   numr   r   r   )
r    r   r   r   r@   rS   r   r\  rT   r   )	r   rc  prA   p_matq_matresult1result2r   s	            r   test_invr  5  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   r   r   r   r  r   rZ   )r    r   r   r   r@   rS   dotrT   r   r   r\  )r   r  rA   r  r  res1res2rT   r   r   x_matrixy_matrixr  r  r   s                  r   test_inv_single_rotationr  G  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 )Nr~   r   )r   r   identity	magnituder@   )rc  s    r   test_identity_magnituder  e  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@   r   r   r   test_single_identity_magnituder  k  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 Nr~   r   r  	r   r   r  r   r#   r@   r  r    r   rc  r  r   s      r   test_identity_invariancer  p  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  {  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    rT   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    rT   r  r   r   )r%   r  r  s      r   test_magnitude_single_rotationr    sR    266!9%AdnnGGRUU#dnnGGQr   c                  L   t         j                  j                  d      } t        j                  d|       }t        j                  d|       }||j	                         z  }|j                         }t        j                  |      }t        |j                  ||      ||k         y )Nr   r~   r  )	r    r   r   r   r@   r  medianr   approx_equal)rngr  rA   r%   r_magr>   s         r   test_approx_equalr    sw    
))


"C-A-A	AEEGAKKME99UD4(54<9r   c                     t        j                  g d      } t        j                  t        j                  d            }| j                  |d         sJ | j                  |d         rJ | j                  |d   d      rJ | j                  |d   dd	      rJ t        j                  t        d
      5  | j                  |d   d      sJ 	 d d d        y # 1 sw Y   y xY w)N)r   r   rk  r   r   r   绽|=r=   g:0yE>T)r>   r   zatol must be setr   r   )	r   r   r"   r    rT   r  rG   rz  r{  )r  rA   s     r   !test_approx_equal_single_rotationr    s    \*A266!9%A>>!A$~~ad### ~~ad~///~~adt~<<<	k);	< 2~~adD~1112 2 2s   8CC$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   r3   r   r  r=   )
r    concatenaterT   linspacer   r   r   r   rI  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   r   r   r3   r   r   weightsr  r=   )
r    r!   r  r   r   r   rI  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  )	rG   rH   rI   r   r"   r    rT   rI  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  r~   rM   Tr  r   r   r0   r   )r    r   r   r   r  r   r  	enumerater8   r#   ru   moveaxisr]   argmaxrL  r@   r  r   )r  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 )Nr[   r   r   r   r   rE   rj   r   r   r3   r0   r   Tinverser    r!   r   rp   expand_dimsr   apply)	rV   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 )Nr[   r   r   r   r   rE   r   r   rL   r  )r   rL   r  )r   rL   Tr  r  )rV   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r3   r   r   r[   r   r   r   r  r   r   rE   r   r  )r   r   r3   r  )r   r   rj   Tr  )r    r\  r!   r   rp   r  r   r  )rV   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  r[   r   r   r   r  r   r   rE   r  r  )r   ir   r  )r   rL   r  Tr  )r    r\  r!   r   rp   r   r  )rV   r%   r  r  r  s        r   -test_apply_multiple_rotations_multiple_pointsr  B  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  r[   r   r   r   r  r   r   rY  r=   r0   r   )r    r\  r!   r   rp   r   rS   r  rV   r%   s     r   test_getitemr  X  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   )rG   rH   	TypeErrorr   r  r   r   r   test_getitem_singler  k  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  rG   rH   r  r  s    r   test_setitem_singler  p  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   seedr~   r  r   r   rL   r    r   r   r   r   r#   r  r   r  s      r   test_setitem_slicer  v  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  r~   r  r   r  r  s      r   test_setitem_integerr  ~  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)Nr~   r   r  zRotation objectr   r   )r   r   rG   rH   r  r  s    r   test_setitem_wrong_typer    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  r[   r   r   r   r  r   r   r3   r0   )r    r\  r!   r   rp   r   r  r  s     r   test_n_rotationsr!    s    
((9
CXX CF
 XX CF
 	S!AQQsVa r   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   rt   r   r   r   )r    r   r   r   r   r#   r]   )r   s    r   test_random_rotation_shaper%    s    
))


"Cc2::<BBDIC8@@BHH$O5==?EEvN5==?EEvNr   c                     t        j                  g dg dg      } | j                         }t        j                  | |      \  }}t        |j                         t        j                  d             t        |dd       y )NrE   r  r   r   r  r=   )	r    r!   copyr   align_vectorsr   rS   rT   r   )r   r   r%   rssds       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   r   r  r=   )	r    r   r   r   normalr  r(  r   r#   )r   cr  r  estr)  s         r   test_align_vectors_no_noiser0    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_rotationr2    sl    
679 	:A
68: 	;A &&q!,ICAsyy|$/D!$'r   c                     d} t        j                  g dg dg dg      }d}g dg dg dg}g dg d	g d
g}t        j                  ||d      \  }}}t        j                  || |      sJ t        j
                  |||      sJ y )NgbQ?)r   r   r   )r         ?r   )r   r   r   r  r   r   r   r   r   )r   g?r   )r   g?r   Treturn_sensitivityr=   )r    r!   r   r(  iscloseallclose)rssd_expectedsens_expectedr>   r  r  r   r)  senss           r   #test_align_vectors_rssd_sensitivityr=    s    %MHHn,,. /M D	Iy)A	K-A,,QdKOCt::dM555;;t]666r   c                     d} t        j                  | d      j                  g d      }t        j                  | d      j                  g d      }d}t        j                  ||t	        j
                  |       d      \  }}}t        j                  |||t	        j
                  |       z  d      \  }}}	t        |j                         |j                                t        t	        j                  |      |z  |d	       t        ||	       y )
Nr~   r   r  r   r   r3   Tr  r=   )	r   r   r  r(  r    r  r   rS   r?   )
rc  r  r  scaleest1rssd1cov1est2rssd2cov2s
             r   !test_align_vectors_scaled_weightsrF    s    
A*00;A*00;AE ..q!RWWQZFD% ..q!URWWQZ5GND%DNN$dnn&67BGGENU*E=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   r   r  r   r,  r   r4  )r   r?  Tr6  r=   r3   r   r   )r   r   )r3   r3   ro   )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_noiserR    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                  t        d      5  t        j                  g dg d       d d d        t        j                  t        d      5  t        j                  g d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 dgg dg dgg d
       d d d        t        j                  t        d      5  t        j                  g dgg dgdg
       d d d        t        j                  t        d      5  t        j                  g dg dgg dg dgt
        j                  t
        j                  g
       d d d        t        j                  t        d      5  t        j                  g dgg dg       d d d        t        j                  t        d      5  t        j                  g dg dgg dg dgdt
        j                  d	g       d d d        t        j                  t        d      5  t        j                  g dgg dgd       d d d        y # 1 sw Y   xY w# 1 sw Y   mxY w# 1 sw Y   7x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   4x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   rh   rE   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 valuesz)`weights` may not contain negative valuesr0   z#Only one infinite weight is allowedz(Cannot align zero length primary vectorsr   z Cannot return sensitivity matrixT)r7  r  r6  )rG   rH   rI   r   r(  r    infr   r   r    test_align_vectors_invalid_inputrU  	  s   	z)K	L 8|Y78 
z)K	L 8y,78 
z *? 
@ C	)4ykBC 
zE
G H	{YK1#GH 
zJ
L 2	95	97M'0	22
 
zH
J G	{YK"FG 
zB
D 9	95	97M(*'7	99
 
zG
I 9	{YK89 
z?
A M	95	97M26	MM
 
z?
A 8	{YK26	88 8K8 88 8C CH H2 2
G G9 9
9 9M M
8 8sx   KK  K-!K:'L) L,AL!L.6L;+MK K*-K7:LLL!L+.L8;MMc                     d} g dg dg}g dg dg}t        j                  g dg dg dg      }t        j                  ||t         j                  dg      \  }}t        |j                         || 	       t        |j                  |      || 	       t        j                  |d
| 	      sJ g dg dg}d}t        j                  ||t         j                  dg      \  }}t        |j                         || 	       t        |j                  |      d
   |d
   | 	       t        j                  ||| 	      sJ g dg dg}t        |j                  |      || 	       g dg dg}g dg dg}d}t        j                  ||t         j                  dg      \  }}t        |j                  |      d
   |d
   | 	       t        j                  ||| 	      sJ y )NrV  r   )r   r   r   r   r5  r   r   r  r=   r   )r   r3   r   r   r   r   r3   rE   )rj   r   r0   )r0   r   r3   r   g4i?)	r    r!   r   r(  rT  r   rS   r  r8  )r>   r  r  
m_expectedRr)  r:  
a_expecteds           r   "test_align_vectors_align_constrainr[  5  s   
 D	IA	IA9$$& 'J $$QBFFA;?GAtAKKM:D9AGGAJ-::dAD))) 
IAM$$QBFFA;?GAtAKKM:D9AGGAJqM1Q4d3::dM555Y'JAGGAJ
6 
K A	Z A&M$$QBFFA;?GAtAGGAJqM1Q4d3::dM555r   c                     d} g }t        d      D ]9  }|j                  t        j                  | d|z         j	                                ; t        |       D ]  }d|d   |   d   z  d|d   |   d   z  g}d|d   |   d   z  d	|d   |   d   z  g}t        j
                  ||d
dg      \  }}t        j
                  ||t        j                  dg      \  }}t        |j	                         |j	                         d        t        |       D ]  }d|d   |   d   z  d|d   |   d   z  d|d   |   d   z  g}d	|d   |   d   z  d|d	   |   d   z  d|d   |   d   z  g}t        j
                  ||g d      \  }}t        j
                  ||t        j                  ddg      \  }}t        |j	                         |j	                         d        y )Nr   rL   r~   r  r   r   r3   r   r       _Br  -C6?r=   r   )r]  r3   r   )	r   appendr   r   rS   r(  r    rT  r   )rc  matsr   r  r  rY  _R2s           r   test_align_vectors_near_infrc  X  s    	AD1X IHOOABF;EEGHI 1X BtAwqz!}_aQ
1o.tAwqz!}_aQ
1o.%%aT1I>1&&q!bffa[AAr||~DAB 1X BtAwqz!}_aQ
1oqaA?tAwqz!}_aQ
1oqaA?%%aLA1&&q!bffa^DAr||~DABr   c                  D   d} g dg dg}g dg dg}t        j                  g dg dg dg      }t        j                  ||t         j                  dg      \  }}t        |j                         ||        t        j                  |d	   |d	         \  }}t        |j                         ||        t        |j                  |d	         |d	   |        g dg dg}t        j                  g dg dg dg      }t        j                  ||t         j                  dg      \  }}t        |j                         ||        t        j                  |d	   |d	         \  }}t        |j                         ||        t        |j                  |d	         |d	   |        y )
NrV  r   r   rb   r   r   r  r=   r   )r    r!   r   r(  rT  r   rS   r  )r>   r  r  rX  rY  ra  s         r   test_align_vectors_parallelre  t  s[   D	IA	IA9%$& 'J !!!Q<DAqAKKM:D9!!!A$!-DAqAKKM:D9AGGAaDM1Q4d3	IA9$$& 'J !!!Q<DAqAKKM:D9!!!A$!-DAqAKKM:D9AGGAaDM1Q4d3r   c                     d} t        j                  g dg dgg dg dgg dg dgg      }|D cg c]  }|d    |d   g }}t        ||      D ]}  \  }}t        j                  ||t         j
                  dg      \  }}t        |j                         t         j                  |        t        |j                  |d         |d   |         t        j                  d	d
      }t        j                  |j                         dz        }g dg dg}g dg dg}g }|D ]*  }	|j                  |	j                  |d         |d   g       , |D ]t  }t        j                  ||t         j
                  dg      \  }}t        j                  ||ddg      \  }
}t        |j                         |
j                         |        v y c c}w )NrV  r   r   r   r   r   r  r=   r   r  r^  rb   r]  )r    r!   zipr   r(  rT  r   r  r   r  r   r   r   r_  rS   )r>   
as_to_testr  
bs_to_testr  rY  ra  RsdRsdRrb  s              r   test_align_vectors_antiparallelrm    s   DIy1%y1%y13 4J *44AAaD5!A$-4J4J
+ 81%%aRVVQK@1ruu48!qt$78 
1	-B


r||~d2
3C	YA	YAJ 2288AaD>1Q4012 B%%aRVVQK@1&&q!dAY?Ar||~DAB 5s   G	c                  x   d} t        j                  dd      j                         }t        j                  dd      j                         }t        ||      D ]_  \  }}|d   }|d   }t        j                  ||      \  }}t        |j                  |      ||        t        j                  |d|       r_J  y )NrV  r   r   r  r   r=   )	r   r   rS   rg  r(  r   r  r    r8  )	r>   mats_amats_bmat_amat_br  r  rY  r)  s	            r   test_align_vectors_primary_onlyrs    s    D__Sq1;;=F__Sq1;;=FFF+ .u!H!H ((A.4
AD1zz$---.r   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   r$  r,  )r   r   r3   r   r   )r   ro   r8  r   r4  r3   g      @r   g      
@g@r   r0   r   r   r   r3   rM   r~   r   )r    r   r   r   r"   r]  r#   r   r   rI  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_slerpr    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   rh   r/   r   r   )rG   rH   r  r    r!   r   )r%   r  s     r   test_slerp_rot_is_rotationr    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   rh   r   rG   rH   rI   r   r"   r   msgr%   s     r   test_slerp_single_rotr    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r  r  r  s     r   test_slerp_rot_len1r    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   )r3   r   r,  r   r3   )rG   rH   rI   r    r   r   r   r"   r]  r!   r   r   r%   r  s      r   test_slerp_time_dim_mismatchr    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   r$  r,  rM   )rG   rH   rI   r    r   r   r   r"   r]  aranger   r  s      r   !test_slerp_num_rotations_mismatchr    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   r$  r,  )r   r   r3   r3   r   
rG   rH   rI   r    r   r   r   r"   r]  r   r  s      r   test_slerp_equal_timesr  
  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)Nr  r   r   r$  r,  )r   r   r   r3   r   r  r  s      r   test_slerp_decreasing_timesr    r  r  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   r$  r,  r   z&`times` must be at most 1-dimensional.r   g      @g@)r    r   r   r   r"   r]  r  r   rG   rH   rI   r!   )r   r%   r  sinterp_timess        r   !test_slerp_call_time_dim_mismatchr    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   r$  r,  r   r   ztimes must be within the ranger   rW  )r   r3   rL   )r    r   r   r   r"   r]  r  r   rG   rH   rI   )r   r%   r  r  s       r   !test_slerp_call_time_out_of_ranger  '  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   r8  rs  rl  r=   )r   r   r   r@   r   r  )r%   r  r_interpolatedr_interpolated_expecteddeltas        r   test_slerp_call_scalar_timer  3  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  rT  r   r   )r   r   r   r    r9   r:   r#   )qsrsrA   s      r   test_multiplication_stabilityr  ?  sa    	!	,B	A	.B A
a"f		rzz|!<a@Ar   c                     d} t        j                  dd      }|j                         }dD ]  }||z  }t        j                  d      }t	        t        |            D ]  }|dkD  r||z  }||z  } ||j                         z  j                         }t        j                  || k        sJ t        j                  g d      }||z  j                         j                  dk(  sJ t        j                  g dg      }||z  j                         j                  dk(  rJ  d	D ]W  }||z  }t        j                  ||j                         z        }t        |j                         |j                         | 
       Y t        j                  g d      }d}||z  }t        j                  ||j                         z        }t        |j                         |j                         | 
       y )NrZ  r~   r   r  )r  rj   r0   r   r   r3   r   r/   r#  rt   )g      r>  g       r   ro   r4  r=   )rV  r   r   r   )r   r   r@   r  r   r8   r  r    rL  r"   r#   r]   r   r   r   )r>   r  p_invrc  rA   r%   ra  angs           r   test_powr  G  s   D+AEEGE% 0Fb!s1v 	A1uEI		
 1557{%%'vvcDj!!! |,1~~%%---~.1~~%%///!0& / =F  Q[[]!23		QYY[t<= 	]+A	A	QAQ./AAIIK48r   c                      t        j                  d      } t        j                  t        d      5  t        | dd       d d d        y # 1 sw Y   y xY w)Nr   r  zmodulus not supportedr   r   )r   r   rG   rH   NotImplementedErrorpow)r  s    r   test_pow_errorsr  l  s@    Q'A	*2I	J Aq!  s   A		A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  r3   r   r   )r3   )r   )r   r3   rH  )r   r   )dtype)r   r3   )	r   r   r    r!   r   r]   r   rS   object)singlemultipler!   s      r    test_rotation_within_numpy_arrayr  r  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   rY  r=   )r   r"   r    sinr   r   pickledumpsloadsr   rS   )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   rS   )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   Nr   .0r   s     r   	<genexpr>z+test_as_euler_contiguous.<locals>.<genexpr>       *!qAv*   c              3   &   K   | ]	  }|d k\    ywr  r   r  s     r   r  z+test_as_euler_contiguous.<locals>.<genexpr>  r  r  )r   r"   r`  flagsrL  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 )Nr~   r   r  )r   r3   r   r   r   )	r   r   listr    cumsumrg  r  r   r#   )re  sizesstartsr   rc  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   )rG   rH   r  r   r  r  r   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   r3   zSingle rotation has no len().r   )r   r    r\  r  rG   rH   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c                      g d} t        j                  | ddd      j                         }t        j                  g d      t        j
                  d      z  }t        ||       y )Nr   r  r  Tr   rY   r3   )r   from_davenportr#   r    r!   r?   r   )r   rU   r&   s      r   #test_from_davenport_single_rotationr    sO    D""4b+/118 	HH\*RWWQZ7MD-(r   c                     g d} g d}t        j                  t        j                  |       t        j                  z  dz        }t        j
                  | dt        j                  dz        }t        |j                  d      |j                  d             t        j                  t        j                  |       t        j                  z  dz  g      }t        j
                  | gdt        j                  dz  g      }t        |j                  d      |j                  d             t        j                  t        j                  |       t        j                  z  dz  t        j                  |      t        j                  z  dz  g      }|d   |d	   z  }t        j
                  || gdt        j                  dz  t        j                  dz  g      }t        |j                  d      |j                  d             t        j                  t        j                  |       t        j                  z  dz  t        j                  |       t        j                  z  dz  g      }t        j
                  | gdt        j                  dz  t        j                  dz  g      }t        |j                  d      |j                  d             y )
Nr   r   r   eTr6   rL   r   r   )r   r   r    r!   r   r  r   r#   )ezeyr   rot_davs       r   #test_from_davenport_one_or_two_axesr    s    	B	B 

rxx|bee3A5
6C%%b#ruuQw7GCKK$K/OOdO35 

ruu 4Q 67
8C%%rdC"%%';GCKK$K/OOdO35 

ruu 4Q 6 "ruu 4Q 6 8 9C
a&3q6/C%%r2hbeeAgruuQw5GHGCKK$K/OOdO35 

ruu 4Q 6 "ruu 4Q 6 8 9C%%rdC"%%'25571CDGCKK$K/OOdO35r   c                  t   g d} g d}g d}t        j                  t        d      5  t        j                  | |gdddg       d d d        t        j                  t        d      5  t        j                  | ||gdg d       d d d        t        j                  t        d	      5  t        j                  | gd
dg       d d d        t        j                  t        d      5  t        j                  | || gdg d       d d d        y # 1 sw Y   xY w# 1 sw Y   xY w# 1 sw Y   bxY w# 1 sw Y   y xY w)Nr   r   r5  zmust be orthogonalr   r  r   r   zorder should ber   zExpected `angles`)r   r   r3   r   )rG   rH   rI   r   r  )r  r  ezys      r   !test_from_davenport_invalid_inputr    s   	B	B
C	z)=	> 8S	3A78	z)=	> ?RsI>?	z):	; 2eaS12	z)<	= ARc<@A A8 8? ?2 2A As/   D
'D'D"$D.
DD"D+.D7c                  N   t         j                  j                  d      } d}t        j                  |df      }| j	                  t         j
                   t         j
                  |f      |d d df<   | j	                  dt         j
                  |f      }| j	                  t         j
                   t         j
                  |f      |d d df<   | j	                  dt         j
                  d      }t        j                  g d      }t        j                  g d      }|D ]  }||t        j                  ||z        j                  |      g}||z
  |d d d	f<   d
D ]F  }	|	dk(  r|n|d d d   }
t        j                  |
|	|      }|j                  |
|	      }t        ||       H  y )Nr   r   r   r  r3   )rs  r   r   r   r  r	  r0   )r    r   r   r\  r]  r   r!   r   r   r  r  as_davenportr   )r   rc  r)  angles_middlelambdasr  r  lambax_lamborderaxr   
angles_davs                r   test_as_davenportr    sg   
))


"CAXXq!fF;;BEE6QD;AF1a4LKKABEEK=M;;BEE6QD;AF1a4Lkkabee%k8G	)	B	)	B 0r8//R8>>rBC$t+q!t/ 	0E![0gddmB))"eV<C))"e4JJ/		00r   c                  J   t         j                  j                  d      } d}t        j                  |df      }| j	                  t         j
                   t         j
                  |f      |d d df<   t        |      D cg c]#  }| j                  dt         j
                  g      % }}| j	                  t         j
                   t         j
                  |f      |d d df<   | j	                  dt         j
                  d      }t        j                  g d      }t        j                  g d      }|D ]  }||t        j                  ||z        j                  |      g}	||z
  |d d d	f<   d
D ]  }
|
dk(  r|	n|	d d d   }t        j                  ||
|      }t        j                  t        d      5  |j!                  ||
      }d d d        |j#                         }t        j                  ||
      j#                         }t%        ||         y c c}w # 1 sw Y   TxY w)Nr   r   r   r  r3   )r   r   r   r   r  r	  r0   rx  r   )r    r   r   r\  r]  r   r   choicer!   r   r   r  r  rG   rz  r{  r  rS   r   )r   rc  r)  r   r  r  r  r  r  r  r  r  r   r  r}  r  s                   r   test_as_davenport_degenerater    s    ))


"C	AXXq!fF ;;BEE6QD;AF1a4L5:1X>SZZBEE
+>M>;;BEE6QD;AF1a4Lkkabee$k7G	)	B	)	B 
Cr8//R8>>rBC$t+q!t/ 	CE![0gddmB))"eV<Ck? 9 --b%8
9==?L$33BzJTTVM%lMB	C
C ?9 9s   ?(H3HH"c                     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<   dD ]  }t        d      D ]  }d	j                  |d   |d   |d   g      }|D cg c]  }t        |       }}|d
k(  r|j                         }t        j                  ||      }t        j                  |||      }	t        |j                  d      |	j                  d      d         |d d dfxx   t         j
                  dz  z  cc<   dD ]  }t        d      D ]  }d	j                  |      }|D cg c]  }t        |       }}|d
k(  r|j                         }t        j                  ||      }t        j                  |||      }	t        |j                         |	j                         d         y c c}w c c}w )Nr   r   r   r  r   r3   r  r   rU  r	  Tr6   rV  rX  )r    r   r   r\  r]  r   r	   r^  r   r_  r   r   r  r   r#   )
r   rc  r)  r  rD  rd  r   r  euldavs
             r   &test_compare_from_davenport_from_eulerr  2  s   
))


"CAXXq!fF ;;BEE6QD;AF1a4L;;1255t;<F1a4L;;BEE6QD;AF1a4L+ 	(%e, 	(I''9Q<1y|DEC(+,1)A,,B,#iik%%c62C))"eV<CCKK$K7t9T!&(	(	( 1a4LBEEAIL+ F%e, 	FI'')$C(+,1)A,,B,#iik%%c62C))"eV<CCKKM3;;=uE	FF - -s   	I I%c                     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<   dD ]  }t        d      D ]  }d	j                  |d   |d   |d   g      }|D cg c]  }t        |       }}|d
k(  r|j                         }t        j                  ||      }|j                  |      }	|j                  ||      }
t        |	|
d         |d d dfxx   t         j
                  dz  z  cc<   dD ]  }t        d      D ]  }d	j                  |      }|D cg c]  }t        |       }}|d
k(  r|j                         }t        j                  ||      }|j                  |      }	|j                  ||      }
t        |	|
d         y c c}w c c}w )Nr   r   r   r  r   r3   r  r   rU  r	  rV  r  )r    r   r   r\  r]  r   r	   r^  r   r_  r   r   r`  r  r   )r   rc  r)  r  rD  rd  r   r  r   r   r  s              r   "test_compare_as_davenport_as_eulerr  S  s   
))


"CAXXq!fF ;;BEE6QD;AF1a4L;;1255t;<F1a4L;;BEE6QD;AF1a4L+ 	2%e, 	2I''9Q<1y|DEC(+,1)A,,B,#iik%%c62C,,s#C""2u-CC51	2	2 1a4LBEEAIL+ 	2%e, 	2I'')$C(+,1)A,,B,#iik%%c62C,,s#C""2u-CC51	2	2 - -s   	H<I)rG   numpyr    numpy.testingr   r   r   scipy.spatial.transformr   r   scipy.statsr   	itertoolsr	   r  r'  r   r'   r*   r,   r4   r;   rC   rJ   rO   rQ   rW   r_   rf   rl   rr   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  r*  r9  r@  rC  markparametrizerh  rm  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r
  r  r  r  r  r  r  r  r!  r%  r*  r0  r2  r=  rF  rR  rU  r[  rc  re  rm  rs  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r   r   r   <module>r     s     A ) 3 + "  ::::
>/@0
.	4181>		+J
2?? 22	"%
E44*	??	O"
O..J)*16666 l5&9:m42 5 ;2> l5&9:m42 5 ;2: l5&9:m4; 5 ;;0 l5&9:m4; 5 ;;2 l5&9:m4> 5 ;>B.$.<?6
??@ :2=
C$(
54A8:*:89,T&
#20!$O((	(
7 $ON)8X 6FB840B4.)/X
		6A"9J&<F?+7=&)5@A0,C:FB2r   