
    \h.                         S SK Jr  S SKJr  S SKJrJr  S SK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r " S	 S
\5      r " S S\5      r " S S\5      r " S S\5      r " S S\5      r " S S\5      rS rg)    )Basic)sympify)cossin)eye	rot_axis1	rot_axis2	rot_axis3)ImmutableDenseMatrix)cacheit)StrNc                       \ rS rSrSrS rSrg)Orienter   z'
Super-class for all orienter classes.
c                     U R                   $ )z>
The rotation matrix corresponding to this orienter
instance.
)_parent_orientselfs    N/var/www/auris/envauris/lib/python3.13/site-packages/sympy/vector/orienters.pyrotation_matrixOrienter.rotation_matrix   s    
 """     N)__name__
__module____qualname____firstlineno____doc__r   __static_attributes__r   r   r   r   r      s    #r   r   c                   b   ^  \ rS rSrSrU 4S jrS r\S 5       r\	S 5       r
\	S 5       rSrU =r$ )	AxisOrienter   z#
Class to denote an axis orienter.
c                    > [        U[        R                  R                  5      (       d  [	        S5      e[        U5      n[        TU ]  XU5      nXl        X#l	        U$ )Nzaxis should be a Vector)

isinstancesympyvectorVector	TypeErrorr   super__new___angle_axis)clsangleaxisobj	__class__s       r   r*   AxisOrienter.__new__   sN    $ 3 344566goc$/
	
r   c                     g)aI  
Axis rotation is a rotation about an arbitrary axis by
some angle. The angle is supplied as a SymPy expr scalar, and
the axis is supplied as a Vector.

Parameters
==========

angle : Expr
    The angle by which the new system is to be rotated

axis : Vector
    The axis around which the rotation has to be performed

Examples
========

>>> from sympy.vector import CoordSys3D
>>> from sympy import symbols
>>> q1 = symbols('q1')
>>> N = CoordSys3D('N')
>>> from sympy.vector import AxisOrienter
>>> orienter = AxisOrienter(q1, N.i + 2 * N.j)
>>> B = N.orient_new('B', (orienter, ))

Nr   )r   r.   r/   s      r   __init__AxisOrienter.__init__(   s    8 	r   c                    [         R                  R                  U R                  U5      R	                  5       nUR                  U5      nU R                  n[        S5      X"R                  -  -
  [        U5      -  [        SUS   * US   /US   SUS   * /US   * US   S//5      [        U5      -  -   X"R                  -  -   nUR                  nU$ )z
The rotation matrix corresponding to this orienter
instance.

Parameters
==========

system : CoordSys3D
    The coordinate system wrt which the rotation matrix
    is to be computed
   r         )r%   r&   expressr/   	normalize	to_matrixr.   r   Tr   Matrixr   )r   systemr/   thetaparent_orients        r   r   AxisOrienter.rotation_matrixF   s     ||##DIIv6@@B~~f%

a&4&&=0CJ>!d1gXtAw!7"&q'1tAwh!7#'7(DGQ!7!9 :<?JGG 	'
 &r   c                     U R                   $ N)r+   r   s    r   r.   AxisOrienter.angle_   s    {{r   c                     U R                   $ rD   )r,   r   s    r   r/   AxisOrienter.axisc   s    zzr   r   )r   r   r   r   r   r*   r4   r   r   propertyr.   r/   r   __classcell__r1   s   @r   r!   r!      sN    	<  0    r   r!   c                   l   ^  \ rS rSrSrU 4S jr\S 5       r\S 5       r\S 5       r	\S 5       r
SrU =r$ )	ThreeAngleOrienterh   z+
Super-class for Body and Space orienters.
c           	        > [        U[        5      (       a  UR                  nSnUn[        U5      R	                  5       n[        U5      S:X  d  [        S5      eU Vs/ s H  owR                  SS5      PM     nnU Vs/ s H  owR                  SS5      PM     nnU Vs/ s H  owR                  SS	5      PM     nnS
R                  U5      nXE;  a  [        S5      e[        US   5      n[        US   5      n	[        US   5      n
[        U5      n[        U5      n[        U5      nU R                  (       a$  [        X5      [        X5      -  [        X5      -  nO#[        X5      [        X5      -  [        X5      -  nUR                  n[        TU ]=  XX#[        U5      5      nXl        X,l        X<l        Xll        Xl        U$ s  snf s  snf s  snf )N)123231312132213321121131212232313323 r7   z%rot_order should be a str of length 3X1Y2Z3r[   zInvalid rot_type parameterr   r9   r8   )r$   r   namestrupperlenr(   replacejoinintr   	_in_order_rotr=   r)   r*   _angle1_angle2_angle3
_rot_orderr   )r-   angle1angle2angle3	rot_orderapproved_ordersoriginal_rot_orderia1a2a3rA   r0   r1   s                r   r*   ThreeAngleOrienter.__new__m   s   i%%!I- '	N((*	I!#CDD2;<)QYYsC()	<2;<)QYYsC()	<2;<)QYYsC()	<GGI&	+899111==!"-!"-.!"-.M ""-!"-.!"-.M &goY9+*
= =<<s   G?G Gc                     U R                   $ rD   )rk   r   s    r   ro   ThreeAngleOrienter.angle1       ||r   c                     U R                   $ rD   )rl   r   s    r   rp   ThreeAngleOrienter.angle2   r|   r   c                     U R                   $ rD   )rm   r   s    r   rq   ThreeAngleOrienter.angle3   r|   r   c                     U R                   $ rD   )rn   r   s    r   rr   ThreeAngleOrienter.rot_order   s    r   r   )r   r   r   r   r   r*   rH   ro   rp   rq   rr   r   rI   rJ   s   @r   rL   rL   h   s^    )V        r   rL   c                   (    \ rS rSrSrSrS rS rSrg)BodyOrienter   z"
Class to denote a body-orienter.
Tc                 4    [         R                  XX#U5      nU$ rD   rL   r*   r-   ro   rp   rq   rr   r0   s         r   r*   BodyOrienter.__new__        ((f)24
r   c                     g)aZ  
Body orientation takes this coordinate system through three
successive simple rotations.

Body fixed rotations include both Euler Angles and
Tait-Bryan Angles, see https://en.wikipedia.org/wiki/Euler_angles.

Parameters
==========

angle1, angle2, angle3 : Expr
    Three successive angles to rotate the coordinate system by

rotation_order : string
    String defining the order of axes for rotation

Examples
========

>>> from sympy.vector import CoordSys3D, BodyOrienter
>>> from sympy import symbols
>>> q1, q2, q3 = symbols('q1 q2 q3')
>>> N = CoordSys3D('N')

A 'Body' fixed rotation is described by three angles and
three body-fixed rotation axes. To orient a coordinate system D
with respect to N, each sequential rotation is always about
the orthogonal unit vectors fixed to D. For example, a '123'
rotation will specify rotations about N.i, then D.j, then
D.k. (Initially, D.i is same as N.i)
Therefore,

>>> body_orienter = BodyOrienter(q1, q2, q3, '123')
>>> D = N.orient_new('D', (body_orienter, ))

is same as

>>> from sympy.vector import AxisOrienter
>>> axis_orienter1 = AxisOrienter(q1, N.i)
>>> D = N.orient_new('D', (axis_orienter1, ))
>>> axis_orienter2 = AxisOrienter(q2, D.j)
>>> D = D.orient_new('D', (axis_orienter2, ))
>>> axis_orienter3 = AxisOrienter(q3, D.k)
>>> D = D.orient_new('D', (axis_orienter3, ))

Acceptable rotation orders are of length 3, expressed in XYZ or
123, and cannot have a rotation about about an axis twice in a row.

>>> body_orienter1 = BodyOrienter(q1, q2, q3, '123')
>>> body_orienter2 = BodyOrienter(q1, q2, 0, 'ZXZ')
>>> body_orienter3 = BodyOrienter(0, 0, 0, 'XYX')

Nr   r   ro   rp   rq   rr   s        r   r4   BodyOrienter.__init__   s    n 	r   r   N	r   r   r   r   r   ri   r*   r4   r   r   r   r   r   r      s     I
7r   r   c                   (    \ rS rSrSrSrS rS rSrg)SpaceOrienter   z#
Class to denote a space-orienter.
Fc                 4    [         R                  XX#U5      nU$ rD   r   r   s         r   r*   SpaceOrienter.__new__   r   r   c                     g)a  
Space rotation is similar to Body rotation, but the rotations
are applied in the opposite order.

Parameters
==========

angle1, angle2, angle3 : Expr
    Three successive angles to rotate the coordinate system by

rotation_order : string
    String defining the order of axes for rotation

See Also
========

BodyOrienter : Orienter to orient systems wrt Euler angles.

Examples
========

>>> from sympy.vector import CoordSys3D, SpaceOrienter
>>> from sympy import symbols
>>> q1, q2, q3 = symbols('q1 q2 q3')
>>> N = CoordSys3D('N')

To orient a coordinate system D with respect to N, each
sequential rotation is always about N's orthogonal unit vectors.
For example, a '123' rotation will specify rotations about
N.i, then N.j, then N.k.
Therefore,

>>> space_orienter = SpaceOrienter(q1, q2, q3, '312')
>>> D = N.orient_new('D', (space_orienter, ))

is same as

>>> from sympy.vector import AxisOrienter
>>> axis_orienter1 = AxisOrienter(q1, N.i)
>>> B = N.orient_new('B', (axis_orienter1, ))
>>> axis_orienter2 = AxisOrienter(q2, N.j)
>>> C = B.orient_new('C', (axis_orienter2, ))
>>> axis_orienter3 = AxisOrienter(q3, N.k)
>>> D = C.orient_new('C', (axis_orienter3, ))

Nr   r   s        r   r4   SpaceOrienter.__init__   s    ` 	r   r   Nr   r   r   r   r   r      s     I
0r   r   c                   r   ^  \ rS rSrSrU 4S jrS r\S 5       r\S 5       r	\S 5       r
\S 5       rS	rU =r$ )
QuaternionOrienteri.  z(
Class to denote a quaternion-orienter.
c           	        > [        U5      n[        U5      n[        U5      n[        U5      n[        US-  US-  -   US-  -
  US-  -
  SX#-  X-  -
  -  SX-  X$-  -   -  /SX#-  X-  -   -  US-  US-  -
  US-  -   US-  -
  SX4-  X-  -
  -  /SX$-  X-  -
  -  SX-  X4-  -   -  US-  US-  -
  US-  -
  US-  -   //5      nUR                  n[        TU ]  XX#U5      nXl        X&l        X6l        XFl        XVl	        U$ )Nr8   )
r   r>   r=   r)   r*   _q0_q1_q2_q3r   )r-   q0q1q2q3rA   r0   r1   s          r   r*   QuaternionOrienter.__new__3  s_   R[R[R[R["'B!G"3bAg"="$'#*"#rw'8"9"#rw'8"9"; #$rw'8"9"$'B!G"3"$'#*,.!G#4"#rw'8"9"; #$rw'8"9"#rw'8"9"$'B!G"3"$'#*,.!G#4"5!6 7 &gocrr2*
r   c                     g)a  
Quaternion orientation orients the new CoordSys3D with
Quaternions, defined as a finite rotation about lambda, a unit
vector, by some amount theta.

This orientation is described by four parameters:

q0 = cos(theta/2)

q1 = lambda_x sin(theta/2)

q2 = lambda_y sin(theta/2)

q3 = lambda_z sin(theta/2)

Quaternion does not take in a rotation order.

Parameters
==========

q0, q1, q2, q3 : Expr
    The quaternions to rotate the coordinate system by

Examples
========

>>> from sympy.vector import CoordSys3D
>>> from sympy import symbols
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = CoordSys3D('N')
>>> from sympy.vector import QuaternionOrienter
>>> q_orienter = QuaternionOrienter(q0, q1, q2, q3)
>>> B = N.orient_new('B', (q_orienter, ))

Nr   r   s        r   r4   QuaternionOrienter.__init__O  s    J 	r   c                     U R                   $ rD   )r   r   s    r   r   QuaternionOrienter.q0v      xxr   c                     U R                   $ rD   )r   r   s    r   r   QuaternionOrienter.q1z  r   r   c                     U R                   $ rD   )r   r   s    r   r   QuaternionOrienter.q2~  r   r   c                     U R                   $ rD   )r   r   s    r   r   QuaternionOrienter.q3  r   r   r   )r   r   r   r   r   r*   r4   rH   r   r   r   r   r   rI   rJ   s   @r   r   r   .  sc    8%N        r   r   c                     U S:X  a  [        [        U5      R                  5      $ U S:X  a  [        [        U5      R                  5      $ U S:X  a  [        [	        U5      R                  5      $ g)z)DCM for simple axis 1, 2 or 3 rotations. r9   r8   r7   N)r>   r   r=   r	   r
   )r/   r.   s     r   rj   rj     s^    qyi&(())	i&(())	i&(()) 
r   )sympy.core.basicr   sympy.core.sympifyr   (sympy.functions.elementary.trigonometricr   r   sympy.matrices.denser   r   r	   r
   sympy.matrices.immutabler   r>   sympy.core.cacher   sympy.core.symbolr   sympy.vectorr%   r   r!   rL   r   r   r   rj   r   r   r   <module>r      sz    " & ? G G C $ ! 
#u 
#M8 M`> >BC% CL<& <~V Vr*r   