o
    eO                     @   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   Z/var/www/myenv/lib/python3.10/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    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   sD   t t ttg d W d    d S 1 sw   Y  d S )Nr    r$   r
   pytestraises
ValueErrorr   r   r   r   r   r   r   r   test_malformed_1d_from_quat>   s   "r2   c                   C   sL   t t ttg dg dg W d    d S 1 sw   Y  d S )N)r    r$   r
   r   r   )r   r            r.   r   r   r   r   test_malformed_2d_from_quatC   s   
"r6   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   r   )r   r   r/   r0   r1   r   r   )r   r   r   r   test_zero_norms_from_quatK   s   "r7   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   r8   r   shaper   r   r   )r:   r;   expected_matr   r   r   #test_as_matrix_single_2d_quaternion\   s   
rE   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@   rA   r   r   r    r   r!   r   r   r    r$   r
   )	r   r   r8   r   rC   r   r   r   r9   )quatsr;   	expected0	expected1r   r   r    test_as_matrix_from_square_inputh   s*   rL   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=   rF   r    r$   r
   r   )r
   r
   r
   r?   r@   rA   r   rG   rH   r    )皙?g@)gffffff@r    rN   )r!   r$   r$   r
   r$   rB   )rI   r;   rJ   rK   	expected2r   r   r   !test_as_matrix_from_generic_input   s4   rQ   c                  C   s6   g dg dg dg} g d}t t|  | d S )NrA   r@   rG         ?rS   rS   rS   )r   r   from_matrixr   r;   r   r   r   r   test_from_single_2d_matrix   s   rV   c                  C   sN   t g dg dg dgd} t g dd}tt|  | d S )NrA   r@   rG   r>   rR   r    r   )r   r   reshaper   r   rT   r   rU   r   r   r   test_from_single_3d_matrix   s   rY   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    r3   r    '   )3~˷g6iпnzo?)r\   r[   6i?)r]   r\   g3~˷?r>   rW   )r   r   sqrtr   r   rT   r   rX   )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   rvsr   r   rT   r8   )r;   r   r   r    test_matrix_calculation_pipeline   s   rd   c                  C   sp   t jd} | d}t| }t d||d}t 	d}t
dD ]}d|d d ||f< q%t|| d S )Nr   )d   r
   r
   ...ij,...jk->...ik)r   r$   r    r
         ?)r   randomRandomStaterandom_sampler   rT   r8   einsum	transposezerosranger   )rndr;   	ortho_matmult_resulteye3dir   r   r   test_from_matrix_ortho_output   s   

rt   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_rotvec      
r|   c                  C   6   g dg} t g dg}t| }t| | d S rv   rw   ry   r   r   r   test_from_2d_single_rotvec      

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    r$   r$   r    r!   rS   r   r   r   )g.pG?cU(G?r   gQH?)gNZ]O?gNZ]OݿgNZ]O?g0i?r   )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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?rN   r   )r   r
   r    r   rS   r    r
   g@w?)g>}J?g.e?g>}J?r$   r   )r   r   r^   r   rx   r   r   r   )rz   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 )Nrg   r
   Tdegrees)r   cbrtr   rx   r   deg2radr   )rotvec1rot1quat1rotvec2rot2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 shapematchr    r$   r/   r0   r1   r   rx   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   r   rM   r   r3   r4   r5   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    r$   r!   rS   r    r!   r    ga2U0*3?r   r    r&   r
   r$   )r
   r
   )r   r   r*   r+   r   r   	as_rotvecr   coscrossrm   )r:   rz   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    r$   r$   gj*0	x?g$x?g:2(r
   r   r   r   r   r   r   rC   r   r:   expected_rotvec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_inputG  
   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 )
