B
    [1B                 @   s   d dl mZ d dlmZ d dlmZ d dlmZmZmZ d dlm	Z	m
Z
mZmZmZmZmZ d dlmZ d dlmZmZ d dlmZmZmZ d d	lmZmZ d d
lmZ d dlmZ d dlmZ d dlmZ G dd deZ dS )    )print_function)Expr)Rational)reim	conjugate)sqrtsincosacosasinexpln)trigsimp)diff	integrate)MatrixAddMul)symbolssympify)latex)
StrPrinter)Integer)
SYMPY_INTSc               @   sT  e Zd ZdZdZdZdKddZedd	 Zed
d Z	edd Z
edd Zedd Zedd Zedd Zedd Zdd Zdd Zdd Zdd Zd d! Zd"d# Zd$d% Zd&d' Zd(d) Zd*d+ Zd,d- Zed.d/ Zd0d1 Zd2d3 Zd4d5 Z d6d7 Z!d8d9 Z"d:d; Z#d<d= Z$d>d? Z%d@dA Z&dBdC Z'edDdE Z(dFdG Z)dLdIdJZ*dHS )M
Quaterniona  Provides basic quaternion operations.
    Quaternion objects can be instantiated as Quaternion(a, b, c, d)
    as in (a + b*i + c*j + d*k).

    Example
    ========

    >>> from sympy.algebras.quaternion import Quaternion
    >>> q = Quaternion(1, 2, 3, 4)
    >>> q
    1 + 2*i + 3*j + 4*k

    Quaternions over complex fields can be defined as :
    ========
    >>> from sympy.algebras.quaternion import Quaternion
    >>> from sympy import symbols, I
    >>> x = symbols('x')
    >>> q1 = Quaternion(x, x**3, x, x**2, real_field = False)
    >>> q2 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
    >>> q1
    x + x**3*i + x*j + x**2*k
    >>> q2
    (3 + 4*I) + (2 + 5*I)*i + 0*j + (7 + 8*I)*k
    g      &@Fr   Tc             C   s|   t |}t |}t |}t |}tdd ||||gD rDtdn4t| ||||}||_||_||_||_||_	|S d S )Nc             s   s   | ]}|j d kV  qdS )FN)is_commutative).0i r   8lib/python3.7/site-packages/sympy/algebras/quaternion.py	<genexpr>7   s    z%Quaternion.__new__.<locals>.<genexpr>z arguments have to be commutative)
r   any
ValueErrorr   __new___a_b_c_d_real_field)clsabcd
real_fieldobjr   r   r    r$   1   s    
zQuaternion.__new__c             C   s   | j S )N)r%   )selfr   r   r    r+   B   s    zQuaternion.ac             C   s   | j S )N)r&   )r1   r   r   r    r,   F   s    zQuaternion.bc             C   s   | j S )N)r'   )r1   r   r   r    r-   J   s    zQuaternion.cc             C   s   | j S )N)r(   )r1   r   r   r    r.   N   s    zQuaternion.dc             C   s   | j S )N)r)   )r1   r   r   r    r/   Q   s    zQuaternion.real_fieldc             C   s   |\}}}t |d |d  |d  }|| || ||   }}}t|tdd }t|tdd }|| }	|| }
|| }| ||	|
| S )a_  Returns a rotation quaternion given the axis and the angle of rotation.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import pi, sqrt
        >>> q = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
        >>> q
        1/2 + 1/2*i + 1/2*j + 1/2*k
              )r   r	   r   r
   	normalize)r*   Zvectoranglexyznormsr+   r,   r-   r.   r   r   r    from_axis_angleU   s    
zQuaternion.from_axis_anglec             C   s  |  tdd }t||d  |d  |d  d }t||d  |d  |d  d }t||d  |d  |d  d }t||d  |d  |d  d }yLt||d |d  }t||d	 |d
  }t||d |d  }W n tk
