o
    i                     @   s  d dl Z d dlZd dlmZmZ d dlmZ d dlmZm	Z	 d dl
mZ d dlmZ d dlZd dlZdd Zd	d
 Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd Zdd  Zd!d" Zd#d$ Zd%d& Zd'd( Z d)d* Z!d+d, Z"d-d. Z#d/d0 Z$d1d2 Z%d3d4 Z&d5d6 Z'd7d8 Z(d9d: Z)d;d< Z*d=d> Z+d?d@ Z,dAdB Z-dCdD Z.dEdF Z/dGdH Z0dIdJ Z1dKdL Z2dMdN Z3dOdP Z4dQdR Z5dSdT Z6dUdV Z7dWdX Z8dYdZ Z9d[d\ Z:d]d^ Z;d_d` Z<dadb Z=dcdd Z>dedf Z?dgdh Z@didj ZAdkdl ZBdmdn 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   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   f/var/www/edux/Edux_v2/venv/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   s\   t g dg dg dg dg} t| }| t 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   test_from_square_quat_matrix"   s   
r#   c                   C   sD   t t ttg d W d    d S 1 sw   Y  d S )Nr       r
   pytestraises
ValueErrorr   r   r   r   r   r   r   r   test_malformed_1d_from_quat/   s   "r*   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_quat4   s   
"r.   c                  C   sV   t g dg dg dg} tt t|  W d    d S 1 s$w   Y  d S )Nr	   )r   r   r   r   r   )r   r   r'   r(   r)   r   r   )r   r   r   r   test_zero_norms_from_quat<   s   "r/   c                  C   s*   g d} t |  }t|td d S )Nr   r
   )r   r   	as_matrixr   r   eye)quatmatr   r   r   #test_as_matrix_single_1d_quaternionF   s   r4   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   r0   r   shaper   r   r   )r2   r3   expected_matr   r   r   #test_as_matrix_single_2d_quaternionM   s   
r=   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 )Nr5   r   r    r   r    r   r!   )r   r
   r
   r7   r8   r9   r   r   r    r   r"   r   r   r    r%   r
   )	r   r   r0   r   r;   r   r   r   r1   )quatsr3   	expected0	expected1r   r   r    test_as_matrix_from_square_inputY   s*   rD   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 )Nr5   r>   r    r%   r
   r   )r
   r
   r
   r7   r8   r9   r   r?   r@   r    )皙?g@)gffffff@r    rF   )r"   r%   r%   r
   r%   r:   )rA   r3   rB   rC   	expected2r   r   r   !test_as_matrix_from_generic_inputu   s4   rI   c                  C   s6   g dg dg dg} g d}t t|  | d S )Nr9   r8   r?         ?rK   rK   rK   )r   r   from_matrixr   r3   r   r   r   r   test_from_single_2d_matrix   s   rN   c                  C   sN   t g dg dg dgd} t g dd}tt|  | d S )Nr9   r8   r?   r6   rJ   r    r   )r   r   reshaper   r   rL   r   rM   r   r   r   test_from_single_3d_matrix   s   rQ   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    r+   r    '   )3~˷g6iпnzo?)rT   rS   6i?)rU   rT   g3~˷?r6   rO   )r   r   sqrtr   r   rL   r   rP   )r   r3   r   r   r   test_from_matrix_calculation   s   rW   c                  C   s(   t jdddd} tt|  |  d S )Nr
   
   r   )sizerandom_state)r   rvsr   r   rL   r0   )r3   r   r   r    test_matrix_calculation_pipeline   s   r\   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   rL   r0   einsum	transposezerosranger   )rndr3   	ortho_matmult_resulteye3dir   r   r   test_from_matrix_ortho_output   s   

rl   c                  C   2   g d} t g d}t| }t| | d S Nr8   )gJ?r   r   g<(?r   r   r   from_rotvecr   r   rotvecr   resultr   r   r   test_from_1d_single_rotvec      
rt   c                  C   6   g dg} t g dg}t| }t| | d S rn   ro   rq   r   r   r   test_from_2d_single_rotvec      

