
    > j                        S r SSKrSSKrSSKJr  SSKrSSKJrJ	r	J
r
JrJrJrJrJr  SSKJr  SSKJr  SSKJr  SSKJs  Jr    SZSS	.S
\	S\S\S\S\	4
S jjjrS[S\	S\S\	4S jjrS\	S\	4S jrS[S\	S\S\	4S jjrS\	S\	4S jrS[S\ S\	S\S\	4S jjr! S[S\	S\ S\	\"-  S\S\	4
S jjr# S[SS	.S
\	S\S\S\	4S  jjjr$S
\	S\	4S! jr%S[S
\	S\S\	4S" jjr&S
\	S\	4S# jr' S[SS$.S
\	S\ S\S%\S\	4
S& jjjr( S[SS$.S
\	S\
S\ S\S%\S\	4S' jjjr)S
\	S\	4S( jr*S
\	S\	4S) jr+ S\S
\	S*\	S+\"S-  S\S\	4
S, jjr,  S]S
\	S-\
S-  S.S\--  \.\-S/4   -  S\	4S0 jjr/  S]S
\	S1\	S-  S2\	S-  S\.\	\	S-  \	S-  4   4S3 jjr0S[S
\	S4\	S5\S\	4S6 jjr1S
\	S7\	S8\-\2-  \-  S-  S\	4S9 jr3 S\S:\	S;\	S-\	S-  S<\S\.\	\	\	4   4
S= jjr4S:\	S;\	S-\	S\.\	\	\	4   4S> jr5S:\	S;\	S-\	S\.\	\	\	4   4S? jr6S
\	S@\"\	-  S\	4SA jr7S
\	S\	4SB jr8S
\	S\	4SC jr9S.\ S\-4SD jr:S
\	SE\	SF\	SG\	SH\S%\S\	4SI jr;S\<\-   S\	SJ\S\	4SK jr=S.\-SL\	S\	4SM jr>SH\SN\SO\-SP\"S:\	S;\	SQ\	SR\	S%\S\	4SS jr?ST\	SU\	S\	4SV jr@SU\	S\.\	\	4   4SW jrAS\	S\	4SX jrBS\	S\	4SY jrCg)^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                    [        U 5      nU(       a  UR                  U SSS9n U(       a  U(       d  UR                  U SS9n U(       a  [        U 5      n U $ )NaxisTr   )r   rollasarray_normalize_quaternion)r   r   r   r   xps        u/root/GenerationalWealth/GenerationalWealth/venv/lib/python3.13/site-packages/scipy/spatial/transform/_rotation_xp.py	from_quatr      sN     
	BwwtRbw)Izz$Tz*$T*K    matrixassume_validc           
         [        U 5      n[        U 5      nU(       Gd  UR                  R                  U 5      S:*  n[	        U5      nU(       d`  UR                  U5      (       aJ  [        UR                  [        R                  " USUS95      S   S   5      n[        SU SXS4    S35      eU(       a   UR                  US   UR                  U 5      n XR                  U 5      -  nUR                  S	U R                  US
9nUR!                  [        R"                  " XxSUS9SS9n	U(       a6  UR                  R%                  U SS9u  pnUR                  U	S   X
U-  5      n ObUR!                  U	5      (       dL  U	) nUR                  R%                  X   SS9u  pn[        R&                  " U 5      U   R)                  X-  5      n [+        U 5      $ )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//77ffQfll6f:KKEb9  
  yy}}V5}AHA"XXmO<f"fMF&&!.yy}}V%>e}THA"VVF^$56::16BF"6**r    c                    [        U 5      n[        U 5      nU S   U S   -   U S   -   nUR                  U S   U S   U S   U/SS9nUR                  USSS9nUR	                  / U R
                  SS	 QS
P7U R                  US9nUR                  SUS   -
  SU S   -  -   U S   U S   -   U S   U S   -   U S   U S   -
  /SS9nUR                  US:H  Xv5      nUR                  U S   U S   -   SUS   -
  SU S   -  -   U S   U S   -   U S   U S   -
  /SS9nUR                  US:H  X5      nUR                  U S   U S   -   U S   U S   -   SUS   -
  SU S   -  -   U S   U S   -
  /SS9n	UR                  US:H  X5      nUR                  U S   U S   -
  U S   U S   -
  U S   U S   -
  SUS   -   /SS9n
UR                  US:H  X5      n[        U5      $ )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                    [        U 5      nU R                  S   S:w  a  [        SU R                   35      eU(       a  [        U 5      OU n [	        U SSUS9nUS:*  nUS-  nSUS	-  -
  US-  S
-  -   nX2R                  XCR                  S9-   nUR                  US-  5      U-  nUR                  XFU5      n	UR                  X	-  UR                  US-  5      /SS9n
U
$ )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
 

;kk
BBI&&#i/KHH[{;E99fnbffUQY&78r9BDKr    mrpc                     [        U 5      nU R                  S   S:w  a  [        SU R                   35      eUR                  R	                  X SS9S   S-   nUR                  SU SS S24   -  SU-
  /SS9nX2-  nU$ )	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                   ^  [        U5      n[        T 5      nUS:  d  US:  a  [        ST  35      e[        R                  " ST 5      S Ln[        R                  " ST 5      S LnU(       d  U(       d  [        ST  35      e[        U 4S j[        US-
  5       5       5      (       a  [        ST  35      eU(       a  [        U5      n[        R                  " USUS	9nUR                  S
   U:w  a  [        SUR                  S
    S35      eT R                  5        Vs/ s H  n[        U5      PM     nn[        XU5      n	U	$ s  snf )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   @   >#    U  H  nTU   TUS -      :H  v   M     g7fr$   N .0iru   s     r   	<genexpr>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      sK   		 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< y9AH =s   Er   orderc                    [        U 5      n[        U 5      nUS;   a  SnOUS;   a  SnO[        SU 35      eU R                  S   S:w  a  [        S5      e[        R
                  " U S	US
9n [        R
                  " USUS
9nU R                  S   nUS:  d  US:  a  [        SU 35      eU [        U SSUS9-  n UR                  U R                  S S UR                  US9nUS:  a7  UUR                  UR                  U SSS S 24   U SSS S 24   5      5      S:  -  nUS	:  a7  UUR                  UR                  U SSS S 24   U SS	S S 24   5      5      S:  -  n[        U5      (       d!  UR                  U5      (       a  [        S5      eUR                  US   UR                  U 5      n U(       a  [        U5      n[!        U R                  S S UR                  5      (       a   U R                  S   UR                  S   :w  a&  [        SUR                   SU R                   S35      eUR                  S S S-   n	UR                  XR"                  [        U5      S9n
[        R$                  " U
5      S   R'                  S5      n
[)        U5       H<  n[+        USUS 4   U SUS S 24   -  5      nU(       a  [-        X5      O
[-        X5      n
M>     U
$ )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
 ,--"&&9L2M2M?@@xx+O<bffdK&! $**Sb/6<<88::b>V\\"--=fll^ L::,f&
 	

 ll3B$&G
Yv5FGAq	&a A8_Q-S!QY?@#,L,q2E  Hr    	canonicalc                p    [        U 5      nU(       a  [        U 5      n U(       a  UR                  U SSS9n U $ )Nr$   r   r   )r   _quat_canonicalr   )r   r   r   r   s       r   as_quatr      s7     
	Bt$wwtQRw(Kr    c           
         [        U 5      nU S   nU S   nU S   nU S   nX"-  nX3-  nXD-  nXU-  n	X#-  n
XE-  nX$-  nX5-  nX4-  nX%-  nXg-
  U-
  U	-   SX-
  -  SX-   -  SX-   -  U* U-   U-
  U	-   SX-
  -  SX-
  -  SX-   -  U* U-
  U-   U	-   /	nUR                  UR                  USS9/ U R                  S S QSPSP75      nU$ )	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   +  s&   		BVAVAVAVA	
B	
B	
B	
B	
B	
B	
B	
B	
B	
B 	"r	RW	RW	RW	b2	RW	RW	RW	b2
O ZZr:<Tdjj"o<Tq<TRS<TUFMr    c                    [        U 5      n[        U 5      n [        U SS S24   SSUS9nSUR                  X0S   S   5      -  nUS	:*  nUS-  nSUS
-  -   SUS-  -  S-  -   nUR	                  US-  5      UR                  XTR                  S9-   nXH-  n	UR                  XWU	5      n
U(       a  [        U
5      n
XSS S24   -  nU$ )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#rr']"FMr    c                     [        U 5      nUR                  S[        U 5      U R                  S9nUR	                  U S   S:  SU5      nSX0S   -  -   nX0SS S24   -  U-  $ )N      ?r   r+   ).r)   Nr   r   .r)   )r   r   r/   r+   r8   )r   r   onesigndenominators        r   as_mrpr   b  sp    		B
**S4

*
CC88D&*B4DL111KsBQBw-+--r    )suppress_warningsr   c                  ^ [        U 5      n[        T5      S:w  a  [        ST S35      e[        R                  " ST5      S Ln[        R                  " ST5      S LnU(       d  U(       d  [        ST 35      e[        U4S j[        S5       5       5      (       a  [        S	T 35      e[        U 5      nTR                  5        Vs/ s H  n[        U5      PM     n	nU(       a  U	OU	S S S
2   n	U	u  pnX:H  nU(       a  SU
-
  U-
  OUnUR                  XS9nUR                  X-
  X-
  -  X-
  -  S-  U R                  US9nUR                  XS   U S   U SU4   -
  5      nUR                  XSU
4   U SU
4   U SU4   U-  -   5      nUR                  XSU4   U SU4   U S   -   5      nUR                  XSU4   U-  U SU4   U-  U SU
4   -
  5      n[        XmXR                  S-  UUUUU5	      nU(       a  [        U5      $ U$ s  snf )Nr)   Expected 3 axes, got r'   rx   ry   rz   c              3   @   >#    U  H  nTU   TUS -      :H  v   M     g7fr|   r}   r~   s     r   r   as_euler.<locals>.<genexpr>y  s"     
2A3q6SQZr   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  s1    
	B 3x1}0Q788#.d:I#.d:I%
 	
 
2q
222J3%PQQt_F03		<1#A&D<4$tt*DGA!IA	AA::i:/D::qu'151Q6djjQW:XD
F|T&\DaL%@AA
CF|T#q&\DaL44G%GHA
CF|T#q&\DL%@AA
CF|d*DaL4,?$sAv,,NOAdEEAIq!Q;LF  '8F2F2# =s   Hc          
      f   [        U 5      nUS;   a  SnOUS;   a  SnO[        SU 35      eUR                  S   S:w  a  [        SUR                   S	35      eUR                  S
   S:w  a  [        S5      e[        UR                  S S U R                  S S
 5      (       d&  [        SUR                   SU R                   S35      eU[	        US
SUS9-  nUR                  USSS S 24   USSS S 24   5      nUR                  USSS S 24   USSS S 24   5      nUS:  US:  -  n	[        U	5      (       a!  UR                  U	S   UR                  U5      nO!UR                  U	5      (       a  [        S5      e[        U USSS S 24   USSS S 24   USSS S 24   UU5      n
U(       a  [        U
5      n
U
$ )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                 X    [         R                  " U 5      SS S24   R                  SSS9$ )N.r)   r   Tr   )r5   r?   multiply)r   s    r   invr     s-    66$<RaR ))"4)88r    c                     [        U 5      n[        U SS S24   SUS9nUR                  U S   5      nSUR                  X#5      -  nU$ )N.r)   r   r   r   r   rM   )r   r   r   r   )r   r   sin_qcos_qrv   s        r   	magnituder     sP    		B4RaR=rb9EFF4< E%''FMr    otherr,   c                 T   Uc  U(       a  [         R                  " SSS9  SnOU(       a  [        U5      n[        U R                  UR                  5      (       d,  [        SU R                  S S  SUR                  S S  S35      e[        U[        U 5      5      n[        U5      nXR:  $ )	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1CDJJsPROCT U&&+kk#2&6%77SU
 	

 uc$i0K{#F=r    weightsr   .c                 R  ^ ^ [        T 5      n[        T 5      n[        T SUS9nT R                  S   S:X  a  [	        S5      e[        [        T R                  S-
  5      5      nTc  UmO[        T[        5      (       a  T4m[        T[
        5      (       d  [	        S5      eTS:w  aX  [        T5      T R                  S-
  * :  d  [        T5      T R                  S-
  :  a  [	        S	T S
T R                  S S  S35      e[        [        [        U 4S jT 5       5      5      5      m[        T 5      nT SS S S 24   nUc  UR                  U5      U-  n	OUR!                  XUS9nUS:  n
U(       d!  UR#                  U
5      (       a  [	        S5      eU(       a  UR%                  XR&                  U5      n[)        T R                  S S UR                  5      (       d)  [	        SUR                   ST R                  S S  S35      eUS   U-  nUR                  U5      U-  n	[        U4S jU 5       5      nUT-   nUR+                  XU5      nUR                  S [-        U5       S-   nUR/                  UR1                  X5      SS9n	UR2                  R5                  U	5      u  nnU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   F   >#    U  H  oTR                   S -
  -  v   M     g7fr|   )r&   )r   r   r   s     r   r   mean.<locals>.<genexpr>  s     >A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   6   >#    U  H  oT;  d  M
  Uv   M     g 7f)Nr}   )r   r   r   s     r   r   r     s     ;Ad]aa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,{:**W&*Ak{++>?? hh{FFG<GTZZ_gmm<<V==/tzz#2&7{D   0;>.< ;;;IT!J++aX6K!!"2C	N3j@I


;2<A99>>!DAqW:r    leftrightc           
         Uc  Uc  U S S 4$ [        U 5      n[        R                  " U SUS9n Uc  UR                  U 5      nUc  UR                  U 5      nU n[	        XC5      u  pV[	        X5      u  px[	        X#5      u  pUSS S 2S S 4   USS S S 2S 4   -  U	SS S S S 24   -  nUR                  USS S 2S S S 24   U
SS S S 2S S 24   -  SS9nUSS S 2S S 4   USS S S 2S S 24   -  nUR                  USS S 2S S S 24   U
SS S S 2S S 24   -  SS9nUSS S S 2S 4   USS S 2S S S 24   -  nUR                  USS S 2S S S 24   USS S S 2S S 24   -  SS9nU	SS S S S 24   USS S 2S S 2S 4   -  nUSS S 2S S S 24   nUSS S S 2S S 24   nUR                  R                  UU5      nUR                  USS S 2S S 2S S S 24   U
SS S S S 2S S 24   -  SS9nUR                  X-
  U-
  U-
  U-
  5      nUR                  UR                  USS5      UR                  S   S45      nUR                  UR                  UUR                  S   S45      SS9nUU
R                  S   -  nUU
R                  S   -  nUR                  UR                  UR                  S   5      S5      nUR                  US	5      nUUU4   nUR                  US	5      nUUU4   n[        U[        XB5      5      nUc  S nUc  S nUUU4$ )
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   reducer0  )  sg   
 |T4 
	B>>$Q2.D|||D!}T" 	AQ#FBT&FBU'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                 $   [        U 5      n[        U 5      nUS   n[        UR                  UR                  5      (       d,  [	        SU R                  S S  SUR                  S S  S35      eU(       a  UR                  U5      U-  S   $ XA-  S   $ )Nrp   zCannot broadcast r   z rotations to 	 vectors.r   )r   r   r   rS   r7   r:   )r   r1  r2  r   mats        r   applyr6  t  s    		B
D/C IFFLL11

3B0v||CR?P>Q R 
 	
 ##C(616::L&!!r    valueindexerc                 V    [         R                  " U 5      US4   R                  U5      $ N.)r5   r?   r@   )r   r7  r8  s      r   setitemr;    s&     66$<%))%00r    r   r   return_sensitivityc                 L   [        U 5      n[        XSUS9nUR                  XS9nUR                  XS9n[        R                  " USUS9n [        R                  " USUS9nU R
                  S   S:w  a  [        SUR
                   35      eUR
                  S   S:w  a  [        S	UR
                   35      eU R
                  UR
                  :w  a&  [        S
UR
                   SUR
                   S35      eU R                  S:  d  UR                  S:  a&  [        SUR
                   SUR
                   S35      eU R
                  S   nUc%  UR                  U[        U 5      U R                  S9nOUR                  U[        U 5      U R                  S9nUR                  S:w  a  [        SUR
                   S35      eUS:  a2  UR
                  S   U:w  a  [        SUR
                  S    SU S35      eUS:  n	[        U	5      (       d!  UR                  U	5      (       a  [        S5      eUR                  XR                  U5      nUS:X  a  UR                  S/5      OX$R                  :H  n
UR!                  UR#                  XR                  5      5      n[        U5      (       d)  US:  a  [        S5      eUS:X  a  U(       a  [        S5      eUR                  US:  UR                  U5      nUR                  U
SS9n[        U5      (       aY  [%        XU5      u  pn['        XU5      u  nnnUR                  UUU5      nUR                  UUU5      nUR                  UUU5      nO5UR                  U5      (       a  ['        XU5      u  pnO[%        XU5      u  pnXU4$ )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 r4  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   r<  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_vectorsrP    s    
	B1<EA+JA+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:J3K3KHII((+VVW= +,q&BJJv&g6GMFF299]GG45E 19BCCA:,5 
 hhuqy"&&'2GB/J Z  #1!#@ [/CA'/R,	8_Y6xx
Hd3hhz?KH66*';A''J$E'5aG'D$E##r    c           
         [        U 5      n[        U 5      nUS S 2S 4   U -  R                  U-  nUR                  R	                  U5      u  pgnUR                  R                  Xh-  5      S:  n	[        R                  " U5      S   R                  UR                  XS   * US   5      5      n[        R                  " U5      SS S 2S4   R                  UR                  XSS S 2S4   * USS S 2S4   5      5      nXh-  n
UR                  X#R                  US-  U S-  -   SS9-  SS9SUR                  USS9-  -
  nUR                  UR                  XR                  SUS95      5      S	   nUS	   US
   -   US
   US   -   -  US   US	   -   -  nUS	   US
   -  US
   US   -  -   US   US	   -  -   nUR                  SU R                  US9nUR!                  U5      U-  X-  XUR                  -  -   -  n[#        U
5      nUUU4$ )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ssdrK  zetakappar;   rL  rJ  s                     r   rA  rA    s#   		Bq\F	D	A	!!A%Ayy}}QHA" iimmAF#a'Gq	'rxxG*ajIJAq	#q"*!!"((7sArz]NAc1bjM"RSA	A
 &&66!Q$A+B677b&
AA	 IO I E C 772::c88Af8#=>?GD fI&	!ai!F)&;<&	AfI@UVDfI&	!AfI&	$99AfI&	<QQE
&&!''&&
1C'''"T)U[1tt8-CDK#A&E$##r    c                    [        U 5      n[        U 5      nU R                  S   nUS:X  a  UR                  S/US9OX#R                  :H  nUR                  UR                  XcR                  5      5      n[        U5      (       d  [        U5      nUR                  X* SS9nUR                  X* SS9n	UR                  X'* SS9n
US   S   nU	S   S   n[        USSUS	9n[        USSUS	9n[        U5      (       d=  UR                  US:H  5      (       d  UR                  US:H  5      (       a  [        S
5      e[        R                   " U5      US:H     R#                  UR$                  5      n[        R                   " U5      US:H     R#                  UR$                  5      nX-  nX-  nUR&                  R)                  USSS S 24   USSS S 24   5      n[        USUS9nUR+                  UUR-                  USSS S 24   USSS S 24   -  SS95      nSnUR                  / SQUR.                  US9nUR0                  U-
  U:  nUR3                  UR5                  USSS S 24   5      5      nUR7                  UR9                  US:H  US   UR9                  US:H  US   * S5      5      UR9                  US:H  US   UR9                  US:H  US   * S5      5      UR9                  US:H  US   UR9                  US:H  US   * S5      5      /5      nUR9                  US:H  UU5      nUR9                  U[;        U[        UUS9-  UR0                  -  5      U5      nUR9                  UUR0                  U-
  U5      nUR9                  UU* U5      nUU-  nUR5                  U5      U:  nUSUS-  -   UU-  S-  S-  -   -  nUUR                  UUR.                  S9-   nUU-  UR=                  U5      -  nUR9                  UUU5      n[?        [;        U5      U5      nUSS 2S4   nU	SS 2S4   nU
SS  n[A        UU5      n UR&                  RC                  UR&                  R)                  U U5      U5      n!UR&                  RC                  U U5      UR&                  RC                  U U5      UR&                  RC                  UU5      -  -
  n"UR+                  UR-                  UU!-  5      UR-                  UU"-  5      5      n#[;        U#US   -  5      n$US:X  a  UO[?        U$U5      n%US:  US   UR                  :H  -  U-  n&[        R                   " UU&5      R#                  SSUS9n'[A        U%U5      n(URE                  UR-                  U'U U(-
  S-  -  5      5      n)UR                  URG                  U5      SS9n&UR9                  U&UR$                  U%5      n%U%U)UR                  UR$                  US94$ )Nr   r$   Tr   )shiftr   )r   .r:  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   r6  rq   rS  isnan)*r   r   r   r   r   rE  rG  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_secrJ  rB   weights_inf_zeroa_estrK  s*                                             r   rB  rB    s    
	Bq\F	
A9:aBJJvfJ5WPVPVEVM ii		-:;G "::g,wwqqw1Hwwqqw1HWWWH1W=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E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                 n   [        U 5      n[        U 5      n[        U5      (       a>  UR                  S:X  a  US   nOUR                  S:w  a  [        S5      e[        XSUS9u  p[        U5      (       a  [        U[        U 5      -  5      nUR                  / U R                  S S QSP7U R                  US9n[        R                  " U5      S	   R                  S
5      nUR                  US:H  XT5      nUR                  US:H  [!        U 5      U5      nUR                  US
:H  X5      nU$ US:X  aU  UR                  / U R                  S S QSP7U R                  US9n[        R                  " U5      S	   R                  S
5      $ US:X  a  [!        U 5      $ US
:X  a  U $ [        U[        U 5      -  5      $ )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[>??TTbA QQ401881djj"o1q1F8S66(#F+//2!q&(3!r'3t9f5!q&$/Av881djj"o1q1F8Svvh'++A..Bw4yAvq9T?*++r    c                     [        U 5      n[        U SSUS9nUS:H  n[        U5      (       a   UR                  X1R                  U 5      n X-  $ UR                  U5      (       a  [        S5      eX-  $ )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	6640  
			ABBr    c                     [        U 5      nU S   S:  nU S   S:H  nX#U S   S:  -  -  nX0S   S:H  -  nX$U S   S:  -  -  nX@S   S:H  -  nX%U S   S:  -  -  nUR                  US   U * U 5      $ )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T&\A-./DV)*Gd6lQ./0Dv,!+,HtF|a/01D88DOdUD11r    c                 H    U S:X  a  gU S:X  a  gU S:X  a  g[        SU  35      e)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                 r   [        U 5      nU(       a  X4OX14u  pUR                  R                  X5      nUR                  UR	                  X75      UR	                  X15      5      nUS:  n	UR                  U	S   U* U5      nUR                  X* U5      nUR                  U	S   U* U5      nU	n
UR                  UR                  US-  5      S   U-  UR                  US-  5      S   4SS9n[        X5      nUS   nUR                  R	                  USS S24   U5      nUR                  R	                  USS S24   U5      nUR                  R	                  USS S24   U5      n[        US	S
XXUU5	      n[        R                  " U5      S   R                  UR                  U
US   * US   5      5      nU$ )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"bXFBiioob%G88BIIb*BIIb,=>D !8D	$y/B3	+B88D%&DhhtI':GK			q	)	$r	)266$(+;I+FGb  I 9+GA
		bqb)2.A
		bqb)2.A
		bqb)73AE1dqQ@QRFVVF^F#''
vf~ovf~>F Mr    r   c                     [        U5      n[        U5      n[        U S   US   XCS9n[        S[	        U 5      5       H2  n[        X   USU4   XCS9nU(       a  [        XW5      O
[        Xu5      nM4     U$ )Nr   r   )r   r   r$   .)r   r/   _make_elementary_quatr   r   r   )r   rv   r   r   r   r   r   ax_quats           r   r   r     ss    		 BvF a&.OD1c$i 'QV.7|D*\'=X ! Kr    rf   c                 F   UR                  / UR                  QSP7UR                  US9n[        R                  " U5      S   R                  UR                  US-  5      5      n[        R                  " U5      SU 4   R                  UR                  US-  5      5      nU$ )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	           
      (   [        U5      n	[        U5      n
SnU	R                  XT5      nU	R                  Xv5      nU	R                  / UR                  QSP7UR
                  U
S9n[        R                  " U5      S   R                  SU	R                  U	R                  Xg5      U	R                  XE5      5      -  5      nU (       a  SOSnU (       a  SOSnU	R                  US   5      U:*  nU	R                  US   U	R                  -
  5      U:*  nUU-  ) nU(       d<  [        U5      (       d,  U	R                  U) 5      (       a  [        R                  " SSS9  U	R!                  USU-  SU-  U (       a  S	OS
-  5      n[        R                  " U5      S   R                  U5      nU	R!                  UX-
  USU4   5      n[        R                  " U5      SU4   R                  U5      nU	R!                  UX-   USU4   5      nU(       d3  UU-  n[        R                  " U5      S   R                  US   U-
  5      n[        R                  " U5      SU4   R                  U5      nXR                  -   SU	R                  -  -  U	R                  -
  nU$ )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     sA    
	Bq\F
Cxx~HI XXmmm1776XBFVVF^F#''BHHRXXa^RXXa^,T(TUF !aK !aK FF6&>"c)EFF6&>BEE)*c1EemE]5%9%9bffeVnn 		
 
%Xq9}iQ'O	PBVVF^F#''+F 
%-vc;6F/G	HBVVF^C,-11"5F 
%-vc;6F/G	HB$Y'++F6NT,ABVVF^C,-11"5FuunRUU+bee3FMr    r  r   c                    [        U 5      nUR                  R                  U SS S24   USS S24   5      nU S   US   -  US   U S   -  -   US   -   nU S   US   -  US   U S   -  -   US   -   nU S   US   -  US   U S   -  -   US   -   nU S   US   -  U S   US   -  -
  U S   US   -  -
  U S   US   -  -
  nUR                  XEXg/SS9nU$ )	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  s3   		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
	   88RR$28.DKr    c                 J    [         R                  " U SUS9n U S   U SS S24   4$ )NrM   r%   r   .r   )r5   r6   )r   r   s     r   r  r  g  s/    qqR(AW:qcrc{""r    c                 .    U [         R                  S-  -  $ Ng     f@npr   rv   s    r   rb   rb   l  s    RUUU]##r    c                 .    U S[         R                  -  -  $ 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  r0  r6  slicer;  rP  rA  rB  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    