r   Y nX t||||S )aI  Returns the equivalent quaternion of a matrix. The quaternion will be normalized
        only if the matrix is special orthogonal (orthogonal and det(M) = 1).

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import Matrix, symbols, cos, sin, trigsimp
        >>> x = symbols('x')
        >>> M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
        >>> q = trigsimp(Quaternion.from_rotation_matrix(M))
        >>> q
        sqrt(2)*sqrt(cos(x) + 1)/2 + 0*i + 0*j + sqrt(-2*cos(x) + 2)/2*k
        r3      )r   r   )r3   r3   )r2   r2   r2   )r2   r3   )r3   r2   )r   r2   )r2   r   )r3   r   )r   r3   )Zdetr   r   r   _Quaternion__copysign	Exception)r*   MZabsQr+   r,   r-   r.   r   r   r    from_rotation_matrixm   s    $$$$zQuaternion.from_rotation_matrixc             C   s"   |dkrdS | | dkr| S |  S )Nr   r   )r6   r7   r   r   r    Z
__copysign   s    zQuaternion.__copysignc             C   s
   |  |S )N)add)r1   otherr   r   r    __add__   s    zQuaternion.__add__c             C   s
   |  |S )N)rA   )r1   rB   r   r   r    __radd__   s    zQuaternion.__radd__c             C   s   |  |d S )N)rA   )r1   rB   r   r   r    __sub__   s    zQuaternion.__sub__c             C   s   |  | |S )N)_generic_mul)r1   rB   r   r   r    __mul__   s    zQuaternion.__mul__c             C   s   |  || S )N)rG   )r1   rB   r   r   r    __rmul__   s    zQuaternion.__rmul__c             C   s
   |  |S )N)pow)r1   pr   r   r    __pow__   s    zQuaternion.__pow__c             C   s   t | j | j | j | j S )N)r   r%   r&   r'   r.   )r1   r   r   r    __neg__   s    zQuaternion.__neg__c             G   s
   | j | S )N)r   )r1   argsr   r   r    _eval_Integral   s    zQuaternion._eval_Integralc             O   s
   | j | S )N)r   )r1   r   kwargsr   r   r    
_eval_diff   s    zQuaternion._eval_diffc             C   s   | }t |}t|tsn|jrT|jrHtt||j t||j |j	|j
S t||S nt|j| |j|j	|j
S t|j|j |j|j |j	|j	 |j
|j
 S )a  Adds quaternions.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import symbols
        >>> q1 = Quaternion(1, 2, 3, 4)
        >>> q2 = Quaternion(5, 6, 7, 8)
        >>> q1.add(q2)
        6 + 8*i + 10*j + 12*k
        >>> q1 + 5
        6 + 2*i + 3*j + 4*k
        >>> x = symbols('x', real = True)
        >>> q1.add(x)
        (x + 1) + 2*i + 3*j + 4*k

        Quaternions over complex fields :
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import I
        >>> q3 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
        >>> q3.add(2 + 3*I)
        (5 + 7*I) + (2 + 5*I)*i + 0*j + (7 + 8*I)*k
        )r   
isinstancer   r/   
is_complexr   r+   r   r,   r-   r.   r   )r1   rB   q1q2r   r   r    rA      s    
&$zQuaternion.addc             C   s   |  | |S )a  Multiplies quaternions.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import symbols
        >>> q1 = Quaternion(1, 2, 3, 4)
        >>> q2 = Quaternion(5, 6, 7, 8)
        >>> q1.mul(q2)
        (-60) + 12*i + 30*j + 24*k
        >>> q1.mul(2)
        2 + 4*i + 6*j + 8*k
        >>> x = symbols('x', real = True)
        >>> q1.mul(x)
        x + 2*x*i + 3*x*j + 4*x*k

        Quaternions over complex fields :
        ========
        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import I
        >>> q3 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
        >>> q3.mul(2 + 3*I)
        (2 + 3*I)*(3 + 4*I) + (2 + 3*I)*(2 + 5*I)*i + 0*j + (2 + 3*I)*(7 + 8*I)*k
        )rG   )r1   rB   r   r   r    mul   s    zQuaternion.mulc             C   s  t | } t |}t| ts,t|ts,| | S t| ts|jrh| jr\|tt| t| dd S t| |S n&t| |j | |j	 | |j
 | |j S t|ts| jr|jr| tt|t|dd S t| |S n&t|| j || j	 || j
 || j S t| j	 |j	 | j
|j
  | j|j  | j|j  | j	|j | j
|j  | j|j
  | j|j	  | j	 |j | j
|j  | j|j	  | j|j
  | j	|j
 | j
