
    Pi                        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 	 	 d[d	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-d0f         z  de	fd1Z/	 	 d^de	d2e	dz  d3e	dz  de.e	e	dz  e	dz  f         fd4Z0d\de	d5e	d6ede	fd7Z1de	d8e	d9e-e2z  ez  dz  de	fd: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	dAe"e	z  de	fdBZ7de	de	fdCZ8de	de	fdDZ9d/e de-fdEZ:de	dFe	dGe	dHe	dIed&ede	fdJZ;de<e-         de	dKede	fdLZ=d/e-dMe	de	fdNZ>dIedOedPe-dQe"d;e	d<e	dRe	dSe	d&ede	fdTZ?dUe	dVe	de	fdWZ@dVe	de.e	e	f         fdXZAde	de	fdYZBde	de	fdZZCdS )_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_objTF)scalar_firstquat	normalizecopyr   returnc                    t          |           }|r|                    | dd          } |r|s|                    | d          } |rt          |           } | S )NaxisTr   )r   rollasarray_normalize_quaternion)r   r   r   r   xps        x/home/jaya/work/projects/VOICE-AGENT/VIET/agent-env/lib/python3.11/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                            |           dk    }t	          |          }|sv|                    |          rat          |                    t          j	        |d|                    d         d                   }t          d| d| |df          d          |r"|                    |d         |j        |           } | |                    |           z  }|                    d	| j        |
          }|                    t          j        ||d|          d          }	|rA|j                            | d          \  }
}}|                    |	d         | |
|z            } nn|                    |	          sY|	 }|j                            | |         d          \  }
}}t          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 Cy}}V$$)T"" 	Et 	Ebjj1!D!D!DEEaHKLLC>#&> >*0c*:> > >    	EXXd?3RVVDDFB//777ffQfl6f::K#Eb999  
 
  
	C y}}V5}AAHAq"XXmO<fa"fMMFF&& 	C!.y}}V,=%>e}TTHAq"VF^^$56::1r6BBF"6***r    c                     t          |           }t          |           }| d         | d         z   | d         z   }|                    | d         | d         | d         |gd          }|                    |dd          }|                    g | j        dd	         d
R | j        |          }|                    d|d         z
  d| d         z  z   | d         | d         z   | d         | d         z   | d         | d         z
  gd          }|                    |dk    ||          }|                    | d         | d         z   d|d         z
  d| d         z  z   | d         | d         z   | d         | d         z
  gd          }|                    |dk    ||          }|                    | d         | d         z   | d         | d         z   d|d         z
  d| d         z  z   | d         | d         z
  gd          }	|                    |dk    |	|          }|                    | d         | d         z
  | d         | d         z
  | d         | d         z
  d|d         z   gd          }
|                    |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88F88+fl3B3'+++6<8O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k    rt          d| j                   |rt          |           n| } t	          | dd|          }|dk    }|dz  }d|d	z  z
  |dz  d
z  z   }||                    ||j                  z   }|                    |dz            |z  }|                    |||          }	|	                    | |	z  |
                    |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LflLL
 
 	
 ")4XffF6TbAAAE4-KAXF#fai$&66K
 

;ek
BBBI&&##i/KHH[+{;;E99funbffUQY&7&78r9BBDKr    mrpc                    t          |           }| j        d         dk    rt          d| j                   |j                            | | d          d         dz   }|                    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
y}RsyRRSSS)""3""55i@1DK		1s37|+a+o?b	III{"DKr    seqanglesc                     t          |          }t                     }|dk     s|dk    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
         |k    rt          d|j
        d
          d          d                                  D             }t          |||          }|S )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   B   K   | ]}|         |d z            k    V  dS r$   N .0iru   s     r   	<genexpr>zfrom_euler.<locals>.<genexpr>   s4      
