U
    ?h                     @   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΄ ZsdS )    N)assert_equalassert_array_almost_equal)assert_allclose)RotationSlerp)special_ortho_group)permutationsc                  C   sP   t ddddgddddgg} t| }| t dgdgg }t| | d S )N      r            nparrayr   	from_quatr   as_quatxrexpected_quat r   ]/var/www/html/venv/lib/python3.8/site-packages/scipy/spatial/transform/tests/test_rotation.pytest_generic_quat_matrix   s    
r   c                  C   s6   t ddddg} t| }| d }t| | d S Nr	   r
   r   r   r   r   r   r   r   test_from_single_1d_quaternion   s    
r   c                  C   s8   t ddddgg} t| }| d }t| | d S r   r   r   r   r   r   test_from_single_2d_quaternion   s    
r   c               	   C   sl   t ddddgddddgddddgddddgg} t| }| t dgdgdgdgg }t| | d S )	Nr	   r   r
   r   r      r   r   r   r   r   r   test_from_square_quat_matrix"   s    




r   c                	   C   s0   t t ttdddg W 5 Q R X d S )Nr      r	   pytestraises
ValueErrorr   r   r   r   r   r   r   r   test_malformed_1d_from_quat/   s    r%   c                   C   sB   t t. ttdddddgdddddgg W 5 Q R X d S )	Nr   r    r	   r
   r            r!   r   r   r   r   test_malformed_2d_from_quat4   s
    r)   c               	   C   sL   t ddddgddddgddddgg} tt t|  W 5 Q R X d S )Nr	   r
   r   r   r   )r   r   r"   r#   r$   r   r   )r   r   r   r   test_zero_norms_from_quat<   s    


r*   c                  C   s.   ddddg} t |  }t|td d S )Nr   r   r	   )r   r   	as_matrixr   r   eye)quatmatr   r   r   #test_as_matrix_single_1d_quaternionF   s    r/   c                  C   s\   ddddgg} t |  }t|jd tdddgdddgdddgg}t|d | d S )Nr   r   r   r	   r	   r   r   r   r+   r   shaper   r   r   )r-   r.   expected_matr   r   r   #test_as_matrix_single_2d_quaternionM   s    r4   c                  C   s   ddddgddddgddddgddddgg} t |  }t|jd tdddgdddgdddgg}t|d | tdddgdddgdddg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   r2   r   r   r   r,   )quatsr.   	expected0	expected1r   r   r    test_as_matrix_from_square_inputY   s*    



r8   c                  C   s   ddddgddddgddddgg} t |  }t|jd tdddgdddgdddgg}t|d | tdddgdddgdddgg}t|d | tdd	d
gdddgdddggd }t|d | d S )Nr   r   r    r	   r
   )r	   r	   r	   r   皙?g@gffffff@r1   )r5   r.   r6   r7   Z	expected2r   r   r   !test_as_matrix_from_generic_inputu   s4    


r;   c                  C   s@   dddgdddgdddgg} ddddg}t t|  | d S )Nr   r         ?)r   r   from_matrixr   r.   r   r   r   r   test_from_single_2d_matrix   s    r?   c                  C   sX   t dddgdddgdddggd} t ddddgd}tt|  | d S )Nr   r   r0   r<   r   r
   )r   r   reshaper   r   r=   r   r>   r   r   r   test_from_single_3d_matrix   s    rB   c                  C   sv   t ddddgt d } t dddgdddgdddgg}tt| |  tt|d	 | d
 d S )Nr   r&   '   g3~˷g6iпgnzo?g6i?g3~˷?r0   r@   )r   r   sqrtr   r   r=   r   rA   )r   r.   r   r   r   test_from_matrix_calculation   s    rE   c                  C   s(   t jdddd} tt|  |  d S )Nr	   
   r   )sizerandom_state)r   Zrvsr   r   r=   r+   )r.   r   r   r    test_matrix_calculation_pipeline   s    rI   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   r=   r+   einsumZ	transposezerosranger   )rndr.   Z	ortho_matZmult_resulteye3dir   r   r   test_from_matrix_ortho_output   s    

