
    UiVT                    ,   d dl Z d dlZd dlZd dlm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mZmZmZmZmZmZ d dlmc mZ d dlZd dlZej                            d          Zd	 Zd
efdZ d Z!d Z"d Z#d Z$d Z%d Z&d Z'd Z(d Z)d Z*d Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d  Z5d! Z6d" Z7d# Z8d$ Z9d% Z:d& Z;d' Z<d( Z=d) Z>d* Z?d+ Z@d, ZAd- ZBd. ZCd/ ZDd0 ZEd1 ZFd2 ZGd3 ZHd4 ZId5 ZJd6 ZKd7 ZLd8 ZMd9 ZNd: ZOd; ZPd< ZQd= ZRd> ZSd? ZTd@ ZUdA ZVdB ZWdC ZXdD ZYdE ZZdF Z[dG Z\dH Z]dI Z^ej        _                    dJ edK                    ej        _                    dLdM          dN                         Z`ej        _                    dJ edK                    ej        _                    dLdM          dO                         Zaej        jb        ej        _                    dJ edK                    ej        _                    dLdM          dP                                     Zcej        jb        ej        _                    dJ edK                    ej        _                    dLdM          dQ                                     Zdej        jb        ej        _                    dJ edK                    ej        _                    dLdM          dR                                     ZedS ZfdT ZgdU ZhdV ZidW ZjdX ZkdY ZldZ Zmd[ Znej        jb        d\             Zod] Zpd^ Zqd_ Zrd` Zsda Ztdb Zudc Zvdd Zwde Zxdf Zydg Zzdh Z{di Z|dj Z}dk Z~dl Zdm Zdn Zdo Zdp Zdq Zdr Zds Zdt Zdu Zdv Zdw Zdx Zdy Zdz Zd{ Zd| Zd} Zd~ Zd Zd Zd Zd ZdZd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zej        jb        d             Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd Zd ZdS )    N)assert_equal)RotationSlerp)special_ortho_group)permutationsproduct)xp_assert_equalis_numpyis_lazy_arrayxp_vector_normxp_assert_closeeager_warnsxp_default_dtypeT)np_onlyc                 B    | dk    rg dS | dk    rg dS | dk    rg dS d S )Nx   r   r   yr   r   r   zr   r   r    axiss    /var/www/development/aibuddy-work/election-extract/venv/lib/python3.11/site-packages/scipy/spatial/transform/tests/test_rotation.py	basis_vecr      sB    s{{yy	yy	yy 
    rc                 t    t          j        |                    |                                                     S )N)r   	from_quatasarrayas_quat)r   xps     r   rotation_to_xpr%   %   s'    bjj55666r   c                  F    t          d           t          g d           d S )Nr   r   r   r   )r   r   r   r   test_init_non_arrayr(   )   s(    \\\\r   c                     |                      g dg dg          }t          j        |          }||                      dgdgg          z  }t          |                                |           d S )N      @   r   r   )      r   r         @g      *@r"   r   r!   r   r#   r$   r   r   expected_quats       r   test_generic_quat_matrixr3   .   sl    


NNNMMM233A1A

SED6?333MAIIKK/////r   c                     |                      g d          }t          j        |          }|dz  }t          |                                |           d S Nr*   r-   r0   r1   s       r   test_from_single_1d_quaternionr6   5   sN    


>>>""A1AEMAIIKK/////r   c                     |                      g dg          }t          j        |          }|dz  }t          |                                |           d S r5   r0   r1   s       r   test_from_single_2d_quaternionr8   <   sQ    


NNN#$$A1AEMAIIKK/////r   c                 <   t           j                            d          }t          j        |                     g d          d          }t          |                                |                     d          dd           | 	                    |                     g d          d	          }t          j        |d          }t          |                                | 	                    |                     d          d
          dd           |                     |
                    dd                    }|t          |d          d d d f         z  }t          |j        d                   D ]Z}||df         }t          j        |d          }t          |                     |                                d          |d           [t          j        |d          }t          |                     |                                dd          |d           d S )Nr   r   r   r   r   Tscalar_first   V瞯<缉ؗҜ<rtolatol
   r   )rD   r   r   d   r,   r   r   .rA   )nprandomRandomStater   r!   r"   r   	as_matrixeyetilerandnr   rangeshaperollr#   r$   rngr   qiqis         r   test_from_quat_scalar_firstrV   C   s   
)


"
"C2::lll33$GGGAAKKMM266!995uEEEE


<<<(('22A14000A	rwwrvvayy*55E    	

399S!$$%%A	"	"	"111d7	++A171: A Aq#vYr555		Q//%@@@@@14000ABGGAIIKKG33QUCCCCCCr   c                     t           j                            d          } t          j        |           }t          j        |                                                                          }|                    |d          sJ t          j        d|           }t          j        |                                                                          }t          j        |                    |d                    sJ d S N{   rR   -q=rB   r=   )	rG   rH   default_rngr   r!   r#   tolistapprox_equalallrR   
r_expectedr   s      r   test_from_quat_array_likerc   Z   s    
)


$
$CS)))J:--//668899A""15"11111 ,,,J:--//668899A6*))!%)889999999r   c                     t          j        |                     g d                    }|                                j        t          |           k    sJ d S )Nr:   )r   r!   r"   r#   dtyper   r$   r   s     r   test_from_quat_int_dtyperg   g   sM    2::lll3344A99;; 0 4 4444444r   c           	      T   t           j                            d          }t          j        d|                     d                    }t          |                    d          |                     g d          dd	           t          j        d|                     d
                    }t          |                    d          | 	                    |                     g d          d          dd	           |                     |
                    dd                    }|t          |d          d d d f         z  }t          |j        d                   D ]}||df         }t          j        |          }t          |                    d          |                     |d          d           t          |                    dd          |                     |                    d          d          d           t          j        |          }t          |                    d          |                     |dd          d           t          |                    dd          |                     |                    d          dd          d           d S )Nr   xyzr=   Tr;   )      ?r   r   r   r>   r?   r@   )rD   r=   rC   rE   r,   r   r   .rF   )	canonicalr<   rk   )rG   rH   rI   r   
from_eulerzerosr   r#   r"   rL   rM   r   rN   rO   r!   rP   rQ   s         r   test_as_quat_scalar_firstro   l   s   
)


"
"CE288A;;//AAII4I00"**^^^2L2LU, , , , 	E288G#4#455AAII4I00GGBJJ~~~66@@U, , , , 	

399S!$$%%A	"	"	"111d7	++A171: $ $q#vYr""		t	44bggb!nn"	$ 	$ 	$ 	$ 			Dt	DD		D	 9 91=="	$ 	$ 	$ 	$ 	$ 	1AAII4I00"''!QQ'2G2G        AII4I@@GGAIII55qqGAAO O O O O Or   c           	         |                      g dg dg dg dg dg dg          }t          j        |          }||                      dgdgd	gd
gd	gd
gg          z  }t          |                                |           d S )N)r+   r   r   r,   r-   r   r.   r   r'   )rr   rr   r   r   r   r   rr   rr   rr   rr   rr   r/      r      r0   r1   s       r   test_from_square_quat_matrixrw      s    



 	 	A 	1A

SEB4!qcA3#DEEEMAIIKK/////r   c                     |                      g dg dg dg dg dg          }t          j        |          }|                     |          t	          |d          d d d f         z  }t          |                    d	          |           d S )
N)      r   r   r   )r   rr   r   r   )r   r   rr   r   rs   rt   r   r   Trl   )r"   r   r!   absr   r   r#   r1   s       r   *test_quat_double_to_canonical_single_coverr{      s    



 	 	A 	1AFF1IIqq 9 9 9!!!T' BBMAIII--}=====r   c                 l   |                      g d          }t          j        |          }t          ||                    d                     |                      g d          t          j        d          z  }t          j        |          }||z  |z  }t          |                    d          t          j        d          z  |                      g d                     t          |                                                    d          t          j        d          z  |                      g d                     t          |                    d          t          j        d          z  |                      g d                     t          |                                                    d          t          j        d          z  |                      g d                     t          ||                                z                      d          |                      g d	          d
           t          ||                                z                      d          |                      g d	          d
           t          ||z                      d          |                      g d          d
           t          |                                |                                z                      d          |                      g d          d
           d S )N)        r   r   rr   Frl   )rj   r   r   r   rv   )ry   r   r   r   )rj   r   r   rr   )ry   r   r   rr   r}   r   r   r   gؗҬ<r\   )	r"   r   r!   r	   r#   mathsqrtr   inv)r$   rS   r   r3s       r   test_quat_double_coverr      s    	

???##A1AAqyy5y11222


>>>""49Q<</A1A	
1QBAIII..ty||;JJ~~~..0 0 0AEEGGOOeO44TYq\\AJJ//1 1 1BJJJ//	!<JJ//1 1 1BFFHH$$u$55dillBJJ///002 2 2 QquuwwY''%'88JJ~~~..U< < < <R[))E)::JJ~~~..U< < < <QrTNNUN33JJ//e= = = =QUUWWrvvxx'0050AAJJ//e= = = = = =r   c                 D   t          j        t          d          5  t          j        |                     g d                     d d d            n# 1 swxY w Y   t          j        t          d          5  t          j        |                     g dg dg                     d d d            n# 1 swxY w Y   t          j        t          d          5  t          j        |                     g dgg dgg                     d d d            d S # 1 swxY w Y   d S )NzExpected `quat` to have shapematchr   rv   r=   )r   rv   r=   r,   r-   )r,   r-            r   rv   r=   r,   )r,   r-   r   r   )pytestraises
ValueErrorr   r!   r"   r$   s    r   test_from_quat_wrong_shaper      s   	z)H	I	I	I 2 22::iii001112 2 2 2 2 2 2 2 2 2 2 2 2 2 2 
z)H	I	I	I  2::OOOO'   	 	 	               
z)H	I	I	I  2::\\N\\N'   	 	 	                 s5   *AAA8.B22B69B60DDDc                    |                      g dg dg dg          }t          |          rV|                     |                     t	          j        |                                          d                             sJ d S t          j        t                    5  t	          j        |           d d d            d S # 1 swxY w Y   d S )N)r=   r,   r   r   )r   r   r   r   rq   r   .)
r"   r   r`   isnanr   r!   r#   r   r   r   )r$   r   s     r   test_zero_norms_from_quatr      s   


LLLLMM 	 	A
 Q "vvbhhx1!44<<>>vFGGHHHHHHH]:&& 	" 	"q!!!	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	" 	"s   B??CCc                     |                      g d          }t          j        |                                          }t	          ||                     d                     d S )Nr'   r=   )r"   r   r!   rJ   r   rK   )r$   quatmats      r   #test_as_matrix_single_1d_quaternionr      sS    ::lll##D

T
"
"
,
,
.
.CC#####r   c                    |                      g dg          }t          j        |                                          }t	          |j        d           |                      g dg dg dg          }t          |d         |           d S )Nr   r   r   r   r   r=   r=   r}   rr   r   r   r   r   .r"   r   r!   rJ   r   rO   r   )r$   r   r   expected_mats       r   #test_as_matrix_single_2d_quaternionr      s    ::|||n%%D

T
"
"
,
,
.
.CI&&&::				
  L
 CK.....r   c                 2   |                      g dg dg dg dg          }t          j        |                                          }t	          |j        d           |                      g dg dg dg          }t          |d	         |           |                      g d
g dg dg          }t          |d         |           t          |d         |                     d                     t          |d         |                     d                     d S )Nr   r   r   r   r   r'   rs   )r,   r=   r=   r   r   r   r   r}   r   r   r   rr   r   r   r   rv   .r=   r=   .)r"   r   r!   rJ   r   rO   r   rK   )r$   quatsr   	expected0	expected1s        r    test_as_matrix_from_square_inputr      s.   JJLLLLLLMM	  E 
U
#
#
-
-
/
/CI&&&

				
  I
 CK+++

		


  I
 CK+++CK+++CK+++++r   c                    |                      g dg dg dg          }t          j        |                                          }t	          |j        d           |                      g dg dg dg          }t          |d         |           |                      g d	g d
g dg          }t          |d         |           |                      g dg dg dg          dz  }t          |d         |           d S )Nr   r   r   )r=   r=   r=   r   r   r   r   r   r   r   r   )皙?g@)gffffff@r   r   )rr   rv   rv   r=   r   r   )r$   r   r   r   r   	expected2s         r   !test_as_matrix_from_generic_inputr     s8   JJLLLLLL  E
 
U
#
#
-
-
/
/CI&&&

				
  I
 CK+++

		


  I
 CK+++




   	I
 CK+++++r   c                     |                      g dg dg dg          }|                      g d          }t          t          j        |                                          |           d S Nr   r   r   )      ?r   r   r   r"   r   r   from_matrixr#   r$   r   r2   s      r   test_from_single_2d_matrixr   6  sv    
**IIIIII  C
 JJ33344MH(--5577GGGGGr   c                     |                      g dg dg dgg          }|                      g dg          }t          t          j        |                                          |           d S r   r   r   s      r   test_from_single_3d_matrixr   @  s~    
**						
   C
 JJ 4 4 4566MH(--5577GGGGGr   c                    d}|                      g d          t          j        d          z  }|                      g dg dg dg          }t          t	          j        |                                          ||           t          t	          j        |                     |d                                                    |                     |d	          |           d S )
N:0yE>)rj   r   r   r   '   )3~˷g6iпnzo?)r   r   6i?)r   r   g3~˷?r\   r   r   r,   )r"   r   r   r   r   r   r#   reshape)r$   rB   r2   r   s       r   test_from_matrix_calculationr   J  s    DJJ~~~..2>M
**///...---  C
 H(--5577TRRRRH(C)C)CDDLLNNJJ}f55     r   c                     |                      t          j        ddd                    }t          t	          j        |                                          |           d S )Nr=   rD   r   )sizerandom_state)r"   r   rvsr   r   r   rJ   r$   r   s     r    test_matrix_calculation_pipeliner   X  sS    
