o
    8Va-                     @   s   d dl m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ZG dd deZG dd	 d	eZG d
d deZG dd deZG dd deZG dd deZdd ZdS )    )Basic)sympifyeyesincos	rot_axis1	rot_axis2	rot_axis3ImmutableMatrix)cacheit)StrNc                   @   s   e Zd ZdZdd ZdS )Orienterz/
    Super-class for all orienter classes.
    c                 C      | j S )zV
        The rotation matrix corresponding to this orienter
        instance.
        )_parent_orientself r   8/usr/lib/python3/dist-packages/sympy/vector/orienters.pyrotation_matrix   s   zOrienter.rotation_matrixN)__name__
__module____qualname____doc__r   r   r   r   r   r   	   s    r   c                       sL   e Zd ZdZ fddZdd Zedd Zedd	 Z	ed
d Z
  ZS )AxisOrienterz+
    Class to denote an axis orienter.
    c                    s>   t |tjjstdt|}t | ||}||_||_	|S )Nzaxis should be a Vector)

isinstancesympyvectorZVector	TypeErrorr   super__new___angle_axis)clsangleaxisobj	__class__r   r   r      s   zAxisOrienter.__new__c                 C      dS )a  
        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$   r   r   r   __init__&   s   zAxisOrienter.__init__c                 C   s   t j| j| }||}| j}td||j  t	| t
d|d  |d g|d d|d  g|d  |d dggt|  ||j  }|j}|S )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   Zexpressr$   	normalizeZ	to_matrixr#   r   Tr   Matrixr   )r   systemr$   Zthetaparent_orientr   r   r   r   D   s   
zAxisOrienter.rotation_matrixc                 C   r   N)r    r   r   r   r   r#   ]      zAxisOrienter.anglec                 C   r   r2   )r!   r   r   r   r   r$   a   r3   zAxisOrienter.axis)r   r   r   r   r   r)   r   r   propertyr#   r$   __classcell__r   r   r&   r   r      s    

r   c                       sP   e Zd ZdZ fddZedd Zedd Zedd	 Zed
d Z	  Z
S )ThreeAngleOrienterz3
    Super-class for Body and Space orienters.
    c                    s<  t |tr|j}d}|}t| }t|dkstddd |D }dd |D }dd |D }d|}||vr>td	t|d
 }t|d }t|d }	t	|}t	|}t	|}| j
rot||t|| t|	| }
nt|	|t|| t|| }
|
j}
t | |||t|}||_||_||_||_|
|_|S )N)Z123Z231Z312Z132Z213Z321Z121Z131Z212Z232Z313Z323 r*   z%rot_order should be a str of length 3c                 S      g | ]}| d dqS )X1replace.0ir   r   r   
<listcomp>v       z.ThreeAngleOrienter.__new__.<locals>.<listcomp>c                 S   r8   )Y2r;   r=   r   r   r   r@   w   rA   c                 S   r8   )Z3r;   r=   r   r   r   r@   x   rA   r7   zInvalid rot_type parameterr   r,   r+   )r   r   namestrupperlenr   joinintr   	_in_order_rotr.   r   r   _angle1_angle2_angle3
_rot_orderr   )r"   angle1angle2angle3	rot_orderZapproved_ordersZoriginal_rot_orderZa1Za2a3r1   r%   r&   r   r   r   k   sP   

zThreeAngleOrienter.__new__c                 C   r   r2   )rN   r   r   r   r   rR      r3   zThreeAngleOrienter.angle1c                 C   r   r2   )rO   r   r   r   r   rS      r3   zThreeAngleOrienter.angle2c                 C   r   r2   )rP   r   r   r   r   rT      r3   zThreeAngleOrienter.angle3c                 C   r   r2   )rQ   r   r   r   r   rU      r3   zThreeAngleOrienter.rot_order)r   r   r   r   r   r4   rR   rS   rT   rU   r5   r   r   r&   r   r6   f   s    +


r6   c                   @   $   e Zd ZdZdZdd Zdd ZdS )BodyOrienterz*
    Class to denote a body-orienter.
    Tc                 C      t | ||||}|S r2   r6   r   r"   rR   rS   rT   rU   r%   r   r   r   r         zBodyOrienter.__new__c                 C   r(   )a  
        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   rR   rS   rT   rU   r   r   r   r)      s   7zBodyOrienter.__init__Nr   r   r   r   rL   r   r)   r   r   r   r   rX      
    rX   c                   @   rW   )SpaceOrienterz+
    Class to denote a space-orienter.
    Fc                 C   rY   r2   rZ   r[   r   r   r   r      r\   zSpaceOrienter.__new__c                 C   r(   )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]   r   r   r   r)      s   0zSpaceOrienter.__init__Nr^   r   r   r   r   r`      r_   r`   c                       sX   e Zd ZdZ fddZdd Zedd Zedd	 Zed
d Z	edd Z
  ZS )QuaternionOrienterz0
    Class to denote a quaternion-orienter.
    c                    s0  t |}t |}t |}t |}t|d |d  |d  |d  d|| ||   d|| ||   gd|| ||   |d |d  |d  |d  d|| ||   gd|| ||   d|| ||   |d |d  |d  |d  gg}|j}t | ||||}||_||_||_||_||_	|S )Nr+   )
r   r/   r.   r   r   _q0_q1_q2_q3r   )r"   q0q1q2q3r1   r%   r&   r   r   r   1  sF   zQuaternionOrienter.__new__c                 C   r(   )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]   r   r   r   r)   M  s   %zQuaternionOrienter.__init__c                 C   r   r2   )rb   r   r   r   r   rf   t  r3   zQuaternionOrienter.q0c                 C   r   r2   )rc   r   r   r   r   rg   x  r3   zQuaternionOrienter.q1c                 C   r   r2   )rd   r   r   r   r   rh   |  r3   zQuaternionOrienter.q2c                 C   r   r2   )re   r   r   r   r   ri     r3   zQuaternionOrienter.q3)r   r   r   r   r   r)   r4   rf   rg   rh   ri   r5   r   r   r&   r   ra   ,  s    '


ra   c                 C   sF   | dkrt t|jS | dkrt t|jS | dkr!t t|jS dS )z)DCM for simple axis 1, 2 or 3 rotations. r,   r+   r*   N)r/   r   r.   r   r	   )r$   r#   r   r   r   rM     s   rM   )Zsympy.core.basicr   r   r   r   r   r   r   r   r	   r
   r/   Zsympy.core.cacher   Zsympy.core.symbolr   Zsympy.vectorr   r   r6   rX   r`   ra   rM   r   r   r   r   <module>   s    (PAF?Y