
    ri                        d Z ddlZddlZddlmZ ddlZddlmZm	Z	m
Z
mZmZmZmZmZ ddlmZ ddlmZ ddlmZ ddlmc mZ 	 	 dZdd	d
e	dededede	f
dZd[de	dede	fdZde	de	fdZd[de	dede	fdZde	de	fdZd[de de	dede	fdZ!	 d[de	de de	e"z  dede	f
dZ#	 d[dd	d
e	dedede	fd Z$d
e	de	fd!Z%d[d
e	dede	fd"Z&d
e	de	fd#Z'	 d[dd$d
e	de ded%ede	f
d&Z(	 d[dd$d
e	de
de ded%ede	fd'Z)d
e	de	fd(Z*d
e	de	fd)Z+	 d\d
e	d*e	d+e"dz  dede	f
d,Z,	 	 d]d
e	d-e
dz  d.de-z  e.e-d/f   z  de	fd0Z/	 	 d]d
e	d1e	dz  d2e	dz  de.e	e	dz  e	dz  f   fd3Z0d[d
e	d4e	d5ede	fd6Z1d
e	d7e	d8e-e2z  ez  dz  de	fd9Z3	 d\d:e	d;e	d-e	dz  d<ede.e	e	e	f   f
d=Z4d:e	d;e	d-e	de.e	e	e	f   fd>Z5d:e	d;e	d-e	de.e	e	e	f   fd?Z6d
e	d@e"e	z  de	fdAZ7d
e	de	fdBZ8d
e	de	fdCZ9d.e de-fdDZ:d
e	dEe	dFe	dGe	dHed%ede	fdIZ;de<e-   de	dJede	fdKZ=d.e-dLe	de	fdMZ>dHedNedOe-dPe"d:e	d;e	dQe	dRe	d%ede	fdSZ?dTe	dUe	de	fdVZ@dUe	de.e	e	f   fdWZAde	de	fdXZBde	de	fdYZCy)^zArray API backend for the `Rotation` class.

This module provides generic, functional implementations of the `Rotation` class methods
that work with any Array API-compatible backend.
    N)EllipsisType)array_namespaceArray	ArrayLikeis_lazy_arrayxp_vector_normxp_result_type
xp_promoteis_jax)broadcastabledevice)is_array_api_objF)scalar_firstquat	normalizecopyr   returnc                    t        |       }|r|j                  | dd      } |r|s|j                  | d      } |rt        |       } | S )NaxisTr   )r   rollasarray_normalize_quaternion)r   r   r   r   xps        b/var/www/auto_recruiter/arenv/lib/python3.12/site-packages/scipy/spatial/transform/_rotation_xp.py	from_quatr      sN     
	BwwtRbw)Izz$Tz*$T*K    matrixassume_validc           	         t        |       }t        |       }|s|j                  j                  |       dk  }t	        |      }|s^|j                  |      rMt        |j                  t        j                  |d|            d   d         }t        d| d| |df    d      |r |j                  |d   |j                  |       } | |j                  |       z  }|j                  d	| j                  |
      }|j!                  t        j"                  ||d|      d      }	|rE|j                  j%                  | d      \  }
}}|j                  |	d   | |
|z        } t+        |       S |j!                  |	      sQ|	 }|j                  j%                  | |   d      \  }
}}t        j&                  |       |   j)                  |
|z        } t+        |       S )Nr      ndimr   zSNon-positive determinant (left-handed or null coordinate frame) in rotation matrix z: ...NN   dtyper   g-q=)atolr   )r   r   F)full_matrices)r   	xp_devicelinalgdetr   anyintnonzeroxpx
atleast_nd
ValueErrorwherenanmatrix_transposeeyer+   allisclosesvdatset_from_matrix_orthogonal)r!   r"   r   r   masklazyindgramiansr;   is_orthogonalU_Vtis_not_orthogonals                 r   from_matrixrK   0   s   		 BvFyy}}V$)T"tbjj1!DEaHKLC##&%r&c*:);1>  XXd?3RVVVDFB//77ffQfll6f:KK#Eb9  
  yy}}V5}AHAq"XXmO<fa"fMF #6** &!.yy}}V,=%>e}THAq"VVF^$56::1r6BF"6**r    c                    t        |       }t        |       }| d   | d   z   | d   z   }|j                  | d   | d   | d   |gd      }|j                  |dd      }|j	                  g | j
                  dd	 d
| j                  |      }|j                  d|d   z
  d| d   z  z   | d   | d   z   | d   | d   z   | d   | d   z
  gd      }|j                  |dk(  ||      }|j                  | d   | d   z   d|d   z
  d| d   z  z   | d   | d   z   | d   | d   z
  gd      }|j                  |dk(  ||      }|j                  | d   | d   z   | d   | d   z   d|d   z
  d| d   z  z   | d   | d   z
  gd      }	|j                  |dk(  |	|      }|j                  | d   | d   z
  | d   | d   z
  | d   | d   z
  d|d   z   gd      }