rU   c                  C   s8   dddg} t ddddg}t| }t| | d S Nr   r   gJ?g<(?r   r   r   from_rotvecr   r   rotvecr   resultr   r   r   test_from_1d_single_rotvec   s    

r\   c                  C   s<   dddgg} t ddddgg}t| }t| | d S rV   rW   rY   r   r   r   test_from_2d_single_rotvec   s    
r]   c                  C   s\   dddgdddgdddgg} t ddddgd	d
ddgddddgg}tt|  | d S )Nr   r    r   r<   r   g.pG?gcU(G?gQH?gNZ]O?gNZ]OݿgNZ]O?g0i?)r   r   r   r   rX   r   )rZ   r   r   r   r   test_from_generic_rotvec   s    


r^   c                  C   s   t dt d dt d dt d gdddgdddgg} t|  }t|d d	 t|dd df | d d
  t|d d t|d	d df t dddg t|d t dddd	g d S )NgMb@?r	   gMb@皙?g333333?r9   r   )r   r	   r   r<   r   r	   g@w?g>}J?g.e?g>}J?r    )r   r   rD   r   rX   r   r   r   )rZ   r-   r   r   r   test_from_rotvec_small_angle   s$    &ra   c                  C   sh   dt d dt d dt d g} tj| dd}| }t | }t|}| }t|| d S )NrL   r	   Tdegrees)r   Zcbrtr   rX   r   deg2radr   )Zrotvec1Zrot1Zquat1Zrotvec2Zrot2Zquat2r   r   r   test_degrees_from_rotvec  s    (

re   c                	   C   s,   t jtdd tddg W 5 Q R X d S )N Expected `rot_vec` to have shapematchr   r    r"   r#   r$   r   rX   r   r   r   r   test_malformed_1d_from_rotvec  s    rj   c                	   C   s<   t jtdd$ tddddgddd	d
gg W 5 Q R X d S )Nrf   rg   r   r    r	   r
   r   r&   r'   r(   ri   r   r   r   r   test_malformed_2d_from_rotvec  s
    

rk   c                  C   s   t ddddgddddgddddg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 )
Nr   r    r   r<   a2U0*3?r   Zaxisr	   )r	   r	   )r   r   linalgnormr   r   	as_rotvecr   coscrossrP   )r-   rZ   angler   r   r   test_as_generic_rotvec  s    


  rt   c                  C   sJ   t ddddg} t dddg}t|  }t|jd t|| d S )Nr   r    j*0	x?$x?:2(r	   r   r   r   r   rp   r   r2   r   r-   Zexpected_rotvecZactual_rotvecr   r   r   test_as_rotvec_single_1d_input.  s
    r|   c                  C   sN   t ddddgg} t dddgg}t|  }t|jd t|| d S )Nr   r    ru   rv   rw   rx   r`   rz   r{   r   r   r   test_as_rotvec_single_2d_input8  s
    r}   c                  C   sp   dddgdddgdddgg} t | }|jdd}tj|}t|d t|d |d  t|d |d  d S )Nr   r   Trb   g      ^@r    )r   r=   rp   r   rn   ro   r   )r.   rotrZ   rs   r   r   r   test_as_rotvec_degreesB  s    

r   c                  C   sV   t dddgdddgdddgg} tt|  |  ttj| dd	jdd	|  d S )
Nr   r   r   r    ga2U0*3gǺ6?ga2U0*?Trb   )r   r   r   r   rX   rp   )rZ   r   r   r   test_rotvec_calc_pipelineM  s    r   c                  C   s8   dddg} t ddddg}t| }t| | d S Nr   rL   r   r   r   r   from_mrpr   r   mrpr   r[   r   r   r   test_from_1d_single_mrpX  s    

r   c                  C   s<   dddgg} t ddddgg}t| }t| | d S r   r   r   r   r   r   test_from_2d_single_mrp_  s    
r   c                  C   sb   t dddgdddgdddgg} t ddddgd	d
ddgddddgg}tt|  | d S )Nr   r    r   r<   r   r_   r9   gg}+;?g}+;g}+;?gߨӉؿ)r   r   r   r   r   r   )r   r   r   r   r   test_from_generic_mrpf  s    


r   c                	   C   s,   t jtdd tddg W 5 Q R X d S )NExpected `mrp` to have shaperg   r   r    r"   r#   r$   r   r   r   r   r   r   test_malformed_1d_from_mrpr  s    r   c                	   C   s<   t jtdd$ tddddgddd	d
gg W 5 Q R X d S )Nr   rg   r   r    r	   r
   r   r&   r'   r(   r   r   r   r   r   test_malformed_2d_from_mrpw  s
    

r   c                  C   s   t ddddgddddgddddgg} | t jj| ddd d d f  } t dd	d
gdddgdddgg}tt|  | d S )Nr   r    r   r<   rl   r   rm   gQUU?gvWUU?QUUտguFx?guFx)r   r   rn   ro   r   r   r   as_mrp)r-   expected_mrpr   r   r   test_as_generic_mrp  s    


 r   c                  C   sF   t t t jd d  ddg} ttjddddgdd |  d S )	Nr    r
   g        r   xyzi  Trb   )r   r   tanpir   r   
from_eulerr   )r   r   r   r   test_past_180_degree_rotation  s    "r   c                  C   sJ   t ddddg} t dddg}t|  }t|jd t|| d S )Nr   r    ru   ??O޿ry   r   r   r   r   r   r   r2   r   r-   r   
actual_mrpr   r   r   test_as_mrp_single_1d_input  s
    r   c                  C   sN   t ddddgg} t dddgg}t|  }t|jd t|| d S )Nr   r    ru   r   r   r   r`   r   r   r   r   r   test_as_mrp_single_2d_input  s
    r   c                  C   sl   t dddgdddgdddgdddgg} t dddgdd	d