=
=A3q6SQZ
=
=
=
=
=
=r    /Expected consecutive axes to be different, got r%   r   zTExpected last dimension of `angles` to match number of sequence axes specified, got r'   c                 ,    g | ]}t          |          S r}   _elementary_basis_indexr   xs     r   
<listcomp>zfrom_euler.<locals>.<listcomp>   !    <<<1#A&&<<<r    )r   lenr7   rematchr2   rangerb   r5   r6   rS   lower_elementary_quat_compose)	ru   rv   r\   r   num_axes	intrinsic	extrinsicaxesqs	   `        r   
from_eulerr      s   		 	 B3xxH!||x!||7147 7
 
 	

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

 
=
=
=
=x!|)<)<
=
=
=== RP3PPQQQ "&!!^Fr222F|B8##2$l2.2 2 2
 
 	
 =<		<<<D vy99AHr    r   orderc                 2   t          |           }t          |           }|dv rd}n|dv rd}nt          d|           | j        d         dk    rt          d          t	          j        | d	|
          } t	          j        |d|
          }| j        d         }|dk     s|dk    rt          d|           | t          | dd|          z  } |                    | j        d d         |j        |          }|dk    rF||	                    |
                    | ddd d f         | ddd d f                             dk    z  }|d	k    rF||	                    |
                    | ddd d f         | dd	d d f                             dk    z  }t          |          s$|                    |          rt          d          |                    |d         |j        |           } |rt          |          }t!          | j        d d         |j                  r| j        d         |j        d         k    r t          d|j         d| j         d          |j        d d         dz   }	|                    |	|j        t          |                    }
t	          j        |
          d                             d          }
t)          |          D ]J}t+          |d|d f         | d|d d f         z            }|rt-          ||
          nt-          |
|          }
K|
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      sp    
		Bt__F"""			$	$	$		@8=@ @
 
 	

 z"~<===>$Q2...D^Fr222Fz"~H!||x!||AxAABBB.B"EEEED ((4:crc?"'&(QQ!||1FF299T#q!!!)_d31119o>>??$F
 !||1FF299T#q!!!)_d31119o>>??$F
 ,-- L"&&9L2M2M L?@@@xx+O<bfdKK "&!! $*SbS/6<88
:b>V\"---&fl & &:& & &
 
 	

 l3B3$&G
Yv5F5FGGAq		&a  A8__ F FQ-S!QQQY?@@#,ELQ,q"2E2EHr    	canonicalc                z    t          |           }|rt          |           } |r|                    | dd          } | S )Nr$   r   r   )r   _quat_canonicalr   )r   r   r   r   s       r   as_quatr      sJ     
		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	}|                    |                    |d          g | j        d d         ddR           }|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   +  sT   			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::<Tdj"o<Tq<TRS<T<TUUFMr    c                    t          |           }t          |           } t          | dd df         dd|          }d|                    || d         d                   z  }|d	k    }|dz  }d|d
z  z   d|dz  z  dz  z   }|                    |dz            |                    ||j                  z   }||z  }	|                    |||	          }
|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JJJG$v,y"9:::E4-KAXFfrk/A	MD$88K
 ffUS[!!BJJ{%+J$N$NNG'/KHH[+{;;E  T#rr']"FMr    c                     t          |           }|                    dt          |           | 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  s}    			B
**S4
*
C
CC88D&*B44DtL111K$sBQBw-+--r    )suppress_warningsr   c                   t          |           }t                    dk    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          |           }d
                                 D             }|r|n
|d d d         }|\  }	}
}|	|k    }|rd|	z
  |
z
  n|}|	                    ||          }|	                    |	|
z
  |
|z
  z  ||	z
  z  dz  | j
        |          }|                    || d         | d         | d|
f         z
            }|                    || d|	f         | d|	f         | d|f         |z  z             }|                    || d|
f         | d|
f         | d         z             }|                    || d|f         |z  | d|f         |z  | d|	f         z
            }t          ||||j        dz  |||||	  	        }|rt          |          n|S )Nr)   Expected 3 axes, got r'   rx   ry   rz   c              3   B   K   | ]}|         |d z            k    V  dS r|   r}   r~   s     r   r   zas_euler.<locals>.<genexpr>y  s4      
2
2A3q6SQZ
2
2
2
2
2
2r    rM   r   c                 ,    g | ]}t          |          S r}   r   r   s     r   r   zas_euler.<locals>.<listcomp>}  r   r    r   r   r*   r   .)r   r   r7   r   r   r2   r   r/   r   r   r+   r8   _get_anglespir   )r   ru   r\   r   r   r   r   r   r   r   jk	symmetricrB   r   abcdrv   s    `                  r   as_eulerr   j  s    
		B 3xx1}}7777888#..d:I#..d:I 
 
 
 
 	
 
2
2
2
2q
2
2
222 RP3PPQQQt__F<<		<<<D,44$ttt*DGAq!QI%A		AA::i://D::q1uQ'1q51Q6djQW:XXD
tF|T&\DaL%@AAA
tCF|T#q&\DaL44G%GHHA
tCF|T#q&\DL%@AAA
tCF|d*DaL4,?$sAv,,NOOA9dBEAIq!Q;L F  '28FF2r    c          
         t          |           }|dv rd}n|dv rd}nt          d|           |j        d         dk    rt          d|j         d	          |j        d
         dk    rt          d          t          |j        d d         | j        d d
                   s t          d|j         d| j         d          |t	          |d
d|          z  }|                    |ddd d f         |ddd d f                   }|                    |ddd d f         |ddd d f                   }|dk    |dk    z  }	t          |	          r#|                    |	d         |j        |          }n$|	                    |	          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     sT    
		B """			$	$	$		5-25 5
 
 	
 z"~>>>>???z"~<===CRC$*SbS/:: 