|j                  |dk(  |
|      }t        |      S )z6Convert known orthogonal rotation matrix to quaternion).r   r   ).r$   r$   ).   rM   r   r   T)r   keepdimsNr-      r*   r$   .rM   ).r$   r   ).r   r$   ).rM   r   ).r   rM   ).rM   r$   ).r$   rM   r   r)   )	r   r/   stackargmaxemptyshaper+   r8   r   )r!   r   r   matrix_tracedecisionchoicer   quat_0quat_1quat_2quat_3s              r   rA   rA   V   s   		 BvF)$vi'886);LLLxx			F9-vi/@,O  H YYxb4Y8F88+fll3B'++6<<8OD XXS!!Ay(9$999y 119y 119y 11		
   F 88FaK.D XX9y 11S!!Ay(9$999y 119y 11		
   F 88FaK.D XX9y 119y 11S!!Ay(9$999y 11		
   F 88FaK.D XX9y 119y 119y 11S!!		
   F 88FaK.D &&r    rotvecdegreesc                    t        |       }| j                  d   dk7  rt        d| j                         |rt        |       n| } t	        | dd|      }|dk  }|dz  }d|d	z  z
  |dz  d
z  z   }||j                  ||j                        z   }|j                  |dz        |z  }|j                  |||      }	|j                  | |	z  |j                  |dz        gd      }
|
S )Nr   r)   z/Expected `rot_vec` to have shape (..., 3), got Tr   rN   r   MbP?rM   g      ?0   i   r+   r   )r   rS   r7   _deg2radr   r   r+   sinr8   concatcos)r[   r\   r   anglesmall_angleangle2small_scale	div_anglelarge_scalescaler   s              r   from_rotvecrm      s    		 B||B1=fll^L
 	
 ")XffF6TbAE4-KAXF#fai$&66K
 

;ekk
BBI&&#i/KHH[+{;E99funbffUQY&78r9BDKr    mrpc                    t        |       }| j                  d   dk7  rt        d| j                         |j                  j	                  | | d      d   dz   }|j                  d| dd df   z  d|z
  gd      }||z  }|S )	Nr   r)   z+Expected `mrp` to have shape (..., 3), got r   .Nr$   rM   .)r   rS   r7   r0   vecdotrd   )rn   r   mrp2_plus_1	q_no_normr   s        r   from_mrprt      s    		B