gdddgdddgg}tt|  | d S )Nr   r   r   r    gzpy?g?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ddddgtd }t| | d S )NzZ   Trb   r   r   r    )r   r   r   r   r   rD   r   )r-   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   Trb   Z)r   r   r+   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      )r&   r	   lowhighrG   r   r   Trb   ZZYX)r   rM   rN   randintr   r   r   r   )rR   abr   yr   r   r   test_from_euler_rotation_order  s    r   c                  C   sH   t jdddgdd } tdddgdddgdddgg}t| | d S )NZzxr   Trb   r   r   r   )r   r   r+   r   r   r   )r.   r3   r   r   r   -test_from_euler_elementary_extrinsic_rotation  s    r   c               	   C   s   dddgdddgdddgg} t jd| dd }t|d tdd	d
gdddgdddgg t|d tdd	dgdddgd	ddgg t|d tdddgdddgdddgg d S )N   <   -   ZZXYTrb   r   g# ?      пgΙ6d?g$+H?+Pz?PpƿgPpֿ+Pz?gPp?r   g+a?g(s=?g&(ٿr    ga}@?gF6g^@?g0	8?gF6?g4?      r<   r   r   r+   r   r   r   anglesr.   r   r   r   &test_from_euler_intrinsic_rotation_312  s(    r   c               	   C   s   dddgdddgdddgg} t jd| dd }t|d tdd	d
gdddgdddgg t|d tddd
gdddgd
ddgg t|d tdddgdddgd
ddgg d S )Nr   r   r   ZZXZTrb   r   >d?g @r   l>?^|@r   X?r<   r         ? , ,?      ?      ?r    r   g4&d'e?cH?Xӿg'eֿ      ?r   r   r   r   r   r   &test_from_euler_intrinsic_rotation_313  s(    r   c               	   C   s   dddgdddgdddgg} t jd| dd }t|d tdd	d
gdddgddd
gg t|d tdddgdddgdddgg t|d tdddgdddgdddgg d S )Nr   r   r   ZzxyTrb   r   4&d?gPp?r   r   r   g+Pzr   r   r   g
?g\2׳g ,?r    r   r   r   r   g      g>dۿ @?r   r   r   r   r   &test_from_euler_extrinsic_rotation_312  s(    r   c               	   C   s   dddgdddgdddgg} t jd| dd }t|d tdd	d
gdddgdddgg t|d tdddgdddgdddgg t|d tdddgdddgdddgg d S )Nr   r   r   ZzxzTrb   r   r   gl>r   r   r   gXr   r   r<   r   r   r   r   r   r   r    r   gcHr   r   r   r   r   r   r   r   r   r   &test_from_euler_extrinsic_rotation_313'  s(    r   c                  C   s   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 ]F}d|}t|t	
||| | }t|t	
||| qd S )	Nr   rF   r	   r   r    r   r    )r   rM   rN   emptyuniformr   r   joinr   r   r   as_eulerupper)rR   nr   	seq_tupleseqr   r   r   test_as_euler_asymmetric_axesB  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|d d df< | jt j t j|fd|d d df< dD ]\}dD ]R}||krq|| | }t|t||	| |
 }t|t||	| qqd S )Nr   rF   r	   r   r   r    r   r   r   )r   rM   rN   r   r   r   r   r   r   r   r   )rR   r   r   axis1axis2r   r   r   r   test_as_euler_symmetric_axesS  s(    $ $  r   c               	   C   s   t dddgdddgdddgdddg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|| qBW 5 Q R X d S )Nr   r   #   i         Gimbal lockrg   r   r   Trb   )r   r   r"   warnsUserWarningr   r   r   r   r+   r   r   r   )r   r   r   rotationmat_expectedangle_estimatesmat_estimatedr   r   r   (test_as_euler_degenerate_asymmetric_axesi  s8    
  

  
