o
    i[                    @   s  d dl Z d dlZd dlmZmZ d dlmZ d dlmZm	Z	 d dl
mZ d dlmZ d dlZd dlZdd Zd	d
 Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd  Zd!d" Zd#d$ Zd%d& Zd'd( Z d)d* Z!d+d, Z"d-d. Z#d/d0 Z$d1d2 Z%d3d4 Z&d5d6 Z'd7d8 Z(d9d: Z)d;d< Z*d=d> Z+d?d@ Z,dAdB Z-dCdD Z.dEdF Z/dGdH Z0dIdJ Z1dKdL Z2dMdN Z3dOdP Z4dQdR Z5dSdT Z6dUdV Z7dWdX Z8dYdZ Z9d[d\ Z:d]d^ Z;d_d` Z<dadb Z=dcdd Z>dedf Z?dgdh Z@didj ZAdkdl ZBdmdn ZCe jDEdoedpe jDEdqdrdsdt ZFe jDEdoedpe jDEdqdrdudv ZGe jDjHe jDEdoedpe jDEdqdrdwdx ZIe jDjHe jDEdoedpe jDEdqdrdydz ZJe jDjHe jDEdoedpe jDEdqdrd{d| ZKd}d~ ZLdd ZMdd ZNdd ZOdd ZPdd ZQdd ZRdd ZSdd ZTe jDjHdd ZUdd ZVdd ZWdd ZXdd ZYdd ZZdd Z[dd Z\dd Z]dd Z^dd Z_dd Z`dd Zadd Zbdd Zcdd Zddd Zedd Zfdd Zgdd Zhdd Zidd Zjdd Zkdd Zldd Zmdd ZnddĄ ZoddƄ ZpddȄ Zqddʄ Zrdd̄ Zsdd΄ ZtddЄ ZudZvddӄ ZwddՄ Zxddׄ Zyddل Zzddۄ Z{dd݄ Z|dd߄ Z}dd Z~dd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zd d Ze jDjHdd Zdd Zdd Zdd	 Zd
d Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd ZdS (       N)assert_equalassert_array_almost_equal)assert_allclose)RotationSlerp)special_ortho_group)permutationsc                 C   s4   | 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    axisr   r   e/home/kim/smarthome/.venv/lib/python3.10/site-packages/scipy/spatial/transform/tests/test_rotation.py	basis_vec   s   r   c                  C   sH   t g dg dg} t| }| t dgdgg }t| | d S )N      r   r   )      r   r   r      nparrayr   	from_quatr   as_quatr	   rexpected_quatr   r   r   test_generic_quat_matrix   s   
r#   c                  C   s2   t g d} t| }| d }t| | d S Nr   r   r   r    r   r   r   test_from_single_1d_quaternion   s   
r%   c                  C   s4   t g dg} t| }| d }t| | d S r$   r   r    r   r   r   test_from_single_2d_quaternion#   s   
r&   c                  C   s  t jd} tjg ddd}t| t dddd tjt g dd	dd}t| t t dd
ddd | 	dd}|t j
j|ddd d d f  }|D ]}tj|dd}tt | d|dd qRtj|dd}tt j| ddd|dd d S )Nr   r   r   r   r   Tscalar_firstr   V瞯<缉ؗҜ<rtolatol
   r   )r0   r   r   d   r   r   r   r-   )r   randomRandomStater   r   r   	as_matrixeyetilerandnlinalgnormrollr   rngr!   qZqir   r   r   test_from_quat_scalar_first*   s    "r?   c                  C   sR  t jd} tdt d}t|jddg dddd	 tdt d
}t|jddt g ddddd	 | 	dd}|t j
j|ddd d d f  }|D ]*}t|}t|jddt |ddd t|jdddt |jddddd qPt|}t|jddt j|ddddd t|jdddt j|jddddddd d S )Nr   xyzr   Tr(   r'   r*   r+   r,   )r0   r   r/   r1   r   r   r   r2   )	canonicalr)   rA   )r   r3   r4   r   
from_eulerzerosr   r   r7   r8   r9   r:   r   r;   r<   r   r   r   test_as_quat_scalar_first>   s6    


rE   c               	   C   sp   t g dg dg dg dg dg dg} t| }| t dgdgd	gd
gd	gd
gg }t| | d S )N)r   r   r   r   r   r   r   r   r   r   r   r   )rH   rH   r   r   r   r   rH   rH   rH   rH   rH   r   r   r      r   r    r   r   r   test_from_square_quat_matrix\   s   
&rL   c                  C   sn   t g dg dg dg dg dg} t| }t | t jj| ddd d d f  }t|jdd	| d S )
N)rH   r   r   r   )r   rH   r   r   )r   r   rH   r   rI   rJ   r   r   TrB   )	r   r   r   r   absr9   r:   r   r   r    r   r   r   *test_quat_double_to_canonical_single_coverk   s   
&rN   c                  C   sl  t g d} t| }t| |jdd t g dt d } t| }|| | }t|jddt d g d t| jddt d g d t|jddt d g d t| jddt d g d t||  jddg d	d
d t||  jddg d	d
d t|| jddg dd
d t| |  jddg dd
d d S )NrI   FrB   )r   r   r   r   rK   )rH   r   r   r   )r   r   r   rH   )rH   r   r   rH   rG   gؗҬ<r.   )	r   r   r   r   r   r   sqrtr   inv)r>   r!   r3r   r   r   test_quat_double_coverx   s<   


rS   c                   C   s   t jtdd ttg d W d    n1 sw   Y  t jtdd ttg dg dg W d    n1 sAw   Y  t jtdd ttg dgg dgg W d    d S 1 siw   Y  d S )NzExpected `quat` to have shapematchr   rK   r   )r   rK   r   r   r   )r   r            r   rK   r   r   )r   r   rW   rX   )pytestraises
ValueErrorr   r   r   r   r   r   r   r   test_from_quat_wrong_shape   s   

