ó
u™xTc           @   s-  d  d l  m Z d  d l m Z m Z d  d l m Z d  d l Z d e f d „  ƒ  YZ	 d e f d „  ƒ  YZ
 d	 e f d
 „  ƒ  YZ d e f d „  ƒ  YZ d e f d „  ƒ  YZ d e f d „  ƒ  YZ d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d S(   iÿÿÿÿ(   t   division(   t   center_of_mass_from_sitest   T_as_X(   t   matrixNt   six_dof_alignmentc           B   s   e  Z d  „  Z RS(   c      
   C   sV   t  d | ƒ |  _ d  |  _ t j d |  j f ƒ |  _ t j d |  j f ƒ |  _ d  S(   Nt   sitesi   i    (	   i   i    i    i    i   i    i    i    i   (	   i   i    i    i    i   i    i    i    i   (   R   t   pivott   Nonet   normalR   t   rtt   T0bt   Tb0(   t   OR   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   __init__
   s    	(   t   __name__t
   __module__R   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR      s   t   six_dofc           B   sn   e  Z e j d  d ƒ Z e j d  d ƒ Z e d „ Z d „  Z d „  Z	 d „  Z
 d „  Z d „  Z d „  Z RS(	   t   ni   c         C   s‚  | d k s t  ‚ | d k rH t | j ƒ d k ro t d | ƒ } qo n' t | j ƒ d k ro t d | ƒ } n  | |  _ | |  _ | |  _ | |  _ t | ƒ t | ƒ |  _	 | d k rß | j
 ƒ  |  _ t d |  j ƒ |  _ n t d | ƒ |  _ | r| |  _ n |  j j ƒ  | |  _ t j |  j |  j |  j f ƒ |  _ t j |  j j ƒ  |  j f ƒ |  _ t |  j ƒ |  _ d  |  _ d  S(   Nt   euler_paramst   euler_angles_xyzi   t   qEi   t   q(   R   R   (   t   AssertionErrort   lent   elemst&   euler_angles_xyz_qE_as_euler_params_qEt&   euler_params_qE_as_euler_angles_xyz_qEt   typeR   t   qrt   r_is_qrt   q_sizet	   normalizet   unit_quaterniont   RBDA_Eq_4_12t   Et   RBDA_Eq_4_7t   rt	   transposeR   R	   t   Tpst   TspR   t   XjR   t   S(   R   R   R   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR      s,    				&$c         C   s   |  j  |  j f S(   N(   R(   R)   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   Xj_S2   s    c   	   	   C   sú   t  j | j d  | j d g ƒ \ } } |  j d k rh t d |  j ƒ | } |  j | | j ƒ  } n' t d |  j ƒ | } |  j | | } |  j	 r® |  j
 j ƒ  | } n | | j |  j ƒ } |  j | | } t d |  j d | d | d |  j	 ƒ S(   Ni   R   R   R   R   R   R   (   R   t   col_listR   R   t   RBDA_Eq_4_13R    R   R   t   RBDA_Eq_4_8_invR   R"   R%   t   crossR   R   (	   R   t   qdt   delta_tt   w_body_framet   v_body_framet   qEdt   new_qEt   qrdt   new_qr(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   time_step_position5   s    )	c         C   s   | | | S(   N(    (   R   R/   t   qddR0   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   time_step_velocityD   s    c         C   sÛ   |  j  d k r> t d |  j ƒ } | d t d |  j ƒ } n t d |  j ƒ j ƒ  } t j | j	 d  | j	 d g ƒ \ } } |  j
 r¨ | | |  j j ƒ  | f } n  | | |  j j | ƒ | f } t j | ƒ j ƒ  S(   NR   R   i   i   (   R   t   d_unit_quaternion_d_qE_matrixR   R,   R    t   RBDA_Eq_4_8R%   R   R+   R   R   R"   R   R.   t   colt   resolve_partitions(   R   t   taut   dt   cR   t   ft   result(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   tau_as_d_pot_d_qG   s    )	   c         C   s   |  j  j |  j j S(   N(   R   R   R   (   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   get_qR   s    c      	   C   sZ   t  |  j j ƒ } t j | |  | | f ƒ \ } } t d |  j d | d | d |  j ƒ S(   NR   R   R   R   (   R   R   R   R   R+   R   R   R   (   R   R   t   iR4   R6   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   new_qU   s    #(   R   R   R   t   zerost   qd_zerot   qdd_zerot   FalseR   R*   R7   R9   RC   RD   RF   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR      s   					t   spherical_alignmentc           B   s   e  Z d  „  Z RS(   c      
   C   sV   t  d | ƒ |  _ d  |  _ t j d |  j f ƒ |  _ t j d |  j f ƒ |  _ d  S(   NR   i   i    (	   i   i    i    i    i   i    i    i    i   (	   i   i    i    i    i   i    i    i    i   (   R   R   R   R   R   R	   R
   R   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   \   s    	(   R   R   R   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRK   Z   s   t	   sphericalc           B   sk   e  Z e j d  d ƒ Z e j d  d ƒ Z d „  Z d „  Z d „  Z d „  Z	 d „  Z
 d „  Z d „  Z RS(	   R   i   c         C   s?  | d k s t  ‚ | d k rH t | j ƒ d k ro t d | ƒ } qo n' t | j ƒ d k ro t d | ƒ } n  | |  _ | |  _ t | ƒ |  _ | d k rÃ | j ƒ  |  _	 t
 d |  j	 ƒ |  _ n t d | ƒ |  _ t j |  j d f ƒ |  _ t j |  j j ƒ  d f ƒ |  _ t |  j ƒ |  _ t j d d	 d ƒ|  _ d  S(   NR   R   i   R   i   R   i    i   R   i   (   s   euler_paramss   euler_angles_xyz(   i    i    i    (   i    i    i    (   i   i    i    i    i   i    i    i    i   i    i    i    i    i    i    i    i    i    (   i   i   (   R   R   R   R   R   R   R   R   R   R    R!   R"   R#   R   R	   R&   R%   R'   R   R(   t   recR)   (   R   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   g   s.    		!     c         C   s   |  j  |  j f S(   N(   R(   R)   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR*   „   s    c         C   s…   | } |  j  d k rE t d |  j ƒ | } |  j | | j ƒ  } n* t d |  j j ƒ | } |  j | | } t d |  j  d | ƒ S(   NR   R   R   R   (   R   R,   R    R   R   R-   R   RL   (   R   R/   R0   R1   R3   R4   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR7   ‡   s    c         C   s   | | | S(   N(    (   R   R/   R8   R0   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR9   ‘   s    c         C   sd   |  j  d k r> t d |  j ƒ } | d t d |  j ƒ } n t d |  j ƒ j ƒ  } | } | | S(   NR   R   i   (   R   R:   R   R,   R    R;   R%   (   R   R>   R?   R@   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRC   ”   s    c         C   s
   |  j  j S(   N(   R   R   (   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRD   �   s    c         C   s   t  d |  j d t j | ƒ ƒ S(   NR   R   (   RL   R   R   R<   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRF       s    (   R   R   R   RG   RH   RI   R   R*   R7   R9   RC   RD   RF   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRL   b   s   			
				t   revolute_alignmentc           B   s   e  Z d  „  Z RS(   c         C   s]   | |  _  | |  _ | j ƒ  } t j | | | f ƒ |  _ t j | j ƒ  | f ƒ |  _ d  S(   N(   R   R   t   vector_to_001_rotationR   R	   R
   R%   R   (   R   R   R   R$   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   ¥   s
    		(   R   R   R   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRN   £   s   t   revolutec           B   sb   e  Z e j d  d ƒ Z e j d  d ƒ Z d „  Z d „  Z d „  Z d „  Z	 d „  Z
 d „  Z RS(   R   i   c      
   C   sã   | |  _  t | ƒ |  _ t j | d ƒ t j | d ƒ } } t j | | d | | d d d d f	 ƒ |  _ t j	 d ƒ |  _
 t j |  j d f ƒ |  _ t j |  j j ƒ  d f ƒ |  _ t |  j ƒ |  _ t j	 d ƒ |  _ d  S(   Ni    i   (   i    i    i    (   i    i    i    (   i    i    i    (   i    i    i   i    i    i    (   R   R   R   t   matht   cost   sinR   t   sqrR"   R<   R$   R	   R&   R%   R'   R   R(   R)   (   R   R   R@   t   s(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   ±   s    	'.!c         C   s   |  j  |  j f S(   N(   R(   R)   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR*   ¾   s    c         C   s   |  j  | | } t d | ƒ S(   NR   (   R   RP   (   R   R/   R0   R4   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR7   Á   s    c         C   s   | | | S(   N(    (   R   R/   R8   R0   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR9   Å   s    c         C   s
   |  j  j S(   N(   R   R   (   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRD   È   s    c         C   s   t  d t j | ƒ ƒ S(   NR   (   RP   R   R<   (   R   R   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRF   Ë   s    (   R   R   R   RG   RH   RI   R   R*   R7   R9   RD   RF   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRP   ¬   s   					c         C   sÇ   |  \ } } } d „  } | | ƒ \ } } | | ƒ \ } } | | ƒ \ }	 }
 t  j | | | | | | | |
 | |	 | | |
 | |	 | |
 | | |	 | |
 | | |	 | |
 | |	 f	 ƒ S(   Nc         S   s   t  j |  ƒ t  j |  ƒ f S(   N(   RQ   RR   RS   (   t   a(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   csÐ   s    (   R   RT   (   R   t   q1t   q2t   q3RW   t   c1t   s1t   c2t   s2t   c3t   s3(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR#   Î   s    	-c   	   
   C   sn   |  \ } } } d „  } | | ƒ \ } } | | ƒ \ } } t  j | d d | | | d | | | d f	 ƒ S(   Nc         S   s   t  j |  ƒ t  j |  ƒ f S(   N(   RQ   RR   RS   (   RV   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRW   Û   s    i    i   (   R   RT   (	   R   RX   RY   RZ   RW   R]   R^   R_   R`   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR;   Ù   s    	
c   	      C   sž   |  \ } } } d „  } | | ƒ \ } } | | ƒ \ } } t  | ƒ d k  r] t d ƒ ‚ n  t j d | | | | d | | d | | | | | | f	 ƒ S(   s½   
  Inverse of RBDA Eq. 4.8 (corresponding to Shabana G^-1)
  Mathematica code:
    ew = {{-s2, 0, 1}, {c2*s3, c3, 0}, {c2*c3, -s3, 0}}
    -Det[ew]
    Simplify[Inverse[ew] * (-Det[ew])]
  c         S   s   t  j |  ƒ t  j |  ƒ f S(   N(   RQ   RR   RS   (   RV   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyRW   ì   s    g�íµ ÷Æ°>s#   Euler angle near singular position.i    i   (   t   abst   RuntimeErrorR   RT   (	   R   RX   RY   RZ   RW   R]   R^   R_   R`   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR-   ã   s    	
c         C   s¶   |  \ } } } } t  j | d | d d | | | | | | | | | | | | | d | d d | | | | | | | | | | | | | d | d d f	 ƒ d S(   Ni   g      à?(   R   RT   (   R   t   p0t   p1t   p2t   p3(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR!   ö   s
    11c         C   sS   |  \ } } } } t  j | | | | | | | | | | | | f d d ƒd S(   NR   i   i   g      à?(   i   i   (   R   RM   (   R   Rc   Rd   Re   Rf   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR,   ý   s    

c         C   s  |  \ } } } } | d | d | d | d f \ } } } } | | | | d }	 | | | }
 | | | } | | | } | | | } | | } | | } | | } | | } | | } | | } t  j |
 | | | | | | | | | | | | | | | f ƒ |	 S(   s5  
  Coefficent matrix for converting gradients w.r.t. normalized Euler
  parameters to gradients w.r.t. non-normalized parameters, as produced
  e.g. by a minimizer in the line search.
  Mathematica code:
    nsq = p0^2+p1^2+p2^2+p3^2
    p0p = p0 / Sqrt[nsq]
    p1p = p1 / Sqrt[nsq]
    p2p = p2 / Sqrt[nsq]
    p3p = p3 / Sqrt[nsq]
    n3 = (p0^2+p1^2+p2^2+p3^2)^(3/2)
    FortranForm[FullSimplify[D[p0p,p0]*n3]]
    FortranForm[FullSimplify[D[p1p,p0]*n3]]
    FortranForm[FullSimplify[D[p2p,p0]*n3]]
    FortranForm[FullSimplify[D[p3p,p0]*n3]]
    FortranForm[FullSimplify[D[p0p,p1]*n3]]
    FortranForm[FullSimplify[D[p1p,p1]*n3]]
    FortranForm[FullSimplify[D[p2p,p1]*n3]]
    FortranForm[FullSimplify[D[p3p,p1]*n3]]
    FortranForm[FullSimplify[D[p0p,p2]*n3]]
    FortranForm[FullSimplify[D[p1p,p2]*n3]]
    FortranForm[FullSimplify[D[p2p,p2]*n3]]
    FortranForm[FullSimplify[D[p3p,p2]*n3]]
    FortranForm[FullSimplify[D[p0p,p3]*n3]]
    FortranForm[FullSimplify[D[p1p,p3]*n3]]
    FortranForm[FullSimplify[D[p2p,p3]*n3]]
    FortranForm[FullSimplify[D[p3p,p3]*n3]]
  i   i   g       @g      ø?(   R   RT   (   R   Rc   Rd   Re   Rf   t   p0st   p1st   p2st   p3st   n3t   c00t   c11t   c22t   c33t   c01t   c02t   c03t   c12t   c13t   c23(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR:     s$    .c         C   s   t  j t d t d |  ƒ ƒ ƒ S(   Niÿÿÿÿi   (   RQ   t   acost   maxt   min(   RV   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt	   safe_acos5  s    c         C   s   t  j t d t d |  ƒ ƒ ƒ S(   Niÿÿÿÿi   (   RQ   t   asinRw   Rx   (   RV   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt	   safe_asin8  s    c         C   s,   |  d k r | d k r d St  j |  | ƒ S(   Ng        (   RQ   t   atan2(   t   yt   x(    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt
   safe_atan2;  s     c         C   s²   |  \ } } } } t  d | | | | ƒ } | | | | d } | | | | } t | | ƒ } | | | | d }	 | | | | }
 t |
 |	 ƒ } t j | | | f ƒ S(   Niþÿÿÿg      à?(   R{   R   R   R<   (   R   Rc   Rd   Re   Rf   RY   t   c1c2t   s1c2RX   t   c2c3t   c2s3RZ   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   ?  s    c         C   sÇ   g  |  D] } t  j | d ƒ ^ q \ } } } g  |  D] } t  j | d ƒ ^ q6 \ } } } t j | | | | | | | | | | | | | | | | | | | | | | | | f ƒ S(   Ng       @(   RQ   RS   RR   R   R<   (   R   R   R\   R^   R`   R[   R]   R_   (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyR   J  s    //(   t
   __future__R    t   scitbx.rigid_body.proto.utilsR   R   t   scitbxR   RQ   t   objectR   R   RK   RL   RN   RP   R#   R;   R-   R!   R,   R:   Ry   R{   R   R   R   (    (    (    sO   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/joint_lib.pyt   <module>   s(   JA	"		
				0				