+ + +:+ + +
 
 	
 .B"EEEED99T#q!!!)_d31119o>>L99T#q!!!)_d31119o>>L$&<4+?@JZ   Axx
?3RVTBB	
		 A?@@@)S!QQQYS!QQQYS!QQQY F  "&!!Mr    c                 h    t          j        |           dd df                             dd          S )N.r)   r   Tr   )r5   r?   multiply)r   s    r   invr     s0    6$<<RaR ))"4)888r    c                     t          |           }t          | dd df         d|          }|                    | d                   }d|                    ||          z  }|S )N.r)   r   r   r   r   rM   )r   r   r   r   )r   r   sin_qcos_qrv   s        r   	magnituder     sa    			B4RaR=rb999EFF4<  E%'''FMr    otherr,   c                 V   ||rt          j        dd           d}n|rt          |          }t          | j        |j                  s0t          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     | 	MW    	 ~~U[11 
UDJsPRsO U U&+k#2#&6U U U
 
 	

 uc$ii0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k    rZt                     j        dz
   k     st                     j        dz
  k    r#t	          d	 d
 j        d d          d          t          t          t           fdD                                           t                     } dd d d f         }||                    |          |z  }	n|                    |||          }|dk     }
|s$|                    |
          rt	          d          |r|                    |
|j        |          }t)           j        d d         |j                  s(t	          d|j         d j        d d          d          |d         |z  }|                    |          |z  }	t          fd|D                       }|z   }|                    |	||          }|j        d t-          |                   dz   }|                    |                    ||          d          }	|j                            |	          \  }}|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   2   K   | ]}|j         d z
  z  V  dS r|   )r&   )r   r   r   s     r   r   zmean.<locals>.<genexpr>  s.      >>AAQ/>>>>>>r    .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|V  d S )Nr}   )r   r   r   s     r   r   zmean.<locals>.<genexpr>  s'      ;;AQd]]a]]]];;r    )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    s5   
 
		Bt__F4<<<Ez!}FGGG U49q=))**H|	D#		 wdE"" HFGGGrzzs4yyTY]#333s4yyDIPQM7R7RVDVVDJsPRsOVVV
 
 	
 >>>>>>>>>??@@DDsD!!!|$K,,{:**WE&*AAk 	={++ 	=>??? 	= hh{BFG<<GTZ_gm<< 	D=D D'+z#2#D D D    0;>..< ;;;;;;;;;IT!J++aX66K!"2C	NN"23j@I


;	22<<A9>>!DAqW:r    leftrightc           
         ||| d d fS t          |           }t          j        | d|          } ||                    |           }||                    |           }| }t	          ||          \  }}t	          ||          \  }}t	          ||          \  }	}
|dd d d d f         |dd d d d f         z  |	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         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         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         z  }|dd d d d d f         }|dd d d d d f         }|j                            ||          }|                    |dd d d d d d d f         |
dd d d d d d f         z  d          }|                    ||z
  |z
  |z
  |z
            }|	                    |
                    |dd          |j        d         df          }|                    |	                    ||j        d         df          d          }||
j        d         z  }||
j        d         z  }|	                    |                    |j        d                   d          }|	                    |d	          }|||f         }|	                    |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   reducer2  )  s7   
 |T4 
		B>$Q2...D|||D!!}T"" 	AQ##FBT2&&FBUB''FB sAAAtT!"RT111d(:%;;bdDRSRSRSAS>TTE
&&CD!!!O$r#tQQQ/'::&
D
DCsAAAtT!"SdAAAqqq%99E
&&CD!!!O$r#tQQQ/'::&
D
DCsD!!!T!"Saaaqqq%99E
&&CD!!!O$r#tQQQ/'::&
D
DCsD$!"SaaaD%99E
 S!!!T111_%KS$111_%Ky{K88HFF8CAAAtQQQ./"S$aaa5J2KKRTFUUE	%-5	6	6B	BKKAq))BHQK+<	=	=B ii

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

:w//I)W$%E 4a!7!788G|	}
Iz))r    pointsinversec                 8   t          |           }t          |           }|d         }t          |j        |j                  s0t	          d| j        d d          d|j        d d          d          |r|                    |          |z  d         S ||z  d         S )Nrp   zCannot broadcast r   z rotations to 	 vectors.r   )r   r   r   rS   r7   r:   )r   r3  r4  r   mats        r   applyr8  t  s    			B
D//C IFFL11 

3B3  v|CRC?P   
 
 	
  ;##C((616::&L&!!r    valueindexerc                 `    t          j        |           |df                             |          S N.)r5   r?   r@   )r   r9  r:  s      r   setitemr=    s)     6$<<%))%000r    r   r   return_sensitivityc                    t          |           }t          | |d|          }|                    | |          }|                    ||          }t          j        |d|          } t          j        |d|          }| j        d         dk    rt          d|j                   |j        d         dk    rt          d	|j                   | j        |j        k    r t          d
|j         d|j         d          | j        dk    s|j        dk    r t          d|j         d|j         d          | j        d         }|+|                    |t          |           | j
                  }n|                    |t          |           | j
                  }|j        dk    rt          d|j         d          |dk    r2|j        d         |k    r!t          d|j        d          d| d          |dk     }	t          |	          s$|                    |	          rt          d          |                    |	|j        |          }|dk    r|                    dg          n
||j        k    }
|                    |                    |
| j
                            }t          |          s,|dk    rt          d          |dk    r|rt          d          |                    |dk    |j        |          }|                    |
d          }t          |          rpt%          | ||          \  }}}t'          | ||          \  }}}|                    |||          }|                    |||          }|                    |||          }n@|                    |          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 r6  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_vectorsrR    sl    
		B1a<<<EAU++JAU++Jzb111Azb111Awr{aVJDTVV
 
 	
 	wr{aVJDTVV
 
 	
 	w!'M%M M,6,<M M M
 
 	
 	vzzQVaZZGG G&0&6G G G
 
 	
 	

A ''!IaLL'@@**WYq\\*II<1UW]UUU   q55gmA&!++K 'a 0K K>?K K K   #Q;-.. 	J266:J3K3K 	JHIII((+RVW== +,q&&BJJv&&&g6GMFF299]AG4455E  199BCCCA::,:5  
 hhuqy"&'22GB//J Z   
E#1!Q#@#@ t[/CAq'/R/R,	8_Y66xx
Hd33hhz?KHH66* 	E';Aq''J'J$E4'5aG'D'D$E4$##r    c           
         t          |           }t          |           }|d d d f         | z  j        |z  }|j                            |          \  }}}|j                            ||z            dk     }	t          j        |          d                             |	                    |	|d          |d                             }t          j        |          dd d df                             |	                    |	|dd d df          |dd d df                             }||z  }
|
                    ||
                    |dz  | dz  z   d          z  d          d|
                    |d          z  z
  }|                    |                    ||                    d|                              d	         }|d	         |d
         z   |d
         |d         z   z  |d         |d	         z   z  }|d	         |d
         z  |d
         |d         z  z   |d         |d	         z  z   }|                    d| 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ssdrM  zetakappar;   rN  rL  s                     r   rC  rC    so   			Bq\\F	D	A	!A%Ay}}QHAq" immAF##a'Gq		'rxx!G*ajIIJJAq		#qqq"*!!"((7QsAAArz]NAc111bjM"R"RSSA	BA
 &&266!Q$A+B6777b&
A
AA	 IO I I E C 772::c288Af8#=#=>>??GD fI&	!ai!F)&;<&	AfI@UVDfI&	!AfI&	$99AfI&	<QQE
&&!'&&
1
1C'''""T)US[1qt8-CDK#A&&E$##r    c                    t          |           }t          |           }| j        d         }|dk    r|                    dg|          n
||j        k    }|                    |                    ||j                            }t          |          st          |          }|
                    | | d          }|
                    || d          }	|
                    || d          }