"r^   c                  C   sV   t g dg dg dg} tt t|  W d    d S 1 s$w   Y  d S )Nr   )r   r   r   r   rF   )r   r   r[   r\   r]   r   r   )r	   r   r   r   test_zero_norms_from_quat   s   "r_   c                  C   s*   g d} t |  }t|td d S )NrG   r   )r   r   r5   r   r   r6   )quatmatr   r   r   #test_as_matrix_single_1d_quaternion   s   rb   c                  C   sR   g dg} t |  }t|jd tg dg dg dg}t|d | d S )Nr   r   r   r   r   r   r   r   rH   r   r
   r   r   r   r   r5   r   shaper   r   r   )r`   ra   expected_matr   r   r   #test_as_matrix_single_2d_quaternion   s   
ri   c                  C   s   g dg dg dg dg} t |  }t|jd tg dg dg dg}t|d	 | tg dg d
g dg}t|d | t|d td t|d td d S )Nrc   r   r   r   r   rG   rI   )r   r   r   re   r
   r   r   r   rH   r   r   r   rK   r   )	r   r   r5   r   rg   r   r   r   r6   )quatsra   	expected0	expected1r   r   r    test_as_matrix_from_square_input   s*   ro   c                  C   s   g dg dg dg} t |  }t|jd tg dg dg dg}t|d | tg dg d	g d
g}t|d | tg dg dg dgd }t|d | d S )Nrc   rj   rZ   )r   r   r   re   r
   r   r   r   rk   r   )皙?g@)gffffff@r   rp   )rH   rK   rK   r   rK   rf   )rl   ra   rm   rn   Z	expected2r   r   r   !test_as_matrix_from_generic_input   s4   rr   c                  C   s6   g dg dg dg} g d}t t|  | d S )Nr   r
   r         ?rt   rt   rt   )r   r   from_matrixr   ra   r"   r   r   r   test_from_single_2d_matrix  s   rw   c                  C   sN   t g dg dg dgd} t g dd}tt|  | d S )Nr   r
   r   rd   rs   r   r   )r   r   reshaper   r   ru   r   rv   r   r   r   test_from_single_3d_matrix  s   rz   c                  C   sl   t g dt d } t g dg dg dg}tt| |  tt|d | d d S )N)r   r   rW   r   '   )3~˷g6iпnzo?)r}   r|   6i?)r~   r}   g3~˷?rd   rx   )r   r   rP   r   r   ru   r   ry   )r"   ra   r   r   r   test_from_matrix_calculation  s   r   c                  C   s(   t jdddd} tt|  |  d S )Nr   r0   r   )sizerandom_state)r   Zrvsr   r   ru   r5   ra   r   r   r    test_matrix_calculation_pipeline+  s   r   c                  C   s   t jd} | d}t j|}tt|D ]}|| dk r&||  ||< qt	|
 }t d||d}t d}tdD ]}d|d d ||f< qAt|| d S )Nr   )r1   r   r   ...ij,...jk->...ik)r   rK   r   r         ?)r   r3   r4   Zrandom_sampler9   Zdetrangelenr   ru   r5   einsumZ	transposerD   r   )rndra   ZdetsiZ	ortho_matZmult_resulteye3dr   r   r   test_from_matrix_ortho_output0  s   

r   c                  C   s   t g dg dg dg} t g dg dg dg}tt|  |dd t g d	g d
g dg} t g dg dg dg}tt|  |dd d S )Nr   r   r   r   r   )5_%?gx'?        )gx'ܿr   r   )r   r   r   ư>rO   )r         r   )rt   r   r   )r   r   rt   re   r
   )r   r   r   r   ru   r5   )ra   expectedr   r   r   test_from_matrix_normalizeD  s(   

r   c                  C   s   t d} d| d< tjtdd t|  W d    n1 s w   Y  d| d< tjtdd t|  W d    d S 1 sAw   Y  d S )Nr   r   r   r   zNon-positive determinantrT   rH   )r   r6   r[   r\   r]   r   ru   r   r   r   r   )test_from_matrix_non_positive_determinantX  s   
"r   c                  C   2   g d} t g d}t| }t| | d S Nr
   )gJ?r   r   g<(?r   r   r   from_rotvecr   r   rotvecr"   resultr   r   r   test_from_1d_single_rotvecc     
r   c                  C   6   g dg} t g dg}t| }t| | d S r   r   r   r   r   r   test_from_2d_single_rotvecj     

r   c                  C   sJ   g dg dg dg} t g dg dg dg}tt|  | d S )Nr   rK   rK   r   rH   rt   r   r   r   )g.pG?cU(G?r   gQH?)gNZ]O?gNZ]OݿgNZ]O?g0i?rG   )r   r   r   r   r   r   )r   r"   r   r   r   test_from_generic_rotvecq  s   r   c                  C   s   t dt d dt d dt d gg dg dg} t|  }t|d d t|dd df | d d	  t|d
 d t|dd df t g d t|d t g d d S )NgMb@?r   gMb@)皙?g333333?rp   r   r   r   r   r   rt   r   r   g@w?)g>}J?g.e?g>}J?rK   rG   )r   r   rP   r   r   r   r   r   )r   r`   r   r   r   test_from_rotvec_small_angle  s   &r   c                  C   sh   dt d dt d dt d g} tj| dd}| }t | }t|}| }t|| d S )Nr   r   Tdegrees)r   Zcbrtr   r   r   deg2radr   )Zrotvec1Zrot1Zquat1Zrotvec2Zrot2Zquat2r   r   r   test_degrees_from_rotvec  s   (

r   c                   C   B   t jtdd tddg W d    d S 1 sw   Y  d S )N Expected `rot_vec` to have shaperT   r   rK   r[   r\   r]   r   r   r   r   r   r   test_malformed_1d_from_rotvec     "r   c                   C   J   t jtdd tg dg dg W d    d S 1 sw   Y  d S )Nr   rT   rZ   r   rW   rX   rY   r   r   r   r   r   test_malformed_2d_from_rotvec     "r   c                  C   s   t g dg dg dg} | t jj| ddd d d f  } t|  }t jj|dd}t| d d df t |d  tt 	|| d d d df t 
d d S )	Nr   rK   rH   rt   r   rH   r   ga2U0*3?rG   r   r   r   rK   )r   r   )r   r   r9   r:   r   r   	as_rotvecr   coscrossrD   )r`   r   angler   r   r   test_as_generic_rotvec  s     ,r   c                  C   D   t g d} t g d}t|  }t|jd t|| d S )Nr   rK   rK   gj*0	x?g$x?g:2(r   r   r   r   r   r   r   rg   r   r`   Zexpected_rotvecZactual_rotvecr   r   r   test_as_rotvec_single_1d_input  
   r   c                  C   H   t g dg} t g dg}t|  }t|jd t|| d S )Nr   r   r   r   r   r   r   r   test_as_rotvec_single_2d_input  
   r   c                  C   sj   g dg dg dg} t | }|jdd}tj|}t|d t|d |d  t|d |d	  d S )
Nr   r
   r   Tr   g      ^@r   r   rK   )r   ru   r   r   r9   r:   r   )ra   rotr   r   r   r   r   test_as_rotvec_degrees  s   

r   c                  C   sP   t g dg dg dg} tt|  |  ttj| ddjdd|  d S )Nr   r   rH   rK   )ga2U0*3gǺ6?ga2U0*?Tr   )r   r   r   r   r   r   )r   r   r   r   test_rotvec_calc_pipeline  s   r   c                  C   r   N)r   r   r   )r   r   r   r   r   r   r   from_mrpr   r   mrpr"   r   r   r   r   test_from_1d_single_mrp  r   r   c                  C   r   r   r   r   r   r   r   test_from_2d_single_mrp  r   r   c                  C   sP   t g dg dg dg} t g dg dg dg}tt|  | d S )Nr   r   r   )r   rp   rp   g)g}+;?g}+;g}+;?gߨӉؿrG   )r   r   r   r   r   r   )r   r"   r   r   r   test_from_generic_mrp  s   r   c                   C   r   )NExpected `mrp` to have shaperT   r   rK   r[   r\   r]   r   r   r   r   r   r   test_malformed_1d_from_mrp  r   r   c                   C   r   )Nr   rT   rZ   r   r   r   r   r   r   test_malformed_2d_from_mrp  r   r   c                  C   sp   t g dg dg dg} | t jj| ddd d d f  } t g dg dg dg}tt|  | d S )	Nr   r   rG   r   r   )gQUU?gvWUU?QUUտ)uFx?guFxr   r   )r   r   r9   r:   r   r   r   as_mrp)r`   expected_mrpr   r   r   test_as_generic_mrp  s    r   c                  C   sD   t t t jd d  ddg} ttjdg ddd |  d S )	NrK   r   r   r   r@   )i  r   r   Tr   )r   r   tanpir   r   rC   r   )r   r   r   r   test_past_180_degree_rotation!  s
   "r   c                  C   r   )Nr   g?g?gO޿r   r   r   r   r   r   r   rg   r   r`   r   