yy}FsyykRSS))""3""5i@1DK		1s37|+a+o?b	II{"DKr    seqanglesc                 Z    t        |      }t               }|dk  s|dkD  rt        d        t        j                  d       d u}t        j                  d       d u}|s|st        d        t         fdt        |dz
        D              rt        d        |rt        |      }t        j                  |d|	      }|j                  d
   |k7  rt        d|j                  d
    d       j                         D cg c]  }t        |       }}t        |||      }	|	S c c}w )Nr$   r)   zPExpected axis specification to be a non-empty string of up to 3 characters, got ^[XYZ]{1,3}$^[xyz]{1,3}$LExpected axes from `seq` to be from ['x', 'y', 'z'] or ['X', 'Y', 'Z'], got c              3   :   K   | ]  }|   |d z      k(    ywr$   N .0iru   s     r   	<genexpr>zfrom_euler.<locals>.<genexpr>   s"     
=A3q6SQZ
=   /Expected consecutive axes to be different, got r%   r   zTExpected last dimension of `angles` to match number of sequence axes specified, got r'   )r   lenr7   rematchr2   rangerb   r5   r6   rS   lower_elementary_basis_index_elementary_quat_compose)
ru   rv   r\   r   num_axes	intrinsic	extrinsicxaxesqs
   `         r   
from_eulerr      sI   		 B3xH!|x!|1147
 	

 #.d:I#.d:I,,/52
 	

 
=x!|)<
==J3%PQQ&!^^Fr2F||B8#$ll2./q2
 	
 14		<1#A&<D< vy9AH =s   D(r   orderc                    t        |       }t        |       }|dv rd}n|dv rd}nt        d|       | j                  d   dk7  rt        d      t	        j
                  | d	|
      } t	        j
                  |d|
      }| j                  d   }|dk  s|dkD  rt        d|       | t        | dd|      z  } |j                  | j                  d d |j                  |      }|dkD  r7||j                  |j                  | ddd d f   | ddd d f               dkD  z  }|d	kD  r7||j                  |j                  | ddd d f   | dd	d d f               dkD  z  }t        |      s|j                  |      rt        d      |j                  |d   |j                  |       } |rt        |      }t!        | j                  d d |j                        r| j                  d   |j                  d   k7  r&t        d|j                   d| j                   d      |j                  d d dz   }	|j                  |	|j"                  t        |            }
t	        j$                  |
      d   j'                  d      }
t)        |      D ]8  }t+        |d|d f   | d|d d f   z        }|rt-        ||
      nt-        |
|      }
: |
S )Ner   Tr   r   Fhorder should be 'e'/'extrinsic' for extrinsic sequences or 'i'/'intrinsic' for intrinsic sequences, got r   r)   !Axes must be vectors of length 3.rM   r%   r$   r-   zExpected up to 3 axes, got r^   r*   .r   Hz>$Consecutive axes must be orthogonal.r(   z/Expected `angles` to match number of axes, got z angles and z axes.)rO   .r)   )r   r/   r7   rS   r5   r6   r   zerosboolabsrq   r   r2   r8   r9   rb   r   r+   r?   r@   r   rm   compose_quat)r   r   rv   r\   r   r   r   r   axes_not_orthogonalq_shaper   r   qis                r   from_davenportr      s    
	Bt_F""		$	$	88=w@
 	

 zz"~<==>>$Q2.D^^Fr2Fzz"~H!|x!|6xjABB.B"EED ((4::cr?"''&(Q!|1FF299T#q!)_d319o>?$F
 !|1FF299T#q!)_d319o>?$F
 ,-"&&9L2M?@@xx+O<bffdK&! $**Sb/6<<8::b>V\\"--=fll^ L::,f&
 	

 ll3B$&G
Yv5FGAq	&a A8_ FQ-S!QY?@#,LQ,q"2EF Hr    	canonicalc                b    t        |       }|rt        |       } |r|j                  | dd      } | S )Nr$   r   r   )r   _quat_canonicalr   )r   r   r   r   s       r   as_quatr      s7     
	Bt$wwtQRw(Kr    c           
         t        |       }| d   }| d   }| d   }| d   }||z  }||z  }||z  }||z  }	||z  }
||z  }||z  }||z  }||z  }||z  }||z
  |z
  |	z   d|
|z
  z  d||z   z  d|
|z   z  | |z   |z
  |	z   d||z
  z  d||z
  z  d||z   z  | |z
  |z   |	z   g	}|j                  |j                  |d      g | j                  d d dd      }|S )	N.r   .r$   .rM   r   rM   r   r   r)   )r   reshaperP   rS   )r   r   r   yzwx2y2z2w2xyzwxzywyzxwmatrix_elementsr!   s                     r   	as_matrixr   +  sH   		BVAVAVAVA	
QB	
QB	
QB	
QB	
QB	
QB	
QB	
QB	
QB	
QB 	R"r	R"W	R"W	R"W	b2	R"W	R"W	R"W	b2
O ZZr:<Tdjj"o<Tq<TRS<TUFMr    c                    t        |       }t        |       } t        | dd df   dd|      }d|j                  || d   d         z  }|d	k  }|dz  }d|d
z  z   d|dz  z  dz  z   }|j	                  |dz        |j                  ||j                        z   }||z  }	|j                  |||	      }
|rt        |
      }
|
| dd df   z  }|S )N.r)   r   Tr^   rM   r   rp   r_         i@         @ra   )	r   r   r   atan2rc   r   r+   r8   _rad2deg)r   r\   r   ax_normrf   rg   rh   ri   div_sinrk   rl   r[   s               r   	as_rotvecr   M  s    		B4 DT#rr']drJG$v,y"9::E4-KAXFfrk/A	MD$88K
 ffUS[!BJJ{%++J$NNG'/KHH[+{;ET#rr']"FMr    c                     t        |       }|j                  dt        |       | j                        }|j	                  | d   dk  d|      }d|| d   z  z   }|| dd df   z  |z  S )N      ?r   r+   ).r)   Nr   r   .r)   )r   r   r/   r+   r8   )r   r   onesigndenominators        r   as_mrpr   b  st    		B
**S4

*
CC88D&*B4DtL111K$sBQBw-+--r    )suppress_warningsr   c                   t        |       }t              dk7  rt        d d      t        j                  d      d u}t        j                  d      d u}|s|st        d       t        fdt        d      D              rt        d	       t        |       }j                         D cg c]  }t        |       }	}|r|	n|	d d d
   }	|	\  }
}}|
|k(  }|rd|
z
  |z
  n|}|j                  ||      }|j                  |
|z
  ||z
  z  ||
z
  z  dz  | j                  |      }|j                  || d   | d   | d|f   z
        }|j                  || d|
f   | d|
f   | d|f   |z  z         }|j                  || d|f   | d|f   | d   z         }|j                  || d|f   |z  | d|f   |z  | d|
f   z
        }t        ||||j                  dz  |||||	      }|rt        |      S |S c c}w )Nr)   Expected 3 axes, got r'   rx   ry   rz   c              3   :   K   | ]  }|   |d z      k(    ywr|   r}   r~   s     r   r   zas_euler.<locals>.<genexpr>y  s"     
2A3q6SQZ
2r   rM   r   r   r   r*   r   .)r   r   r7   r   r   r2   r   r/   r   r   r   r+   r8   _get_anglespir   )r   ru   r\   r   r   r   r   r   r   r   r   jk	symmetricrB   r   abcdrv   s    `                   r   as_eulerr   j  sE    
	B 3x1}0Q788#.d:I#.d:I%
 	
 
2q
22J3%PQQt_F03		<1#A&<D<4$tt*DGAq!QIA	AA::i:/D::q1uQ'1q51Q6djjQW:XD
tF|T&\DaL%@AA
tCF|T#q&\DaL44G%GHA
tCF|T#q&\DL%@AA
tCF|d*DaL4,?$sAv,,NOA9dBEEAIq!Q;LF  '8F2F2# =s   4G4c          
      8   t        |       }|dv rd}n|dv rd}nt        d|       |j                  d   dk7  rt        d|j                   d	      |j                  d
   dk7  rt        d      t        |j                  d d | j                  d d
       s&t        d|j                   d| j                   d      |t	        |d