r   c               	   C   s  t dddgdddgdddgdddgg} tjtd	d
 dD ]}dD ]}||krTqF|| | }tj|| dd}| }|j|dd}tj||dd }t	|| |
 }tj|| dd}| }|j|dd}tj||dd }t	|| qFq>W 5 Q R X d S )Nr   r   r   r   K   r   iLr   r   rg   r   Trb   )r   r   r"   r   r   r   r   r+   r   r   r   )r   r   r   r   r   r   r   r   r   r   r   'test_as_euler_degenerate_symmetric_axes  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   rF   numrH   rK   r	   )
r   rM   rN   r   invr+   rO   r   r,   r   )	rR   r   pqp_matq_matresult1result2rS   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 )Nr   rH   r	   r   r   rK   r0   )r   rM   rN   r   r   r+   dotr,   r   rO   r   )rR   r   r   r   r   Zres1Zres2r,   r   r   Zx_matrixZy_matrixr   r   rS   r   r   r   test_inv_single_rotation  s(    




r   c                  C   s4   d} t t|  d t t|   d d S )NrF   r   )r   r   identity	magnituder   )r   r   r   r   test_identity_magnitude  s    r   c                   C   s0   t   dkstt    dks,td S )Nr   )r   r   r   AssertionErrorr   r   r   r   r   test_single_identity_magnitude  s    r   c                  C   sV   d} t j| dd}|t |  }t| |  ||  }t| t|  d S NrF   r   r   	r   rM   r   r   r   r   r   r   rP   r   r   r[   r   r   r   test_identity_invariance  s    r   c                  C   sT   d} t j| dd}|t   }t| |  ||  }t| t|  d S r   r   r   r   r   r   test_single_identity_invariance  s    r   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   r,   r   r   r   )r   r[   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   r,   r   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    rJ   绽|=Zatol)
r   concatenater,   linspacer   r   rX   r   meanr   )axesthetastr   r   r   r   	test_mean  s
    r	  c                  C   s   t dddgdddgdddgg} t dt jd d}|D ]Z}t|| d d  }|jddgd}t||  }| }t||  	 ddd q:d S )Nr   r   r    rJ   weightsr  r  )
r   r   r  r   r   rX   r  r   r   r   )r  r  r  rwmwr   mr   r   r   test_weighted_mean#  s    "r  c               	   C   sB   t jtdd* ttd} | jtd d W 5 Q R X d S )Nznon-negativerg   r
   r
  )	r"   r#   r$   r   r   r   r,   r  onesr   r   r   r   test_mean_invalid_weights0  s    r  c                  C   s"   t  jdd} t| t std S )NFZreturn_indices)r   r   reduce