**(,QRaHHH
I
ICH(--77993?????r   c                    d}t           j                            d          }|                     |                    d                    }| j                            |          }t          |j        d                   D ]F}||         dk     r8t          j
        |          |df                             ||df                    }Gt          j        |                                          }|                     ||                     |                    }|                     d          |                     d          z   }t'          |||           d S )Nr[   r   )rE   r=   r=   .r=   r\   )rG   rH   rI   r"   random_samplelinalgdetrN   rO   xpxatsetr   r   rJ   matmulmatrix_transposern   rK   r   )	r$   rB   rndr   detsrT   	ortho_matmult_resulteye3ds	            r   test_from_matrix_ortho_outputr   ]  s   D
)


"
"C
**S&&{33
4
4C9==D4:a=!! 8 87Q;;&++af%))3q#v;,77C$S))3355I))Ir':':9'E'EFFKHH[!!BFF1II-EKT222222r   c                    |                      g dg dg dg          }|                      g dg dg dg          }t          t          j        |                                          |d           |                      g d	g d
g dg          }|                      g dg dg dg          }t          t          j        |                                          |d           d S )Nr   r   r   r   r   )5_%?gx'?r}   )gx'ܿr   r}   )r}   r}   rj   ư>r\   )r         r   )r   r   r   )r   r   r   r   r   )r"   r   r   r   rJ   )r$   r   expecteds      r   test_from_matrix_normalizer   n  s!   
**						  C zz5555555557 8 8H H(--77998$OOOO
**  C zz<<<'<<'<<) * *H H(--77998$OOOOOOr   c                 |   |                      d          }t          j        |          d                             d          }t	          |          rO|                     |                     t          j        |          	                                                    sJ nGt          j        t          d          5  t          j        |           d d d            n# 1 swxY w Y   t          j        |          d                             d          }t	          |          rP|                     |                     t          j        |          	                                                    sJ d S t          j        t          d          5  t          j        |           d d d            d S # 1 swxY w Y   d S )Nr=   r   r   r   zNon-positive determinantr   rr   )rK   r   r   r   r   r`   r   r   r   rJ   r   r   r   r   s     r   )test_from_matrix_non_positive_determinantr     s   
&&))C
&++d



"
"CS &vvbhhx3C88BBDDEEFFFFFF]:-GHHH 	& 	& %%%	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& &++d



#
#CS &vvbhhx3C88BBDDEEFFFFFFF]:-GHHH 	& 	& %%%	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	& 	&s$   <CC!$C!F11F58F5c                     t           j                            d          } t          j        |           }t          j        |                                                                          }|                    |d          sJ t          j        d|           }t          j        |                                                                          }t          j        |                    |d                    sJ d S rX   )	rG   rH   r]   r   r   rJ   r^   r_   r`   ra   s      r   test_from_matrix_array_liker         
)


$
$CS)))JZ1133::<<==A""15"11111 ,,,JZ1133::<<==A6*))!%)889999999r   c                     |                      g dg dg dg          }t          j        |          }|                                j        t          |           k    sJ d S )Nr   r   r   )r"   r   r   r#   re   r   r$   r   r   s      r   test_from_matrix_int_dtyper     sa    
**iiiIII6
7
7CS!!A99;; 0 4 4444444r   c                     d}|                      g d          }|                      g d          }t          j        |          }t          |                                ||           d S NHz>r   )gJ?r   r   g<(?r\   r"   r   from_rotvecr   r#   r$   rB   rotvecr2   results        r   test_from_1d_single_rotvecr     si    DZZ			""FJJ;;;<<M!&))FFNN$$m$??????r   c                     d}|                      g dg          }|                      g dg          }t          j        |          }t          |                                ||           d S r   r   r   s        r   test_from_2d_single_rotvecr     sn    DZZ$$FJJ < < <=>>M!&))FFNN$$m$??????r   c                     d}|                      g dg dg dg          }|                      g dg dg dg          }t          t          j        |                                          ||           d S )	Nr   r   rv   rv   r   rr   r   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$   rB   r   r2   s       r   test_from_generic_rotvecr     s    DZZIILLII  F JJ444555 
  M
 H(0088::MPTUUUUUUr   c                    |                      dt          j        d          z  dt          j        d          z  dt          j        d          z  gg dg dg          }t          j        |                                          }t          |d         |                      d          d                    t          |d	d df         |d
         dz             t          |d         |                      d          d                    t          |dd df         |                      g d                     t          |d         |                      g d                     d S )NgMb@?r=   gMb@)皙?g333333?r   r   r   r=   rj   r   r   r   r   r   r=   g@w?r   )g>}J?g.e?g>}J?r   r~   )r"   r   r   r   r   r#   r   r	   )r$   r   r   s      r   test_from_rotvec_small_angler     sP   ZZ		!	edill2D49Q<<4GH		
  F ''//11DDJ

3 3444DBQBK#!5666DJ

9 5 5b 9:::DBQBKJJ % % % & &' ' ' DL"**^^^"<"<=====r   c                     t           j                            d          } t          j        |           }t          j        |                                                                          }|                    |d          sJ t          j        d|           }t          j        |                                                                          }t          j        |                    |d                    sJ d S rX   )	rG   rH   r]   r   r   	as_rotvecr^   r_   r`   ra   s      r   test_from_rotvec_array_liker     r   r   c                     |                      g d          }t          j        |          }|                                j        t          |           k    sJ d S )Nr   )r"   r   r   r#   re   r   )r$   r   r   s      r   test_from_rotvec_int_dtyper    sR    ZZ			""FV$$A99;; 0 4 4444444r   c                 <   |                      dgdz            }t          j        |d          }|                                }|                      |dz  | j        z            }t          j        |          }|                                }t          ||           d S )Ngvô0?r=   Tdegrees   )r"   r   r   r#   pir   )r$   rotvec1rot1quat1rotvec2rot2quat2s          r   test_degrees_from_rotvecr    s    jj.)A-..G666DLLNNE jj3.//G((DLLNNEE5!!!!!r   c                     t          j        t          d          5  t          j        |                     ddg                     d d d            d S # 1 swxY w Y   d S )N Expected `rot_vec` to have shaper   r   rv   r   r   r   r   r   r"   r   s    r   test_malformed_1d_from_rotvecr    s    	z)K	L	L	L 1 1RZZA//0001 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1   *AAAc                     t          j        t          d          5  t          j        |                     g dg dg                     d d d            d S # 1 swxY w Y   d S )Nr  r   r   r-   r   r   r   r  r   s    r   test_malformed_2d_from_rotvecr     s    	z)K	L	L	L  RZZLLLL)   	 	 	                    .AAAc           	         |                      g dg dg dg          }|t          |dd          z  }t          j        |                                          }t          |d          }t          |d d df         |                     |d	z                       t          | j                            ||d d d df                   | 	                    d
          d           d S )Nr   rv   rr   r   r   rr   r   ga2U0*3?r'   rr   T)r   keepdimsr   r=   rv   )r=   r=   r>   r\   )
r"   r   r   r!   r   r   cosr   crossrn   )r$   r   r   angles       r   test_as_generic_rotvecr    s    ::OOLL  D
 	N4b48888D%%//11F6+++EDAJuqy 1 1222BIOOFDBQBK88"((6:J:JQVWWWWWWr   c                     |                      g d          }|                      g d          }t          j        |                                          }t	          |j        d           t          ||           d S )Nr   rv   rv   gj*0	x?g$x?g:2(r=   r"   r   r!   r   r   rO   r   r$   r   expected_rotvecactual_rotvecs       r   test_as_rotvec_single_1d_inputr(    st    ::mmm$$Djj!C!C!CDDO&t,,6688M$d+++M?33333r   c                     |                      g dg          }|                      g dg          }t          j        |                                          }t	          |j        d           t          ||           d S )Nr   r"  r   r$  r%  s       r   test_as_rotvec_single_2d_inputr*  !  sy    ::}}}o&&Djj"D"D"D!EFFO&t,,6688M$f---M?33333r   c                 z   |                      g dg dg dg          }t          j        |          }|                    d          }t	          |d          }t          ||                      d          d	                    t          |d
         |d                    t          |d         |d                    d S )Nr   r   r   Tr  rr   r   g      ^@r   r   r   rv   )r"   r   r   r   r   r   )r$   r   rotr   r  s        r   test_as_rotvec_degreesr-  +  s    
**iiiIII6
7
7C

s
#
#C]]4]((F6+++EE2::e,,R0111F1Ivay)))F1Ivay)))))r   c                    |                      g dg dg dg          }t          t          j        |                                          |           t          t          j        |d                              d          |           d S )Nr   r   rr   rv   )ga2U0*3gǺ6?ga2U0*?Tr  )r"   r   r   r   r   )r$   r   s     r   test_rotvec_calc_pipeliner0  6  s    ZZ		


  F
 H(00::<<fEEEH(>>>HHQUHVV    r   c                     |                      g d          }|                      g d          }t          j        |          }t          |                                |d           d S )Nr   r   rj   r}   r   r   r   r[   r\   r"   r   from_mrpr   r#   r$   mrpr2   r   s       r   test_from_1d_single_mrpr8  B  sa    
**[[[
!
!CJJ~~~..Ms##FFNN$$m%@@@@@@r   c                     |                      g dg          }|                      g dg          }t          j        |          }t          |                                |           d S )Nr2  r3  r4  r6  s       r   test_from_2d_single_mrpr:  I  sa    
**kkk]
#
#CJJ/00Ms##FFNN$$m44444r   c                     t           j                            d          } t          j        |           }t          j        |                                                                          }|                    |d          sJ t          j        d|           }t          j        |                                                                          }t          j        |                    |d                    sJ d S rX   )	rG   rH   r]   r   r5  as_mrpr^   r_   r`   ra   s      r   test_from_mrp_array_liker=  P  s    
)


$
$CS)))J*++--446677A""15"11111 ,,,J*++--446677A6*))!%)889999999r   c                     |                      g d          }t          j        |          }|                                j        t          |           k    sJ d S )Nr   )r"   r   r5  r#   re   r   )r$   r7  r   s      r   test_from_mrp_int_dtyper?  ]  sR    
**YYY

C#A99;; 0 4 4444444r   c                     |                      g dg dg dg          }|                      g dg dg dg          }t          t          j        |                                          |           d S )Nr   r   r   )r   r   r   g)g}+;?g}+;g}+;?gߨӉؿr'   )r"   r   r   r5  r#   )r$   r7  r2   s      r   test_from_generic_mrprA  c  s    
**				  C JJ:::   M H%c**2244mDDDDDr   c                     t          j        t          d          5  t          j        |                     ddg                     d d d            d S # 1 swxY w Y   d S )NExpected `mrp` to have shaper   r   rv   r   r   r   r   r5  r"   r   s    r   test_malformed_1d_from_mrprE  o  s    	z)G	H	H	H . ."**aV,,---. . . . . . . . . . . . . . . . . .r  c                     t          j        t          d          5  t          j        |                     g dg dg                     d d d            d S # 1 swxY w Y   d S )NrC  r   r   r  rD  r   s    r   test_malformed_2d_from_mrprG  t  s    	z)G	H	H	H  "**LLLL&   	 	 	                 r  c                 "   |                      g dg dg dg          }|t          |d          d d d f         z  }|                      g dg dg dg          }t          t          j        |                                          |           d S )	Nr  r  r'   r   r   )gQUU?gvWUU?QUUտ)uFx?guFxrJ  r   )r"   r   r   r   r!   r<  )r$   r   expected_mrps      r   test_as_generic_mrprL  |  s    ::  D 	N4a(((D11D::------		  L H&t,,3355|DDDDDr   c           	      
   |                      t          j        | j        dz  dz             ddg          }t	          t          j        d|                      g d          d                                          |           d S )	Nrv   r,   r}   r   ri   )i  r   r   Tr  )r"   r   tanr  r   r   rm   r<  )r$   rK  s     r   test_past_180_degree_rotationrO    s     ::Q 7 77a@AALE2::kkk#:#:DIIIPPRR    r   c                     |                      g d          }|                      g d          }t          j        |                                          }t	          |j        d           t          ||           d S )Nr   g?g?gO޿r#  r"   r   r!   r<  r   rO   r   r$   r   rK  
actual_mrps       r   test_as_mrp_single_1d_inputrU    st    ::mmm$$D::CCCDDL#D))0022J!4(((J-----r   c                     |                      g dg          }|                      g dg          }t          j        |                                          }t	          |j        d           t          ||           d S )Nr   rQ  r   rR  rS  s       r   test_as_mrp_single_2d_inputrW    sy    ::}}}o&&D::DDDEFFL#D))0022J!6***J-----r   c                     |                      g dg dg dg dg          }|                      g dg dg dg dg          }t          t          j        |                                          |           d S )Nr   r/  )gzpy?r   r   )皙?r   rY  )gK}\UUſgK}\UU?rI  )r"   r   r   r5  r<  )r$   rT  rK  s      r   test_mrp_calc_pipelinerZ    s    		

	  J
 ::		...	  L
 H%j1188::LIIIIIr   c                     t          j        d|                     d          d                                          }|                     g d          t	          j        d          z  }t          ||           d S )Nr   Z   Tr  r}   r   r   r   rv   )r   rm   r"   r#   r   r   r   )r$   r   r2   s      r   test_from_euler_single_rotationr^    sg    sBJJrNNDAAAIIKKDJJ~~~..1=MD-(((((r   c                    t          j        d|                     d          d                                          }t          j        d|                     d          d                                          }t	          ||           d S )Nr   r\  Tr  Zr   rm   r"   rJ   r   )r$   	extrinsic	intrinsics      r   (test_single_intrinsic_extrinsic_rotationrd    sr    #CBFFFPPRRI#CBFFFPPRRIIy)))))r   c                    t           j                            d          }|                     |                    ddd                    }|                     |d          }t          j        d|d	                                          }t          j        d
|d	                                          }t          ||           d S )Nr   r  )r   r=   lowhighr   rr   r   ri   Tr  ZYX)
rG   rH   rI   r"   randintflipr   rm   r#   r   )r$   r   abr   r   s         r   test_from_euler_rotation_orderrn    s    
)


