a
    ReO                     @   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 ZCdodp ZDdqdr ZEdsdt ZFdudv ZGdwdx ZHdydz ZId{d| ZJd}d~ ZKdd ZLdd ZMdd ZNdd ZOdd ZPdd ZQdd ZRdd ZSdd ZTdd 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҄ ZuddԄ Zvddք ZwdS )    N)assert_equalassert_array_almost_equal)assert_allclose)RotationSlerp)special_ortho_group)permutationsc                  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xrexpected_quat r   _/var/www/sunrise/env/lib/python3.9/site-packages/scipy/spatial/transform/tests/test_rotation.py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   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!   r!   r    r   r   r   r!   r!   r!   r!   r!   r   r   r       r   r   r   r   r   test_from_square_quat_matrix"   s    
&r%   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)r!   r   r   r   )r   r!   r   r   )r   r   r!   r   r"   r#   r    ZaxisT)	canonical)	r   r   r   r   abslinalgnormr   r   r   r   r   r   *test_quat_double_to_canonical_single_cover1   s    
&r+   c                   C   sB   t t$ ttg d W d    n1 s40    Y  d S )Nr    r$   r
   pytestraises
ValueErrorr   r   r   r   r   r   r   r   test_malformed_1d_from_quat>   s    r1   c                   C   sJ   t t, ttg dg dg W d    n1 s<0    Y  d S )N)r    r$   r
   r   r   )r   r            r-   r   r   r   r   test_malformed_2d_from_quatC   s
    r5   c                  C   sT   t g dg dg dg} tt t|  W d    n1 sF0    Y  d S )Nr	   )r   r   r   r   r   )r   r   r.   r/   r0   r   r   )r   r   r   r   test_zero_norms_from_quatK   s    r6   c                  C   s*   g d} t |  }t|td d S )Nr   r
   )r   r   	as_matrixr   r   eye)quatmatr   r   r   #test_as_matrix_single_1d_quaternionU   s    r;   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   r!   r   r    r   r   r   r   r    r   r   r   r7   r   shaper   r   r   )r9   r:   expected_matr   r   r   #test_as_matrix_single_2d_quaternion\   s    
rD   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 )Nr<   r   r    r   r    r   r"   )r   r
   r
   r>   r?   r@   r   r   r    r   r!   r   r   r    r$   r
   )	r   r   r7   r   rB   r   r   r   r8   )quatsr:   	expected0	expected1r   r   r    test_as_matrix_from_square_inputh   s*    rK   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 )Nr<   rE   r    r$   r
   r   )r
   r
   r
   r>   r?   r@   r   rF   rG   r    )皙?g@)gffffff@r    rM   )r!   r$   r$   r
   r$   rA   )rH   r:   rI   rJ   Z	expected2r   r   r   !test_as_matrix_from_generic_input   s4    rO   c                  C   s6   g dg dg dg} g d}t t|  | d S )Nr@   r?   rF         ?rQ   rQ   rQ   )r   r   from_matrixr   r:   r   r   r   r   test_from_single_2d_matrix   s    rT   c                  C   sN   t g dg dg dgd} t g dd}tt|  | d S )Nr@   r?   rF   r=   rP   r    r   )r   r   reshaper   r   rR   r   rS   r   r   r   test_from_single_3d_matrix   s    rW   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    r2   r    '   )3~˷g6iпnzo?)rZ   rY   6i?)r[   rZ   g3~˷?r=   rU   )r   r   sqrtr   r   rR   r   rV   )r   r:   r   r   r   test_from_matrix_calculation   s    r]   c                  C   s(   t jdddd} tt|  |  d S )Nr
   
   r   )sizerandom_state)r   Zrvsr   r   rR   r7   )r:   r   r   r    test_matrix_calculation_pipeline   s    ra   c                  C   sp   t jd} | d}t| }t d||d}t 	d}t
dD ]}d|d d ||f< qJt|| d S )Nr   )d   r
   r
   ...ij,...jk->...ik)r   r$   r    r
         ?)r   randomRandomStateZrandom_sampler   rR   r7   einsumZ	transposezerosranger   )rndr:   Z	ortho_matZmult_resulteye3dir   r   r   test_from_matrix_ortho_output   s    