isinstancer   )r[   r   r   r   test_reduction_no_indices6  s    r  c                  C   sV   t  jdd} t| tks tt| dks0t| \}}}|d ksFt|d ksRtd S )NTr  r	   )r   r   r  typetupler   len)r[   reduced	left_best
right_bestr   r   r   test_reduction_none_indices;  s    
r  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*t||k s<t|| | ||  }| |  }t|t t| d S )Nr   r   r   rF   r'   Tr  r	   r   r   rm   )r   rM   rN   r   r  rP   r  	enumerateabsr   rA   Zmoveaxisr2   Zargmaxallr   r   r   r   )rnglr   r   r  r  r  ZscalarsrT   ZlijZpjkZrkZmax_indZleft_best_checkZright_best_checkZreduced_checkmagr   r   r   !test_reduction_scalar_calculationE  s&    * r&  c            	      C   s.  t dddgdddgdddgg} t| }tt j| dd}t dddg}t j|dd}t dddg}t j|dd}t||| t||| t||| t||| t dddg}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   rm   r    r	   r:   TZinverser   r   r   r=   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*    
r+  c                  C   s   t dddgdddgdddgg} t| }tt j| dd}t dddgddd	gg}t d
ddgddd	gg}t||| t||| t dddgddd	gg}t|j|dd| t|j|dd| d S )Nr   r   r   rm   r    r	   r
   r   r&   r:   Tr'  r(  )r.   r1r2v	v_rotated	v_inverser   r   r   *test_apply_single_rotation_multiple_points{  s    
r3  c                  C   s   t d} t dddgdddgdddgg| d< t dddgdddgdddgg| d< t| }t dddg}t j|dd}t dddgdd	dgg}t||| t||| t dddgdddgg}t|j|d
d| t|j|d
d| d S )Nr    r	   r	   r   r   r   r    r	   rm   r:   ru   Tr'  )r   r   r   r   r=   r)  r   r*  )r.   r   Zv1Zv2r1  r2  r   r   r   *test_apply_multiple_rotations_single_point  s(    



r5  c                  C   s   t d} t dddgdddgdddgg| d< t dddgdddgdddgg| d< t| }t dddgddd	gg}t d
ddgdddgg}t||| t dddgdd	dgg}t|j|dd| d S )Nr4  r   r   r   r    r	   r
   r   r&   r:   ir,  Tr'  )r   r   r   r   r=   r   r*  )r.   r   r0  r1  r2  r   r   r   -test_apply_multiple_rotations_multiple_points  s"    



r6  c                  C   s   t d} t dddgdddgdddgg| d< t dddgdddgdddg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 )Nr4  r   r   r   V瞯<r  rm   )r   r   r   r   r=   r   r+   r)  r.   r   r   r   r   test_getitem  s    



r9  c                	   C   s*   t jtdd t d  W 5 Q R X d S Nznot subscriptablerg   r   )r"   r#   	TypeErrorr   r   r   r   r   r   test_getitem_single  s    r<  c               	   C   s2   t  } tjtdd t  | d< W 5 Q R X d S r:  )r   r   r"   r#   r;  r  r   r   r   test_setitem_single  s    r=  c                  C   sT   t jjdd} tjd| d}tjd| d}||dd< t|dd  |  d S )Nr   seedrF   r   r   r   r&   r   rM   rN   r   r   r   r!  r.  r/  r   r   r   test_setitem_slice  s
    rB  c                  C   sJ   t jjdd} tjd| d}tj| d}||d< t|d  |  d S )Nr   r>  rF   r   r   r@  rA  r   r   r   test_setitem_integer  s
    rC  c               	   C   s4   t jddd} tjtdd d| d< W 5 Q R X d S )NrF   r   r   zRotation objectrg   r   )r   rM   r"   r#   r;  r  r   r   r   test_setitem_wrong_type  s    rD  c                  C   s   t d} t dddgdddgdddgg| d< t dddgdddgdddgg| d< t| }tt|d tt|d d d d S )Nr4  r   r   r   r    )r   r   r   r   r=   r   r  r8  r   r   r   test_n_rotations  s    



rE  c                  C   sX   t dddgdddgg} |  }t| |\}}t| t d t|ddd	 d S )
Nr   r    r	   r
   r   r&   r   ư>r  )	r   r   copyr   align_vectorsr   r+   r,   r   )r   r   r   rmsdr   r   r   test_align_vectors_no_rotation  s
    rJ  c                  C   sb   t jd} tj| d}| jdd}||}t||\}}t| |  t|ddd d S )Nr   r   r   r	   rG   Hz>r  )	r   rM   rN   r   normalr*  rH  r   r   )rR   cr   r   estrI  r   r   r   test_align_vectors_no_noise  s    