"
"C


3;;13V;<<==A
AE1d333;;==AE1d333;;==AAqr   c                     d}t          j        d|                     ddg          d                                          }|                     g dg dg dg          }t	          |||	           d S )
Nr[   zxr\  Tr  r   r   r   rr   r   r\   ra  )r$   rB   r   r   s       r   -test_from_euler_elementary_extrinsic_rotationrr    s    D

dBJJBx$8$8$
G
G
G
Q
Q
S
SC::

		  L
 CD111111r   c           	         d}|                      g dg dg dg          }t          j        d|d                                          }t	          |d         |                      g d	g d
g dg          |           t	          |d         |                      g dg dg dg          |           t	          |d         |                      g dg dg dg          |           d S )Nr      <   -   ru  rv  ru  rw  ru  rv  ZXYTr  r   )g# ?      пgΙ6d?)g$+H?+Pz?Ppƿ)gPpֿ+Pz?gPp?r\   r   )g+a?r{  (s=?)r  r|  g&(ٿ)r{  r~  r|  r   )ga}@?gF6g^@?)g0	8?gF6?g4?)      r   r|  r"   r   rm   rJ   r   )r$   rB   anglesr   s       r   &test_from_euler_intrinsic_rotation_312r    s]   DZZ
  F
 
eVT
:
:
:
D
D
F
FCCK*********- " " 	    CK*********- " " 	    CK***)))***- " " 	     r   c           	         |                      g dg dg dg          }t          j        d|d                                          }t	          |d         |                      g dg d	g d
g                     t	          |d         |                      g dg dg dg                     t	          |d         |                      g dg dg dg                     d S )Nrt  rx  ry  ZXZTr  r   )>d?g @r|  )l>?^|@r  )X?r  r   r   g      ?g ,r|  g ,?g      ?r  r|        ?r   r   )r}  g4&d'e?)cH?Xӿg'eֿ)r|        ?r~  r  r$   r  r   s      r   &test_from_euler_intrinsic_rotation_313r    s:   ZZ
  F
 
eVT
:
:
:
D
D
F
FCCK,,,,,,***- " "    CK+++***'''- " "    CK---...***- " "     r   c           	         |                      g dg dg dg          }t          j        d|d                                          }t	          |d         |                      g dg d	g d
g                     t	          |d         |                      g dg d	g dg                     t	          |d         |                      g dg dg dg                     d S )Nrt  rx  ry  zxyTr  r   )4&d?gPp?r  )r  r|  g+Pz)r  r  r  r   )g
?\2׳r  )r  g ,?r|  r   )r  r  r  )r  r  r   )g>dۿ @?r|  r  r  s      r   &test_from_euler_extrinsic_rotation_312r    s:   ZZ
  F
 
eVT
:
:
:
D
D
F
FCCK++++++---- " "    CK+++++++++- " "    CK,,,,,,,,,- " "     r   c           	         |                      g dg dg dg          }t          j        d|d                                          }t	          |d         |                      g dg d	g d
g                     t	          |d         |                      g dg dg d
g                     t	          |d         |                      g dg dg dg                     d S )Nrt  rx  ry  zxzTr  r   )r  gl>r  )r  r  gXr  r   r  r  r   )r}  gcHr|  )r  r  r{  )r  r  r~  r  r  s      r   &test_from_euler_extrinsic_rotation_313r  *  s:   ZZ
  F
 
eVT
:
:
:
D
D
F
FCCK---...)))- " "    CK,,,+++)))- " "    CK,,,,,,+++- " "     r   c                     t           j                            d          } d}t          j        |           }t          j        ||                    |                                                    }|                    |d          sJ t          j        d|           }t          j        ||                    |                                                    }t          j        |                    |d                    sJ d S )NrY   ri   rZ   r[   r\   r=   )	rG   rH   r]   r   rm   as_eulerr^   r_   r`   )rR   orderrb   r   s       r   test_from_euler_array_liker  E  s    
)


$
$CES)))JE:#6#6u#=#=#D#D#F#FGGA""15"11111 ,,,JE:#6#6u#=#=#D#D#F#FGGA6*))!%)889999999r   c                     t           j                            d          } |                     dd          }t	          j        d|d          }t	          j        dt          |          d          }|                    |d	          sJ d S )
NrY   Lr  )rg  rh  r   Tr  r[   r\   )rG   rH   r]   uniformr   rm   floatr_   )rR   degrb   r   s       r   test_from_euler_scalarr  S  s    
)


$
$C
++$S+
)
)C$S#t<<<JCsT:::A""15"1111111r   	seq_tupleri   rc  )FTc                 T     fd}t           j                            d          }d}t          j        |df          }|                    t           j         t           j        |f          |d d df<   |                    t           j         dz  t           j        dz  |f          |d d df<   |                    t           j         t           j        |f          |d d df<                        |          }d                    |          }|r|                                }t          j
        ||          }|                    |          }	|                    |          }
t          ||	dd	
           t          ||
dd	
            ||	|z
  dd            ||
|z
  dd           d S )Nc                 "                        | d          }                    | d          }                    ||          }                                        |          |k               sJ                     ||k               sJ d S Nr   r   meanstdhypotr`   rz   errormean_maxrms_maxr  r  rmsr$   s         r   
test_statsz1test_as_euler_asymmetric_axes.<locals>.test_stats_      wwu1w%%ffUf##hhtS!!vvbffTllX-.....vvcGm$$$$$$$r   r     r=   rf  rv   r    r[   rB   rA   r>   +=rG   rH   rI   emptyr  r  r"   joinupperr   rm   r  _as_euler_from_matrixr   r$   r  rc  r  r   nr  seqrotationangles_quat
angles_mats   `          r   test_as_euler_asymmetric_axesr  [  s   % % % % % )


"
"CAXq!fF;;BE6QD;AAF111a4L;;BE6A:BEAIQD;IIF111a4L;;BE6QD;AAF111a4LZZF
'')

C  iikk"3//H##C((K //44JFKae<<<<FJQU;;;;J{V#UE222JzF"E511111r   c                 \     fd}t           j                            d          }d}t          j        |df          }|                    t           j         t           j        |f          |d d df<   |                    dt           j        |f          |d d df<   |                    t           j         t           j        |f          |d d df<                        |          }d                    |d         |d         |d         g          }|r|                                }t          j
        ||          }|                    |          }	|                    |          }
t          ||	dd	
           t          ||
dd
            ||	|z
  dd            ||
|z
  dd	           d S )Nc                 "                        | d          }                    | d          }                    ||          }                                        |          |k               sJ                     ||k               sJ d S r  r  r  s         r   r  z0test_as_euler_symmetric_axes.<locals>.test_stats  r  r   r   r  r=   rf  r   rv   r  gvIh%<=r  &.>r?   r  r>   r  r  s   `          r   test_as_euler_symmetric_axesr    s   % % % % % )


"
"CAXq!fF;;BE6QD;AAF111a4L;;125t;<<F111a4L;;BE6QD;AAF111a4LZZF ''9Q<1y|<
=
=C iikk"3//H##C((K//44JFKae<<<<FJQT::::J{V#UE222JzF"E511111r   c                    d}|                      g dg dg dg dg          }d                    |          }|r|                                }t          j        ||d          }|                                }t          |t          d	
          5  |                    |d          }d d d            n# 1 swxY w Y   t          j        ||d                                          }	t          ||	|           d S )Nr[   rw  r\  #   r     r  r\     r  r     r  Tr  Gimbal lockr   r\   
r"   r  r  r   rm   rJ   r   UserWarningr  r   
r$   r  rc  rB   r  r  r  mat_expectedangle_estimatesmat_estimateds
             r   (test_as_euler_degenerate_asymmetric_axesr    sS    D ZZ	  F '')

C  iikk"3===H%%''L 
\;m	D	D	D ? ?"++C+>>? ? ? ? ? ? ? ? ? ? ? ? ? ? ?'_dKKKUUWWML-d;;;;;;s   B55B9<B9c                 $   d}|                      g dg dg dg dg          }d                    |d         |d         |d         g          }|r|                                }t          j        ||d	
          }|                                }t          |t          d          5  |                    |d	
          }d d d            n# 1 swxY w Y   t          j        ||d	
                                          }	t          ||	|           d S )Nr[   r  r   rv  r  r   K   rv  r  r  r  r  r  r  r   r   Tr  r  r   r\   r  r  s
             r   'test_as_euler_degenerate_symmetric_axesr    sf    D ZZ	  F ''9Q<1y|<
=
=C  iikk"3===H%%''L 
\;m	D	D	D ? ?"++C+>>? ? ? ? ? ? ? ? ? ? ? ? ? ? ?'_dKKKUUWWML-d;;;;;;s   &C

CCc                 6   |                      g dg dg dg dg          }d                    |          }|r|                                }t          j        ||d          }t          |t          d	          5  |                    |d          }|                    |d          }d d d            n# 1 swxY w Y   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           |                      g dg dg dg dg          }|d d df         d
k    }d                    |d
         |d         |d
         g          }|r|                                }t          j        ||d          }t          |t          d	          5  |                    |d          }d d d            n# 1 swxY w Y   t          |t          d	          5  |                    |d          }d d d            n# 1 swxY w Y   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           d S )Nr  r  r  r  r  Tr  r  r   r   rv   r[   r  r   r   r  r  r  r  r   r\   )
r"   r  r  r   rm   r   r  r  r  r   )	r$   r  rc  r  r  r,  estimates_matrixestimates_quatidxs	            r   +test_as_euler_degenerate_compare_algorithmsr    s    ZZ	  F '')

C  iikk

c64
8
8
8C	S+]	;	;	; 9 944S$4GGc4889 9 9 9 9 9 9 9 9 9 9 9 9 9 9 QF#^AAA1vI%>QU    $QQQT*N111a4,@qtTTTT ZZ	  F A,!
C ''9Q<1y|<
=
=C  iikk

c64
8
8
8C	S+]	;	;	; H H44S$4GGH H H H H H H H H H H H H H H	S+]	;	;	; 9 9c4889 9 9 9 9 9 9 9 9 9 9 9 9 9 9QF#^AAA1vI%>QU    #q!>3$'#:    a .a"8t     s6   ;/B66B:=B:2GGG8HH #H c                    d}t           j                            d          }d}t          j        ||          }t          j        |                     |                                                    }|                                }|                                }|                                }|                     t          j	        d||                    }|                     t          j	        d||                    }	| 
                    |ddf          }
t          j        |
          dd dd df                             |                     d                    }
t          ||
|           t          |	|
|           d S )	Nr[   r   rD   )numr   z...ij,...jk->...ikr=   .r\   )rG   rH   rI   r   r!   r"   r#   r   rJ   einsumr  r   r   r   rK   r   )r$   rB   r   r  prS   p_matq_matresult1result2r   s              r   test_invr    s?   D
)


"
"C
AAC000A2::aiikk2233A	AKKMMEKKMMEjj#7FFGGGjj#7FFGGGHHaAYEF5MM#rr2A2+&**266!9955EGU....GU......r   c                    d}t           j                            d          }t          j        |          }t          j        |                     |                                                    }|                                }|                                }|                                }t          j	        ||          }t          j	        ||          }| 
                    d          }	t          ||	|           t          ||	|           t          j        d|          }
t          j        |                     |
                                                    }
|
                                }|
                                }|                                }| j                            ||          }| j                            ||          }|                     d          }t          j        |          d	d dd df                             | 
                    d                    }t          |||           t          |||           d S )
Nr[   	   #;h29s+,g0Hn rZ   r=   r\   r   )r  rR   r   .)rG   rH   r]   r   r!   r"   r#   r   rJ   dotrK   r   r   r   r  r   r   r   )r$   rB   rR   r  rS   r  r  res1res2rK   r   r   x_matrixy_matrixr  r  r   s                    r   test_inv_single_rotationr  4  s   D
)

 G
H
HCC   A2::aiikk2233A	AKKMMEKKMME6%D6%D
&&))CD#D))))D#D))))A3'''A2::aiikk2233A	A{{}}H{{}}Hix22Gix22GHHYEF5MM#rr2A2+&**266!9955EGU....GU......r   c                 z   d}t          j        |          }t          j        |                     |                                                    }|                     |          }t          |                                |           t          |                                                                |           d S )NrD   )	r   identityr!   r"   r#   rn   r   	magnituder   )r$   r  r   r   s       r   test_identity_magnituder  U  s    
A!A2::aiikk2233Axx{{HAKKMM8,,,AEEGG%%''22222r   c                 &   t          j        |                     t          j                                                                        }|                                dk    sJ |                                                                dk    sJ d S )Nr   )r   r!   r"   r  r#   r  r   rf   s     r   test_single_identity_magnituder  ^  sw    2::h&7&9&9&A&A&C&CDDEEA;;==A5577!######r   c                 P   d}d}t          j        |d          }t          j        |                     |                                                    }t          j        |                     t          j        |                                                              }||z  }t          |                                |                                           ||                                z  }t          |                                | 	                    |          |           d S Nr[   rD   r   rZ   r\   
r   rH   r!   r"   r#   r  r   r   r  rn   r$   rB   r  r  rS   r   s         r   test_identity_invariancer  d  s    D
Aq!!!A2::aiikk2233A2::h&7&:&:&B&B&D&DEEFFAUFAIIKK!1!1222aeeggFF$$&&$??????r   c                 N   d}d}t          j        |d          }t          j        |                     |                                                    }t          j        |                     t          j                                                                        }||z  }t          |                                |                                           ||                                z  }t          |                                | 	                    |          |           d S r   r  r  s         r   test_single_identity_invariancer  q  s    D
Aq!!!A2::aiikk2233A2::h&7&9&9&A&A&C&CDDEEAUFAIIKK!1!1222aeeggFF$$&&$??????r   c           	         t          j        |                     d                    }|                                }t	          ||                     | j        | j        | j        dg                     t          j        |                     d                     }|                                }t	          ||                     | j        | j        | j        dg                     d S )Nr,   r   )r   r!   rK   r  r   r"   r  r$   r   r   s      r   test_magnituder    s    266!99%%A[[]]FFBJJrubeQ'?@@AAABFF1II:&&A[[]]FFBJJrubeQ'?@@AAAAAr   c                    t          j        |                     d                    }|d                                         }t	          || j                   |d                                         }t	          |d           d S )Nr,   r   r=   r}   )r   r!   rK   r  r   r  )r$   r   r  r  s       r   test_magnitude_single_rotationr
    sj    266!99%%AdnnGGRU###dnnGGS!!!!!r   c                 t   t           j                            d          }t          j        d|          }t          j        d|          }t          j        |                     |                                                    }t          j        |                     |                                                    }||                                z  }|                                }|                     t          j	        |                    }t          |                    ||          ||k                d S )Nr  rD   rZ   )rG   rH   r]   r   r!   r"   r#   r   r  medianr	   r_   )r$   rR   r  rS   r   r_magrB   s          r   test_approx_equalr    s    
)

 G