rw   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"   rK   r   r   r   )g.pG?cU(G?r|   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   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?rF   r{   )r   r
   r    r   rK   r    r
   g@w?)g>}J?g.e?g>}J?r%   r   )r   r   rV   r   rp   r   r   r   )rr   r2   r   r   r   test_from_rotvec_small_angle   s   &r   c                  C   sh   dt d dt d dt d g} tj| dd}| }t | }t|}| }t|| d S )Nr_   r
   Tdegrees)r   cbrtr   rp   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'   r(   r)   r   rp   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   rE   r   r+   r,   r-   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"   rK   r    r"   r    ga2U0*3?r   r    axisr
   r%   )r
   r
   )r   r   linalgnormr   r   	as_rotvecr   coscrossre   )r2   rr   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   r;   r   r2   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_input8  
   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 )
Nr9   r8   r?   Tr   g      ^@r   r    r%   )r   rL   r   r   r   r   r   )r3   rotrr   r   r   r   r   test_as_rotvec_degreesB  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   rp   r   )rr   r   r   r   test_rotvec_calc_pipelineM  s    r   c                  C   rm   N)r   r   r_   )r   r   r    r   r   r   r   from_mrpr   r   mrpr   rs   r   r   r   test_from_1d_single_mrpX  ru   r   c                  C   rv   r   r   r   r   r   r   test_from_2d_single_mrp_  rx   r   c                  C   sP   t g dg dg dg} t g dg dg dg}tt|  | d S )Nry   rz   r{   )r~   rF   rF   g)g}+;?g}+;g}+;?gߨӉؿr   )r   r   r   r   r   r   )r   r   r   r   r   test_from_generic_mrpf  s   r   c                   C   r   )NExpected `mrp` to have shaper   r    r%   r'   r(   r)   r   r   r   r   r   r   test_malformed_1d_from_mrpr  r   r   c                   C   r   )Nr   r   rE   r   r   r   r   r   r   test_malformed_2d_from_mrpw  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)r2   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   r;   r   r2   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   r5   r%   )r   r   r   r   r   rV   r   )r2   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   r0   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      )r+   r
   lowhighrY   r"   r   Tr   ZYX)r   r`   ra   randintr   r   r   r   )rg   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   r7   r   r   r"   r8   )r   r   r0   r   r   r   )r3   r<   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?)      rK   r   r   r   r0   r   r   r   anglesr3   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   rK   r    g      ?g ,r   g ,?g      ?r   r         ?rK   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_313'  r   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 ]#}d|}t|t	
||| | }t|t	
||| qMd S )	Nr   rX   r
   r   r%   r    r    )r   r`   ra   emptyuniformr   r   joinr   r   r   as_eulerupper)rg   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 ])}||krPqI|| | }t|t||	| |
 }t|t||	| qIqEd S )Nr   rX   r
   r   r    r%   r   r   r   )r   r`   ra   r  r  r   r   r   r   r  r  )rg   r  r   axis1axis2r  r   r   r   test_as_euler_symmetric_axesS  s(   $ $r  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   r0   r  r   r  )r   r  r  rotationmat_expectedangle_estimatesmat_estimatedr   r   r   (test_as_euler_degenerate_asymmetric_axesi  s<   

"r(  c                  C   s  t g dg dg dg dg} tjtddf dD ]Z}dD ]U}||kr&q|| | }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qW d    d S 1 sw   Y  d S )
N)r   r   r   )r  r   K   )r   r   r  )r   iLr  r!  r   r  Tr   )r   r   r'   r"  r#  r   r   r0   r  r   r  )r   r  r  r  r$  r%  r&  r'  r   r   r   'test_as_euler_degenerate_symmetric_axes  sD   
"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   rX   numrZ   r^   r
   )