rQ  c                  C   sj   t dddgdddgg} t ddd	gd
ddgg}t| |\}}t| ||dd t|ddd d S )Ngq?gH*fܿgHlB?gMs^E?g+Obp޿gn^wg=2)p?g[[VKg?gw7?gѿgGvgprF  r  r   rM  )r   r   r   rH  r   r*  )r   r   rP  rI  r   r   r   $test_align_vectors_improper_rotation  s    rR  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   rK  rL  r   Tr    )r   rM   rN   r   rN  r*  rH  r  r   r+   rD   )
r!  rO  r   r   Zest1Zrmsd1Zcov1Zest2Zrmsd2Zcov2r   r   r   !test_align_vectors_scaled_weights(  s    
 rS  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   rJ   r   r	   rL  r         ?)rG   scaleT)Zreturn_sensitivityr  r    r   r   )r   r   )r    r    r<   )r   rM   rN   r   rN  r*  rd   rX   rH  r   rp   r   sum)rR   Z	n_vectorsr~   Zvectorsr[   sigmaZ	tolerancenoiseZnoisy_resultrP  rI  ZcovZerror_vectorr   r   r   test_align_vectors_noise6  s4    

	
rZ  c               	   C   sL   t jtdd4 tdddggdddgg\} }t|ddd W 5 Q R X d S )NzOptimal rotation is notrg   r   r   r   缉ؗҜ<r  )r"   r   r   r   rH  r   )Z
r_estimaterI  r   r   r    test_align_vectors_single_vector]  s     r\  c                	   C   sb  t jtdd  tdddgdddgg W 5 Q R X t jtdd  tdddggdddg W 5 Q R X t jtdd* tdddgdd	d
ggdddgg W 5 Q R X t jtdd* tjdddggdddggdggd W 5 Q R X t jtdd* tjdddggdddggddgd W 5 Q R X t jtdd( tjdddggdddggdgd W 5 Q R X d S )Nz Expected input `a` to have shaperg   r   r    r	   z Expected input `b` to have shapez/Expected inputs `a` and `b` to have same shapesr
   r   r&   z&Expected `weights` to be 1 dimensionalr
  z+Expected `weights` to have number of valuesz)`weights` may not contain negative valuesr   )r"   r#   r$   r   rH  r   r   r   r    test_align_vectors_invalid_inputc  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   r   )r
   r   r@   r   r   r
   )r   rM   rN   r   r   r   r2   )rR   r   r   r   test_random_rotation_shape{  s
    r_  c                  C   s  t jd} t| jdd}| }dddddg}t||}ddd	dd
ddddddg}||}| }||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^  rL  r   r    r	   r
   r<   r   rT  g      @g      
@g@r   r   r'   rF   )r   rM   rN   r   r   r   r   r   r   rW  r   r  )rR   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    
  

ra  c               	   C   s<   t jtdd$ tddddg} tdg|  W 5 Q R X d S )Nzmust be a sequence of rotationsrg   r   r    r	   r
   )r"   r#   r$   r   r   r   r  r   r   r   test_slerp_single_rot  s    rb  c               	   C   sX   t jtdd@ tjd} t| jdd}t	dgdgg}t
|| W 5 Q R X d S )Nz.times to be specified in a 1 dimensional arrayrg   r   )r    r
   rL  r   r    )r"   r#   r$   r   rM   rN   r   r   r   r   r   rR   r   r  r   r   r   test_slerp_time_dim_mismatch  s    rd  c               	   C   sP   t jtdd8 tjd} t| jdd}t	d}t
|| W 5 Q R X d S )Nz7number of rotations to be equal to number of timestampsrg   r   r^  rL  r'   )r"   r#   r$   r   rM   rN   r   r   r   aranger   rc  r   r   r   !test_slerp_num_rotations_mismatch  s
    
rf  c               	   C   sT   t jtdd< tjd} t| jdd}dddddg}t	|| W 5 Q R X d S )	Nstrictly increasing orderrg   r   r^  rL  r   r    r
   
r"   r#   r$   r   rM   rN   r   r   r   r   rc  r   r   r   test_slerp_equal_times  s
    ri  c               	   C   sT   t jtdd< tjd} t| jdd}ddddd	g}t	|| W 5 Q R X d S )