rm   c                  C   s2   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_rotvec   s    
rt   c                  C   s6   g dg} t g dg}t| }t| | d S rn   ro   rq   r   r   r   test_from_2d_single_rotvec   s    

ru   c                  C   sJ   g dg dg dg} t g dg dg dg}tt|  | d S )Nr    r$   r$   r    r!   rQ   r   r   r   )g.pG?cU(G?ry   gQH?)gNZ]O?gNZ]OݿgNZ]O?g0i?r   )r   r   r   r   rp   r   )rr   r   r   r   r   test_from_generic_rotvec   s    rz   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?rM   rx   )r   r
   r    r   rQ   r    r
   g@w?)g>}J?g.e?g>}J?r$   r   )r   r   r\   r   rp   r   r   r   )rr   r9   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 )Nrd   r
   Tdegrees)r   Zcbrtr   rp   r   deg2radr   )Zrotvec1Zrot1Zquat1Zrotvec2Zrot2Zquat2r   r   r   test_degrees_from_rotvec  s    (

r   c                   C   s@   t jtdd tddg W d    n1 s20    Y  d S )N Expected `rot_vec` to have shapematchr    r$   r.   r/   r0   r   rp   r   r   r   r   test_malformed_1d_from_rotvec!  s    r   c                   C   sH   t jtdd& tg dg dg W d    n1 s:0    Y  d S )Nr   r   rL   r   r2   r3   r4   r   r   r   r   r   test_malformed_2d_from_rotvec&  s
    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    r$   r!   rQ   r    r!   r    ga2U0*3?r   r    r&   r
   r$   )r
   r
   )r   r   r)   r*   r   r   	as_rotvecr   cosZcrossrh   )r9   rr   angler   r   r   test_as_generic_rotvec.  s      r   c                  C   sD   t g d} t g d}t|  }t|jd t|| d S )Nr    r$   r$   gj*0	x?g$x?g:2(r
   r   r   r   r   r   r   rB   r   r9   Zexpected_rotvecZactual_rotvecr   r   r   test_as_rotvec_single_1d_input=  s
    r   c                  C   sH   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_inputG  s
    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?   rF   Tr~   g      ^@r   r    r$   )r   rR   r   r   r)   r*   r   )r:   rotrr   r   r   r   r   test_as_rotvec_degreesQ  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 )Nrx   r    r!   r$   )ga2U0*3gǺ6?ga2U0*?Tr~   )r   r   r   r   rp   r   )rr   r   r   r   test_rotvec_calc_pipeline\  s    r   c                  C   s2   g d} t g d}t| }t| | d S N)r   r   rd   )r   r   r    r   r   r   r   from_mrpr   r   mrpr   rs   r   r   r   test_from_1d_single_mrpg  s    
r   c                  C   s6   g dg} t g dg}t| }t| | d S r   r   r   r   r   r   test_from_2d_single_mrpn  s    

r   c                  C   sP   t g dg dg dg} t g dg dg dg}tt|  | d S )Nrv   rw   rx   )r{   rM   rM   g)g}+;?g}+;g}+;?gߨӉؿr   )r   r   r   r   r   r   )r   r   r   r   r   test_from_generic_mrpu  s    r   c                   C   s@   t jtdd tddg W d    n1 s20    Y  d S )NExpected `mrp` to have shaper   r    r$   r.   r/   r0   r   r   r   r   r   r   test_malformed_1d_from_mrp  s    r   c                   C   sH   t jtdd& tg dg dg W d    n1 s:0    Y  d S )Nr   r   rL   r   r   r   r   r   r   test_malformed_2d_from_mrp  s
    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   r   r    r&   )gQUU?gvWUU?QUUտ)uFx?guFxr   rx   )r   r   r)   r*   r   r   r   as_mrp)r9   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 )	Nr$   r   g        r   xyz)i  r   r   Tr~   )r   r   tanpir   r   