NrA   r@   rG   Tr   g      ^@r   r    r$   )r   rT   r   r   r*   r+   r   )r;   rotrz   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 )Nr   r    r!   r$   )ga2U0*3gǺ6?ga2U0*?Tr   )r   r   r   r   rx   r   )rz   r   r   r   test_rotvec_calc_pipeline\  s    r   c                  C   ru   N)r   r   rg   )r   r   r    r   r   r   r   from_mrpr   r   mrpr   r{   r   r   r   test_from_1d_single_mrpg  r}   r   c                  C   r~   r   r   r   r   r   r   test_from_2d_single_mrpn  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   rN   rN   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   r   )NExpected `mrp` to have shaper   r    r$   r/   r0   r1   r   r   r   r   r   r   test_malformed_1d_from_mrp  r   r   c                   C   r   )Nr   r   rM   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   r   r    r&   )gQUU?gvWUU?QUUտ)uFx?guFxr   r   )r   r   r*   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 )	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   r   )Nr   g?g?gO޿r   r   r   r   r   r   r   rC   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_input  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 )NzZ   Tr   r=   r$   )r   r   r   r   r   r^   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   Tr   Z)r   r   r8   r   )	extrinsic	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      )r3   r
   lowhighra   r!   r   Tr   ZYX)r   rh   ri   randintr   r   r   r   )ro   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 )Nzxr   Tr   r?   r   r   r!   r@   )r   r   r8   r   r   r   )r;   rD   r   r   r   -test_from_euler_elementary_extrinsic_rotation  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   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?)      rS   r   r   r   r8   r   r   r   anglesr;   r   r   r   &test_from_euler_intrinsic_rotation_312  (   r   c                  C   r   )Nr   r   r   ZXZTr   r   )>d?g @r   )l>?^|@r   )X?r  rS   r    g      ?g ,r   g ,?g      ?r   r         ?rS   r$   )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   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  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   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  r   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 ]k}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 qQd 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allr)   errormean_maxrms_maxr  r  rmsr   r   r   
test_statsS  
   z1test_as_euler_asymmetric_axes.<locals>.test_statsr     r
   r   r$   r    r    -q=atolrtolV瞯<+=r   rh   ri   emptyuniformr   r   joinr   r   as_euler_as_euler_from_matrixr   upper	r  ro   nr   	seq_tupleseqrotationangles_quat
angles_matr   r   r   test_as_euler_asymmetric_axesQ  s2   $,$




r6  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 ]t}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 qKd S )Nc                 S   r  r  r  r  r   r   r   r  y  r  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  s2   $ $



r8  c                  C   s   t g dg dg dg dg} tjtdd] tdD ]O}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    d S 1 sxw   Y  d S )Nr   r   #   r:     r:  r      r?  r<     Gimbal lockr   r   r!  Tr   r   r   r/   warnsUserWarningr   r+  r   r   r8   r,  r   r.  r   r1  r2  r3  mat_expectedangle_estimatesmat_estimatedr   r   r   (test_as_euler_degenerate_asymmetric_axes  s<   