H
HC$$$A$$$A2::aiikk2233A2::aiikk2233A	AEEGGAKKMME::bi&&''DANN1d++edl<<<<<r   c                 @   t          j        |                     g d                    }t          j        |                     d                    }|                    |d                   sJ |                    |d                   rJ |                    |d         d          rJ |                    |d         dd	          rJ t          j        t          d
          5  |                    |d         d          sJ 	 d d d            d S # 1 swxY w Y   d S )N)r   r   r  r,   r=   r   绽|=r\   r   T)rB   r  zatol must be setr   r  )	r   r   r"   r!   rK   r_   r   warnsr  )r$   r  rS   s      r   !test_approx_equal_single_rotationr    sW    	RZZ5566A266!99%%A>>!A$~~ad##### ~~ad~/////~~adt~<<<<<	k);	<	<	< 2 2~~adD~1111112 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2s   % DDDc                    |                      |                     d           |                     d          f          }t          j        t          j        d           t          j        d          f          }|                     d| j        dz  d          }|D ]O}t          j        ||z            }t          |	                                
                                dd           Pd S )Nr=   r   rv   rE   r}   r  r\   )concatrK   rG   concatenatelinspacer  r   r   r   r  r  )r$   axesthetastr   s        r   	test_meanr    s    99rvvayyj"&&)),--D>BF1II:rvayy122D[[BEAIs++F ? ? T****,,c>>>>>? ?r   c                    |                      g dg dg dg          }|                     d| j        dz  d          }|D ]}t          j        ||d ddf         z            }|                    ddg          }t          j        ||z            }|                                }t          ||                                z                                  d	d
           d S )N)r}   r   r   r   r   rv   rE   .r   weightsr}   r  r\   )	r"   r  r  r   r   r  r   r   r  )r$   r  r  r  rwmwr   ms           r   test_weighted_meanr!    s    ::{{{IIIyyy9::D[[BEAIs++F E E!!d2A2s7m"344WWaVW$$ T**FFHHRVVXX0022CeDDDDDE Er   c                    t          j        |                     d                    }t          |                                          rU|                    |                     d                     }t          |                     |j	                            sJ d S t          j        t          d          5  |                    |                     d                      d d d            d S # 1 swxY w Y   d S )Nr,   r  znon-negativer   )r   r!   rK   r   r#   r  onesr`   r   _quatr   r   r   )r$   r   r   s      r   test_mean_invalid_weightsr%    s   266!99%%AQYY[[!! (FFBGGAJJ;F''288AG$$%%%%%%%]:^<<< 	( 	(FFBGGAJJ;F'''	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	( 	(s   9+C11C58C5c                     t          j        |                     g d                    }|                    d          }t	          |t                     sJ d S )Nr}   r}   r}   rj   Freturn_indices)r   r!   r"   reduce
isinstancer  s      r   test_reduction_no_indicesr,    sT    2::&:&:&:;;<<AXXUX++Ffh'''''''r   c                     t          j        |                     g d                    }|                    d          }t	          |          t
          u sJ t          |          dk    sJ |\  }}}|J |J d S )Nr'  Tr(  r=   )r   r!   r"   r*  typetuplelen)r$   r   r   reduced	left_best
right_bests         r   test_reduction_none_indicesr4    s    2::&:&:&:;;<<AXXTX**F<<5    v;;!%+"GY
r   c                 r   d}t           j                            d          }t          j        |                     t          j        d|                                                              }t          j        |                     t          j        d|                                                              }t          j        |                     t          j        d|                                                              }|                    ||d          \  }}}t          j        t          |          t          |          t          |          f          }	t          |          D ]g\  }
}t          |          D ]R\  }}t          |          D ]=\  }}t          j        ||z  |z                                  d	                   |	|
||f<   >Sht          j        t          j        |	d
d          |	j        d
         df          }	t          j        t          j        |	t          |          df          d
          }|                     |t          |          z            }|                     |t          |          z            }|                     ||k              sJ |                     ||k              sJ ||         |z  ||         z  }|                                |z                                  }t'          ||                     t          |                    |           d S )Nr[   r  r-   rZ   rD   r   Tr(  r=   r   r   rr   r   r\   )rG   rH   r]   r   r!   r"   r#   r*  rn   r0  	enumeraterz   r   moveaxisrO   argmaxr`   r   r  r   )r$   rB   rR   lr   r  r1  r2  r3  scalarsrT   lijpjkrkmax_indleft_best_checkright_best_checkreduced_checkmags                        r   !test_reduction_scalar_calculationrE    s   D
)

 G
H
HC2::hoaS&A&A&A&I&I&K&KLLMMA2::hobc&B&B&B&J&J&L&LMMNNA2::hoaS&A&A&A&I&I&K&KLLMMA%&XXa4X%H%H"GY
 hAAA/00G1 G G2q\\ 	G 	GEAr"1 G G2#%627R<*@*@*B*B1*E#F#F1a  G	G jWa33gmA6F5KLLGi
7SVVRL99BBBGjjCFF!233Ozz'CFF"23366)./////66* 0011111o&*Q/?-@@M;;===(
3
3
5
5CC#a&&))555555r   c                    |                      g dg dg dg          }t          j        |          }t          j        |                     |d                    }|                      g d          }|                     |d          }|                      g d          }|                     |d          }t	          |                    |          |           t	          |                    |          |           t	          |                    |          |           t	          |                    |          |           |                      g d          }|                     |d          }	t	          |                    |d	
          |           t	          |                    |d	
          |	           t	          |                    |d	
          |	           t	          |                    |d	
          |	           d S )Nr   rr   r   r   r   r   r   )rj   rv   r=   g       r   r=          @rr   r=   Tinverser"   r   r   expand_dimsr   apply)
r$   r   r_1dr_2dv_1dv_2dv1d_rotatedv2d_rotatedv1d_inversev2d_inverses
             r   'test_apply_single_rotation_single_pointrX    s   
**

				  C
 $$Ds ; ;<<D::kkk""D>>$Q>''D**\\\**K..1.55KDJJt$$k222DJJt$$k222DJJt$$k222DJJt$$k222**\\\**K..1.55KDJJtTJ22K@@@DJJtTJ22K@@@DJJtTJ22K@@@DJJtTJ22K@@@@@r   c                 ~   |                      g dg dg dg          }t          j        |          }t          j        |                     |d                    }|                      g dg dg          }|                      g dg d	g          }t	          |                    |          |           t	          |                    |          |           |                      g d
g dg          }t	          |                    |d          |           t	          |                    |d          |           d S )NrG  r   r   r   r   r   r,   r-   r   rH  )r,   r   rI  )r-   r   TrK  rM  )r$   r   r1r2v	v_rotated	v_inverses          r   *test_apply_single_rotation_multiple_pointsrb    s5   
**

				  C
 
	c	"	"B		bnnSqn99	:	:B


IIIyyy)**A

LLL***566IBHHQKK+++BHHQKK+++

LLL***566IBHHQH--y999BHHQH--y9999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        |          }|                     g d	          }|                     |d
          }|                     g dg dg          }t          |                    |          |           t          |                    |          |           |                     g dg dg          }t          |                    |d          |           t          |                    |d          |           d S )Nrv   r=   r=   rG  r   r   r   rq  r   r   r   r   rH  )r   r!  rv   rI  )r   r=   r   TrK  )	rG   r  arrayr"   r   r   rN  r   rO  )r$   r   r   v1v2r`  ra  s          r   *test_apply_multiple_rotations_single_pointrh  *  sr   
(9

CX

				  CF
 X		

		  CF
 **S//CS!!A	III		B		#	#B

LLL***566IAGGBKK+++AGGBKK+++

LLL***566IAGGBG--y999AGGBG--y99999r   c                 2   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        |          }|                     g d	g d
g          }|                     g dg dg          }t          |                    |          |           |                     g dg dg          }t          |                    |d          |           d S )Nrd  rG  r   r   r   rq  r   r   r   rZ  rH  )r,   ir-   rI  )r,   r   r[  TrK  )rG   r  re  r"   r   r   r   rO  )r$   r   r   r_  r`  ra  s         r   -test_apply_multiple_rotations_multiple_pointsrj  G  s,   
(9

CX

				  CF
 X		

		  CF
 **S//CS!!A


IIIyyy)**A

LLL***566IAGGAJJ	***

LLL***566IAGGAtG,,i88888r   c                 h   |                      g d          }|                      |g          }|                      ||g          }|                     d          }|                      |g          }|                      ||g          }t          |||g|||g          D ]\  }}t          j        |          }	|j        }
|	j        s'|j        dk    s|j        dk    rt          |	          df}
|	                    |          }|j        |
k    sJ |	                    |d          }|j        |
k    sJ d S )N)rj   rJ  r+   r=   r#  r   TrK  )	r"   rK   r   r   r   rO   singler0  rO  )r$   vector0vector1vector2matrix0matrix1matrix2r   r_  r   rO   r   s               r   test_apply_shapesrs  ^  s@   jj))Gjj'##Gjj'7+,,GffQiiGjj'##Gjj'7+,,G'73gw5PQQ    1 ##x 	 QW__60A0AVVQKEGGAJJw%GGAtG$$w%   r   c                     t           j                            d          } t          j        |           }|                     ddd          }|                    |                                          }|                    |          }t          ||d           |                     ddd	          }|                    |                                          }|                    |          }t          ||d           d S )
NrY   rZ   irE   r#  r   r[   r\   rv   r=   )rG   rH   r]   r   r  rO  r^   r   )rR   r   r  r_  
v_expecteds        r   test_apply_array_likerx  q  s    
)


$
$CC   AD#D))A	

AJAz....D#F++A	

AJAz......r   c                 N   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                                         |d         d	
           t          |d                                         |d         d	
           t          |d d                                         |                     |d         d          d	
           d S )Nrd  rG  r   r   r   rq  r   r   r>   r\   r   rr   r   r   )	rG   r  re  r"   r   r   r   rJ   rN  r   s      r   test_getitemrz    s$   
(9

CX

				  CF
 X		

		  CF
 **S//CS!!AAaDNN$$c!f59999AaDNN$$c&k>>>>AcrcF$$&&s6{(K(KRWXXXXXXr   c                     t          j        t          d          5  t          j        |                     g d                    d          d d d            d S # 1 swxY w Y   d S )Nnot subscriptabler   r'   r   )r   r   	TypeErrorr   r!   r"   r   s    r   test_getitem_singler~    s    	y(;	<	<	< 8 82::lll3344Q778 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8 8s   0AA Ac                  J   t          j        g dg dg dgg dg dg dgg          } t          j        |           }t	          |dg                                         | dg         d           t	          |dd	g                                         | dd	g         d           d S )
Nr   r   r   rq  r   r   r>   r\   r   )rG   re  r   r   r   rJ   )r   r   s     r   test_getitem_array_liker    s    
(\\\YYYY  YYZZYY ! " "C 	S!!AAqcF$$&&QCu====Aq!fI''))31v;UCCCCCCr   c                    t          j        |                     g d                    }t          j        t
          d          5  t          j        |                     g d                    |d<   d d d            d S # 1 swxY w Y   d S )Nr'   r|  r   r   )r   r!   r"   r   r   r}  rf   s     r   test_setitem_singler    s    2::lll3344A	y(;	<	<	< < <!"**\\\":":;;!< < < < < < < < < < < < < < < < < <s   -A??BBc                    t           j                            d          }t          j        |                     t          j        d|                                                              }t          j        |                     t          j        d|                                                              }||dd<   t          |dd                                         |                                           d S )Nr  rD   rZ   r-   r   r   rG   rH   r]   r   r!   r"   r#   r	   r$   rR   r]  r^  s       r   test_setitem_slicer    s    
)

 G