from_eulerr   )r   r   r   r   test_past_180_degree_rotation  s    "r   c                  C   sD   t g d} t g d}t|  }t|jd t|| d S )Nr   g?g?gO޿r   r   r   r   r   r   r   rB   r   r9   r   
actual_mrpr   r   r   test_as_mrp_single_1d_input  s
    r   c                  C   sH   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_mrp_single_2d_input  s
    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 )Nrx   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 )NzZ   Tr~   r<   r$   )r   r   r   r   r   r\   r   )r9   r   r   r   r   test_from_euler_single_rotation  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   r   r7   r   )Z	extrinsicZ	intrinsicr   r   r   (test_single_intrinsic_extrinsic_rotation  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      )r2   r
   lowhighr_   r!   r   Tr~   ZZYX)r   re   rf   randintr   r   r   r   )rj   abr   yr   r   r   test_from_euler_rotation_order  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~   r>   r   r   r!   r?   )r   r   r7   r   r   r   )r:   rC   r   r   r   -test_from_euler_elementary_extrinsic_rotation  s    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 )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   r$   )ga}@?gF6g^@?)g0	8?gF6?g4?)      rQ   r   r   r   r7   r   r   r   anglesr:   r   r   r   &test_from_euler_intrinsic_rotation_312  s(    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   )>d?g @r   )l>?^|@r   )X?r   rQ   r    g      ?g ,r   g ,?g      ?r   r         ?rQ   r$   )r   g4&d'e?)cH?Xӿg'eֿ)r         ?r   r   r   r   r   r   &test_from_euler_intrinsic_rotation_313   s(    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   r$   )r   r   r   )r   r   g      )g>dۿ @?r   r   r   r   r   r   &test_from_euler_extrinsic_rotation_312  s(    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   r$   )r   gcHr   )r   r   r   )r   r   r   r   r   r   r   r   &test_from_euler_extrinsic_rotation_3136  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t j d t jd |fd|d d df< |jt j t j|fd|d d df< td	D ]}d
|}t	||}|
|}||}t||ddd t||ddd | || dd | || dd | }t	||}|
|}||}t||ddd t||ddd | || dd | || dd qd S )Nc                 S   sV   t j| dd}t j| dd}t ||}t t ||k s@J t ||k sRJ d S Nr   r&   r   meanstdhypotallr(   errorZmean_maxZrms_maxr   r   Zrmsr   r   r   
test_statsS  s
    z1test_as_euler_asymmetric_axes.<locals>.test_statsr     r
   r   r$   r    r    -q=atolZrtolV瞯<+=r   re   rf   emptyuniformr   r   joinr   r   as_euler_as_euler_from_matrixr   upper	r   rj   nr   	seq_tupleseqrotationZangles_quatZ
angles_matr   r   r   test_as_euler_asymmetric_axesQ  s0    $,$




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< td	D ]}d
|d |d |d g}t	||}|
|}||}t||ddd t||ddd | || dd | || dd | }t	||}|
|}||}t||ddd t||ddd | || dd | || dd qd S )Nc                 S   sV   t j| dd}t j| dd}t ||}t t ||k s@J t ||k sRJ d S r   r   r   r   r   r   r   y  s
    z0test_as_euler_symmetric_axes.<locals>.test_statsr   r   r
   r   r    r$   r   r   gvIh%<=r   g&.>缉ؗҜ<r   r   r   r  r   r   r   test_as_euler_symmetric_axesw  s0    $ $



r	  c                  C   s   t g dg dg dg dg} tjtdd tdD ]}d|}tj|| d	d
}|	 }|j
|d	d
}tj||d	d
	 }t|| | }tj|| d	d
}|	 }|j
|d	d
}tj||d	d
	 }t|| q:W d    n1 s0    Y  d S )Nr   r   #   r     r  r      r  r     Gimbal lockr   r   r   Tr~   r   r   r.   warnsUserWarningr   r   r   r   r7   r   r   r  r   r  r  r  Zmat_expectedZangle_estimatesZmat_estimatedr   r   r   (test_as_euler_degenerate_asymmetric_axes  s0    