actual_mrpr   r   r   test_as_mrp_single_1d_input+  r   r   c                  C   r   )Nr   r   r   r   r   r   r   r   test_as_mrp_single_2d_input5  r   r   c                  C   s\   t g dg dg dg dg} t g dg dg dg dg}tt|  | d S )Nr   r   )gzpy?r   r   )皙?r   r   )gK}\UUſgK}\UU?r   )r   r   r   r   r   r   )r   r   r   r   r   test_mrp_calc_pipeline?  s   r   c                  C   s:   t jdddd } tg dtd }t| | d S )Nr   Z   Tr   rc   rK   )r   rC   r   r   r   rP   r   )r`   r"   r   r   r   test_from_euler_single_rotationM  s   r   c                  C   s6   t jdddd } t jdddd }t| | d S )Nr   r   Tr   Z)r   rC   r5   r   	extrinsic	intrinsicr   r   r   (test_single_intrinsic_extrinsic_rotationS  s   r   c                  C   sh   t jd} | jdddd}|d d d d df }tjd|dd }tjd	|dd }t|| d S )
Nr      )rW   r   lowhighr   rH   r@   Tr   ZZYX)r   r3   r4   randintr   rC   r   r   )r   abr	   r   r   r   r   test_from_euler_rotation_orderY  s   r   c                  C   sB   t jdddgdd } tg dg dg dg}t| | d S )NZzxr   Tr   re   r   r   rH   r
   )r   rC   r5   r   r   r   )ra   rh   r   r   r   -test_from_euler_elementary_extrinsic_rotationc  s   r   c                  C      g dg dg dg} t jd| dd }t|d tg dg d	g d
g t|d tg dg dg dg t|d tg dg dg dg d S )N   <   -   r   r   r   r   r   r   ZZXYTr   r   )g# ?      пgΙ6d?)g$+H?+Pz?Ppƿ)gPpֿ+Pz?gPp?r   )g+a?r  (s=?)r  r  g&(ٿ)r  r  r  rK   )ga}@?gF6g^@?)g0	8?gF6?g4?)      rt   r  r   rC   r5   r   r   r   anglesra   r   r   r   &test_from_euler_intrinsic_rotation_312n  (   r  c                  C   r   )Nr   r  r  ZZXZTr   r   )>d?g @r  )l>?^|@r  )X?r  rt   r   g      ?g ,r  g ,?g      ?r  r        ?rt   rK   )r  g4&d'e?)cH?Xӿg'eֿ)r        ?r  r	  r
  r   r   r   &test_from_euler_intrinsic_rotation_313  r  r  c                  C   s   g dg dg dg} t jd| dd }t|d tg dg d	g d
g t|d tg dg d	g dg t|d tg dg dg dg d S )Nr   r  r  ZzxyTr   r   )4&d?gPp?r  )r  r  g+Pz)r  r  r  r   )g
?\2׳r  )r  g ,?r  rK   )r  r  r  )r  r  r   )g>dۿ @?r  r	  r
  r   r   r   &test_from_euler_extrinsic_rotation_312  r  r  c                  C   s   g dg dg dg} t jd| dd }t|d tg dg d	g d
g t|d tg dg dg d
g t|d tg dg dg dg d S )Nr   r  r  ZzxzTr   r   )r  gl>r  )r  r  gXr  r   r  r  rK   )r  gcHr  )r  r  r  )r  r  r  r	  r
  r   r   r   &test_from_euler_extrinsic_rotation_313  r  r  	seq_tupler@   r   )FTc           
      C   s  dd }t jd}d}t |df}|jt j t j|fd|d d df< |jt j d t jd |fd|d d df< |jt j t j|fd|d d df< d	| }|rX| }t	||}|
|}||}	t||dd
d t||	dd
d ||| dd ||	| dd d S )Nc                 S   V   t j| dd}t j| dd}t ||}t t ||k s J t ||k s)J d S Nr   r   r   meanstdhypotallrM   errorZmean_maxZrms_maxr$  r%  Zrmsr   r   r   
test_stats  
   z1test_as_euler_asymmetric_axes.<locals>.test_statsr     r   r   rK   r    -q=r.   r-   r*   +=r   r3   r4   emptyuniformr   joinupperr   rC   as_euler_as_euler_from_matrixr   
r   r   r*  r   nr  seqrotationZangles_quatZ
angles_matr   r   r   test_as_euler_asymmetric_axes  s"   $,$


r<  c           
      C   s  dd }t jd}d}t |df}|jt j t j|fd|d d df< |jdt j|fd|d d df< |jt j t j|fd|d d df< d	| d | d | d g}|r[| }t	||}|
|}||}	t||dd
d t||	ddd ||| dd ||	| dd
 d S )Nc                 S   r!  r"  r#  r(  r   r   r   r*    r+  z0test_as_euler_symmetric_axes.<locals>.test_statsr   r,  r   r   r   rK   r-  gvIh%<=r/  &.>r+   r0  r*   r1  r8  r   r   r   test_as_euler_symmetric_axes  s"   $ $

r>  c                 C   s   t g dg dg dg dg}d| }|r| }tj||dd}| }tjt	dd	 |j
|dd}W d    n1 sAw   Y  tj||dd }t|| d S )
Nr   r   #   r@     r@  r      rE  rB     r-  Tr   Gimbal lockrT   r   r   r4  r5  r   rC   r5   r[   warnsUserWarningr6  r   r   r   r  r:  r;  mat_expectedZangle_estimatesmat_estimatedr   r   r   (test_as_euler_degenerate_asymmetric_axes  s    
rO  c                 C   s   t g dg dg dg dg}d| d | d | d g}|r%| }tj||dd	}| }tjt	d
d |j
|dd	}W d    n1 sJw   Y  tj||dd	 }t|| d S )NrG  r   r   r@  r   K   r   r   r@  rG  iLrE  r-  r   r   Tr   rH  rT   rI  rL  r   r   r   'test_as_euler_degenerate_symmetric_axes5  s    rU  c                 C   st  t g dg dg dg dg}d| }|r| }tj||dd}tjtdd	 |j	|dd}W d    n1 s=w   Y  tjtdd	 |j
|dd}W d    n1 s[w   Y  t|d d d
dgf |d d d
dgf d
dd t|d d df |d d df d
dd t g dg dg dg dg}|d d df d
k}d| d
 | d | d
 g}|r| }tj||dd}tjtdd	 |j	|dd}W d    n1 sw   Y  tjtdd	 |j
|dd}W d    n1 sw   Y  t|d d d
dgf |d d d
dgf d
dd t|| df || df d
dd t||df ||df dd d S )Nr?  rA  rD  rF  r-  Tr   rH  rT   r   rK   r.  r/  r   Hz>rP  rQ  rS  rT  r   rO   )r   r   r4  r5  r   rC   r[   rJ  rK  r7  r6  r   )r   r   r  r:  r   Zestimates_matrixZestimates_quatidxr   r   r   +test_as_euler_degenerate_compare_algorithmsQ  s\   
(((
rX  c            	      C   s   t jd} d}tj|| d}| }| }| }t d||}t d||}t |ddf}t d|d d < t	|| t	|| d S )Nr   r0   )numr   r   r   )
r   r3   r4   r   rQ   r5   r   r2  r6   r   )	r   r9  pr>   p_matq_matresult1result2r   r   r   r   test_inv  s   
r_  c                  C   s   t jd} tj| d}| }| }| }t ||}t ||}t d}t|| t|| tjd| d}| }	| }
|	 }t 	d|
|}t 	d||
}t 
d}t d|d d < t|| t|| d S )N	   #;h29s+,g0Hn r=   r   r   )rY  r=   r   rd   )r   r3   default_rngr   rQ   r5   dotr6   r   r   r2  )r=   rZ  r>   r[  r\  Zres1Zres2r6   r	   r   Zx_matrixZy_matrixr]  r^  r   r   r   r   test_inv_single_rotation  s(   




rd  c                  C   s4   d} t t|  d t t|   d d S )Nr0   r   )r   r   identity	magnituderQ   )r9  r   r   r   test_identity_magnitude  s   rg  c                   C   s0   t   dks
J t    dksJ d S )Nr   )r   re  rf  rQ   r   r   r   r   test_single_identity_magnitude  s   rh  c                  C   sV   d} t j| dd}|t |  }t| |  ||  }t| t|  d S Nr0   r   ra  	r   r3   re  r   r   rQ   rf  r   rD   r9  rZ  r   r   r   r   test_identity_invariance  s   rl  c                  C   sT   d} t j| dd}|t   }t| |  ||  }t| t|  d S ri  rj  rk  r   r   r   test_single_identity_invariance  s   rm  c                  C   sf   t td} |  }t|tjtjtjdg t td } |  }t|tjtjtjdg d S )Nr   r   )r   r   r   r6   rf  r   r   )r!   r   r   r   r   test_magnitude  s   rn  c                  C   sB   t td} | d  }t|tj | d  }t|d d S )Nr   r   r   )r   r   r   r6   rf  r   r   )r!   r]  r^  r   r   r   test_magnitude_single_rotation  s
   ro  c                  C   s`   t jd} tjd| d}tjd| d}||  }| }t |}t|||||k  d S )Nr`  r0   ra  )	r   r3   rb  r   rQ   rf  Zmedianr   approx_equal)r=   rZ  r>   r!   Zr_magr.   r   r   r   test_approx_equal  s   
rq  c                  C   s   t g d} t td}| |d sJ | |d r!J | j|d ddr,J | j|d ddd	r8J tjtd
d | j|d ddsKJ W d    d S 1 sVw   Y  d S )N)r   r   r=  r   r   r   绽|=rO   g:0yE>T)r.   r   zatol must be setrT   r   )	r   r   r   r   r6   rp  r[   rJ  rK  )rZ  r>   r   r   r   !test_approx_equal_single_rotation  s   "rs  c                  C   sb   t t d t df} t dt jd d}|D ]}t||  }t| 	 ddd qd S )Nr   r   rK   r1   rr  rO   )
r   concatenater6   linspacer   r   r   r   r$  rf  )axesthetastr!   r   r   r   	test_mean  s   ry  c                  C   s   t g dg dg dg} t dt jd d}|D ]-}t|| d d  }|jddgd}t||  }| }t||  	 ddd	 qd S )
Nr   r
   r   rK   r1   r   weightsrr  rO   )
r   r   ru  r   r   r   r$  r   rQ   rf  )rv  rw  rx  rwmwr!   mr   r   r   test_weighted_mean  s   r  c                  C   sX   t jtdd ttd} | jtd d W d    d S 1 s%w   Y  d S )Nznon-negativerT   r   rz  )	r[   r\   r]   r   r   r   r6   r$  onesr!   r   r   r   test_mean_invalid_weights(  s   "r  c                  C   s"   t  jdd} t| t sJ d S )NFZreturn_indices)r   re  reduce
isinstance)r   r   r   r   test_reduction_no_indices.  s   r  c                  C   sV   t  jdd} t| tu sJ t| dksJ | \}}}|d u s#J |d u s)J d S )NTr  r   )r   re  r  typetupler   )r   reduced	left_best
right_bestr   r   r   test_reduction_none_indices3  s   
r  c                  C   st  t jd} tjd| d}tjd| d}tjd| d}|j||dd\}}}t t|t|t|f}t|D ](\}}	t|D ]\}
}t|D ]\}}t |	| | 	 d |||
|f< qHq@q8t 
t |d	d
|jd	 df}t jt 
|t|dfd	d}|t| }|t| }||k sJ ||k sJ || | ||  }| |  }t|t t| d S )Nr`  r   ra  r0   rX   Tr  r   r   r   rH   r   )r   r3   rb  r   r  rD   r   	enumeraterM   r   ry   Zmoveaxisrg   Zargmaxr'  rQ   rf  r   )r=   lr!   rZ  r  r  r  Zscalarsr   ZlijZpjkZrkZmax_indZleft_best_checkZright_best_checkZreduced_checkmagr   r   r   !test_reduction_scalar_calculation=  s*   & r  c            	      C   s"  t g dg dg dg} t| }tt j| dd}t g d}t j|dd}t g d}t j|dd}t||| t||| t||| t||| t g d}t j|dd}t|j|d	d
| t|j|d	d
| t|j|d	d
| t|j|d	d
| d S )Nre   r
   r   r   r   rV   rq   r   r   rK   rH   r   TZinverser   r   r   ru   expand_dimsr   apply)	ra   Zr_1dZr_2dZv_1dZv_2dZv1d_rotatedZv2d_rotatedZv1d_inverseZv2d_inverser   r   r   'test_apply_single_rotation_single_pointW  s*   