d|      z  }|j                  |ddd d f   |ddd d f         }|j                  |ddd d f   |ddd d f         }|dk\  |dk\  z  }	t        |	      r!|j                  |	d   |j                  |      }n|j                  |	      rt        d      t        | |ddd d f   |ddd d f   |ddd d f   ||      }
|rt        |
      }
|
S )Nr   Tr   Fr   r-   r)   r   r'   r   r   z2Expected `axes` to match number of rotations, got z
 axes and  rotations.r^   .r   r$   rM   r   r(   r   )r   r7   rS   r   r   rq   r   r8   r9   r2   _compute_davenport_from_quatr   )r   r   r   r\   r   r   r   vdot_ax0_ax1vdot_ax1_ax2
is_invalidrv   s              r   as_davenportr     s    
	B ""		$	$	--2G5
 	
 zz"~0A>??zz"~<==CR$**Sb/:@ M::,k+
 	
 .B"EED99T#q!)_d319o>L99T#q!)_d319o>L$&<4+?@JZ xx
?3RVVTB	
	?@@)S!QYS!QYS!QYF &!Mr    c                 \    t        j                  |       dd df   j                  dd      S )N.r)   r   Tr   )r5   r?   multiply)r   s    r   invr     s-    66$<RaR ))"4)88r    c                     t        |       }t        | dd df   d|      }|j                  | d         }d|j                  ||      z  }|S )N.r)   r   r   r   r   rM   )r   r   r   r   )r   r   sin_qcos_qrv   s        r   	magnituder     sR    		B4RaR=rb9EFF4< E%''FMr    otherr,   c                 :   ||rt        j                  dd       d}n|rt        |      }t        | j                  |j                        s,t        d| j                  d d  d|j                  d d  d      t        |t        |             }t        |      }||k  S )	NzEatol must be set to use the degrees flag, defaulting to 1e-8 radians.rM   
stacklevelg:0yE>z5Expected broadcastable shapes in both rotations, got r   z rotations in first and z rotations in second object.)	warningswarnrb   r   rS   r7   r   r   r   )r   r   r,   r\   quat_resultrv   s         r   approx_equalr     s     |MMW 	~U[[1CDJJsPROCT U&&+kk#2&6%77SU
 	

 uc$i0K{#FD=r    weightsr   .c                      t               }t               }t         d|      } j                  d   dk(  rt	        d      t        t         j                  dz
              }|nt        t              rft        t
              st	        d      dk7  rVt               j                  dz
   k  st               j                  dz
  kD  rt	        d	 d
 j                  d d  d      t        t        t         fdD                          t               } dd d d f   }||j                  |      |z  }	n|j!                  |||      }|dk  }
|s|j#                  |
      rt	        d      |r|j%                  |
|j&                  |      }t)         j                  d d |j                        s)t	        d|j                   d j                  d d  d      |d   |z  }|j                  |      |z  }	t        fd|D              }|z   }|j+                  |	||      }|j                  d t-        |       dz   }|j/                  |j1                  ||      d      }	|j2                  j5                  |	      \  }}|d   S )NTforce_floatingr   r   z+Mean of an empty rotation set is undefined.r$   z+`axis` must be None, int, or tuple of ints.r}   rM   zaxis z* is out of bounds for rotation with shape r   r'   c              3   B   K   | ]  }|j                   d z
  z    ywr|   )r&   )r   r   r   s     r   r   zmean.<locals>.<genexpr>  s     >AAQ/>s   .r*   z`weights` must be non-negative.zDExpected `weights` to be broadcastable to rotation shape, got shape z for r   r(   c              3   ,   K   | ]  }|vs|  y w)Nr}   )r   r   r   s     r   r   zmean.<locals>.<genexpr>  s     ;AQd]a;s   	)r   rO   rO   r   .r   )r   r/   r	   rS   r7   tupler   r&   
isinstancer3   minmaxsortedr@   r   r:   r   r2   r8   r9   r   moveaxisr   meanr   r0   eigh)r   r   r   r   r   r+   all_axesrC   quat_expandKneg_weightsweighted_quat	keep_axes
axes_orderK_reordered	new_shaperH   vs   ` `               r   r   r     s   
 
	Bt_F4<Ezz!}FGG U499q=)*H|	D#	wdE"FGGrzs4yTYY]#33s4yDIIPQM7RD6CDJJsPROCTTUV
 	
 >>>?@DDsD!|$K,{:**WE&*Ak{+>?? hh{BFFG<GTZZ_gmm<V==/tzz#2&7{D   0;>.< ;;;IT!J++aX6K!!"2C	N3j@I


;	2<A99>>!DAqW:r    leftrightc           
         ||| d d fS t        |       }t        j                  | d|      } ||j                  |       }||j                  |       }| }t	        ||      \  }}t	        ||      \  }}t	        ||      \  }	}