H
HC		BJJxrs'C'C'C'K'K'M'MNN	O	OB		BJJxqc'B'B'B'J'J'L'LMM	N	NBBqsGBqsGOO%%rzz||44444r   c                    t           j                            d          }t          j        |                     t          j        d|                                                              }t          j        |                     t          j        |                                                              }||d<   t          |d                                         |                                           d S )Nr  rD   rZ   r   r  r  s       r   test_setitem_integerr    s    
)

 G
H
HC		BJJxrs'C'C'C'K'K'M'MNN	O	OB		BJJx3'?'?'?'G'G'I'IJJ	K	KBBqEBqEMMOORZZ\\22222r   c                    t          j        |                     t          j        dd                                                              }t          j        t          d          5  d|d<   d d d            d S # 1 swxY w Y   d S )NrD   r   rZ   zRotation objectr   r   )r   r!   r"   rH   r#   r   r   r}  rf   s     r   test_setitem_wrong_typer    s    2::hoba&@&@&@&H&H&J&JKKLLA	y(9	:	:	:  !                 s   )A<<B B 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          t          |          d	           t          t          |d d
                   d           d S )Nrd  rG  r   r   r   rq  r   r   rv   rr   )rG   r  re  r"   r   r   r   r0  r   s      r   test_n_rotationsr    s    
(9

CX

				  CF
 X		

		  CF
 **S//CS!!AQQssVa     r   c                     t           j                            d          } t          t	          j        |                                           j        d           t          t	          j        d |                                           j        d           t          t	          j        d|                                           j        d           t          t	          j        d|                                           j        d           d S )Nr  rZ   r,   r   r   r-   r-   r,   )rG   rH   r]   r   r   r#   rO   rZ   s    r   test_random_rotation_shaper    s    
)

 G
H
HCS)))113394@@@3///7799?FFF,,,4466<fEEE,,,4466<fEEEEEr   c                 `   |                      g dg dg          }|                      |d          }t          j        ||          \  }}t          |                                |                     d          d           t          ||                      d          d	         d
d           d S )Nr   rZ  T)copyr=   r[   r\   r}   r   Fr   check_shaperB   )r"   r   align_vectorsr   rJ   rK   )r$   r   r   r   rssds        r   test_align_vectors_no_rotationr    s    


IIIyyy)**A


14
  A$Q**GAtAKKMM266!9959999D"**S//"-5tLLLLLLr   c                 H   t           j                            d          }t          j        |                     t          j        |                                                              }|                     |                    d                    }|                    |          }t          j	        ||          \  }}t          |                                |                                           t          ||                     d          d         dd	           d S )
Nl	   #t}":xL rZ   r-   r=   ru  r}   r   Fr   r  )rG   rH   r]   r   r!   r"   r#   normalrO  r  r   )r$   rR   crm  rl  estr  s          r   test_align_vectors_no_noiser    s    
)

 F
G
GC2::ho#&>&>&>&F&F&H&HIIJJA


3::6:**++A	

A&q!,,ICAIIKK///D"**S//"-5tLLLLLLr   c                 D   |                      g dg dg          }|                      g dg dg          }t          j        ||          \  }}t          ||                    |          d           t          ||                      d          d         d	d
           d S )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   Fr   r  )r"   r   r  r   rO  )r$   r   r   r  r  s        r   $test_align_vectors_improper_rotationr    s    


888999; 	< 	<A


888:::< 	= 	=A &q!,,ICAsyy||$////D"**S//"-5tLLLLLLr   c                 r   |                      d          d         }|                      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          |||           t          |||           d S )NgbQ?r   )r   r}   r}   )r}         ?rj   )r}   rj   rj   r   r   r   r   r   r   )r   g?r   )r   g?r   Treturn_sensitivityr\   )r"   r   r  r   )	r$   rssd_expectedsens_expectedrB   rl  rm  r,  r  senss	            r   #test_align_vectors_rssd_sensitivityr    s    JJ011"5MJJ . . 0 1 1M D


IIIyyy)))455A


III{{{KKK899A,QdKKKOCtD-d3333D-d333333r   c                    d}|                      t          j        |d                              g d                    }|                      t          j        |d                              g d                    }d}t          j        |||                     |          d          \  }}}t          j        ||||                     |          z  d          \  }}	}
t          |                                |                                           t          t          j	        |          |z  |	d	           t          ||
           d S )
NrD   r   rZ   r   r   rv   Tr   r\   )
r"   r   rH   rO  r  r#  r   rJ   r   r   )r$   r  rl  rm  scaleest1rssd1cov1est2rssd2cov2s              r   !test_align_vectors_scaled_weightsr    s!   
A


8?1!,,,22999==>>A


8?1!,,,22999==>>AE .q!RWWQZZFFD% .q!URWWQZZ5GNND%DNN$$dnn&6&6777DIe$$u,e$????D$r   c                 J   t           j                            d          }d}t          t	          j        |          |           }|                     |                    |df                    }|                    |          }t          j        d          }d|z  }t	          j	        |                     |                    |df|                              }|                    |          }	t	          j
        |	|d	
          \  }
}}||
                                z                                  }t          |d         |                     d          d         |           t          |d         |                     d          d         |           t          |d         |                     d          d         |           ||z  }t          |d         |                     d          d         |           t          |d         |                     d          d         |           t          |d         |                     d          d         |           |                     |	|
                    |          z
  dz            dz  }t          ||d           d S )Nr  rE   rZ   r=   ru  r   r  )r   r  Tr  r   r}   r   r\   rv   r   )r   r   )rv   rv   r   F)r  )rG   rH   r]   r%   r   r"   r  rO  deg2radr   r  r   r   r   sum)r$   rR   	n_vectorsr,  vectorsr   sigma	tolerancenoisenoisy_resultr  r  coverror_vector
rssd_checks                  r   test_align_vectors_noiser    sb   
)

 G
H
HCI
S1112
6
6Cjj)Q8899GYYwF JqMMEeI 


3::Q  
 
 	 	 E ;;v&&L+L'?CE E ENCs #''))O..00LLORZZ__R%8yIIIILORZZ__R%8yIIIILORZZ__R%8yIIII 5LCCIrzz#r2CCCCCIrzz#r2CCCCCIrzz#r2CCCC		'(:(::q@AASHJD*%888888r   c                 \   t          j        t          d          5  |                     g d          |                     g d          }}t	          j        ||           d d d            n# 1 swxY w Y   t          j        t          d          5  |                     g d          |                     g d          }}t	          j        ||           d d d            n# 1 swxY w Y   t          j        t          d          5  |                     g dg dg          |                     g dg          }}t	          j        ||           d d d            n# 1 swxY w Y   t          j        t          d          5  |                     g dg          |                     g dg          }}|                     d	gg          }t	          j        |||           d d d            n# 1 swxY w Y   t          j        t          d
          5  |                     g dg dg          |                     g dg dg          }}|                     g d          }t	          j        |||           d d d            n# 1 swxY w Y   |                     g dg          |                     g dg          }}|                     dg          }t          |          r}t	          j        |||          \  }}|                     |                     |	                                                    s
J d            |                     |          s
J d            nIt          j        t          d          5  t	          j        |||           d d d            n# 1 swxY w Y   |                     g dg dg          |                     g dg dg          }}|                     | j
        | j
        g          }t          |          r}t	          j        |||          \  }}|                     |                     |	                                                    s
J d            |                     |          s
J d            nIt          j        t          d          5  t	          j        |||           d d d            n# 1 swxY w Y   |                     g dg          |                     g dg          }}t          |          r|t	          j        ||          \  }}|                     |                     |	                                                    s
J d            |                     |          s
J d            nHt          j        t          d          5  t	          j        ||           d d d            n# 1 swxY w Y   |                     g dg dg          |                     g dg dg          }}|                     | j
        d	g          }t          |          rOt	          j        |||d          \  }}}|                     |                     |                    s
J d            nKt          j        t          d          5  t	          j        |||d           d d d            n# 1 swxY w Y   |                     g dg          |                     g dg          }}t          |          rOt	          j        ||d          \  }}}|                     |                     |                    s
J d            d S t          j        t          d          5  t	          j        ||d           d d d            d S # 1 swxY w Y   d S )Nz Expected input `a` to have shaper   r   r   z Expected input `b` to have shapez/Expected inputs `a` and `b` to have same shapesrZ  z&Expected `weights` to be 1 dimensionalr   z+Expected `weights` to have number of valuesrr   zQuaternion should be nanzRSSD should be nanz)`weights` may not contain negative valuesz#Only one infinite weight is allowedr   z(Cannot align zero length primary vectorsTr  z Sensitivity matrix should be nanz Cannot return sensitivity matrix)r   r   r   r"   r   r  r   r`   r   r#   inf)r$   rl  rm  r  r   r  r  s          r    test_align_vectors_invalid_inputr  A  s$
   	z)K	L	L	L % %zz,,,''III)>)>1q!$$$% % % % % % % % % % % % % % % 
z)K	L	L	L % %zz)))$$bjj&>&>1q!$$$% % % % % % % % % % % % % % % 
z *? 
@ 
@ 
@ % %zz999iii0112::yyyk3J3J1q!$$$% % % % % % % % % % % % % % %
 
zE
G 
G 
G . .zz999+&&

III;(?(?1**qcU##q!W---	. . . . . . . . . . . . . . . 
zJ
L 
L 
L . .zz999iii0112::yyy)))>T3U3U1**YYY''q!W---	. . . . . . . . . . . . . . . ::yyyk""BJJ			{$;$;qAjj"GW 2(Aw774vvbhhqyy{{++,,HH.HHHHxx~~3333333]:!LN N N 	2 	2"1a111	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 ::yyy))),--rzz999iii:P/Q/QqAjj"&"&)**GW 2(Aw774vvbhhqyy{{++,,HH.HHHHxx~~3333333]:!FH H H 	2 	2"1a111	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 	2 ::yyyk""BJJ			{$;$;qAQ )(A..4vvbhhqyy{{++,,HH.HHHHxx~~3333333]:!KM M M 	) 	)"1a(((	) 	) 	) 	) 	) 	) 	) 	) 	) 	) 	) 	) 	) 	) 	) ::yyy))),--rzz999iii:P/Q/QqAjj"&!%%GQ K .q!WQUVVV4vvbhhtnn%%II'IIIII]:!CE E E 	K 	K"1aTJJJJ	K 	K 	K 	K 	K 	K 	K 	K 	K 	K 	K 	K 	K 	K 	K ::yyyk""BJJ			{$;$;qAQ N .q!MMM4vvbhhtnn%%II'IIIIII]:@B B B 	N 	N .q!MMMM	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	N 	Ns   AA,,A03A0AC""C&)C&A	EE!$E!AG--G14G1A$JJ
J;NN"%N"'S

SS+WWW#[[[<^!!^%(^%c                 N   d}|                      g dg dg          }|                      g dg dg          }|                      g dg dg dg          }t          j        |||                      | j        dg          	          \  }}t	          |                                ||
           t	          |                    |          ||
           t          j        |d||           sJ |                      g dg dg          }d}t          j        |||                      | j        dg          	          \  }}t	          |                                ||
           t	          |                    |          d         |d         |
           t          j        ||||           sJ |                      g dg dg          }t	          |                    |          ||
           |                      g dg dg          }|                      g dg dg          }d}t          j        |||                      | j        dg          	          \  }}t	          |                    |          d         |d         |
           t          j        ||||           sJ d S )Nr[   r   r   r}   r   r   r  r   r   r   r  r\   r}   )rB   r$   )r   rv   r   rj   r   r   r   rv   r   )r   r=   rr   )ry   r=   rv   r/  g4i?)	r"   r   r  r  r   rJ   rO  r   isclose)	r$   rB   rm  rl  
m_expectedRr  r  
a_expecteds	            r   "test_align_vectors_align_constrainr    s   
 D


IIIyyy)**A


KKK+,,A[[[&YY&YY( ) )J $Q2::rvqk3J3JKKKGAtAKKMM:D9999AGGAJJ----;tSt333333 	

IIIyyy)**AM$Q2::rvqk3J3JKKKGAtAKKMM:D9999AGGAJJv&&	====;t]"======[[[)))455JAGGAJJ
6666 	

III{{{+,,A


LLL***-..A&M$Q2::rvqk3J3JKKKGAtAGGAJJv&&	====;t]"========r   c                 (   d}g }t          d          D ]@}|                    t          j        |d|z                                                        At          |          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        ||| j        dg          \  }}t          |                                |                                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        ||| j        ddg          \  }}t          |                                |                                d           'd S )NrE   r   rD   rZ   r   r   rv   r=   r,       _Br  -C6?r\   r-   )r  rv   r   )	rN   appendr   rH   rJ   r"   r  r  r   )	r$   r  matsrT   rl  rm  r  _R2s	            r   test_align_vectors_near_infr    se    	AD1XX @ @HOA26222<<>>????1XX B BJJDGAJqM)1tAwqz!}+<=>>JJDGAJqM)1tAwqz!}+<=>>%aT1I>>>1&q!bfa[AAAAr||~~DAAAAA1XX B BJJ$q'!*Q-471:a=!DGAJqM/JKKJJ$q'!*Q-471:a=!DGAJqM/JKK%aLLLAAA1&q!bfa^DDDAr||~~DAAAAAB Br   c                    d}|                      g dg dg          }|                      g dg dg          }|                      g dg dg dg          }t          j        ||| j        dg          \  }}t	          |                                ||	           t          j        |d
         |d
                   \  }}t	          |                                ||	           t	          |                    |d
                   |d
         |	           |                      g dg dg          }|                      g dg dg dg          }t          j        ||| j        dg          \  }}t	          |                                ||	           t          j        |d
         |d
                   \  }}t	          |                                ||	           t	          |                    |d
                   |d
         |	           d S )Nr[   rj   r   r   r   r  r   r   r   r  r\   r   r   )r"   r   r  r  r   rJ   rO  )r$   rB   rl  rm  r  r  r  s          r   test_align_vectors_parallelr    s   D