r  c                  C   s   t g dg dg dg} t| }tt j| dd}t g dg dg}t g dg d	g}t||| t||| t g d
g dg}t|j|dd| t|j|dd| d S )Nre   r
   r   r   r   rV   r   r   rW   r  )r   rW   r  )r   rW   Tr  r  )ra   r1r2v	v_rotated	v_inverser   r   r   *test_apply_single_rotation_multiple_pointss  s   
r  c                  C   s   t d} t g dg dg dg| d< t g dg dg dg| d< t| }t g d	}t j|dd
}t g dg dg}t||| t||| t g dg dg}t|j|dd| t|j|dd| d S )NrK   r   r   re   r
   r   r   r   r   r   rV   r   r  )r   r   rK   r  )r   r   rq   Tr  )r   r2  r   r   ru   r  r   r  )ra   r!   Zv1v2r  r  r   r   r   *test_apply_multiple_rotations_single_point  s(   



r  c                  C   s   t d} t g dg dg dg| d< t g dg dg dg| d< t| }t g d	g d
g}t g dg dg}t||| t g dg dg}t|j|dd| d S )Nr  re   r
   r   r   r   r   r   rV   r  r  )r   ir   r  )r   rW   r  Tr  )r   r2  r   r   ru   r   r  )ra   r!   r  r  r  r   r   r   -test_apply_multiple_rotations_multiple_points  s"   