|dd d d d f   |dd d d d f   z  |	dd d d d f   z  }|j                  |dd d d d d f   |
dd d d d d f   z  d      }|dd d d d f   |dd d d d d f   z  }|j                  |dd d d d d f   |
dd d d d d f   z  d      }|dd d d d f   |dd d d d d f   z  }|j                  |dd d d d d f   |dd d d d d f   z  d      }|	dd d d d f   |dd d d d d f   z  }|dd d d d d f   }|dd d d d d f   }|j                  j                  ||      }|j                  |dd d d d d d d f   |
dd d d d d d f   z  d      }|j                  ||z
  |z
  |z
  |z
        }|j                  |j                  |dd      |j                  d   df      }|j                  |j                  ||j                  d   df      d      }||
j                  d   z  }||
j                  d   z  }|j                  |j                  |j                  d         d      }|j                  |d	      }|||f   }|j                  |d	      }|||f   }t        |t        ||            }|d }|d }|||fS )
NrM   r%   .r   r   r$   r   )r$   r   )r   r$   )r   r5   r6   	ones_like_split_rotationsumr0   crossr   r   r   rS   rQ   aranger   )r   r  r  r   ppspvlslvrsrvterm1prvterm2lrvterm3lpvterm4lv_expandedpv_expandedcross_lpterm5qsmax_ind	left_best
right_bestall_idxleft_idx	right_idxreduceds                                 r   reducer-  )  sw   
 |T4 
	B>>$Q2.D|||D!}T" 	AQ#FBT2&FBUB'FB sAtT!"RT1d(:%;;bdDRSAS>TTE
&&CD!O$r#tQ/'::&
DCsAtT!"SdAq%99E
&&CD!O$r#tQ/'::&
DCsD!T!"Saq%99E
&&CD!O$r#tQ/'::&
DCsD$!"SaD%99E
 S!T1_%KS$1_%Kyy{K8HFF8CAtQ./"S$a5J2KKRTFUE	%-5	6B	BKKAq)BHHQK+<	=B ii

2R'89iBG288A;&I288A;&J jj4::b>2G<Gzz)W-H'!"D

:w/I)W$%E 4a!78G|	}
Iz))r    pointsinversec                    t        |       }t        |       }|d   }t        |j                  |j                        s,t	        d| j                  d d  d|j                  d d  d      |r|j                  |      |z  d   S ||z  d   S )Nrp   zCannot broadcast r   z rotations to 	 vectors.r   )r   r   r   rS   r7   r:   )r   r.  r/  r   mats        r   applyr3  t  s    		B
D/C IFFLL1

3B0v||CR?P>Q R 
 	
 ##C(616::&L&!!r    valueindexerc                 T    t        j                  |       |df   j                  |      S N.)r5   r?   r@   )r   r4  r5  s      r   setitemr8    s&     66$<%))%00r    r   r   return_sensitivityc                 @   t        |       }t        | |d|      }|j                  | |      }|j                  ||      }t        j                  |d|      } t        j                  |d|      }| j
                  d   dk7  rt        d|j
                         |j
                  d   dk7  rt        d	|j
                         | j
                  |j
                  k7  r&t        d
|j
                   d|j
                   d      | j                  dkD  s|j                  dkD  r&t        d|j
                   d|j
                   d      | j
                  d   }|(|j                  |t        |       | j                        }n|j                  |t        |       | j                        }|j                  dk7  rt        d|j
                   d      |dkD  r1|j
                  d   |k7  rt        d|j
                  d    d| d      |dk  }	t        |	      s|j                  |	      rt        d      |j                  |	|j                  |      }|dk(  r|j                  dg      n||j                  k(  }
|j!                  |j#                  |
| j                              }t        |      s"|dkD  rt        d      |dk(  r|rt        d      |j                  |dkD  |j                  |      }|j                  |
d      }t        |      r\t%        | ||      \  }}}t'        | ||      \  }}}|j                  |||      }|j                  |||      }|j                  |||      }n4|j                  |      rt'        | ||      \  }}}nt%        | ||      \  }}}|||fS )NTr   ra   rM   r%   r   r)   z5Expected input `a` to have shape (3,) or (N, 3), got z5Expected input `b` to have shape (3,) or (N, 3), got z5Expected inputs `a` and `b` to have same shapes, got z and z respectively.z>Expected inputs `a` and `b` to have shape (3,) or (N, 3), got r   r   r$   z2Expected `weights` to be 1 dimensional, got shape r'   zRExpected `weights` to have number of values equal to number of input vectors, got z values and r1  z)`weights` may not contain negative valuesz#Only one infinite weight is allowedzKCannot return sensitivity matrix with an infinite weight or one vector pairr   )r   r	   r   r5   r6   rS   r7   r&   onesr/   r+   r   r2   r8   r9   infr  astype_align_vectors_align_vectors_fixed)r   r   r   r9  r   r+   
a_original
b_originalNnegative_weightsweight_is_infn_inf
inf_branchq_optrssdsensitivity	q_opt_infrssd_infsensitivity_infs                      r   align_vectorsrM    s    
	B1a<EAU+JAU+Jzb1Azb1Awwr{aCJDTDTCUV
 	
 	wwr{aCJDTDTCUV
 	
 	ww!''%%&eJ,<,<+=^M
 	
 	vvzQVVaZL j&6&6%7~G
 	
 	

A ''!IaL'@**WYq\*I<<1DW]]OSTU  q5gmmA&!+  'a 01aS	K  #Q;-.266:J3KHII((+RVVW= +,q&BJJv&g6GMFF299]AGG45E 19BCCA:,5 
 hhuqy"&&'2GB/J Z #1!Q#@ t[/CAq'/R,	8_Y6xx
Hd3hhz?KH66*';Aq''J$E4'5aG'D$E4$##r    c           
      8   t        |       }t        |       }|d d d f   | z  j                  |z  }|j                  j	                  |      \  }}}|j                  j                  ||z        dk  }	t        j                  |      d   j                  |j                  |	|d    |d               }t        j                  |      dd d df   j                  |j                  |	|dd d df    |dd d df               }||z  }