r   r`   ra   r   invr0   rc   r  r1   r   )	rg   r  pqp_matq_matresult1result2rj   r   r   r   test_inv  s   
r4  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   rZ   r
   r    r+  r^   r6   )r   r`   ra   r   r-  r0   dotr1   r   rc   r  )rg   r.  r/  r0  r1  res1res2r1   r   r   x_matrixy_matrixr2  r3  rj   r   r   r   test_inv_single_rotation  s(   




r;  c                  C   s4   d} t t|  d t t|   d d S )NrX   r   )r   r   identity	magnituder-  )r  r   r   r   test_identity_magnitude  s   r>  c                   C   s0   t   dks
J t    dksJ d S )Nr   )r   r<  r=  r-  r   r   r   r   test_single_identity_magnitude  s   r?  c                  C   sV   d} t j| dd}|t |  }t| |  ||  }t| t|  d S NrX   r   r5  	r   r`   r<  r   r   r-  r=  r   re   r  r.  rs   r   r   r   test_identity_invariance  s   rC  c                  C   sT   d} t j| dd}|t   }t| |  ||  }t| t|  d S r@  rA  rB  r   r   r   test_single_identity_invariance  s   rD  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   r1   r=  r   r   )r   rs   r   r   r   test_magnitude  s   rE  c                  C   sB   t td} | d  }t|tj | d  }t|d d S )Nr   r   r
   )r   r   r   r1   r=  r   r   )r   r2  r3  r   r   r   test_magnitude_single_rotation  s
   rF  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%   r]   绽|=atol)
r   concatenater1   linspacer   r   rp   r   meanr=  )axesthetastr   r   r   r   	test_mean  s   rP  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{   r8   r   r%   r]   r    weightsrG  rH  )
r   r   rK  r   r   rp   rL  r   r-  r=  )rM  rN  rO  rwmwr   mr   r   r   test_weighted_mean#  s   rV  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   rQ  )	r'   r(   r)   r   r   r   r1   rL  onesr   r   r   r   test_mean_invalid_weights0  s   "rY  c                  C   s"   t  jdd} t| t sJ d S )NFreturn_indices)r   r<  reduce
isinstance)rs   r   r   r   test_reduction_no_indices6  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 )NTrZ  r
   )r   r<  r\  typetuplelen)rs   reduced	left_best
right_bestr   r   r   test_reduction_none_indices;  s   
re  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   r5  rX   r,   TrZ  r
   r    r"   r   )r   r`   ra   r   r\  re   ra  	enumerateabsr   rP   moveaxisr;   argmaxallr-  r=  r   )rnglr   r.  rb  rc  rd  scalarsrk   lijpjkrkmax_indleft_best_checkright_best_checkreduced_checkmagr   r   r   !test_reduction_scalar_calculationE  s*   & rx  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 )Nr7   r8   r9   r   r   r$   rG   r    r
   r%   r"   r
   Tinverser   r   r   rL   expand_dimsr   apply)	r3   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 )Nr7   r8   r9   r   r   r$   r   r   r+   ry  )r   r+   rz  )r   r+   Tr{  r}  )r3   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
   r7   r8   r9   r   r   r?   r    r$   r   ry  )r    r   r%   rz  )r    r
   rG   Tr{  )r   r  r   r   rL   r~  r   r  )r3   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  r7   r8   r9   r   r   r?   r    r$   r  ry  )r   ir   rz  )r   r+   r  Tr{  )r   r  r   r   rL   r   r  )r3   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  r7   r8   r9   r   r   r?   r    V瞯<rH  r"   r   )r   r  r   r   rL   r   r0   r~  r3   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'   r(   	TypeErrorr   r<  r   r   r   r   test_getitem_single  s   "r  c                  C   sH   t  } tjtdd t  | d< W d    d S 1 sw   Y  d S r  )r   r<  r'   r(   r  rX  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   seedrX   r5  r   r    r+   r   r`   ra   r   r   r   rk  r  r  r   r   r   test_setitem_slice  s
   r  c                  C   sJ   t jjdd} tjd| d}tj| d}||d< t|d  |  d S )Nr   r  rX   r5  r    r  r  r   r   r   test_setitem_integer  s
   r  c                  C   sJ   t jddd} tjtdd d| d< W d    d S 1 sw   Y  d S )NrX   r   r5  zRotation objectr   r    )r   r`   r'   r(   r  rX  r   r   r   test_setitem_wrong_type  s   
"r  c                  C   s|   t d} t g dg dg dg| d< t g dg dg dg| d< t| }tt|d	 tt|d d
 d d S )Nr  r7   r8   r9   r   r   r?   r    r%   r"   )r   r  r   r   rL   r   ra  r  r   r   r   test_n_rotations  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   ư>rH  )	r   r   copyr   align_vectorsr   r0   r1   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   r5  r   r
   rY   Hz>rH  )	r   r`   ra   r   normalr  r  r   r   )rg   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  rH  r   r  )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   r5  r  r  r   Tr%   )r   r`   ra   r   r  r  r  rW  r   r0   rV   )
rk  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   r]   r5  r
   r  r          ?)rY   scaleT)return_sensitivityrH  r%   r   r   )r    r    )r%   r%   rK   )r   r`   ra   r   r  r  r   rp   r  r-  r   r   sum)rg   	n_vectorsr   vectorsrs   sigma	tolerancenoisenoisy_resultr  r  coverror_vectorr   r   r   test_align_vectors_noise6  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   缉ؗҜ<rH  )r'   r"  r#  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    rQ  z+Expected `weights` to have number of valuesr%   z)`weights` may not contain negative valuesr"   )r'   r(   r)   r   r  r   r   r   r    test_align_vectors_invalid_inputc  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   r5  )r   r    rO   r   r   r   )r   r`   ra   r   r   r   r;   )rg   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   rK   r  r    r  r%   g      @r
   g      
@g@r   r"   r
   r    r   r%   r,   rX   r   )r   r`   ra   r   r   r  r   r   r   r  r   ra  )rg   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   sN   t jtdd tg d} tdg|  W d    d S 1 s w   Y  d S )Nzmust be a sequence of rotationsr   rE   r    )r'   r(   r)   r   r   r   rX  r   r   r   test_slerp_single_rot  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'   r(   r)   r   r`   ra   r   r   r  r   r   rg   r   rO  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  r,   )r'   r(   r)   r   r`   ra   r   r   r  aranger   r  r   r   r   !test_slerp_num_rotations_mismatch  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'   r(   r)   r   r`   ra   r   r   r  r   r  r   r   r   test_slerp_equal_times     "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   r`   ra   r   r   r  r  r   r'   r(   r)   r   )rg   r   rO  sinterp_timesr   r   r   !test_slerp_call_time_dim_mismatch  s   