|j	  | j|j  | j|j  S )Nr   )r   rR   r   r/   rS   r   r   r   r+   r,   r-   r.   )rT   rU   r   r   r    rG      s(    
&
&2.0zQuaternion._generic_mulc             C   s    | }t |j|j |j |j S )z(Returns the conjugate of the quaternion.)r   r+   r,   r-   r.   )r1   qr   r   r    _eval_conjugate   s    zQuaternion._eval_conjugatec             C   s4   | }t t|jd |jd  |jd  |jd  S )z#Returns the norm of the quaternion.r2   )r   r   r+   r,   r-   r.   )r1   rW   r   r   r    r9   %  s    zQuaternion.normc             C   s   | }|d|    S )z.Returns the normalized form of the quaternion.r3   )r9   )r1   rW   r   r   r    r4   ,  s    zQuaternion.normalizec             C   s,   | }|  stdt|d|  d   S )z&Returns the inverse of the quaternion.z6Cannot compute inverse for a quaternion with zero normr3   r2   )r9   r#   r   )r1   rW   r   r   r    inverse1  s    zQuaternion.inversec             C   st   | }|dkr|  S d}|dk r0|  |  }}t|ttfsBtS x,|dkrn|d@ r\|| }|d? }|| }qDW |S )a  Finds the pth power of the quaternion.
        Returns the inverse if p = -1.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> q = Quaternion(1, 2, 3, 4)
        >>> q.pow(4)
        668 + (-224)*i + (-336)*j + (-448)*k
        rE   r3   r   )rY   rR   r   r   NotImplemented)r1   rK   rW   Zresr   r   r    rJ   8  s    
zQuaternion.powc             C   s   | }t |jd |jd  |jd  }t|jt| }t|jt| |j | }t|jt| |j | }t|jt| |j | }t||||S )a]  Returns the exponential of q (e^q).

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> q = Quaternion(1, 2, 3, 4)
        >>> q.exp()
        E*cos(sqrt(29))
        + 2*sqrt(29)*E*sin(sqrt(29))/29*i
        + 3*sqrt(29)*E*sin(sqrt(29))/29*j
        + 4*sqrt(29)*E*sin(sqrt(29))/29*k
        r2   )	r   r,   r-   r.   r   r+   r
   r	   r   )r1   rW   vector_normr+   r,   r-   r.   r   r   r    r   X  s    "zQuaternion.expc             C   s   | }t |jd |jd  |jd  }| }t|}|jt|j|  | }|jt|j|  | }|jt|j|  | }t||||S )aw  Returns the natural logarithm of the quaternion (_ln(q)).

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> q = Quaternion(1, 2, 3, 4)
        >>> q._ln()
        log(sqrt(30))
        + 2*sqrt(29)*acos(sqrt(30)/30)/29*i
        + 3*sqrt(29)*acos(sqrt(30)/30)/29*j
        + 4*sqrt(29)*acos(sqrt(30)/30)/29*k
        r2   )	r   r,   r-   r.   r9   r   r   r+   r   )r1   rW   r[   Zq_normr+   r,   r-   r.   r   r   r    _lnp  s    "zQuaternion._lnc             C   s0   | }|  \}}t||| }|| |  S )a  Computes the pth power in the cos-sin form.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> q = Quaternion(1, 2, 3, 4)
        >>> q.pow_cos_sin(4)
        900*cos(4*acos(sqrt(30)/30))
        + 1800*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*i
        + 2700*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*j
        + 3600*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*k
        )to_axis_angler   r;   r9   )r1   rK   rW   vr5   rU   r   r   r    pow_cos_sin  s    zQuaternion.pow_cos_sinc             G   s>   t t| jf| t| jf| t| jf| t| jf| S )N)r   r   r+   r,   r-   r.   )r1   rN   r   r   r    r     s    zQuaternion.diffc             G   s>   t t| jf| t| jf| t| jf| t| jf| S )N)r   r   r+   r,   r-   r.   )r1   rN   r   r   r    r     s    zQuaternion.integratec             C   s^   t |tr t|d |d }n| }|td| d | d | d  t| }|j|j|jfS )a^  Returns the coordinates of the point pin(a 3 tuple) after rotation.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import symbols, trigsimp, cos, sin
        >>> x = symbols('x')
        >>> q = Quaternion(cos(x/2), 0, 0, sin(x/2))
        >>> trigsimp(Quaternion.rotate_point((1, 1, 1), q))
        (sqrt(2)*cos(x + pi/4), sqrt(2)*sin(x + pi/4), 1)
        >>> (axis, angle) = q.to_axis_angle()
        >>> trigsimp(Quaternion.rotate_point((1, 1, 1), (axis, angle)))
        (sqrt(2)*cos(x + pi/4), sqrt(2)*sin(x + pi/4), 1)
        r   r3   r2   )	rR   tupler   r;   r4   r   r,   r-   r.   )ZpinrrW   Zpoutr   r   r    rotate_point  s
    