|j                  ||j                  |dz  | dz  z   d      z  d      d|j                  |d      z  z
  }|j                  |j                  ||j                  d|                  d	   }|d	   |d
   z   |d
   |d   z   z  |d   |d	   z   z  }|d	   |d
   z  |d
   |d   z  z   |d   |d	   z  z   }|j                  d| j                  |      }|j!                  |      |z  ||z  ||j                  z  z   z  }t#        |
      }|||fS )Nr   r   .r   rM   r   r$   r   r   r   r   r)   r*   )r   r/   mTr0   r>   r1   r5   r?   r@   r8   r  sqrtmaximumr   r;   r+   r   rA   )r   r   r   r   r   Busvhneg_detCssdrH  zetakappar;   rI  rG  s                     r   r>  r>    s5   		Bq\F	D	A	!!A%Ayy}}QHAq" iimmAF#a'Gq	'rxx!G*ajIJAq	#q"*!!"((7QsArz]NAc1bjM"RSA	BA
 &&266!Q$A+B677b&
AA	 IO I E C 772::c288Af8#=>?GD fI&	!ai!F)&;<&	AfI@UVDfI&	!AfI&	$99AfI&	<QQE
&&!''&&
1C'''"T)US[1qtt8-CDK#A&E$##r    c                    t        |       }t        |       }| j                  d   }|dk(  r|j                  dg|      n||j                  k(  }|j                  |j                  ||j                              }t        |      st        |      }|j                  | | d      }|j                  || d      }	|j                  || d      }
|d   d   }|	d   d   }t        |dd|	      }t        |dd|	      }t        |      s3|j                  |dk(        s|j                  |dk(        rt        d
      t        j                   |      |dk(     j#                  |j$                        }t        j                   |      |dk(     j#                  |j$                        }||z  }||z  }|j&                  j)                  |ddd d f   |ddd d f         }t        |d|      }|j+                  ||j-                  |ddd d f   |ddd d f   z  d            }d}|j                  g d|j.                  |      }|j0                  |z
  |k  }|j3                  |j5                  |ddd d f               }|j7                  |j9                  |dk(  |d   |j9                  |dk(  |d    d            |j9                  |dk(  |d   |j9                  |dk(  |d    d            |j9                  |dk(  |d   |j9                  |dk(  |d    d            g      }|j9                  |dk(  ||      }|j9                  |t;        |t        ||      z  |j0                  z        |      }|j9                  ||j0                  |z
  |      }|j9                  || |      }||z  }|j5                  |      |k  }|d|dz  z   ||z  dz  dz  z   z  }||j                  ||j.                        z   }||z  |j=                  |      z  }|j9                  |||      }t?        t;        |      |      }|dd df   }|	dd df   }|
dd  }tA        ||      } |j&                  jC                  |j&                  j)                  | |      |      }!|j&                  jC                  | |      |j&                  jC                  | |      |j&                  jC                  ||      z  z
  }"|j+                  |j-                  ||!z        |j-                  ||"z              }#t;        |#|d   z        }$|dk(  r|nt?        |$|      }%|dkD  |d   |j                  k(  z  |z  }&t        j                   ||&      j#                  dd|      }'tA        |%|      }(|jE                  |j-                  |'| |(z
  dz  z              })|j                  |jG                  |      d      }&|j9                  |&|j$                  |%      }%|%|)|j                  |j$                  |      fS )Nr   r$   Tr   )shiftr   )r   .r7  r   r^   z(Cannot align zero length primary vectors.r   r   r_   )        r]  r]  r   r*   )r   rM   rM   )r   r$   )r   r   )r      r   ih  ra   )r   r   )$r   r/   rS   r   r<  rQ   r=  uint8r   r3   r   r   r   r2   r7   r5   r?   r@   r9   r0   r  r   r  r+   r   argminr   rP   r8   rm   rc   r   r3  rq   rP  isnan)*r   r   r   r   r   rB  rD  inf_idxa_sortedb_sortedweights_sorteda_prib_pri
a_pri_norm
b_pri_normr  
cross_normtheta	toleranceq_flipflipr   r_componentsrtheta2ri   r_small_scaler_large_scaleq_pria_secb_secweights_secc_secsin_termcos_termphiq_secrG  rB   weights_inf_zeroa_estrH  s*                                             r   r?  r?    s    
	Bq\F	
A9:aBJJvfJ5WPRPVPVEVM ii		-:;G ":g,wwqqw1Hwwqqw1HWWWWH1W=NVY'EVY'EB"EJB"EJ$66*/"bffZ1_&=GHH 
#J!O488@J
#J!O488@JJEJE IIOOE#q!),eCAI.>?EB26JHHZc1ai(85a;K(KRT!UVEIZZ,EKKZOF 555=9$D 			"&&sAqy)*+A 88HHQ!VU4["((16E$K<*KLHHQ!VU4["((16E$K<*KLHHQ!VU4["((16E$K<*KL	
L 	q,6AXXdKN1,D(Druu(LMvVFHHT2555=%0EHHTE65)E U]F&&-)+KQ!^fvo.AC.GGHM
 BJJ{%++J>>EEMBFF5M1M
m];AQ0E
 QRWEQRWE $K %E yy		u =uEHyyu-
		&)9)9%)GGH ((266+01266+:P3Q
RCeFm+,E !VEeU!;E
 UwqzRVV+,=D vvgt,00"0E%OE77266*a%iA-==>?D66"((7#"6-DHHT2665)E$

266&
999r    nc                 f   t        |       }t        |       }t        |      rA|j                  dk(  r|d   }n|j                  dk7  rt        d      t        | |d|      \  } }t        |      rt        |t        |       z        }|j                  g | j                  d d d| j                  |      }t        j                  |      d	   j                  d
      }|j                  |dk(  ||      }|j                  |dk(  t!        |       |      }|j                  |d
k(  | |      }|S |dk(  rW|j                  g | j                  d d d| j                  |      }t        j                  |      d	   j                  d
      S |dk(  rt!        |       S |d
k(  r| S t        |t        |       z        S )N)r$   r   zArray exponent must be a scalarTr   r   rO   r*   r   r$   )r   r/   r   rS   r&   r7   r
   r   rm   r   r   r+   r5   r?   r@   r8   r   )r   r  r   r   resultidentitys         r   powr    s   		Bt_F 77d?!AVVq[>??T1TbAa QQ401881djj"o1q1F8S66(#F+//2!q&(F3!r'3t9f5!q&$/Av881djj"o1q1F8Svvh'++A..Bw4yAvq9T?*++r    c                     t        |       }t        | dd|      }|dk(  }t        |      r"|j                  ||j                  |       } | |z  S |j                  |      rt        d      | |z  S )Nr   Tr^   r   z&Found zero norm quaternions in `quat`.)r   r   r   r8   r9   r2   r7   )r   r   	quat_norm	zero_norms       r   r   r     su    		Bt"tCIQIYxx	26640 ) 
		ABB)r    c                     t        |       }| d   dk  }| d   dk(  }||| d   dk  z  z  }|| d   dk(  z  }||| d   dk  z  z  }|| d   dk(  z  }||| d   dk  z  z  }|j                  |d   |  |       S )Nr   r   r   r   r   rp   )r   r8   )r   r   rB   zero_wzero_wxzero_wxys         r   r   r     s    		B<!D&\QF6T&\A-./DV)*G7d6lQ./0D$v,!+,H8tF|a/01D88DOdUD11r    c                 B    | dk(  ry| dk(  ry| dk(  ryt        d|        )Nr   r   r   r$   r   rM   z.Expected axis to be from ['x', 'y', 'z'], got )r7   r   s    r   r   r     s3    s{		
EdVL
MMr    n1n2n3r   c                    t        |       }|r||fn||f\  }}|j                  j                  ||      }|j                  |j	                  ||      |j	                  ||            }|dk  }	|j                  |	d   | |      }|j                  |	| |      }|j                  |	d   | |      }|	}
|j                  |j                  |dz        d   |z  |j                  |dz        d   fd      }t        ||       }|d   }|j                  j	                  |dd df   |      }|j                  j	                  |dd df   |      }|j                  j	                  |dd df   |      }t        |d	d
||||||	      }t        j                  |      d   j                  |j                  |
|d    |d               }|S )Nr   rp   rM   r   r   r   .r)   Fr$   r   )r   r0   r  r   rq   r8   rd   rc   re   r   r   r5   r?   r@   )r   r  r  r  r   r   r   n_crosslambrB   correct_set	quat_lambq_transr   r   r   r   rv   s                     r   r   r     s    
	B"b"XRFBiioob"%G88BIIb'*BIIb",=>D !8D	$y/B3	+B88D4%&DhhtI':GK			q	)	$r	)266$(+;I+FGb  I 9d+GA
		bqb)2.A
		bqb)2.A
		bqb)73AE1dAq!Q@QRFVVF^F#''
vf~ovf~>F Mr    r   c                     t        |      }t        |      }t        | d   |d   ||      }t        dt	        |             D ]3  }t        | |   |d|f   ||      }|rt        ||      nt        ||      }5 |S )Nr   r   )r   r   r$   .)r   r/   _make_elementary_quatr   r   r   )r   rv   r   r   r   r   r   ax_quats           r   r   r     s    		 BvF a&.BOD1c$i  Y'QQSUV.7|D'*\'SW=XY Kr    rf   c                 H   |j                  g |j                  d|j                  |      }t        j                  |      d   j                  |j                  |dz              }t        j                  |      d| f   j                  |j                  |dz              }|S )NrO   r*   r   r   .)r   rS   r+   r5   r?   r@   re   rc   )r   rf   r   r   r   s        r   r  r    s    88%ekk%1%U[[8HD66$<##BFF53;$78D66$<T	"&&rvveck':;DKr    r   r   r  r   r   c	           
         t        |      }	t        |      }
d}|	j                  ||      }|	j                  ||      }|	j                  g |j                  d|j
                  |
      }t        j                  |      d   j                  d|	j                  |	j                  ||      |	j                  ||            z        }| rdnd}| rdnd}|	j                  |d         |k  }|	j                  |d   |	j                  z
        |k  }||z   }|s4t        |      s)|	j                  |       rt        j                  dd       |	j!                  |d|z  d|z  | rd	nd
z        }t        j                  |      d   j                  |      }|	j!                  |||z
  |d|f         }t        j                  |      d|f   j                  |      }|	j!                  |||z   |d|f         }|s2||z  }t        j                  |      d   j                  |d   |z
        }t        j                  |      d|f   j                  |      }||	j                  z   d|	j                  z  z  |	j                  z
  }|S )Nr   r)   r*   r   rM   r   zlGimbal lock detected. Setting third angle to zero since it is not possible to uniquely determine all angles.r   r   r$   r   .)r   r/   r   r   rS   r+   r5   r?   r@   hypotr   r   r   r2   r   r   r8   )r   r   r   r  r   r   r   r   r   r   r   epshalf_sum	half_diffrv   angle_firstangle_thirdcase1case2case0a0a1a3s                          r   r   r     sK    
	Bq\F
Cxx1~HAI XXmmm1776XBFVVF^F#''BHHRXXa^RXXaQR^,T(TUF !aK !aK FF6&>"c)EFF6&>BEE)*c1EemE]5%9bffeVn 		
 
%Xq9}iQ'O	PBVVF^F#''+F 
%I-vc;6F/G	HBVVF^C,-11"5F 
%I-vc;6F/G	HB$Y'++F6NT,ABVVF^C,-11"5FruunRUU+bee3FMr    r  r   c                    t        |       }|j                  j                  | dd df   |dd df         }| d   |d   z  |d   | d   z  z   |d   z   }| d   |d   z  |d   | d   z  z   |d   z   }| d   |d   z  |d   | d   z  z   |d   z   }| d   |d   z  | d   |d   z  z
  | d   |d   z  z
  | d   |d   z  z
  }|j                  ||||gd      }|S )	N.r)   r   r   r   r   r   r   )r   r0   r  rP   )	r  r   r   r  qxqyqzqwr   s	            r   r   r   W  s7   		BIIOOAc2A2gJ#rr'
3E	
6QvY	6QvY!6	6v	FB	
6QvY	6QvY!6	6v	FB	
6QvY	6QvY!6	6v	FB	&	AfI
F)ai
	 
F)ai
	  F)ai
	   88RR$28.DKr    c                 N    t        j                  | d|      } | d   | dd df   fS )NrM   r%   r   .r   )r5   r6   )r   r   s     r   r  r  g  s/    qqR(AW:qcrc{""r    c                 .    | t         j                  dz  z  S Ng     f@npr   rv   s    r   rb   rb   l  s    RUUU]##r    c                 .    | dt         j                  z  z  S r  r  r  s    r   r   r   p  s    URUU]##r    )TT)F)NF)NN)D__doc__r   r   typesr   numpyr  scipy._lib._array_apir   r   r   r   r   r	   r
   r   scipy._lib._utilr   scipy._lib.array_api_compatr   r/   r   scipy._lib.array_api_extra_libarray_api_extrar5   r   r   rK   rA   rm   rt   strr   floatr   r   r   r   r   r   r   r   r   r   r3   r   r   r-  r3  slicer8  rM  r>  r?  r  r   r   r   r   listr   r  r   r   r  rb   r   r}   r    r   <module>r     s   
   	 	 	 + ; 8 ( (
 
 
 
  $#+ #+T #+e #+LC'E C'e C'L   .% E  C      %  H EJ=
==%*U]==A=
=B $)BG
 ;?
E e DE D U *. .5 . ,1$3OT$3
$3$3$($3HL$3
$3V 	2 $2
2
2 2 	2 2 2j9e 9 9E e  KP
%*T\CG
4 !%)-<
<< *uS#X
&< 	<B H*
H*
$,H* 4<H* 5%$,,-	H*V" "u "t " ""1
11(+el(BT(I1
1 RW^$^$^$!&^$JN^$
5%^$B$e $ $ $%ue@S:T $DJ:J:J:!&J:
5%J:Z,e , ,% ,@ % 	2% 	2E 	2N# N# N)
)) 	) 		)
 ) ) )X49 e  QV  E % ;;; ; 	;
 ; ; ; ; ; ;|E e   #u #U5%<%8 #
$U $u $$U $u $r    