"r  c                  C   s   t jd} t| jdd}t dd }t||}tj	t
dd |g d W d    n1 s3w   Y  tj	t
dd |g d	 W d    d S 1 sQw   Y  d S )
Nr   r  r  r   r    ztimes must be within the ranger   )r   r    r%   )r    r%   r+   )r   r`   ra   r   r   r  r  r   r'   r(   r)   )rg   r   rO  r  r   r   r   !test_slerp_call_time_out_of_range  s   
"r  c                  C   s\   t jdddgdd} tddg| }|d}t jdddd}||  }t| dd	d
 d S )NXr   P   Tr   r    r  r  r  rH  )r   r   r   r-  r   r=  )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   r5  i  r    r   )r   r`   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   r5  r%   r    r   )r%   )r    )r    r%   r  )r   r    )dtype)r
   r%   )	r   r`   r   r   r   r;   r   r0   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  rH  )r   r   r   sinr   r   pickledumpsloadsr   r0   )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   r0   )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   .0rk   r   r   r   	<genexpr>7      z+test_as_euler_contiguous.<locals>.<genexpr>c                 s   r  r  r   r  r   r   r   r	  8  r
  )r   r   r  flagsrj  strides)r   e1e2r   r   r   test_as_euler_contiguous1  s   

r  c                     sb   t jddd g d} dgtt|  } fddt|| D }t |}t  |  d S )NrX   r   r5  )r    r%   r
   r    r
   c                    s    g | ]\}} |||  qS r   r   )r  rk   r  r$  r   r   
<listcomp>?  s     z$test_concatenate.<locals>.<listcomp>)	r   r`   listr   cumsumziprJ  r   r   )sizesstartssplitrs   r   r  r   test_concatenate;  s   
r  c                   C   sH   t jtdd tt dd g W d    d S 1 sw   Y  d S )NzRotation objects onlyr   r    )r'   r(   r  r   rJ  r<  r   r   r   r   test_concatenate_wrong_typeD  s   "r  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  ra  r'   r(   r  )rotation_multi_emptyrotation_multi_onerotation_multirotation_singler   r   r   test_len_and_boolJ  s   
r  )tr'   numpyr   numpy.testingr   r   r   scipy.spatial.transformr   r   scipy.statsr   	itertoolsr   r  r  r   r   r   r#   r*   r.   r/   r4   r=   rD   rI   rN   rQ   rW   r\   rl   rt   rw   r}   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r  r(  r*  r4  r;  r>  r?  rC  rD  rE  rF  rP  rV  rY  r^  re  rx  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   <module>   s    





%)
	
	'	,
	
	