r  c                  C   s  t g dg dg dg dg} tjtdd tdD ]}d|d	 |d
 |d	 g}tj|| dd}|	 }|j
|dd}tj||dd	 }t|| | }tj|| dd}|	 }|j
|dd}tj||dd	 }t|| q:W d    n1 s0    Y  d S )Nr  r   r   r  r   K   r   r   r  r  iLr  r  r   r   r   r   r    Tr~   r  r  r   r   r   'test_as_euler_degenerate_symmetric_axes  s0    


r  c                  C   s\  t g dg dg dg dg} tjtdd: tdD ]}d|}tj|| d	d
}|j	|d	d
}|j
|d	d
}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j|| d	d
}|j	|d	d
}|j
|d	d
}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 q<W d    n1 st0    Y  t g dg dg dg dg} | d d df dk}tjtdd tdD ]f}d|d |d |d g}tj|| d	d
}|j	|d	d
}|j
|d	d
}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 | }tj|| d	d
}|j	|d	d
}|j
|d	d
}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 qW d    n1 sN0    Y  d S )Nr
  r  r  r  r  r   r   r   Tr~   r   r$   r   r   r    Hz>r  r  r  r  ư>r   )r   r   r.   r  r  r   r   r   r   r   r   r   r  )r   r  r  r   Zestimates_matrixZestimates_quatidxr   r   r   +test_as_euler_degenerate_compare_algorithms  s~    
(r#  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   r^   numr`   rc   r
   )
r   re   rf   r   invr7   rg   r   r8   r   )	rj   r  pqp_matq_matresult1result2rk   r   r   r   test_inv1  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 )Nr   r`   r
   r    r$  rc   r=   )r   re   rf   r   r&  r7   dotr8   r   rg   r   )rj   r'  r(  r)  r*  Zres1Zres2r8   r   r   Zx_matrixZy_matrixr+  r,  rk   r   r   r   test_inv_single_rotationC  s(    




r0  c                  C   s4   d} t t|  d t t|   d d S )Nr^   r   )r   r   identity	magnituder&  )r  r   r   r   test_identity_magnitudea  s    r3  c                   C   s0   t   dksJ t    dks,J d S )Nr   )r   r1  r2  r&  r   r   r   r   test_single_identity_magnitudeg  s    r4  c                  C   sV   d} t j| dd}|t |  }t| |  ||  }t| t|  d S Nr^   r   r.  	r   re   r1  r   r   r&  r2  r   rh   r  r'  rs   r   r   r   test_identity_invariancel  s    r8  c                  C   sT   d} t j| dd}|t   }t| |  ||  }t| t|  d S r5  r6  r7  r   r   r   test_single_identity_invariancew  s    r9  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   r8   r2  r   r   )r   rs   r   r   r   test_magnitude  s    r:  c                  C   sB   t td} | d  }t|tj | d  }t|d d S )Nr   r   r
   )r   r   r   r8   r2  r   r   )r   r+  r,  r   r   r   test_magnitude_single_rotation  s
    r;  c                  C   sb   t t d t df} t dt jd d}|D ](}t||  }t| 	 ddd q4d S )Nr
   r   r$   rb   绽|=r!  )
r   concatenater8   linspacer   r   rp   r   r   r2  )axesthetastr   r   r   r   	test_mean  s
    rB  c                  C   s   t g dg dg dg} t dt jd d}|D ]Z}t|| d d  }|jddgd}t||  }| }t||  	 ddd	 q4d S )
Nrx   r?   r   r$   rb   r    weightsr<  r!  )
r   r   r>  r   r   rp   r   r   r&  r2  )r?  r@  rA  rwmwr   mr   r   r   test_weighted_mean  s    rH  c                  C   sV   t jtdd4 ttd} | jtd d W d    n1 sH0    Y  d S )Nznon-negativer   r   rC  )	r.   r/   r0   r   r   r   r8   r   onesr   r   r   r   test_mean_invalid_weights  s    rK  c                  C   s"   t  jdd} t| t sJ d S )NFZreturn_indices)r   r1  reduce
isinstance)rs   r   r   r   test_reduction_no_indices  s    rO  c                  C   sV   t  jdd} t| tks J t| dks0J | \}}}|d u sFJ |d u sRJ d S )NTrL  r
   )r   r1  rM  typetuplelen)rs   reduced	left_best
right_bestr   r   r   test_reduction_none_indices  s    
rV  c                  C   sx  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 ]P\}}	t|D ]>\}
}t|D ],\}}t |	| | 	 d |||
|f< qqqpt 
t |d	d|jd	 d
f}t jt 
|t|d
fd	d}|t| }|t| }||k s*J ||k s<J || | ||  }| |  }t|t t| d S )Nr   r   r.  r^   r3   TrL  r
   r    r!   r&   )r   re   rf   r   rM  rh   rR  	enumerater(   r   rV   ZmoveaxisrB   Zargmaxr   r&  r2  r   )rnglr   r'  rS  rT  rU  Zscalarsrl   lijZpjkZrkZmax_indZleft_best_checkZright_best_checkZreduced_checkZ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 )Nr>   r?   r@   r   r&   r,   rN   r    r
   r$   r!   r
   TZinverser   r   r   rR   expand_dimsr   apply)	r:   Zr_1dZr_2dZv_1dZv_2dZv1d_rotatedZv2d_rotatedZv1d_inverseZv2d_inverser   r   r   'test_apply_single_rotation_single_point  s*    
rd  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 )Nr>   r?   r@   r   r&   r,   r   r   r2   r^  )r   r2   r_  )r   r2   Tr`  ra  )r:   r1r2v	v_rotated	v_inverser   r   r   *test_apply_single_rotation_multiple_points  s    
rm  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r$   r
   r
   r>   r?   r@   r   r   rF   r    r,   r&   r^  )r    r   r$   r_  )r    r
   rN   Tr`  )r   r   r   r   rR   rb  r   rc  )r:   r   Zv1Zv2rk  rl  r   r   r   *test_apply_multiple_rotations_single_point
  s(    



ro  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 )Nrn  r>   r?   r@   r   r   rF   r    r,   re  r^  )r   ir   r_  )r   r2   rf  Tr`  )r   r   r   r   rR   r   rc  )r:   r   rj  rk  rl  r   r   r   -test_apply_multiple_rotations_multiple_points&  s"    