KKK+,,A


KKK+,,A[[['ZZ&YY( ) )J !!Q<<<DAqAKKMM:D9999!!F)QvY77DAqAKKMM:D9999AGGAfI&&&	====


IIIyyy)**A[[[&YY&YY( ) )J !!Q<<<DAqAKKMM:D9999!!F)QvY77DAqAKKMM:D9999AGGAfI&&&	======r   c           	      j   d}t          j        g dg dgg dg dgg dg dgg          }d |D             }t          ||          D ]\  }}|                     |          |                     |          }}t	          j        ||| j        dg          \  }}t          |                                | j	        |	           t          |
                    |d
                   |d
         |	           t	          j        dd          }t	          j        |                                dz            }	g dg dg}g dg dg}g }|	D ]J}
|                    t          j        |

                    |d                   |d         g                     K|D ]}|                     |          |                     |          }}t	          j        ||| j        dg          \  }}t	          j        ||ddg          \  }}t          |                                |                                |	           d S )Nr[   r  r   r   r   c                 0    g | ]}|d           |d         gS r   r   r   ).0rl  s     r   
<listcomp>z3test_align_vectors_antiparallel.<locals>.<listcomp>  s&    444AAaD5!A$-444r   r   r  r\   r   rE   r   rZ   r  r   r  )rG   re  zipr"   r   r  r  r   r  r  rO  rH   r   r   r  rJ   )r$   rB   
as_to_test
bs_to_testrl  rm  r  r  RsdRsdRr  s               r   test_align_vectors_antiparallelr    sN   DKKK3%IIyyy1%IIyyy13 4 4J 54444JJ
++ B B1zz!}}bjjmm1%aRVQK@@@1ru48888&	**AfIDAAAAA 
!	$	$	$B

r||~~d2
3
3C	YYYA	YYYAJ < <"(BHHQqTNNAaD#9::;;;; B Bzz!}}bjjmm1%aRVQK@@@1&q!dAY???Ar||~~DAAAAA	B Br   c                    d}t          j        dd                                          }t          j        dd                                          }t          ||          D ]\  }}|                     |d                   }|                     |d                   }t          j        ||          \  }}	t          |                    |          ||           t          j	        |	d|          sJ d S )Nr[   rE   r   rZ   r   r\   )
r   rH   rJ   r  r"   r  r   rO  rG   r  )
r$   rB   mats_amats_bmat_amat_brl  rm  r  r  s
             r   test_align_vectors_primary_onlyr    s    D_Sa(((2244F_Sa(((2244FFF++ . .uJJuQx  JJuQx   (A..4

AD1111z$-------. .r   c                     t           j                            d          } t          j        |           }|                     d          }|                    |          }t          j        ||          \  }}t          j        |                                |                                          \  }}t          |	                                |	                                           t          ||           d S )NrY   rZ   r  ru  )
rG   rH   r]   r   r  rO  r  r^   r   r#   )rR   r  rm  rl  est_expectedr  r  r  s           r   test_align_vectors_array_liker    s    
)


$
$CC   A


A	

A"*"8A">">L-&qxxzz188::>>ICL((**CKKMM:::D-(((((r   c                     |                      g d          }t          t          j        |                    }t	          |           rd}||k    sJ d S |                    d          sJ d S )Nr'   zRotation.from_matrix(array([[1., 0., 0.],
                            [0., 1., 0.],
                            [0., 0., 1.]]))Rotation.from_matrix()r"   reprr   r!   r
   
startswithr$   rS   actualr   s       r   test_repr_single_rotationr    s~    


<<<  A($Q''((F|| :/ !!!!!!  !89999999r   c                     |                      g dg dg          t          j        d          z  }t          j        |          }t          |           rd}||k    sJ d S |                    d          sJ d S )N)r}   r   r   r   r   rv   a-  Rotation.from_matrix(array([[[ 0.,  0.,  1.],
                             [ 0.,  1.,  0.],
                             [-1.,  0.,  0.]],
                     
                            [[ 0., -1.,  0.],
                             [ 1.,  0.,  0.],
                             [ 0.,  0.,  1.]]]))r  )r"   r   r   r   r!   r
   r  r  s       r   test_repr_rotation_sequencer  ,  s    


NNNLLL122TYq\\AA"1%%)F|| :4 !!!!!!  !89999999r   c                    t           j                            d          }t          j        |                     |                    d                              }|                                }g d}t          ||          }t          |j
        t          |                     d                              sJ g d} ||          }|                                }|d d df         dk     d d d f         }	|                     |	| |          }|d d df         dk     d d d f         }	|                     |	| |          }t          |d         |d                    t          |d         |d	                    t          |d
         |d                    t          |d         |d                    t          |d         |d                    |                     |d         |d         z            }
|                     |d         |d	         z            }t          |
|           |                     |d         |d         z            }|                     |d         |d
         z            }t          ||           |                     |d	         |d         z            }t          |d|
dz  z  dz
             t          t!          |          t!          |                     d S )Nr   r  ru  )r   r   rv   r=   r,   )r   r   r  r   r  rv   g      @r=   g      
@g@r,   rr   r   r   r   )r-   .r   )r   .)rD   .)r,   .rv   r   )rG   rH   rI   r   r!   r"   r  r#   r   r+  timesr.  wherer   r  r   r0  )r$   r   key_rots	key_quats	key_timesinterpolatorr  interp_rotsinterp_quatsmask
cos_theta1
cos_theta2
cos_theta4
cos_theta5
cos_theta3s                  r   
test_slerpr  =  s   
)


"
"C!"**S[[f[-E-E"F"FGGH  ""IIH--Ll($rzz!}}*=*=>>>>>===E,u%%K&&((L B!#QQQW-D88D<->>Laaaeq !!!T'*D	z955I L()F*;<<<L()F*;<<<L()F*;<<<L()F*;<<<L)9V+<=== V,|F/CCDDJV,|F/CCDDJJ
+++V,|F/CCDDJV,|F/CCDDJJ
+++ V,|F/CCDDJJZ] 3a 7888 [!!3u::.....r   c                     t          j        t          d          5  |                     g dg dg          }|                     ddg          }t	          ||           d d d            d S # 1 swxY w Y   d S )Nzmust be a `Rotation` instancer   r   r'   r   r   )r   r   r}  r"   r   )r$   r   r  s      r   test_slerp_rot_is_rotationr  l  s    	y(G	H	H	H  JJ			!		# $ $JJ1va	                 s   AA,,A03A0z*must be a sequence of at least 2 rotationsc                     t          j        |                     g dg                    }t          j        t
          t                    5  t          dg|           d d d            d S # 1 swxY w Y   d S )N)rj   rv   r=   r,   r   r   )r   r!   r"   r   r   r   SLERP_EXCEPTION_MESSAGEr   rf   s     r   test_slerp_single_rotr  w  s    2::~~~&67788A	z)@	A	A	A  qc1                 s   A**A.1A.c                 0   t          j                    }t          j        |                     |                                                    }t          j        t          t                    5  t          g |           d d d            d S # 1 swxY w Y   d S )Nr   
r   rH   r!   r"   r#   r   r   r   r  r   rf   s     r   test_slerp_rot_len0r
  }  s    A2::aiikk2233A	z)@	A	A	A  b!                 s   -BBBc                 4   t          j        d          }t          j        |                     |                                                    }t          j        t          t                    5  t          dg|           d d d            d S # 1 swxY w Y   d S )Nr   r   r	  rf   s     r   test_slerp_rot_len1r    s    A2::aiikk2233A	z)@	A	A	A  qc1                 s   .BBBc                 t   t          j        t          d          5  t          j                            d          }t          j        |                     |	                    d                              }|                     dgdgg          }t          ||           d d d            d S # 1 swxY w Y   d S )Nz.times to be specified in a 1 dimensional arrayr   r   )rv   r,   ru  r   rv   )r   r   r   rG   rH   rI   r   r!   r"   r  r   r$   r   r   r  s       r   test_slerp_time_dim_mismatchr    s    	zM
O 
O 
O  i##A&&rzz#++6+*B*BCCDDJJ  a                 s   BB--B14B1c                 l   t          j        t          d          5  t          j                            d          }t          j        |                     |	                    d                              }| 
                    d          }t          ||           d d d            d S # 1 swxY w Y   d S )Nz7number of rotations to be equal to number of timestampsr   r   r  ru  r   )r   r   r   rG   rH   rI   r   r!   r"   r  aranger   r  s       r   !test_slerp_num_rotations_mismatchr    s    	z *@ 
A 
A 
A  i##A&&rzz#++6+*B*BCCDDIIaLLa                 s   B B))B-0B-c                    t           j                            d          }|                     |                    d                    }t          j        |          }g d}t          |          rAt          ||          }| 	                    | 
                    |j                            sJ d S t          j        t          d          5  t          ||           d d d            d S # 1 swxY w Y   d S )Nr   r  ru  )r   r   rv   rv   r,   strictly increasing orderr   rG   rH   rI   r"   r  r   r!   r   r   r`   r   r  r   r   r   r$   r   rS   r   r  ss         r   test_slerp_equal_timesr       
)


"
"C


3;;F;++,,A1AAQ !QKKvvbhhqw''(((((((]:-HIII 	 	!QKKK	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	   C**C.1C.c                    t           j                            d          }|                     |                    d                    }t          j        |          }g d}t          |          rAt          ||          }| 	                    | 
                    |j                            sJ d S t          j        t          d          5  t          ||           d d d            d S # 1 swxY w Y   d S )Nr   r  ru  )r   r   r=   rv   r,   r  r   r  r  s         r   test_slerp_decreasing_timesr    r  r  c                    t           j                            d          }t          j        |                     |                    d                              }|                     d          }t          ||          }t          j
        t          d          5  |                     dgdgg          } ||           d d d            d S # 1 swxY w Y   d S )	Nr   r  ru  r-   z&`times` must be at most 1-dimensional.r   g      @g@)rG   rH   rI   r   r!   r"   r  r  r   r   r   r   )r$   r   r   r  r  interp_timess         r   !test_slerp_call_time_dim_mismatchr    s   
)


"
"C2::ckkvk&>&>??@@A
		!AaA	zE
G 
G 
G  zzC5$'5#* + +	,	                 s   %CCCc                    t           j                            d          }t          j        |                     |                    d                              }|                     d          dz   }t          ||          }|                     g d          }|                     g d          }t          |          r ||          
                                }|                     ||                     |          k    ||                     |          k              }|                     |                     || df                             sJ |                     |                     ||df                              sJ  ||          
                                }|                     ||                     |          k    ||                     |          k              }|                     |                     || df                             sJ |                     |                     ||df                              sJ d S t!          j        t$          d	
          5   ||           d d d            n# 1 swxY w Y   t!          j        t$          d	
          5   ||           d d d            d S # 1 swxY w Y   d S )Nr   r  ru  r-   r   r  )r   rv   r   .ztimes must be within the ranger   )rG   rH   rI   r   r!   r"   r  r  r   r   r#   logical_andminmaxr`   r   r   r   r   )	r$   r   r   r  r  	times_low
times_highrS   in_ranges	            r   !test_slerp_call_time_out_of_ranger'    s   
)


"
"C2::ckkvk&>&>??@@A
		!qAaA

999%%IIII&&JY AiLL  "">>)rvvayy"8)rvvayy:PQQvvbhhq(C01122222vvrxx(C- 011122222AjMM!!##>>*q		"9:PQ;RSSvvbhhq(C01122222vvrxx(C- 01112222222]:-MNNN 	 	AiLLL	 	 	 	 	 	 	 	 	 	 	 	 	 	 	]:-MNNN 	 	AjMMM	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	s$   ,JJJ*KK
Kc                 r   t          j        d|                     ddg          d          }t          ddg|          } |d          }t          j        d|                     d          d          }||                                z  }|                     |                                dd	
          sJ d S )NXr   P   Tr  r   r  r  r?   r\   )r   rm   r"   r   r   allcloser  )r$   r   r  r_interpolatedr_interpolated_expecteddeltas         r   test_slerp_call_scalar_timer/    s    CQG!4!4dCCCAq!faAQtWWN&1#rzz"~~tTTT488:::E;;u((!%;8888888r   c                    t          j        dd          }t          j        |                     |                                                    }t          j        dd          }t          j        |                     |                                                    }|                     t          |                    }|D ];}|||z  z  }t          t          |                                d          |           <d S )N2   r   rZ   r  r   r   )	r   rH   r!   r"   r#   r#  r0  r   r   )r$   qsrsr   rS   s        r   test_multiplication_stabilityr4    s    		#	#	#B		BJJrzz||44	5	5B	1	%	%	%B		BJJrzz||44	5	5Bwws2wwH H H