Nrg  rg   r   r^  rL  r   r	   r    r
   rh  rc  r   r   r   test_slerp_decreasing_times  s
    rj  c               	   C   sj   t jd} t| jdd}t d}t||}tj	t
dd  t dgdgg}|| W 5 Q R X d S )	Nr   r^  rL  r   z&`times` must be at most 1-dimensional.rg   g      @g@)r   rM   rN   r   r   r   re  r   r"   r#   r$   r   )rR   r   r  sZinterp_timesr   r   r   !test_slerp_call_time_dim_mismatch  s    

rl  c               	   C   s   t jd} t| jdd}t dd }t||}tj	t
dd |dddg W 5 Q R X tj	t
dd |ddd	g W 5 Q R X d S )
Nr   r^  rL  r   r   ztimes must be within the rangerg   r    r&   )r   rM   rN   r   r   r   re  r   r"   r#   r$   )rR   r   r  rk  r   r   r   !test_slerp_call_time_out_of_range  s    
rm  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   Trb   r   r   r   r[  r  )r   r   r   r   r   r   )r   rk  Zr_interpolatedZr_interpolated_expecteddeltar   r   r   test_slerp_call_scalar_time  s    rq  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   i  r   rm   )r   rM   r   r   rn   ro   r   )qsrsr   r   r   r   test_multiplication_stability  s
    ru  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    rV  )r   r   )Zdtype)r	   r    )	r   rM   r   r   r   r2   r   r+   object)ZsingleZmultipler   r   r   r    test_rotation_within_numpy_array  s*    

rw  c               	   C   sX   t ddttjd ttjd g} t| }t|}t	| 
 |
 dd d S Nr   r
   r7  r  )r   r   r   sinr   rq   pickledumpsloadsr   r+   )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 rx  )
r   r   r   ry  r   rq   rG  deepcopyr   r+   )r   r.  r   r   r   test_deepcopy+  s    *
r  c                  C   s~   t ddddg} | d}| d}|jd dks8t|jd dksJttdd |jD sbttd	d |jD sztd S )
Nr   r   r   ZXYZZC_CONTIGUOUSTc                 s   s   | ]}|d kV  qdS r   Nr   .0rT   r   r   r   	<genexpr>7  s     z+test_as_euler_contiguous.<locals>.<genexpr>c                 s   s   | ]}|d kV  qdS r  r   r  r   r   r   r  8  s     )r   r   r   flagsr   r   strides)r   e1e2r   r   r   test_as_euler_contiguous1  s    

r  c                     sh   t jddd dddddg} dgtt|  } fddt|| D }t |}t  |  d S )	NrF   r   r   r   r    r	   c                    s    g | ]\}} |||  qS r   r   )r  rT   r   r   r   r   
<listcomp>?  s     z$test_concatenate.<locals>.<listcomp>)	r   rM   listr   Zcumsumzipr  r   r   )sizesZstartssplitr[   r   r  r   test_concatenate;  s    
r  c                	   C   s2   t jtdd tt dd g W 5 Q R X d S )NzRotation objects onlyrg   r   )r"   r#   r;  r   r  r   r   r   r   r   test_concatenate_wrong_typeD  s    r  c               	   C   s   t td} t ddddgg}t ddddgddddgg}t ddddg}t| dks\tt|dksltt|dks|ttjtdd t| W 5 Q R X | st|st|st|std S )N)r   r
   r   r   r    zSingle rotation has no len().rg   )r   r   r   r  r   r"   r#   r;  )Zrotation_multi_emptyZrotation_multi_oneZrotation_multiZrotation_singler   r   r   test_len_and_boolJ  s    r  )tr"   numpyr   Znumpy.testingr   r   r   Zscipy.spatial.transformr   r   Zscipy.statsr   	itertoolsr   rz  rG  r   r   r   r   r%   r)   r*   r/   r4   r8   r;   r?   rB   rE   rI   rU   r\   r]   r^   ra   re   rj   rk   rt   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+  r3  r5  r6  r9  r<  r=  rB  rC  rD  rE  rJ  rQ  rR  rS  rZ  r\  r]  r_  ra  rb  rd  rf  ri  rj  rl  rm  rq  ru  rw  r}  r  r  r  r  r  r   r   r   r   <module>   s   





%)
	
	'	,
	
	