rp  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 )Nrn  r>   r?   r@   r   r   rF   r    r   r!  r!   r&   )r   r   r   r   rR   r   r7   rb  r:   r   r   r   r   test_getitem<  s    



rr  c                   C   s>   t jtdd t d  W d    n1 s00    Y  d S Nznot subscriptabler   r   )r.   r/   	TypeErrorr   r1  r   r   r   r   test_getitem_singleO  s    ru  c                  C   sF   t  } tjtdd t  | d< W d    n1 s80    Y  d S rs  )r   r1  r.   r/   rt  rJ  r   r   r   test_setitem_singleT  s    rv  c                  C   sT   t jjdd} tjd| d}tjd| d}||dd< t|dd  |  d S )Nr   seedr^   r.  r   r    r2   r   re   rf   r   r   r   rX  rh  ri  r   r   r   test_setitem_sliceZ  s
    r{  c                  C   sJ   t jjdd} tjd| d}tj| d}||d< t|d  |  d S )Nr   rw  r^   r.  r    ry  rz  r   r   r   test_setitem_integerb  s
    r|  c                  C   sH   t jddd} tjtdd d| d< W d    n1 s:0    Y  d S )Nr^   r   r.  zRotation objectr   r    )r   re   r.   r/   rt  rJ  r   r   r   test_setitem_wrong_typej  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 )Nrn  r>   r?   r@   r   r   rF   r    r$   r!   )r   r   r   r   rR   r   rR  rq  r   r   r   test_n_rotationsp  s    



r~  c                  C   sT   t g dg dg} |  }t| |\}}t| t d t|ddd d S )Nr,   re  r
   r   r   r!  )	r   r   copyr   align_vectorsr   r7   r8   r   )r   r   r   rmsdr   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 )Nr   r.  r   r
   r_   r  r!  )	r   re   rf   r   normalrc  r  r   r   )rj   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   r!  r   r  )r   r   r   r  r   rc  )r   r   r  r  r   r   r   $test_align_vectors_improper_rotation  s    