|d         d         }|	d         d         }t          |dd|	          }t          |dd|	          }t          |          sA|                    |dk              s|                    |dk              rt          d
          t          j        |          |dk                                 |j                  }t          j        |          |dk                                 |j                  }||z  }||z  }|j                            |ddd d f         |ddd d f                   }t          |d|          }|                    ||                    |ddd d f         |ddd d f         z  d                    }d}|                    g d|j        |          }|j        |z
  |k     }|                    |                    |ddd d f                             }|                    |                    |dk    |d         |                    |dk    |d          d                    |                    |dk    |d         |                    |dk    |d          d                    |                    |dk    |d         |                    |dk    |d          d                    g          }|                    |dk    ||          }|                    |t;          |t          ||          z  |j        z            |          }|                    ||j        |z
  |          }|                    || |          }||z  }|                    |          |k     }|d|dz  z   ||z  dz  dz  z   z  }||                    ||j                  z   }||z  |                    |          z  }|                    |||          }t?          t;          |          |          }|dd df         }|	dd df         }|
dd          }tA          ||          } |j        !                    |j                            | |          |          }!|j        !                    | |          |j        !                    | |          |j        !                    ||          z  z
  }"|                    |                    ||!z            |                    ||"z                      }#t;          |#|d         z            }$|dk    r|nt?          |$|          }%|dk    |d         |j        k    z  |z  }&t          j        ||&                              dd|          }'tA          |%|          }(|"                    |                    |'| |(z
  dz  z                      })|                    |#                    |          d          }&|                    |&|j        |%          }%|%|)|                    |j        |          fS )Nr   r$   Tr   )shiftr   )r   .r<  r   r^   z(Cannot align zero length primary vectors.r   r   r_   )        rb  rb  r   r*   )r   rM   rM   )r   r$   )r   r   )r      r   ih  ra   )r   r   )$r   r/   rS   r   rA  rQ   rB  uint8r   r3   r   r   r   r2   r7   r5   r?   r@   r9   r0   r  r   r  r+   r   argminr   rP   r8   rm   rc   r   r8  rq   rU  isnan)*r   r   r   r   r   rG  rI  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_secrL  rB   weights_inf_zeroa_estrM  s*                                             r   rD  rD    s    
		Bq\\F	