a"frzz||!<<<hGGGGH Hr   c                    d}t          j        dd          }t          j        |                     |                                                    }|                                }dD ]`}||z  }t          j        d          }t          j        |                     |                                                    }t          t          |                    D ]}|dk    r||z  }||z  }||                                z  	                                }| 
                    ||k               sJ t          j        |                     g d                    }||z                                  j        dk    sJ t          j        |                     g dg                    }||z                                  j        dk    sJ bd	D ]f}||z  }t          j        ||                                z            }t          |                                |                                |
           gt          j        |                     g d                    }d}||z  }t          j        ||                                z            }t          |                                |                                |
           d S )Nr  rD   r   rZ   )r[  r   rr   r   r   rv   r-   r'   r  r   )      r   g       r}   r   r  r\   )r[   r   r   r=   )r   rH   r!   r"   r#   r   r  rN   rz   r  r`   rO   r   r   r   )	r$   rB   r  p_invr  rS   r   r  angs	            r   test_powr9    sm   D"""A2::aiikk2233AEEGGE% 0 0Fb!!rzz!))++6677s1vv 	 	A1uuEI15577{%%''vvcDj!!!!! rzz,,,77881~~%----rzz<<<.99::1~~%///// / = =F Q[[]]!233		QYY[[t<<<<< 	RZZ6677A	A	QAQ.//AAIIKK4888888r   c                 ,   t          j        d          }t          j        |                     |                                                    }t          j        t          d          5  t          |dd           d d d            d S # 1 swxY w Y   d S )Nr   rZ   zmodulus not supportedr   r   )	r   rH   r!   r"   r#   r   r   NotImplementedErrorpow)r$   r  s     r   test_pow_errorsr=    s    AA2::aiikk2233A	*2I	J	J	J  Aq!                 s   *B		BBc                     t          j        d          } t          j        dd          }t          j        |           }t	          |j        d           t          j        |          }t	          |j        d           t          |d                                         |d                                                    t          |d                                         |d                                                    t          j        | g          }t	          |j        d           t	          |d         |            t          j        |g          }t	          |j        d           t          |d	                                         |d                                                    t          |d
                                         |d                                                    t          j        | |gt                    }t	          |j        d           t	          |d         |            t	          |d         |           t          j        |||g          }t	          |j        d           d S )Nr   rZ   rv   r   r   )rv   )r   )r   rv   r   r  re   )r=   rv   )	r   rH   rG   re  r   rO   r   rJ   object)rl  multiplere  s      r    test_rotation_within_numpy_arrayrB  "  s   _###Fqa(((HHVEb!!!HXEd###E!H&&(((1+*?*?*A*ABBBE!H&&(((1+*?*?*A*ABBBHfXEd###q6"""HhZ  Ef%%%E$K))++Xa[-B-B-D-DEEEE$K))++Xa[-B-B-D-DEEEHfh'v666Ed###q6"""q8$$$Hh(344Ef%%%%%r   c                    t          j        |                     ddt          j        t
          j        dz            t          j        t
          j        dz            g                    }t          j	        |          }t          j
        |          }t          |                                |                                d           d S Nr   r,   r>   r\   )r   r!   r"   r   sinrG   r  r  pickledumpsloadsr   rJ   )r$   r   pkl	unpickleds       r   test_picklingrK  A  s     	2::q!TXbeAg->->q@Q@Q&RSSTTA
,q//CS!!IAKKMM9#6#6#8#8uEEEEEEr   c                 l   t          j        |                     ddt          j        t
          j        dz            t          j        t
          j        dz            g                    }t          j	        |          }t          |                                |                                d           d S rD  )r   r!   r"   r   rE  rG   r  r  r  deepcopyr   rJ   )r$   r   r]  s      r   test_deepcopyrN  J  s     	2::q!TXbeAg->->q@Q@Q&RSSTTA	q		BAKKMM2<<>>>>>>>>r   c                  J   t          j        g d          } |                     d          }|                     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 d S )Nr'   ri   XYZC_CONTIGUOUSTc              3   "   K   | ]
}|d k    V  dS r   Nr   r  rT   s     r   	<genexpr>z+test_as_euler_contiguous.<locals>.<genexpr>Y  &      **!qAv******r   c              3   "   K   | ]
}|d k    V  dS rS  r   rT  s     r   rU  z+test_as_euler_contiguous.<locals>.<genexpr>Z  rV  r   )r   r!   r  flagsr`   strides)r   e1e2s      r   test_as_euler_contiguousr\  R  s    <<<((A	
E		B	
E		B8N#t++++8N#t++++**rz**********rz**********r   c                    t          j        dd          t          j        |                                                                         g d}dgt          t          j        |                    z   }fdt          ||          D             }t          j	        |          }t                                          |                                           t          j	                  }t                                          |                                           |usJ t          j        |                     t          j                                                                        }t          j	        |          }t          |                                |                                           d S )NrD   r   rZ   )r   rv   r=   r   r=   c                 0    g | ]\  }}|||z            S r   r   )r  rT   r  r  s      r   r  z$test_concatenate.<locals>.<listcomp>b  s*    >>>41aXaAg>>>r   )r   rH   r!   r"   r#   listrG   cumsumr  r  r	   r  )r$   sizesstartssplitr   r,  r  s         @r   test_concatenaterd  ]  su   rq)))H!"**X-=-=-?-?"@"@AAHOOES4	%(()))F>>>>3vu+=+=>>>E!%((FH$$&&(8(8999 !(++FH$$&&(8(8999 6!!!! 
RZZ(9(;(;(C(C(E(EFF
G
GC!#&&FCKKMM6>>#3#344444r   c                 &   t          j        t          d          5  t          |                     t          j                                                                        }t          j        |dd g           d d d            d S # 1 swxY w Y   d S )NzRotation objects onlyr   r   )r   r   r}  r   r"   r  r#   r  )r$   r,  s     r   test_concatenate_wrong_typerf  s  s    	y(?	@	@	@ - -rzz("3"5"5"="="?"?@@AAc1d^,,,- - - - - - - - - - - - - - - - - -s   ABB
B
c                    t          |                     g dg                    }t          |                     g dg dg                    }t          |                     g d                    }t          |          dk    sJ t          |          dk    sJ t          j        t
          d          5  t          |           d d d            n# 1 swxY w Y   |sJ |sJ |sJ d S )Nr'   r   rv   zSingle rotation has no len().r   )r   r"   r0  r   r   r}  )r$   rotation_multi_onerotation_multirotation_singles       r   test_len_and_boolrk  z  s=   !"**lll^"<"<==bjj,,,)EFFGGNrzz,,,7788O!""a''''~!####	y(G	H	H	H  O               s   7CCCc                     |                      g d          }t          j        |ddd                                          }|                      g d          t	          j        d          z  }t          ||           d S )Nr   rb  r\  Tr  r]  rv   )r"   r   from_davenportr#   r   r   r   )r$   r   r   r2   s       r   #test_from_davenport_single_rotationrn    s{    ::iii  D"4b+/1 1 118 	JJ~~~..1=MD-(((((r   c                    |                      g d          }|                      g d          }t          j        || j        z  dz            }t          j        |d| j        dz            }t          |                    d          |                    d                     |                     |d          }t          j        || j        z  dz            }t          j        |d| j        dz  g          }t          |                    d          |                    d                     |                     ||gd	          }t          j        ||                      | j        dz  g| j        d
z  gg          z            }|d         |d         z  }|                     ||gd	          }t          j        |d| j        d
z  | j        dz  g          }t          |                    d          |                    d                     |                     ||gd	          }t          j        ||                      | j        d
z  g| j        dz  gg          z            }|                     |d          }t          j        |d| j        d
z  | j        dz  g          }t          |                    d          |                    d                     d S )Nr   r  r,   eTrl   r   r   r   r   r   )	r"   r   r   r  rm  r   r#   r   stack)r$   ezeyr,  rot_davr  axes_davs          r   #test_from_davenport_one_or_two_axesrv    s   	KKK	 	 B	KKK	 	 B 
rBEz!|
,
,C%b#ruQw77GCKK$K//OOdO335 5 5 ::b&!!D

tbe|A~
.
.C%dC"%';;GCKK$K//OOdO335 5 5 88RH18%%D

tbjj257)beAgY1G&H&HH
I
IC
a&3q6/CxxRqx))H%hbeAgruQw5GHHGCKK$K//OOdO335 5 5 88RH18%%D

tbjj257)beAgY1G&H&HH
I
ICzz"f%%H%hbeAgruQw5GHHGCKK$K//OOdO335 5 5 5 5r   c                    g d}g d}g d}|                      ||g          }t          |          rUt          j        |dddg                                          }|                     |                     |                    sJ nKt          j        t          d          5  t          j        |dddg           d d d            n# 1 swxY w Y   |                      |||g          }t          |          rUt          j        |dg d                                          }|                     |                     |                    sJ nKt          j        t          d          5  t          j        |dg d           d d d            n# 1 swxY w Y   t          j        t          d	          5  t          j        |                      |g          d
dg           d d d            n# 1 swxY w Y   t          j        t          d          5  t          j        |                      |||g          dg d           d d d            d S # 1 swxY w Y   d S )Nr   r   r  rp  r   zmust be orthogonalr   r   zorder should beri   zExpected `angles`)r   r   rv   r=   )
r"   r   r   rm  r#   r`   r   r   r   r   )r$   rr  rs  ezyr  rS   s         r   !test_from_davenport_invalid_inputry    sL   	B	B
))C::r3i  DT 7#D#1v66>>@@vvbhhqkk""""""]:-ABBB 	7 	7#D#1v666	7 	7 	7 	7 	7 	7 	7 	7 	7 	7 	7 	7 	7 	7 	7::r2sm$$DT :#D#yyy99AACCvvbhhqkk""""""]:-ABBB 	: 	:#D#yyy999	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	: 	:	z):	;	;	; > >

B4 0 0%!===> > > > > > > > > > > > > > >	z)<	=	=	= M M