r  c                  C   s   t d} t g dg dg dg| d< t g dg dg dg| d< t| }t|d  | d d	d
 t|d  | d d	d
 t|d d  t j| d ddd	d
 d S )Nr  re   r
   r   r   r   r   r   r*   rO   rH   r   )r   r2  r   r   ru   r   r5   r  ra   r!   r   r   r   test_getitem  s   



,r  c                   C   s@   t jtdd t d  W d    d S 1 sw   Y  d S Nznot subscriptablerT   r   )r[   r\   	TypeErrorr   re  r   r   r   r   test_getitem_single  s   "r  c                  C   sH   t  } tjtdd t  | d< W d    d S 1 sw   Y  d S r  )r   re  r[   r\   r  r  r   r   r   test_setitem_single  s   "r  c                  C   sR   t jd} tjd| d}tjd| d}||dd< t|dd  |  d S )Nr`  r0   ra  r   r   rW   r   r3   rb  r   r   r   r=   r  r  r   r   r   test_setitem_slice  s
   r  c                  C   sH   t jd} tjd| d}tj| d}||d< t|d  |  d S )Nr`  r0   ra  r   r  r  r   r   r   test_setitem_integer  s
   r  c                  C   sJ   t jddd} tjtdd d| d< W d    d S 1 sw   Y  d S )Nr0   r   ra  zRotation objectrT   r   )r   r3   r[   r\   r  r  r   r   r   test_setitem_wrong_type  s   
"r  c                  C   s|   t d} t g dg dg dg| d< t g dg dg dg| d< t| }tt|d	 tt|d d
 d d S )Nr  re   r
   r   r   r   r   r   rK   rH   )r   r2  r   r   ru   r   r   r  r   r   r   test_n_rotations  s   



r  c                  C   sv   t jd} ttj| d jd ttjd | d jd ttjd| d jd ttjd| d jd d S )Nr`  ra  r   r   rx   r   r   r   )r   r3   rb  r   r   r   rg   ra  r   r   r   test_random_rotation_shape   s
   r  c                  C   sT   t g dg dg} |  }t| |\}}t| t d t|ddd d S )NrV   r  r   r   r   rO   )	r   r   copyr   align_vectorsr   r5   r6   r   )r	   r   r!   rssdr   r   r   test_align_vectors_no_rotation	  s
   r  c                  C   sb   t jd} tj| d}| jdd}||}t||\}}t| |  t|ddd d S )Nl	   #t}":xL ra  )r   r   r   r   rV  rO   )	r   r3   rb  r   normalr  r  r   r   )r=   cr   r   estr  r   r   r   test_align_vectors_no_noise  s   
r  c                  C   sb   t g dg dg} t g dg dg}t| |\}}t| ||dd t|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   rO   r   rV  )r   r   r   r  r   r  )r	   r   r  r  r   r   r   $test_align_vectors_improper_rotation  s   

r  c                  C   s   d} t 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d\}}}t j|| |ds=J t j|||dsGJ d S )NgbQ?)r   r   r   )r         ?r   )r   r   r   r   r   r   r   r   r
   )r   g?r   )r   g?r   Treturn_sensitivityrO   )r   r   r   r  iscloseZallclose)rssd_expectedZsens_expectedr.   r   r   r   r  Zsensr   r   r   #test_align_vectors_rssd_sensitivity)  s   
r  c            
      C   s   d} t j| ddg d}t j| ddg d}d}t ||t| d\}}}t |||t|  d\}}}	t| |  tt|| |dd	 t||	 d S )
Nr0   r   ra  r
   r   rK   Tr   rO   )	r   r3   r  r  r   r  r   r5   rP   )
r9  r   r   scaleZest1Zrssd1Zcov1Zest2Zrssd2Zcov2r   r   r   !test_align_vectors_scaled_weights6  s    r  c                  C   s*  t jd} d}tj| d}| j|dfd}||}t d}d| }t| j|df|d}||}tj||d	d
\}	}
}||		  
 }t|d d|d t|d d|d t|d d|d ||9 }t|d d|d t|d d|d t|d d|d t|
t ||	| d d  d S )Nr`  r1   ra  r   r  r   r  )r   r  Tr  r   rO   rK   r   )r   r   )rK   rK   rt   )r   r3   rb  r   r  r  r   r   r  rQ   r   r   sum)r=   Z	n_vectorsr   Zvectorsr   sigmaZ	tolerancenoiseZnoisy_resultr  r  ZcovZerror_vectorr   r   r   test_align_vectors_noiseD  s4   


	&r  c                   C   s  t jtdd tg dg d W d    n1 sw   Y  t jtdd tg dg d W d    n1 s=w   Y  t jtdd tg dg dgg dg W d    n1 scw   Y  t jtdd tjg dgg dgd	ggd
 W d    n1 sw   Y  t jtdd tjg dg dgg dg dgg dd
 W d    n1 sw   Y  t jtdd tjg dgg dgdgd
 W d    n1 sw   Y  t jtdd  tjg dg dgg dg dgtjtjgd
 W d    n	1 sw   Y  t jtdd tg dgg dg W d    n	1 s1w   Y  t jtdd  tjg dg dgg dg dgdtjd	gd W d    n	1 saw   Y  t jtdd tjg dgg dgdd W d    d S 1 sw   Y  d S )Nz Expected input `a` to have shaperT   rZ   rV   z Expected input `b` to have shapez/Expected inputs `a` and `b` to have same shapesr  z&Expected `weights` to be 1 dimensionalr   rz  z+Expected `weights` to have number of valuesz)`weights` may not contain negative valuesrH   z#Only one infinite weight is allowedz(Cannot align zero length primary vectorsr   z Cannot return sensitivity matrixT)r  r{  r  )r[   r\   r]   r   r  r   infr   r   r   r    test_align_vectors_invalid_inputk  sh    "   
 