"rJ  c                  C   s  t g dg dg dg dg} tjtddf tdD ]X}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    d S 1 sw   Y  d S )NrA  r   r   r:  r   K   r   r   r:  rA  iLr?  rB  r   r   r!  r   r    Tr   rC  rF  r   r   r   'test_as_euler_degenerate_symmetric_axes  s<   
"rP  c                  C   sR  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 sw   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 ]}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    d S 1 sw   Y  d S )Nr9  r;  r>  r@  rB  r   r   r!  Tr   r   r$   r"  r#  r    Hz>rK  rL  rN  rO  ư>r$  )r   r   r/   rD  rE  r   r+  r   r   r-  r,  r   r.  )r   r1  r2  r   estimates_matrixestimates_quatidxr   r   r   +test_as_euler_degenerate_compare_algorithms  s   
$rW  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`   numrb   rf   r
   )
r   rh   ri   r   invr8   rk   r)  r9   r   )	ro   r0  pqp_matq_matresult1result2rr   r   r   r   test_inv1  s   
ra  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   rb   r
   r    rX  rf   r>   )r   rh   ri   r   rZ  r8   dotr9   r   rk   r)  )ro   r[  r\  r]  r^  res1res2r9   r   r   x_matrixy_matrixr_  r`  rr   r   r   r   test_inv_single_rotationC  s(   




rh  c                  C   s4   d} t t|  d t t|   d d S )Nr`   r   )r   r   identity	magnituderZ  )r0  r   r   r   test_identity_magnitudea  s   rk  c                   C   s0   t   dks
J t    dksJ d S )Nr   )r   ri  rj  rZ  r   r   r   r   test_single_identity_magnitudeg  s   rl  c                  C   sV   d} t j| dd}|t |  }t| |  ||  }t| t|  d S Nr`   r   rb  	r   rh   ri  r   r   rZ  rj  r   rm   r0  r[  r{   r   r   r   test_identity_invariancel  s   rp  c                  C   sT   d} t j| dd}|t   }t| |  ||  }t| t|  d S rm  rn  ro  r   r   r   test_single_identity_invariancew  s   rq  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   r9   rj  r   r   )r   r{   r   r   r   test_magnitude  s   rr  c                  C   sB   t td} | d  }t|tj | d  }t|d d S )Nr   r   r
   )r   r   r   r9   rj  r   r   )r   r_  r`  r   r   r   test_magnitude_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   r$   re   绽|=rS  )
r   concatenater9   linspacer   r   rx   r   r  rj  )axesthetastr   r   r   r   	test_mean  s   rz  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   r$   re   r    weightsrt  rS  )
r   r   rv  r   r   rx   r  r   rZ  rj  )rw  rx  ry  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-negativer   r   r{  )	r/   r0   r1   r   r   r   r9   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return_indices)r   ri  reduce
isinstance)r{   r   r   r   test_reduction_no_indices  s   r  c                  C   sV   t  jdd} t| tksJ t| dksJ | \}}}|d u s#J |d u s)J d S )NTr  r
   )r   ri  r  typetuplelen)r{   reduced	left_best
right_bestr   r   r   test_reduction_none_indices  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   rb  r`   r4   Tr  r
   r    r!   r&   )r   rh   ri   r   r  rm   r  	enumerater)   r   rX   moveaxisrC   argmaxr  rZ  rj  r   )rnglr   r[  r  r  r  scalarsrs   lijpjkrkmax_indleft_best_checkright_best_check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 )Nr?   r@   rA   r   r&   r-   rO   r    r
   r$   r!   r
   Tinverser   r   r   rT   expand_dimsr   apply)	r;   r_1dr_2dv_1dv_2dv1d_rotatedv2d_rotatedv1d_inversev2d_inverser   r   r   'test_apply_single_rotation_single_point  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 )Nr?   r@   rA   r   r&   r-   r   r   r3   r  )r   r3   r  )r   r3   Tr  r  )r;   r1r2v	v_rotated	v_inverser   r   r   *test_apply_single_rotation_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 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@   rA   r   r   rG   r    r-   r&   r  )r    r   r$   r  )r    r
   rO   Tr  )r   r)  r   r   rT   r  r   r  )r;   r   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  r?   r@   rA   r   r   rG   r    r-   r  r  )r   ir   r  )r   r3   r  Tr  )r   r)  r   r   rT   r   r  )r;   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  r?   r@   rA   r   r   rG   r    r&  rS  r!   r&   )r   r)  r   r   rT   r   r8   r  r;   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 subscriptabler   r   )r/   r0   	TypeErrorr   ri  r   r   r   r   test_getitem_singleO  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   ri  r/   r0   r  r  r   r   r   test_setitem_singleT  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   seedr`   rb  r   r    r3   r   rh   ri   r   r   r   r  r  r  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   r  r`   rb  r    r  r  r   r   r   test_setitem_integerb  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 )Nr`   r   rb  zRotation objectr   r    )r   rh   r/   r0   r  r  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 )Nr  r?   r@   rA   r   r   rG   r    r$   r!   )r   r)  r   r   rT   r   r  r  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-   r  r
   r   rR  rS  )	r   r   copyr   align_vectorsr   r8   r9   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   rb  r   r
   ra   rQ  rS  )	r   rh   ri   r   normalr  r  r   r   )ro   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rR  rS  r   rQ  )r   r   r   r  r   r  )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   rb  r  r  r   Tr$   )r   rh   ri   r   r  r  r  r  r   r8   r^   )
r  r  r   r   est1rmsd1cov1est2rmsd2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   re   rb  r
   r  r          ?)ra   scaleT)return_sensitivityrS  r$   r   r   )r    r    )r$   r$   rS   )r   rh   ri   r   r  r  r   rx   r  rZ  r   r   sum)ro   	n_vectorsr   vectorsr{   sigma	tolerancenoisenoisy_resultr  r  cov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    d S 1 s(w   Y  d S )NzOptimal rotation is notr   )r    r!   r    )r    r    r!   r   r7  rS  )r/   rD  rE  r   r  r   )
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w   Y  t jtdd tg dg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 sew   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g dgddg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    d S 1 sw   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 shapesr  z&Expected `weights` to be 1 dimensionalr    r{  z+Expected `weights` to have number of valuesr$   z)`weights` may not contain negative valuesr!   )r/   r0   r1   r   r  r   r   r   r    test_align_vectors_invalid_input  s0    "" "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   rb  )r   r    rW   r   r   r   )r   rh   ri   r   r   r   rC   )ro   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   rS   r	  r    r  r$   g      @r
   g      
@g@r   r!   r
   r    r   r$   r4   r`   r   )r   rh   ri   r   r   r*  r   r   r   r  r   r  )ro   key_rots	key_quats	key_timesinterpolatortimesinterp_rotsinterp_quats