r  c            
      C   s   t jd} tj| d}| jdd}||}t||t dd\}}}t||dt d d\}}}	t|	 |	  tt 
d| | t||	 d S )Nr   r.  r  r  r   Tr$   )r   re   rf   r   r  rc  r  rI  r   r7   r\   )
rX  r  r   r   Zest1Zrmsd1Zcov1Zest2Zrmsd2Zcov2r   r   r   !test_align_vectors_scaled_weights  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   rb   r.  r
   r  r          ?)r_   ZscaleT)Zreturn_sensitivityr!  r$   r   r   )r    r    )r$   r$   rQ   )r   re   rf   r   r  rc  r   rp   r  r&  r   r   sum)rj   Z	n_vectorsr   Zvectorsrs   sigmaZ	toleranceZnoiseZnoisy_resultr  r  ZcovZerror_vectorr   r   r   test_align_vectors_noise  s4    

	
r  c                  C   s\   t jtdd: tg dgg dg\} }t|ddd W d    n1 sN0    Y  d S )NzOptimal rotation is notr   )r    r!   r    )r    r    r!   r   r  r!  )r.   r  r  r   r  r   )Z
r_estimater  r   r   r    test_align_vectors_single_vector  s    r  c                   C   s  t jtdd& tg dg dg W d    n1 s:0    Y  t jtdd& tg dgg d W d    n1 s~0    Y  t jtdd. tg dg dgg dg W d    n1 s0    Y  t jtdd0 tjg dgg dgdggd	 W d    n1 s0    Y  t jtd
d0 tjg dgg dgddgd	 W d    n1 sj0    Y  t jtdd. tjg dgg dgdgd	 W d    n1 s0    Y  d S )Nz Expected input `a` to have shaper   r,   z Expected input `b` to have shapez/Expected inputs `a` and `b` to have same shapesre  z&Expected `weights` to be 1 dimensionalr    rC  z+Expected `weights` to have number of valuesr$   z)`weights` may not contain negative valuesr!   )r.   r/   r0   r   r  r   r   r   r    test_align_vectors_invalid_input  s$    44<@@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   r.  )r   r    rU   r   r   r   )r   re   rf   r   r   r   rB   )rj   r   r   r   test_random_rotation_shape  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    r$   r
   r   )r   rQ   r   r    r  r$   g      @r
   g      
