ó
s™xTc           @   s§   d  d l  m Z d  d l m Z d  d l m Z d  d l Z d  d l m Z d  d l m	 Z	 d  d l
 Z
 d d „ Z d	 e j f d
 „  ƒ  YZ d e f d „  ƒ  YZ d S(   iÿÿÿÿ(   t   division(   t   featherstone(   t   body_libN(   t   flex(   t   matrixi   c         C   s?  t  | ƒ t  |  ƒ k s t ‚ g  } t t j t j | ƒ ƒ ƒ } | j ƒ  } xÚt | j ƒ D]É\ } } g  | D] }	 t	 j
 |  |	 ƒ ^ qq }
 g  | D] }	 | |	 ^ q— } | j | } | j | ƒ } | d  k	 r<t  | ƒ d k sÿ t  | ƒ t  | ƒ k rt j d |
 d | ƒ } nt  | ƒ d k rUt j d |
 d | d |  | d ƒ } nÛ t  | ƒ d k r*g  | D] }	 t	 j
 |  |	 ƒ ^ qn} | d } | | d } xˆ |
 D]_ } t | j | | d d ƒƒ } | | k  r¬t j d |
 d | d | d | j ƒ  ƒ } Pq¬q¬Wt j d |
 d | ƒ } n t ‚ d	 | _ nÕ | d d	 k r�t  |
 ƒ d k ryt j d |
 d | ƒ } n t j d |
 d | ƒ } d	 | _ nt g  | D] }	 t	 j
 |  |	 ƒ ^ q¤} t j d |
 d | d | d d | d | d j ƒ  ƒ } | j | d | _ | | _ | j | ƒ q^ Wt j d
 | ƒ | S(   Ni   t   sitest   massesi   t   pivoti    t   value_if_undefinedt   normaliÿÿÿÿt   bodies(   t   lent   AssertionErrort   abst   matht   cost   radianst'   fixed_vertices_given_cluster_index_dictt	   enumeratet   clustersR   t   colt   hinge_edgest   gett   NoneR   t   zero_doft	   sphericalt	   cos_anglet   revolutet	   normalizet   parentt   translationalt   six_doft   cluster_indicest   i_seqst   appendt   set_cb_tree(   R   R   t   cluster_managert*   near_singular_hinges_angular_tolerance_degt   resultt   abs_cos_limitt   fvgcit   ict   clustert   it
   body_sitest   body_massest   het   fixed_verticest   bodyt   normal_sitesR   t   axist   sitet   abs_cos(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   construct_bodies	   sh    &	&
		&	
	t   modelc           B   s†   e  Z d  d „ Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z	 d	 „  Z
 d
 „  Z d „  Z d „  Z d d d „ Z RS(   i   c         C   s{   t  t |  ƒ j d t d | d | d | j d | ƒ ƒ | |  _ | |  _ | |  _ | |  _ | |  _	 | |  _
 |  j ƒ  d  S(   NR
   R   R   R$   R%   (   t   superR6   t   __init__R5   R$   t   labelsR   R   t
   tardy_treet   potential_objR%   t   flag_positions_as_changed(   t   OR9   R   R   R:   R;   R%   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyR8   I   s    	
						c         C   s;   d  |  _ d  |  _ d  |  _ d  |  _ t t |  ƒ j ƒ  d  S(   N(   R   t   _model__sites_movedt   _model__e_pott   _model__d_e_pot_d_sitest   _model__f_ext_arrayR7   R6   R<   (   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyR<   _   s
    				c         C   s    d  |  _ t t |  ƒ j ƒ  d  S(   N(   R   t   _model__qdd_arrayR7   R6   t   flag_velocities_as_changed(   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyRC   f   s    	c         C   sè   |  j  d  k rá |  j ƒ  } d  g t |  j ƒ |  _  d } |  j j j } xw t t |  j	 ƒ ƒ D]` } | | } xM | | D]A } |  j  | d  k s™ t
 ‚ | |  j | |  j  | <| d 7} qz Wq_ W| t |  j ƒ k sá t
 ‚ n  |  j  S(   Ni    i   (   R>   R   t	   aja_arrayR   R   R:   R$   R   t   xrangeR
   R   (   R=   t   O_ajat   n_doneR   t   ibt   ajat   i_seq(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   sites_movedj   s    
c         C   sR   |  j  d  k rK |  j d  k r* d |  _  qK |  j j d |  j ƒ  ƒ |  _  n  |  j  S(   Ni    RK   (   R?   R   R;   t   e_potRK   (   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyRL   y   s    c         C   sk   |  j  d  k rd |  j d  k rC t j d ƒ g t |  j ƒ |  _  qd |  j j d |  j ƒ  ƒ |  _  n  |  j  S(   Ni    RK   (   i    i    i    (	   R@   R   R;   R   R   R   R   t   d_e_pot_d_sitesRK   (   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyRM   ‚   s    %c         C   s  |  j  d  k r|  j ƒ  } |  j ƒ  } g  |  _  |  j j j } xË t |  j ƒ D]· \ } } | j	 j
 } | | } t j d ƒ } t j d ƒ }	 xO | | D]C }
 |  j |
 } | | |
 } | | 7} |	 | | j | ƒ 7}	 qš W|  j  j t j |	 | f ƒ j ƒ  ƒ qO Wn  |  j  S(   Ni    (   i    i    i    (   i    i    i    (   RA   R   t	   jar_arrayRM   R:   R$   R   R   R
   t	   alignmentt   cb_0bR   R   R   t   crossR"   t   resolve_partitions(   R=   t   O_jar_arrayt   O_d_e_pot_d_sitesR   RH   R0   RP   t   jart   ft   ncRJ   t   st   force_bf(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   f_ext_array‹   s"    	

,c         C   sJ   g  t  |  j |  j d |  j ƒ  ƒ ƒ D]! \ } } | j j d | ƒ ^ q% S(   s^   
Gradients of potential energy (defined via f_ext_array) w.r.t. positional
coordinates q.
    RZ   t   tau(   t   zipR
   t   f_ext_as_tauRZ   t   jointt   tau_as_d_e_pot_d_q(   R=   R0   R[   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   d_e_pot_d_qž   s    	c         C   sh   t  j ƒ  } | j |  j ƒ x* |  j ƒ  D] } | j t  j | ƒ ƒ q) W| j ƒ  |  j k sd t ‚ | S(   N(   R   t   doublet   reservet   q_packed_sizeR`   t   extendt   sizeR   (   R=   R&   t   v(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   d_e_pot_d_q_packed§   s    c         C   s@   |  j  d  k r9 |  j d d  d |  j ƒ  d d  ƒ |  _  n  |  j  S(   Nt	   tau_arrayRZ   t	   grav_accn(   RB   R   t   forward_dynamics_abRZ   (   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt	   qdd_array¯   s    	!c         C   s   |  j  ƒ  |  j ƒ  S(   N(   t   e_kinRL   (   R=   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   e_totµ   s    c         C   s™   |  j  ƒ  } x2 |  j D]' } | j j d | j d | ƒ | _ q WxG t |  j | ƒ D]3 \ } } | j j d | j d | d | ƒ | _ qT W|  j ƒ  d  S(   Nt   qdt   delta_tt   qdd(   Rk   R
   R^   t   time_step_positionRn   R\   t   time_step_velocityR<   (   R=   Ro   t   O_qdd_arrayR0   Rp   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   dynamics_step¸   s    c         C   s   t  d |  d | d | ƒ S(   Nt   tardy_modelt   max_iterationst   callback_after_step(   t   refinery(   R=   Rv   Rw   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   minimizationÂ   s    N(   t   __name__t
   __module__R8   R<   RC   RK   RL   RM   RZ   R`   Rg   Rk   Rm   Rt   R   Ry   (    (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyR6   G   s   														
Rx   c           B   s)   e  Z d d d  „ Z d „  Z d „  Z RS(   c      	   C   sê   | |  _  | |  _ | j ƒ  |  _ d |  _ d |  _ d |  _ x¡ t rß |  j } t j	 j
 d |  d t j	 j d | ƒ d t j	 j d t ƒ ƒ |  j | k rž Pn  | d  k	 rÍ |  j | k r½ Pn  | |  j 8} n  |  j d 7_ q? W|  ` d  S(   Ni    t   target_evaluatort   termination_paramsRv   t   exception_handling_paramst-   ignore_line_search_failed_step_at_lower_boundi   (   Ru   t   _refinery__callback_after_stept   pack_qt   xt   function_evaluations_totalt   lbfgs_steps_totalt   lbfgs_restartst   Truet   scitbxt   lbfgst   runt   termination_parameterst   exception_handling_parametersR   (   R=   Ru   Rv   Rw   t   lbfgs_steps_prev(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyR8   Ê   s,    								
c         C   s5   |  j  d 7_  |  j d  k	 r1 |  j d | ƒ n  d  S(   Ni   t	   minimizer(   R„   R€   R   (   R=   R�   (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyRw   â   s    c         C   sM   |  j  d 7_  |  j j d |  j ƒ |  j j ƒ  } |  j j ƒ  } | | f S(   Ni   t   q_packed(   Rƒ   Ru   t   unpack_qR‚   RL   Rg   (   R=   RV   t   g(    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt    compute_functional_and_gradientsç   s
    N(   Rz   R{   R   R8   Rw   R‘   (    (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyRx   È   s   	(   t
   __future__R    t   scitbx.rigid_body.essenceR   R   t   scitbx.lbfgsR‡   t   scitbx.array_familyR   R   R   R5   t   system_modelR6   t   objectRx   (    (    (    sM   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/tardy.pyt   <module>   s   :