cos_theta1
cos_theta2
cos_theta4
cos_theta5
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` instancer   rM   r   r   r    )r/   r0   r  r   r   r   )r   ry  r   r   r   test_slerp_rot_is_rotation*  s   
"r  c                  C   sR   d} t jt| d tg d}tdg| W d    d S 1 s"w   Y  d S Nz*must be a sequence of at least 2 rotationsr   rM   r    r/   r0   r1   r   r   r   msgr   r   r   r   test_slerp_single_rot2  s
   "r  c                  C   sT   d} t jt| d tg dg}tdg| W d    d S 1 s#w   Y  d S r   r  r  r   r   r   test_slerp_rot_len19  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 arrayr   r   )r$   r   r  r    r$   )r/   r0   r1   r   rh   ri   r   r   r*  r   r   ro   r   ry  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 timestampsr   r   r  r  r4   )r/   r0   r1   r   rh   ri   r   r   r*  aranger   r  r   r   r   !test_slerp_num_rotations_mismatchJ  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 orderr   r   r  r  )r   r    r$   r$   r   
r/   r0   r1   r   rh   ri   r   r   r*  r   r  r   r   r   test_slerp_equal_timesS     "r  c                  C   r
  )Nr  r   r   r  r  )r   r    r
   r$   r   r  r  r   r   r   test_slerp_decreasing_times[  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.r   g      @g@)r   rh   ri   r   r   r*  r  r   r/   r0   r1   r   )ro   r   ry  s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 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 ranger   )r   r    r$   )r    r$   r3   )r   rh   ri   r   r   r*  r  r   r/   r0   r1   )ro   r   ry  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=  r7  rS  )r   r   r   rZ  r   rj  )r   r  r_interpolated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   rb  r   r    r&   )r   rh   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   rb  r$   r    r   )r$   )r    )r    r$   r  )r   r    )dtype)r
   r$   )	r   rh   r   r   r   rC   r   r8   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&  rS  )r   r   r   sinr   r   pickledumpsloadsr   r8   )r   pkl	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   r8   )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 )	Nr   r   XYZC_CONTIGUOUSTc                 s       | ]}|d kV  qdS r   Nr   .0rs   r   r   r   	<genexpr>      z+test_as_euler_contiguous.<locals>.<genexpr>c                 s   r/  r0  r   r1  r   r   r   r3    r4  )r   r   r,  flagsr  strides)r   e1e2r   r   r   test_as_euler_contiguous  s   

r9  c                     sb   t jddd g d} dgtt|  } fddt|| D }t |}t  |  d S )Nr`   r   rb  )r    r$   r
   r    r
   c                    s    g | ]\}} |||  qS r   r   )r2  rs   r0  r3  r   r   
<listcomp>  s     z$test_concatenate.<locals>.<listcomp>)	r   rh   listr   cumsumzipru  r   r   )sizesstartssplitr{   r   r:  r   test_concatenate  s   
rB  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 onlyr   r    )r/   r0   r  r   ru  ri  r   r   r   r   test_concatenate_wrong_type  s   "rC  c                  C   s   t td} t g dg}t g dg dg}t g d}t| dks&J t|dks.J t|dks6J tjtdd t| W d    n1 sLw   Y  | sUJ |sYJ |s]J |saJ d S )N)r   r   r   r   r    r$   zSingle rotation has no len().r   )r   r   r)  r  r/   r0   r  )rotation_multi_emptyrotation_multi_onerotation_multirotation_singler   r   r   test_len_and_bool  s   
rH  )xr/   numpyr   numpy.testingr   r   r   scipy.spatial.transformr   r   scipy.statsr   	itertoolsr   r%  r  r   r   r   r%   r,   r2   r6   r7   r<   rE   rL   rQ   rV   rY   r_   rd   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  r6  r8  rJ  rP  rW  ra  rh  rk  rl  rp  rq  rr  rs  rz  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r	  r  r  r  r  r  r  r"  r*  r,  r9  rB  rC  rH  r   r   r   r   <module>   s    





&&$$L
	
	'	,
	
	