BB< 8 8#|||LLLM M M M M M M M M M M M M M M M M MsH   #CCC*FFF5,G--G14G1/IIIc                     t           j                            d          } t          j        g d          }t          j        g d          }t          j        g d          }t	          j        |           }|                    |||gd          }t	          j        |||gd|                                          }|                    |d          sJ t	          j        d	|           }|                    |||gd          }t	          j        |||gd|                                          }t          j	        |                    |d                    sJ d S )
NrY   r   r   r   rZ   rp  r[   r\   rv   )
rG   rH   r]   re  r   as_davenportrm  r^   r_   r`   )rR   rZ  r[  e3rb   r  r   s          r   test_from_davenport_array_liker}    s@   
)


$
$C	)))		B	)))		B	)))		BS)))J$$b"b\377FRc6==??CCA""15"11111 ,,,J$$b"b\377FRc6==??CCA6*))!%)889999999r   c                 0   t           j                            d          }d}t          j        |df          }|                    t           j         t           j        |f          |d d df<   |                    dt           j        |f          }|                    t           j         t           j        |f          |d d df<   |                    dt           j        d          }|                     g d          }|                     g d          }|D ]}|                     t          j        ||z            	                    |                    }	| 
                    |||	gd	          }
||z
  |d d d
f<   dD ]}|dk    r|
n|                     |
d	          }t          j        |                     |          ||          }|                    |                     |          |          }t          ||                     |                     d S )Nr   rE   r=   rf  rv   )r  r  r  r   r   rb  rc  rc  )rG   rH   rI   r  r  r  r"   r   r   rO  rq  rk  rm  r{  r   )r$   r   r  r  angles_middlelambdasrZ  r[  lambr|  ax_lambr  axr,  
angles_davs                  r   test_as_davenportr    s   
)


"
"CAXq!fF;;BE6QD;AAF111a4LKKABEK==M;;BE6QD;AAF111a4Lkkabe%k88G	KKK	 	 B	KKK	 	 B < <ZZ,T"W55;;B??@@((BB<a(00$t+qqq!t/ 	< 	<E![00bgggAg6N6NB)"**R..%HHC))"**R..%@@JJ

6(:(:;;;;		<	< <r   c                    t           j                            d          d}t          j        |df          }                    t           j         t           j        |f          |d d df<   fdt          |          D             }                    t           j         t           j        |f          |d d df<                       dt           j        d          }|                     g d          }|                     g d	          }|D ]b}|                     t          j	        ||z            
                    |                    }|                     |||gd
          }	||z
  |d d df<   dD ]}
|
dk    r|	n
|	d d d         }t          j        |                     |          |
|          }t          |t          d          5  |                    |                     |          |
          }d d d            n# 1 swxY w Y   |                                }t          j        |                     |          |
|          }|                                }t#          ||d           dd S )Nr   r-   r=   rf  c                 R    g | ]#}                     d t          j        g          $S r   )choicerG   r  )r  rT   r   s     r   r  z0test_as_davenport_degenerate.<locals>.<listcomp>	  s+    >>>SZZBE
++>>>r   rv   )r-   r  r  r   r   r  rc  rr   r  r   r[   r\   )rG   rH   rI   r  r  r  rN   r"   r   r   rO  rq  rm  r   r  r{  rJ   r   )r$   r  r  r  r  rZ  r[  r  r|  r  r  r  r,  r  r  rot_estimatedr  r   s                    @r   test_as_davenport_degenerater    s    )


"
"C	AXq!fF ;;BE6QD;AAF111a4L>>>>U1XX>>>M;;BE6QD;AAF111a4Lkkabe$k77G	KKK	 	 B	KKK	 	 B E EZZ,T"W55;;B??@@((BB<a(00$t+qqq!t/ 	E 	EE![00gdddmB)"**R..%HHCS+]CCC E E --bjjnneDD
E E E E E E E E E E E E E E E==??L$3BJJrNNE:VVM)3355ML-eDDDDD	E	E Es   *HH
Hc           	         t           j                            d          }d}t          j        |df          }|                    t           j         t           j        |f          |d d df<   |                    dt           j        |f          |d d df<   |                    t           j         t           j        |f          |d d df<   dD ]}t          d          D ]}d	                    |d         |d         |d         g          }d
 |D             }|dk    r|                                }t          j
        ||                     |                    }t          j        |                     |          ||                     |                    }	t          |                    d          |	                    d          d           |d d dfxx         t           j        dz  z  cc<   dD ]}t          d          D ]}d	                    |          }d |D             }|dk    r|                                }t          j
        ||                     |                    }t          j        |                     |          ||                     |                    }	t          |                                |	                                d           ،d S )Nr   rE   r=   rf  r   rv   r  ri   r  c                 ,    g | ]}t          |          S r   r   rT  s     r   r  z:test_compare_from_davenport_from_euler.<locals>.<listcomp>#	      ,,,1)A,,,,,r   rc  Trl   r[   rF   c                 ,    g | ]}t          |          S r   r  rT  s     r   r  z:test_compare_from_davenport_from_euler.<locals>.<listcomp>0	  r  r   )rG   rH   rI   r  r  r  r   r  r  r   rm   r"   rm  r   r#   )
r$   r   r  r  r  r  r  r  euldavs
             r   &test_compare_from_davenport_from_eulerr  	  s   
)


"
"CAXq!fF ;;BE6QD;AAF111a4L;;125t;<<F111a4L;;BE6QD;AAF111a4L+ 	( 	(%e,, 	( 	(I''9Q<1y|DEEC,,,,,B##iikk%c2::f+=+=>>C)"**R..%FASASTTCCKK$K77t9T9T!&( ( ( ( (	( 111a4LLLBEAILLL+ F F%e,, 	F 	FI'')$$C,,,,,B##iikk%c2::f+=+=>>C)"**R..%FASASTTCCKKMM3;;==uEEEEE	FF Fr   c                 0   t           j                            d          }d}t          j        |df          }|                    t           j         t           j        |f          |d d df<   |                    dt           j        |f          |d d df<   |                    t           j         t           j        |f          |d d df<   dD ]}t          d          D ]}d	                    |d         |d         |d         g          }d
 |D             }|dk    r|                                }t          j
        ||                     |                    }|                    |          }	|                    |                     |          |          }
t          |	|
d           ˌ|d d dfxx         t           j        dz  z  cc<   dD ]}t          d          D ]}d	                    |          }d |D             }|dk    r|                                }t          j
        ||                     |                    }|                    |          }	|                    |                     |          |          }
t          |	|
d           d S )Nr   rE   r=   rf  r   rv   r  ri   r  c                 ,    g | ]}t          |          S r   r  rT  s     r   r  z6test_compare_as_davenport_as_euler.<locals>.<listcomp>D	  r  r   rc  r[   rF   c                 ,    g | ]}t          |          S r   r  rT  s     r   r  z6test_compare_as_davenport_as_euler.<locals>.<listcomp>Q	  r  r   )rG   rH   rI   r  r  r  r   r  r  r   rm   r"   r  r{  r   )r$   r   r  r  r  r  r  r  r,  r  r  s              r   "test_compare_as_davenport_as_eulerr  8	  s   
)


"
"CAXq!fF ;;BE6QD;AAF111a4L;;125t;<<F111a4L;;BE6QD;AAF111a4L+ 	2 	2%e,, 	2 	2I''9Q<1y|DEEC,,,,,B##iikk%c2::f+=+=>>C,,s##C""2::b>>599CC511111	2 111a4LLLBEAILLL+ 	2 	2%e,, 	2 	2I'')$$C,,,,,B##iikk%c2::f+=+=>>C,,s##C""2::b>>599CC511111	2	2 	2r   c                 
   t          j        d          }t          |          dk    sJ t          j        d          }t          |          dk    sJ t          j        d          g          }t          |          dk    sJ t          j        |                     d                    }t          |          dk    sJ t          j        |                     d                    }t          |          dk    sJ t          j        d|                     d                    }t          |          dk    sJ t          j        |                     d                    }t          |          dk    sJ t          j	        | 
                    d          d|                     d                    }t          |          dk    sJ t          j        |                     d                    }	t          |	          dk    sJ d S )	Nr   )r  r=   r   r,   r   r=   r=   ri   r   rb  )r   rH   r0  r  r!   rn   r   rm   r   rm  rK   r5  )
r$   r   r_ider_getr_quatr_matrixr_eulerr_vecr_davr_mrps
             r   test_zero_rotation_constructionr  Z	  s   AAq66Q;;;;!$$$Eu::????O"""2&Eu::???? 0 011Fv;;!#BHHY$7$788Hx==A!%&)9)9::Gw<<1 &!1!122Eu::????#BFF1II{BHHV<L<LMMEu::????bhhv..//Eu::??????r   c                    t          j        |                     d                    }|                                j        dk    sJ |                                j        dk    sJ |                    d          j        dk    sJ |                                j        dk    sJ |                                j        dk    sJ |	                    | 
                    d          d          j        dk    sJ d S )Nr  r  ri   r   r=   rb  )r   r!   rn   r#   rO   rJ   r  r   r<  r{  rK   rf   s     r   !test_zero_rotation_representationr  w	  s    288F++,,A99;;&&&&;;==)++++::e"f,,,,;;==&((((88::v%%%%>>"&&))[1176AAAAAAr   c                    t          j        |                     d                    }|                     g d          }|                    |          }|j        dk    sJ |                     d          }|                    |          }|j        dk    sJ |                     d          }t          j        t          d          5  |                    |           d d d            d S # 1 swxY w Y   d S )Nr  r   r   rv  z/Expected equal numbers of rotations and vectorsr   )
r   r!   rn   r"   rO  rO   r#  r   r   r   )r$   r   r_  r`  v0v0_rotrg  s          r   !test_zero_rotation_array_rotationr  	  s*   288F++,,A


999A

I?f$$$$	&		BWWR[[F<6!!!!	B	K
M 
M 
M  	                 s   C++C/2C/c                    t          j        |                     d                    }t          j        |                     g d                    }||z  }t	          |          dk    sJ ||z  }t	          |          dk    sJ t          j        |                     d                    }||z  }t	          |          dk    sJ d}t          j        d          }t          j        |                     |                                                    }t          j        t          |          5  ||z   d d d            n# 1 swxY w Y   t          j        t          |          5  ||z   d d d            d S # 1 swxY w Y   d S )Nr  r~   r   "Expected equal number of rotationsrv   r   )
r   r!   rn   r"   r0  rH   r#   r   r   r   )	r$   r   r_singler_mult_leftr_mult_rightr0r_multmsg_rotation_errorr^  s	            r   !test_zero_rotation_multiplicationr  	  s   288F++,,A!"**^^^"<"<==Hh,K{q    a<L|!!!!		BHHV,,	-	-BVFv;;!=			B		BJJrzz||44	5	5B	z);	<	<	<  
R               
z);	<	<	<  
R                 s$   0EE	E(E;;E?E?c                 ^   t          j        |                     d                    }t          j        ||g          }t	          |          dk    sJ t          j        |                     g d                    }|                    ||g          }t	          |          dk    sJ t          j        |                     t          j        d                                                              }|                    ||g          }t	          |          dk    sJ t          j        |                     t          j        d                                                              }|                    ||g          }|                    ||g          }t	          |          dk    sJ d S )Nr  r   r~   r   r=   r,   )r   r!   rn   r  r0  r"   rH   r#   )r$   r   r  r]  r   r4s         r   !test_zero_rotation_concatentationr  	  sZ   288F++,,A		q!f	%	%Br77a<<<<		BJJ~~~66	7	7B	
Aw		Br77a<<<<		BJJxq'9'9'A'A'C'CDD	E	EB	
Aw		Br77a<<<<		BJJxq'9'9'A'A'C'CDD	E	EB	
2w		B	
2w		Br77a<<<<<<r   c                     t          j        |                     d                    }dD ]}||z  }t          |          dk    sJ d S )Nr  )r6  rr   r   r   r  r   )r   r!   rn   r0  )r$   r   pppow0s       r   test_zero_rotation_powerr  	  sU    288F++,,A#  "u4yyA~~~~~ r   c                     t          j        |                     d                    }|                                }t	          |          dk    sJ d S )Nr  r   )r   r!   rn   r   r0  )r$   r   r_invs      r   test_zero_rotation_inverser  	  sB    288F++,,AEEGGEu::??????r   c                     t          j        |                     d                    }|                                }|j        dk    sJ d S )Nr  r  )r   r!   rn   r  rO   )r$   r   r  s      r   test_zero_rotation_magnituder  	  sD    288F++,,AI?d""""""r   c                     t          j        |                     d                    }t          j        t
          d          5  |                                 d d d            d S # 1 swxY w Y   d S )Nr  z+Mean of an empty rotation set is undefined.r   )r   r!   rn   r   r   r   r  rf   s     r   test_zero_rotation_meanr  	  s    288F++,,A	z)V	W	W	W  	                 s   A%%A),A)c                    t          j        |                     d                    }t          j        |                     d                    }|                    |          j        dk    sJ t          j        |                     g d                    }|                    |          j        dk    sJ t          j        |                     t          j                                                                        }|                    |          j        dk    sJ d}t          j        |                     t          j        d                                                              }t          j	        t          |          5  |                    |           d d d            n# 1 swxY w Y   t          j	        t          |          5  |                    |           d d d            d S # 1 swxY w Y   d S )Nr  r  r~   r  rv   r   )r   r!   rn   r_   rO   r"   rH   r#   r   r   r   )r$   r   r  r]  r^  
approx_msgr   s          r   test_zero_rotation_approx_equalr  	  s*   288F++,,A		BHHV,,	-	-B>>"#t++++		BJJ~~~66	7	7B>>"#t++++		BJJx'8'8'@'@'B'BCC	D	DB??1#t++++5J		BJJxq'9'9'A'A'C'CDD	E	EB	z	4	4	4  	r               
z	4	4	4  
                 s$   
F,,F03F0G55G9<G9c                    t          j        |                     d                    }||                     g | j                           }t          |          dk    sJ |d d         }t          |          dk    sJ t          j        t                    5  ||                     dg                    d d d            n# 1 swxY w Y   t          j        t                    5  ||                     dg                    d d d            n# 1 swxY w Y   t          j        t                    5  t          j        |                     g d                    |d<   d d d            d S # 1 swxY w Y   d S )Nr  r?  r   Tr'   )	r   r!   rn   r"   boolr0  r   r   
IndexError)r$   r   r  r_slices       r   test_zero_rotation_get_setr  	  s   288F++,,Abjj27j++,Eu::????eGw<<1	z	"	"  	"**aS//               
z	"	"  	"**dV

               
z	"	" < <!"**\\\":":;;!< < < < < < < < < < < < < < < < < <s6   C  CC$DDD1-E++E/2E/c                 f   t          j        |                     t          j        d                                                              }||                     g d                   }t          |          dk    sJ ||                     g d                   }t          |          dk    sJ ||                     g d                   }t          |          dk    sJ t          j        t                    5  ||                     ddg                    d d d            d S # 1 swxY w Y   d S )Nr=   )FFFr   )FTFr   )TTTT)	r   r!   r"   rH   r#   r0  r   r   r  )r$   r   r  r]  r   s        r   test_boolean_indexesr  	  sS   2::hoa&8&8&@&@&B&BCCDDA	
2::+++,,	-Br77a<<<<	
2::***++	,Br77a<<<<	
2::((())	*Br77a<<<<	z	"	" $ $	"**dD\
"
"##$ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $s   ;D&&D*-D*)r   r   numpyrG   numpy.testingr   scipy.spatial.transformr   r   scipy.statsr   	itertoolsr   r   scipy._lib._array_apir	   r
   r   r   r   r   r   scipy._lib.array_api_extra_libarray_api_extrar   rF  r  markskip_xp_backends
pytestmarkr   r%   r(   r3   r6   r8   rV   rc   rg   ro   rw   r{   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r(  r*  r-  r0  r8  r:  r=  r?  rA  rE  rG  rL  rO  rU  rW  rZ  r^  rd  rn  rr  r  r  r  r  r  r  parametrizer  r  thread_unsafer  r  r  r  r  r  r  r  r  r  r
  r  r  r  r!  r%  r,  r4  rE  rX  rb  rh  rj  rs  rx  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  r  r  r  r  r'  r/  r4  r9  r=  rB  rK  rN  r\  rd  rf  rk  rn  rv  ry  r}  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r   r   r   <module>r     s         & & & & & & 3 3 3 3 3 3 3 3 + + + + + + + + + + + + + +                  ) ( ( ( ( ( ( ( (   [))$)77
  7h 7 7 7 7  
0 0 00 0 00 0 0D D D.
: 
: 
:5 5 5
O O O@0 0 0
> 
> 
>= = =>  (
" 
" 
"$ $ $	/ 	/ 	/, , ,8, , ,>H H HH H H  @ @ @
3 3 3"P P P(& & &"
: 
: 
:5 5 5@ @ @@ @ @V V V> > >.
: 
: 
:5 5 5
" 
" 
"1 1 1
  X X X4 4 44 4 4* * *	 	 	A A A5 5 5
: 
: 
:5 5 5	E 	E 	E. . .
  E E E  . . .. . .J J J) ) )* * *  	2 	2 	2  8  6  6  6: : :2 2 2 ll5&9&9::m442 2 54 ;:2D ll5&9&9::m442 2 54 ;:2> ll5&9&9::m44< < 54 ;: <6 ll5&9&9::m44< < 54 ;: <6 ll5&9&9::m44= = 54 ;: =@/ / /*/ / /B3 3 3$ $ $
@ 
@ 
@@ @ @B B B" " "	= 	= 	= 2 2 2? ? ?
E 
E 
E( ( (( ( (  6 6 66A A A8: : :*: : ::9 9 9.     &/ / /Y Y Y(8 8 8
	D 	D 	D< < <5 5 53 3 3  ! ! !&F F FM M MM M M	M 	M 	M
4 
4 
4     %9 %9 %9PKN KN KN\ >  >  >FB B B8> > >0B B B:. . . 	) 	) 	)
: 
: 
:: : :",/ ,/ ,/^   G           
 
 

 
 

 
 
  0	9 	9 	9H H H$9 $9 $9N  & & &>F F F? ? ?+ + +5 5 5,- - -   ) ) ) 5  5  5FM M M2: : :$< < <. E E E>F F FB2 2 2D  :B B B  "  0  (    # # #    $< < <&$ $ $ $ $r   