$r  c                  C   s  d} g dg dg}g dg dg}t g dg dg dg}tj||t jdgd\}}t| || d	 t|||| d	 t j|d
| d	sJJ g dg dg}d}tj||t jdgd\}}t| || d	 t||d
 |d
 | d	 t j||| d	sJ g dg dg}t|||| d	 g dg dg}g dg dg}d}tj||t jdgd\}}t||d
 |d
 | d	 t j||| d	sJ d S )Nr.  r
   r   r   r  r   r   rz  rO   r   )r   rK   r   r   r   r   rK   rV   )rq   r   rH   )rH   r   rK   r   g4i?)	r   r   r   r  r  r   r5   r  r  )r.   r   r   
m_expectedRr  r  Z
a_expectedr   r   r   "test_align_vectors_align_constrain  s2   
r  c                  C   s  d} g }t dD ]}|tj| d| d  qt | D ]N}d|d | d  d|d | d  g}d|d | d  d	|d | d  g}tj||d
dgd\}}tj||tjdgd\}}t| | dd qt | D ]a}d|d | d  d|d | d  d|d | d  g}d	|d | d  d|d	 | d  d|d | d  g}tj||g dd\}}tj||tjddgd\}}t| | dd qpd S )Nr1   rW   r0   ra  r   r   rK   r   r       _Brz  -C6?rO   r   )r  rK   r   )	r   appendr   r3   r5   r  r   r  r   )r9  Zmatsr   r   r   r  _R2r   r   r   test_align_vectors_near_inf  s"   ((::r  c                  C   sT  d} g dg dg}g dg dg}t g dg dg dg}tj||t jdgd\}}t| || d t|d	 |d	 \}}t| || d t||d	 |d	 | d g dg dg}t g dg dg dg}tj||t jdgd\}}t| || d t|d	 |d	 \}}t| || d t||d	 |d	 | d d S )
Nr.  r
   r   rk   r   r   rz  rO   r   )r   r   r   r  r  r   r5   r  )r.   r   r   r  r  r  r   r   r   test_align_vectors_parallel  s,   

 r  c                  C   sd  d} t g dg dgg dg dgg dg dgg}dd |D }t||D ])\}}tj||t jdgd\}}t| t j| d	 t|	|d
 |d
 | d	 q(tj
dd
d}t| d }g dg dg}g dg dg}g }|D ]}	||		|d
 |d g qv|D ]&}tj||t jdgd\}}tj||ddgd\}
}t| |
 | d	 qd S )Nr.  r
   r   r   c                 S   s   g | ]}|d   |d gqS r   r   r   ).0r   r   r   r   
<listcomp>  s    z3test_align_vectors_antiparallel.<locals>.<listcomp>r   rz  rO   r   r1   ra  r  rk   r  )r   r   zipr   r  r  r   rf  r   r  r3   r   r   r  r5   )r.   Z
as_to_testZ
bs_to_testr   r   r  r  ZRsZdRsZdRr  r   r   r   test_align_vectors_antiparallel  s,   r  c            	      C   s   d} t jddd }t jddd }t||D ](\}}|d }|d }t ||\}}t|||| d tj|d| dsAJ qd S )Nr.  r1   r   ra  r   rO   )	r   r3   r5   r  r  r   r  r   r  )	r.   Zmats_aZmats_bZmat_aZmat_br   r   r  r  r   r   r   test_align_vectors_primary_only  s   r  c                  C   s  t jd} t| jdd}| }g d}t||}g d}||}| }||d d df dk   d9  < ||d d df dk   d9  < t|d |d  t|d |d  t|d	 |d
  t|d |d  t|d |d  t 	|d |d
  }t 	|d
 |d  }	t||	 t 	|d |d  }
t 	|d |d	  }t|
| t 	|d |d  }t|d
|d
  d  t
t|t| d S )Nr   r  r  )r   r   rK   r   r   )r   rt   r  r   r  rK   g      @r   g      
@g@r   rH   r   r   r   rK   rX   r0   r   )r   r3   r4   r   r   r3  r   r   r   r  r   r   )r   Zkey_rotsZ	key_quatsZ	key_timesZinterpolatortimesZinterp_rotsZinterp_quatsZ
cos_theta1Z
cos_theta2Z
cos_theta4Z
cos_theta5Z
cos_theta3r   r   r   
test_slerp  s0   
  

r  c                  C   sb   t jtdd  tg dg dg} tddg}t||  W d    d S 1 s*w   Y  d S )Nzmust be a `Rotation` instancerT   rZ   rG   r   r   )r[   r\   r  r   r   r   )r!   rx  r   r   r   test_slerp_rot_is_rotationC  s   
"r  z*must be a sequence of at least 2 rotationsc                  C   sN   t g d} tjttd tdg|  W d    d S 1 s w   Y  d S )NrZ   rT   r   )r   r   r[   r\   r]   SLERP_EXCEPTION_MESSAGEr   r  r   r   r   test_slerp_single_rotL  s   "r  c                  C   sF   t  } tjttd tg |  W d    d S 1 sw   Y  d S )NrT   r   r3   r[   r\   r]   r  r   r  r   r   r   test_slerp_rot_len0R  s   "r  c                  C   sJ   t d} tjttd tdg|  W d    d S 1 sw   Y  d S )Nr   rT   r  r  r   r   r   test_slerp_rot_len1X  s   
"r  c                  C   sn   t jtdd& tjd} t| jdd}t	dgdgg}t
|| W d    d S 1 s0w   Y  d S )Nz.times to be specified in a 1 dimensional arrayrT   r   )rK   r   r  r   rK   )r[   r\   r]   r   r3   r4   r   r   r3  r   r   r   r!   rx  r   r   r   test_slerp_time_dim_mismatch^  s   "r  c                  C   sf   t jtdd" tjd} t| jdd}t	d}t
|| W d    d S 1 s,w   Y  d S )Nz7number of rotations to be equal to number of timestampsrT   r   r  r  rX   )r[   r\   r]   r   r3   r4   r   r   r3  aranger   r  r   r   r   !test_slerp_num_rotations_mismatchh  s   
"r  c                  C   d   t jtdd! tjd} t| jdd}g d}t	|| W d    d S 1 s+w   Y  d S )Nstrictly increasing orderrT   r   r  r  )r   r   rK   rK   r   
r[   r\   r]   r   r3   r4   r   r   r3  r   r  r   r   r   test_slerp_equal_timesq     "r  c                  C   r  )Nr  rT   r   r  r  )r   r   r   rK   r   r  r  r   r   r   test_slerp_decreasing_timesy  r  r  c                  C   s   t jd} t| jdd}t d}t||}tj	t
dd t dgdgg}|| W d    d S 1 s9w   Y  d S )	Nr   r  r  r   z&`times` must be at most 1-dimensional.rT   g      @g@)r   r3   r4   r   r   r3  r  r   r[   r\   r]   r   )r   r!   rx  sZinterp_timesr   r   r   !test_slerp_call_time_dim_mismatch  s   