&zQuaternion.rotate_pointc       	      C   s   | }y|j dk r|d }W n tk
r.   Y nX | }tdt|j  }td|j |j   }t|j| }t|j| }t|j| }|||f}||f}|S )aP  Returns the axis and angle of rotation of a quaternion

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> q = Quaternion(1, 1, 1, 1)
        >>> (axis, angle) = q.to_axis_angle()
        >>> axis
        (sqrt(3)/3, sqrt(3)/3, sqrt(3)/3)
        >>> angle
        2*pi/3
        r   rE   r2   r3   )	r+   BaseExceptionr4   r   r   r   r,   r-   r.   )	r1   rW   r5   r:   r6   r7   r8   r^   tr   r   r    r]     s    

zQuaternion.to_axis_angleNc             C   s  | }|  d }dd| |jd |jd    }d| |j|j |j|j   }d| |j|j |j|j   }d| |j|j |j|j   }dd| |jd |jd    }d| |j|j |j|j   }	d| |j|j |j|j   }
d| |j|j |j|j   }dd| |jd |jd    }|sVt|||g|||	g|
||ggS |\}}}|||  ||  ||  }|||  ||  ||	  }|||
  ||  ||  }d } }}d}t||||g|||	|g|
|||g||||ggS dS )a  Returns the equivalent rotation transformation matrix of the quaternion
        which represents rotation about the origin if v is not passed.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import symbols, trigsimp, cos, sin
        >>> x = symbols('x')
        >>> q = Quaternion(cos(x/2), 0, 0, sin(x/2))
        >>> trigsimp(q.to_rotation_matrix())
        Matrix([
        [cos(x), -sin(x), 0],
        [sin(x),  cos(x), 0],
        [     0,       0, 1]])

        Generates a 4x4 transformation matrix (used for rotation about a point
        other than the origin) if the point(v) is passed as an argument.

        Example
        ========

        >>> from sympy.algebras.quaternion import Quaternion
        >>> from sympy import symbols, trigsimp, cos, sin
        >>> x = symbols('x')
        >>> q = Quaternion(cos(x/2), 0, 0, sin(x/2))
        >>> trigsimp(q.to_rotation_matrix((1, 1, 1)))
         Matrix([
        [cos(x), -sin(x), 0,  sin(x) - cos(x) + 1],
        [sin(x),  cos(x), 0, -sin(x) - cos(x) + 1],
        [     0,       0, 1,                    0],
        [     0,       0, 0,                    1]])
        r3   r2   r   N)r9   r-   r.   r,   r+   r   )r1   r^   rW   r:   Zm00Zm01Zm02Zm10Zm11Zm12Zm20Zm21Zm22r6   r7   r8   Zm03Zm13Zm23Zm30Zm31Zm32Zm33r   r   r    to_rotation_matrix  s*    #          
zQuaternion.to_rotation_matrix)r   r   r   r   T)N)+__name__
__module____qualname____doc__Z_op_priorityr   r$   propertyr+   r,   r-   r.   r/   classmethodr;   r@   staticmethodr=   rC   rD   rF   rH   rI   rL   rM   rO   rQ   rA   rV   rG   rX   r9   r4   rY   rJ   r   r\   r_   r   r   rb   r]   rf   r   r   r   r    r      sJ   
"
,$ 'r   N)!Z
__future__r   Zsympy.core.exprr   Zsympyr   r   r   r   r   r	   r
   r   r   r   r   r   r   r   r   r   r   r   r   Zsympy.printing.latexr   Zsympy.printingr   Zsympy.core.numbersr   Zsympy.core.compatibilityr   r   r   r   r   r    <module>   s   $