@g@r   r!   r
   r    r   r$   r3   r^   r   )r   re   rf   r   r   r   r   r   r   r  r   rR  )rj   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   s`   t jtdd> tg dg dg} tddg}t||  W d    n1 sR0    Y  d S )Nzmust be a `Rotation` instancer   rL   r   r   r    )r.   r/   rt  r   r   r   )r   rA  r   r   r   test_slerp_rot_is_rotation*  s    
r  c                  C   sP   d} t jt| d* tg d}tdg| W d    n1 sB0    Y  d S Nz*must be a sequence of at least 2 rotationsr   rL   r    r.   r/   r0   r   r   r   msgr   r   r   r   test_slerp_single_rot2  s    r  c                  C   sR   d} t jt| d, tg dg}tdg| W d    n1 sD0    Y  d S r  r  r  r   r   r   test_slerp_rot_len19  s    r  c                  C   sl   t jtddJ tjd} t| jdd}t	dgdgg}t
|| W d    n1 s^0    Y  d S )Nz.times to be specified in a 1 dimensional arrayr   r   )r$   r   r  r    r$   )r.   r/   r0   r   re   rf   r   r   r   r   r   rj   r   rA  r   r   r   test_slerp_time_dim_mismatch@  s    r  c                  C   sd   t jtddB tjd} t| jdd}t	d}t
|| W d    n1 sV0    Y  d S )Nz7number of rotations to be equal to number of timestampsr   r   r  r  r3   )r.   r/   r0   r   re   rf   r   r   r   aranger   r  r   r   r   !test_slerp_num_rotations_mismatchJ  s
    
r  c                  C   sb   t jtdd@ tjd} t| jdd}g d}t	|| W d    n1 sT0    Y  d S )Nstrictly increasing orderr   r   r  r  )r   r    r$   r$   r   
r.   r/   r0   r   re   rf   r   r   r   r   r  r   r   r   test_slerp_equal_timesS  s
    r  c                  C   sb   t jtdd@ tjd} t| jdd}g d}t	|| W d    n1 sT0    Y  d S )Nr  r   r   r  r  )r   r    r
   r$   r   r  r  r   r   r   test_slerp_decreasing_times[  s
    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    n1 sp0    Y  d S )	Nr   r  r  r   z&`times` must be at most 1-dimensional.r   g      @g@)r   re   rf   r   r   r   r  r   r.   r/   r0   r   )rj   r   rA  sZinterp_timesr   r   r   !test_slerp_call_time_dim_mismatchc  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 sf0    Y  tj	t
dd |g d	 W d    n1 s0    Y  d S )
Nr   r  r  r   r    ztimes must be within the ranger   )r   r    r$   )r    r$   r2   )r   re   rf   r   r   r   r  r   r.   r/   r0   )rj   r   rA  r  r   r   r   !test_slerp_call_time_out_of_rangep  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   r  r  r!  )r   r   r   r&  r   r2  )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   r.  r   r    r&   )r   re   r   r   r)   r*   r   )qsrsr(  r   r   r   test_multiplication_stability  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   r.  r$   r    r   )r$   )r    )r    r$   r  )r   r    )Zdtype)r
   r$   )	r   re   r   r   r   rB   r   r7   object)single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   r!  )r   r   r   sinr   r   pickledumpsloadsr   r7   )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   r7   )r   rh  r   r   r   test_deepcopy  s    *
r  c                  C   sz   t g d} | d}| d}|jd du s4J |jd du sFJ tdd |jD s^J tdd |jD svJ d S )	Nr   r   ZXYZZC_CONTIGUOUSTc                 s   s   | ]}|d kV  qdS r   Nr   .0rl   r   r   r   	<genexpr>      z+test_as_euler_contiguous.<locals>.<genexpr>c                 s   s   | ]}|d kV  qdS r  r   r  r   r   r   r    r  )r   r   r   flagsr   strides)r   e1e2r   r   r   test_as_euler_contiguous  s    

r  c                     sb   t jddd g d} dgtt|  } fddt|| D }t |}t  |  d S )Nr^   r   r.  )r    r$   r
   r    r
   c                    s    g | ]\}} |||  qS r   r   )r  rl   r  r  r   r   
<listcomp>  r  z$test_concatenate.<locals>.<listcomp>)	r   re   listr   Zcumsumzipr=  r   r   )sizesZstartssplitrs   r   r  r   test_concatenate  s    
r  c                   C   sF   t jtdd$ tt dd g W d    n1 s80    Y  d S )NzRotation objects onlyr   r    )r.   r/   rt  r   r=  r1  r   r   r   r   test_concatenate_wrong_type  s    r  c                  C   s   t td} t g dg}t g dg dg}t g d}t| dksLJ t|dks\J t|dkslJ tjtdd t| W d    n1 s0    Y  | sJ |sJ |sJ |sJ d S )N)r   r   r   r   r    r$   zSingle rotation has no len().r   )r   r   r   rR  r.   r/   rt  )Zrotation_multi_emptyZrotation_multi_oneZrotation_multiZrotation_singler   r   r   test_len_and_bool  s    &r  )xr.   numpyr   Znumpy.testingr   r   r   Zscipy.spatial.transformr   r   Zscipy.statsr   	itertoolsr   r  r  r   r   r   r%   r+   r1   r5   r6   r;   rD   rK   rO   rT   rW   r]   ra   rm   rt   ru   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-  r0  r3  r4  r8  r9  r:  r;  rB  rH  rK  rO  rV  r]  rd  rm  ro  rp  rr  ru  rv  r{  r|  r}  r~  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r   r   r   r   <module>   s   





&&$$L
	
	'	,
	
	