"r  c                  C   s   t jd} t| jdd}t dd }t||}tj	t
dd |g d W d    n1 s3w   Y  tj	t
dd |g d	 W d    d S 1 sQw   Y  d S )
Nr   r  r  r   r   ztimes must be within the rangerT   r  )r   rK   rW   )r   r3   r4   r   r   r3  r  r   r[   r\   r]   )r   r!   rx  r  r   r   r   !test_slerp_call_time_out_of_range  s   
"r  c                  C   s\   t jdddgdd} tddg| }|d}t jdddd}||  }t| dd	d
 d S )NXr   P   Tr   r   r  rC  r+   rO   )r   rC   r   rQ   r   rf  )r!   r  Zr_interpolatedZr_interpolated_expecteddeltar   r   r   test_slerp_call_scalar_time  s   r  c                  C   sP   t jddd} t jddd}| D ]}||| 9 }ttjj| ddd qd S )N2   r   ra  r,  r   r   )r   r3   r   r   r9   r:   r   )qsrsr>   r   r   r   test_multiplication_stability  s   r  c                  C   sN  d} t jddd}| }dD ]W}|| }t d}tt|D ]}|dkr+|| }q || }q ||   }t|| k sAJ t 	g d}|| 
 jdksSJ t 	g dg}|| 
 jdksfJ qd	D ]}|| }t ||  }t|
 |
 | d
 qit g d}d}|| }t ||  }t|
 |
 | d
 d S )Nr0  r0   r   ra  )r  rq   rH   r   r   rK   r   rG   r  rx   )      r   g       r   rt   r  rO   )r.  r   r   r   )r   r3   rQ   re  r   rM   rf  r   r'  r   r   rg   r   r   r   )r.   rZ  Zp_invr9  r>   r!   r  angr   r   r   test_pow  s2   


r  c                  C   sL   t jdd} tjtdd t| dd W d    d S 1 sw   Y  d S )Nr   ra  zmodulus not supportedrT   r   )r   r3   r[   r\   NotImplementedErrorpow)rZ  r   r   r   test_pow_errors  s   "r  c                  C   sF  t jdd} t jddd}t| }t|jd t|}t|jd t|d  |d   t|d  |d   t| g}t|jd t|d |  t|g}t|jd t|d	  |d   t|d
  |d   tj| |gtd}t|jd t|d |  t|d | t|||g}t|jd d S )Nr   ra  rK   r   r   )rK   )r   )r   rK   r   r  )Zdtype)r   rK   )	r   r3   r   r   r   rg   r   r5   object)singleZmultipler   r   r   r    test_rotation_within_numpy_array  s*   

r  c               	   C   sX   t ddttjd ttjd g} t| }t|}t	| 
 |
 dd d S Nr   r   r*   rO   )r   r   r   sinr   r   pickledumpsloadsr   r5   )r!   ZpklZ	unpickledr   r   r   test_pickling  s   *

r  c               	   C   sN   t ddttjd ttjd g} t| }t| 	 |	 dd d S r  )
r   r   r   r  r   r   r  deepcopyr   r5   )r!   r  r   r   r   test_deepcopy  s   *
r  c                  C   sz   t g d} | d}| d}|jd du sJ |jd du s#J tdd |jD s/J tdd |jD s;J d S )	NrG   r@   ZXYZZC_CONTIGUOUSTc                 s       | ]}|d kV  qdS r   Nr   r  r   r   r   r   	<genexpr>
      z+test_as_euler_contiguous.<locals>.<genexpr>c                 s   r  r  r   r  r   r   r   r    r  )r   r   r6  flagsr'  strides)r!   e1e2r   r   r   test_as_euler_contiguous  s   

r  c                     s   t jddd g d} dgtt|  } fddt|| D }t |}t  |  t  }t  |   |usCJ t t 	 }tt 	  |  d S )Nr0   r   ra  )r   rK   r   r   r   c                    s    g | ]\}} |||  qS r   r   )r  r   r9  r;  r   r   r    s     z$test_concatenate.<locals>.<listcomp>)
r   r3   listr   Zcumsumr  rt  r   r   re  )sizesZstartssplitr   r   r  r   test_concatenate  s   

r  c                   C   sH   t jtdd tt dd g W d    d S 1 sw   Y  d S )NzRotation objects onlyrT   r   )r[   r\   r  r   rt  re  r   r   r   r   test_concatenate_wrong_type"  s   "r  c                  C   s   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d t| W d    n1 s=w   Y  | sFJ |sJJ |sNJ d S )NrG   r   rK   zSingle rotation has no len().rT   )r   r   r[   r\   r  )Zrotation_multi_oneZrotation_multiZrotation_singler   r   r   test_len_and_bool(  s   
r  c                  C   sD   g d} t j| dddd }tg dtd }t|| d S )Nr   r   r   Tr   rc   rK   )r   from_davenportr   r   r   rP   r   )r   r`   r"   r   r   r   #test_from_davenport_single_rotation8  s   
r   c                  C   s  g d} g d}t t| tj d }t | dtjd }t|jdd|jdd t t| tj d g}t | gdtjd g}t|jdd|jdd t t| tj d t|tj d g}|d |d	  }t || gdtjd tjd g}t|jdd|jdd t t| tj d t| tj d g}t | gdtjd tjd g}t|jdd|jdd d S )
Nr   r   r   eTrB   rW   r   r   )r   r   r   r   r   r  r   r   )ezeyr   Zrot_davr   r   r   #test_from_davenport_one_or_two_axes@  s6   

"
 
r$  c                  C   s.  g d} g d}g d}t jtdd t| |gdddg W d    n1 s)w   Y  t jtdd t| ||gdg d W d    n1 sLw   Y  t jtd	d t| gd
dg W d    n1 slw   Y  t jtdd t| || gdg d W d    d S 1 sw   Y  d S )Nr   r   r  zmust be orthogonalrT   r!  r   r   zorder should ber@   zExpected `angles`)r   r   rK   r   )r[   r\   r]   r   r  )r"  r#  Zezyr   r   r   !test_from_davenport_invalid_input`  s   "r%  c                  C   s,  t jd} d}t |df}| jt j t j|fd|d d df< | jdt j|fd}| jt j t j|fd|d d df< | jdt jdd}t g d}t g d}|D ]=}||t|| 	|g}|| |d d d	f< d
D ]!}	|	dkry|n|d d d }
t
|
|	|}||
|	}t|| qqqVd S )Nr   r1   r   r   rK   )rC  r
   r   r   r   r   rH   )r   r3   r4   r2  r3  r   r   r   r   r  r  as_davenportr   )r   r9  r  angles_middlelambdasr  r  lambax_lamborderaxr   
angles_davr   r   r   test_as_davenportn  s&   $$r.  c               
      sv  t jd d} t | df} jt j t j| fd|d d df<  fddt| D } jt j t j| fd|d d df<  jdt jdd}t g d	}t g d
}|D ]a}||t	|| 
|g}|| |d d df< dD ]E}|dkrz|n|d d d }	t|	||}
tjtdd |
|	|}W d    n1 sw   Y  |
 }t|	|| }t|| qrqWd S )Nr   r   r   r   c                    s   g | ]
}  d tjgqS r   )choicer   r   r  r   r   r   r    s    z0test_as_davenport_degenerate.<locals>.<listcomp>rK   )r   r
   r   r   r   r   rH   rH  rT   )r   r3   r4   r2  r3  r   r   r   r   r   r  r  r[   rJ  rK  r&  r5   r   )r9  r  r'  r(  r  r  r)  r*  r+  r,  r   r-  rM  rN  r   r1  r   test_as_davenport_degenerate  s.   $$r2  c            	      C   s  t jd} d}t |df}| jt j t j|fd|d d df< | jdt j|fd|d d df< | jt j t j|fd|d d df< dD ]B}tdD ];}d	|d |d |d g}d