A9:aBJJvfJ555WPRPVEVM ii		-::;;G ":: g,,wwqqw11Hwwqqw11HWWWWH1W==NVY'EVY'EB"EEEJB"EEEJ$$ I66*/"" 	IbffZ1_&=&= 	IGHHH 
##J!O488@@J
##J!O488@@JJEJE IOOE#q!!!),eCAAAI.>??EB2666JHHZc1aaai(85a;K(KRT!U!UVVEIZZ,,,EKZOOF 55=9$D 			"&&sAqqqy)**++A 88HHQ!VU4["((16E$K<*K*KLLHHQ!VU4["((16E$K<*K*KLLHHQ!VU4["((16E$K<*K*KLL	
 L 	q,66AXXdKN1,D,D,D(Dru(LMMvVVFHHT255=%00EHHTE65))E U]F&&--)+KQ!^fvo.AC.GGHM
 BJJ{%+J>>>EEMBFF5MM1M
m];;AQ00E
 QRRWEQRRWE $K %E y	u = =uEEHyu--
	&&)9)9%)G)GGH ((266+011266+:P3Q3Q
R
RCeFm+,,E !VVEEeU!;!;E
 UwqzRV+,=D vgt,,00"0EE%OOE77266*a%iA-==>>??D66"((7##"6--DHHT265))E$

26&
9999r    nc                    t          |           }t          |           }t          |          rD|j        dk    r	|d         }n|j        dk    rt          d          t          | |d|          \  } }t          |          rt          |t          |           z            }|
                    g | j        d d         dR | j        |          }t          j        |          d	                             d
          }|                    |dk    ||          }|                    |dk    t!          |           |          }|                    |d