d |D }|dkrj| }t	
||}t	|||}t|jdd|jdddd qKqE|d d df  t jd 8  < dD ]5}tdD ].}d	|}dd |D }|dkr| }t	
||}t	|||}t| | dd qqd S )Nr   r1   r   r   r   rK   r   r@   r-  c                 S      g | ]}t |qS r   r   r  r   r   r   r        z:test_compare_from_davenport_from_euler.<locals>.<listcomp>r   TrB   r.  r2   c                 S   r3  r   r4  r  r   r   r   r    r5  )r   r3   r4   r2  r3  r   r   r4  r5  r   rC   r  r   r   )	r   r9  r  r+  r   r:  r,  euldavr   r   r   &test_compare_from_davenport_from_euler  s<   $ $
r8  c            
      C   s  t jd} d}t |df}| jt j t j|fd|d d df< | jdt j|fd|d d df< | jt j t j|fd|d d df< dD ]>}tdD ]7}d	|d |d |d g}d
d |D }|dkrj| }t	
||}||}|||}	t||	dd qKqE|d d df  t jd 8  < dD ]5}tdD ].}d	|}dd |D }|dkr| }t	
||}||}|||}	t||	dd qqd S )Nr   r1   r   r   r   rK   r   r@   r-  c                 S   r3  r   r4  r  r   r   r   r    r5  z6test_compare_as_davenport_as_euler.<locals>.<listcomp>r   r.  r2   c                 S   r3  r   r4  r  r   r   r   r    r5  )r   r3   r4   r2  r3  r   r   r4  r5  r   rC   r6  r&  r   )
r   r9  r  r+  r   r:  r,  r   r6  r7  r   r   r   "test_compare_as_davenport_as_euler  s<   $ $


r9  c            	      C   s(  t jdd} t| dksJ t jdd}t|dksJ t jddg  }t|dks,J t td}t|dks<J t td}t|dksLJ t dtd}t|dks]J t 	td}t|dksmJ t 
tddtd}t|dksJ t td}t|dksJ d S )	Nr   rY  r   r   r   r   r   r   r@   r   r   )r   r3   r   re  r   r   rD   ru   rC   r   r  r6   r   )	r!   Zr_ider_getZr_quatZr_matrixZr_eulerZr_vecZr_davZr_mrpr   r   r   test_zero_rotation_construction  s$   r>  c                  C   s   t jd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s4J | t	
ddjdksBJ d S )	Nr   r:  r;  r<  r@   r   r   r   )r   r3   r   rg   r5   r6  r   r   r&  r   r6   r  r   r   r   !test_zero_rotation_representation  s    r?  c                  C   s   t jdd} tg d}| |}|jdksJ td}| |}|jdks*J td}tj	t
dd | | W d    d S 1 sGw   Y  d S )Nr   r:  rV   r   )rK   r   z/Expected equal numbers of rotations and vectorsrT   )r   r3   r   r   r  rg   rD   r  r[   r\   r]   )r!   r  r  Zv0Zv0_rotr  r   r   r   !test_zero_rotation_array_rotation  s   



"r@  c                  C   s   t jdd} t  }| | }t|dksJ ||  }t|dks"J t d}| | }t|dks3J d}t d}tjt|d ||  W d    n1 sPw   Y  tjt|d ||  W d    d S 1 slw   Y  d S )Nr   r:  "Expected equal number of rotationsrK   rT   )r   r3   r   r[   r\   r]   )r!   Zr_singleZr_mult_leftZr_mult_rightr0Zr_multZmsg_rotation_errorr  r   r   r   !test_zero_rotation_multiplication  s"   



"rC  c                  C   s   t jdd} t | | g}t|dksJ | t  | g}t|dks&J | t d| g}t|dks8J | | t dg}t|dksJJ d S )Nr   r:  r   r   r   )r   r3   rt  r   )r!   rB  r  rR   Zr4r   r   r   !test_zero_rotation_concatentation4  s   rD  c                  C   s2   t jdd} dD ]}| | }t|dksJ qd S )Nr   r:  )r  rH   r   r   r  )r   r3   r   )r!   ppZpow0r   r   r   test_zero_rotation_powerD  s
   rF  c                  C   s(   t jdd} |  }t|dksJ d S )Nr   r:  )r   r3   rQ   r   )r!   Zr_invr   r   r   test_zero_rotation_inverseK  s   rG  c                  C   s&   t jdd} |  }|jdksJ d S )Nr   r:  r/  )r   r3   rf  rg   )r!   rf  r   r   r   test_zero_rotation_magnitudeQ  s   rH  c                  C   sH   t jdd} tjtdd |   W d    d S 1 sw   Y  d S )Nr   r:  z+Mean of an empty rotation set is undefined.rT   )r   r3   r[   r\   r]   r$  r  r   r   r   test_zero_rotation_meanW  s   
"rI  c                  C   s   t d} | t djdksJ | t  jdksJ t  | jdks*J d}t d}tjt|d | | W d    n1 sHw   Y  tjt|d ||  W d    d S 1 sew   Y  d S )Nr   r/  rA  rK   rT   )r   r3   rp  rg   r[   r\   r]   )r!   Z
approx_msgrR   r   r   r   test_zero_rotation_approx_equal]  s   

"rJ  c                  C   s   t d} | g  }t|dksJ | d d }t|dksJ tt | dg  W d    n1 s4w   Y  tt | dg  W d    n1 sNw   Y  tt t  | d< W d    d S 1 sjw   Y  d S )Nr   Tr   r3   r   r[   r\   
IndexError)r!   r=  Zr_slicer   r   r   test_zero_rotation_get_setl  s   
"rM  c                  C   s   t d} | g d }t|dksJ | g d }t|dks!J | g d }t|dks/J tt | ddg  W d    d S 1 sFw   Y  d S )Nr   )FFFr   )FTFr   )TTTTrK  )r!   rB  r  rR   r   r   r   test_boolean_indexes  s   
"rN  )r[   numpyr   Znumpy.testingr   r   r   Zscipy.spatial.transformr   r   Zscipy.statsr   	itertoolsr   r  r  r   r#   r%   r&   r?   rE   rL   rN   rS   r^   r_   rb   ri   ro   rr   rw   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  markZparametrizer<  r>  Zthread_unsaferO  rU  rX  r_  rd  rg  rh  rl  rm  rn  ro  rq  rs  ry  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  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r   r$  r%  r.  r2  r8  r9  r>  r?  r@  rC  rD  rF  rG  rH  rI  rJ  rM  rN  r   r   r   r   <module>   sF    






A
	


		',#,
	%
 !"