k    | |          }|S |dk    r\|
                    g | j        d d         dR | j        |          }t          j        |          d	                             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  B7d??!AAVq[[>???T1TbAAAa Q Q4011881dj"o1q11F8SS6(##F+//22!q&(F33!r'3t99f55!q&$//Avv881dj"o1q11F8SSvh'++A...Bww4yyAvvq9T??*+++r    c                     t          |           }t          | dd|          }|dk    }t          |          r|                    ||j        |           } n$|                    |          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     s    			Bt"tCCCIQIY Cxx	26400				 CABBB)r    c                 4   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  }|                    |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11r    c                 V    | dk    rdS | dk    rdS | dk    rdS t          d|            )Nr   r   r   r$   r   rM   z.Expected axis to be from ['x', 'y', 'z'], got )r7   r   s    r   r   r     sC    s{{q	q	q
LdLL
M
MMr    n1n2n3r   c                 *   t          |           }|r||fn||f\  }}|j                            ||          }|                    |                    ||          |                    ||                    }|dk     }	|                    |	d         | |          }|                    |	| |          }|                    |	d         | |          }|	}
|                    |                    |dz            d         |z  |                    |dz            d         fd          }t          ||           }|d         }|j                            |dd df         |          }|j                            |dd df         |          }|j                            |dd df         |          }t          |d	d
||||||	  	        }t          j        |          d                             |                    |
|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"0b"XXRFBioob"%%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33AE1dAq!Q@QRRFVF^^F#''
vf~ovf~>> F Mr    r   c                 F   t          |          }t          |          }t          | d         |d         ||          }t          dt	          |                     D ]E}t          | |         |d|f         ||          }|rt          ||          nt          ||          }F|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OOOD1c$ii   Y Y'QQSUVVV.7X|D'***\'SW=X=XKr    rf   c                 d   |                     g |j        dR |j        |          }t          j        |          d                             |                    |dz                      }t          j        |          d| f                             |                    |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%ek%1%%U[8HHD6$<<##BFF53;$7$788D6$<<T	"&&rvveck':':;;DKr    r   r   r  r   r   c	           
         t          |          }	t          |          }
d}|	                    ||          }|	                    ||          }|	                    g |j        dR |j        |
          }t          j        |          d                             d|	                    |		                    ||          |		                    ||                    z            }| rdnd}| rdnd}|	
                    |d                   |k    }|	
                    |d         |	j        z
            |k    }||z   }|s;t          |          s,|	                    |           rt          j        dd           |	                    |d|z  d|z  | rd	nd
z            }t          j        |          d                             |          }|	                    |||z
  |d|f                   }t          j        |          d|f                             |          }|	                    |||z   |d|f                   }|s;||z  }t          j        |          d                             |d         |z
            }t          j        |          d|f                             |          }||	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     s    
		Bq\\F
Cxx1~~HAI XXmmmm176XBBFVF^^F#''BHHRXXa^^RXXaQR^^,T,T(TUUF '!!aK '!!aK FF6&>""c)EFF6&>BE)**c1EemE 
]5%9%9 
bffeVnn 
 		
 	
 	
 	
 
%Xq9}i8NQ'O	P	PBVF^^F#''++F 
%I-vc;6F/G	H	HBVF^^C,-11"55F 
%I-vc;6F/G	H	HB C$Y'++F6NT,ABBVF^^C,-11"55FrunRU+be3FMr    r  r   c                 N   t          |           }|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
  }|                    ||||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  s>   			BIOOAc2A2gJ#rr'
3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                 X    t          j        | d|          } | d         | dd df         fS )NrM   r%   r   .r   )r5   r6   )r   r   s     r   r  r  g  s3    qqR(((AW:qcrc{""r    c                 &    | t           j        dz  z  S Ng     f@npr   rv   s    r   rb   rb   l  s    RUU]##r    c                 &    | dt           j        z  z  S r  r  r  s    r   r   r   p  s    URU]##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  r2  r8  slicer=  rR  rC  rD  r  r   r   r   r   listr   r  r   r   r  rb   r   r}   r    r   <module>r     s	    
			           	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 + * * * * * ; ; ; ; ; ; 8 8 8 8 8 8 ( ( ( ( ( ( ( ( (
 
   
 
     $#+ #+ #+T #+e #+ #+ #+ #+LC'E C'e C' C' C' C'L       .% E       C      %        H EJ= =
==%*U]==A=
= = = =B $)BG  
 ;?
   E e    D E D U    *. .5 . . . . ,1$3OT$3 $3 $3
$3$3$($3HL$3
$3 $3 $3 $3V 	2 $2 2 2
2
2 2 	2 2 2 2 2 2j9e 9 9 9 9 9E e     KP 
%*T\CG
   4 !%)-< <
<< *uS#X
&< 	< < < <B H* H*
H*
$,H* 4<H* 5%$,,-	H* H* H* H*V" " "u "t " " " " ""1
11(+el(BT(I1
1 1 1 1 RW^$ ^$^$^$!&^$JN^$
5%^$ ^$ ^$ ^$B$e $ $ $%ue@S:T $ $ $ $DJ:J:J:!&J:
5%J: J: J: J:Z,e , ,% , , , ,@ %    	2% 	2E 	2 	2 	2 	2N# N# N N N N)
)) 	) 		)
 ) ) ) ) ) )X49 e  QV     E %    ;;; ; 	;
 ; ; ; ; ; ; ; ; ;|E e      #u #U5%<%8 # # # #
$U $u $ $ $ $$U $u $ $ $ $ $ $r    