ó
t™xTc           @   sx  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	 d  d l
 m Z d  d l m Z d  d l m Z d  d	 l m Z d  d
 l m Z m Z d  d l Z d „  Z d „  Z d „  Z d e f d „  ƒ  YZ d e f d „  ƒ  YZ d e f d „  ƒ  YZ d a d g Z d „  Z d „  Z  d d d „ Z! d „  Z" e# d k rte" e j$ d ƒ n  d S(   iÿÿÿÿ(   t   division(   t   featherstone(   t	   joint_lib(   t   spatial_inertia_from_sitest   kinetic_energyt   T_as_X(   t
   test_utils(   t-   create_triangle_with_center_of_mass_at_origin(   t   flex(   t   matrix(   t   approx_equal(   t   null_outt   show_times_at_exitNc         C   s$  xt  d ƒ D]} t j |  j d d ƒ ƒ j ƒ  } t j |  j d d ƒ d ƒ } t j d d d | d	 | ƒ } t j d d
 d | d	 | ƒ } t | j | j ƒ sª t	 ‚ t | j
 | j
 ƒ sÅ t	 ‚ t j d d d | j d	 | ƒ } t | j | j ƒ st	 ‚ t | j
 | j
 ƒ s t	 ‚ q Wd  S(   Ni   t   sizei   i   g      à?t   typet   euler_paramst   qEt   qrt   euler_angles_xyz(   t   xrangeR	   t   colt   random_doublet	   normalizeR   t   six_dofR
   t   Et   AssertionErrort   rR   (   t   mersenne_twistert   i_trialR   R   t   Jt   Jeat   Jep(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt/   exercise_euler_params_qE_as_euler_angles_xyz_qE   s    !!c         C   sM  xFt  d ƒ D]8} t j |  j ƒ  |  j d d ƒ d f ƒ } t j |  j ƒ  |  j d d ƒ d f ƒ } | | } | | } | j ƒ  } | j ƒ  } t | ƒ } t | ƒ }	 t | ƒ }
 t | ƒ } t | ƒ } t | ƒ } t | |	 |
 ƒ sö t ‚ t |	 | | ƒ st ‚ t | j	 ƒ  | ƒ s*t ‚ t |	 j	 ƒ  | ƒ s t ‚ q Wd  S(   Ni
   R   i   g      à?(
   R   R	   t   rtt    random_double_r3_rotation_matrixR   t   inverse_assuming_orthogonal_rR   R
   R   t   inverse(   R   R   t   T1t   T2t   T12t   T21t   T1it   T2it   X1t   X2t   X12t   X21t   X1it   X2i(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   exercise_T_as_X   s*    		

c         C   sG   t  ƒ  } t j |  j d d ƒ d d ƒ } g  | D] } | | ^ q3 S(   NR   i   i   i   (   R   R	   R   R   (   R   t   sitest   tt   site(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt*   create_triangle_with_random_center_of_mass4   s    	#t   featherstone_system_modelc           B   s   e  Z d  „  Z RS(   c         C   sF   d |  _  | g |  _ d g |  _ t | j ƒ g |  _ | g |  _ d  S(   Ni   iÿÿÿÿ(   t   NBt   pitcht   parentR   t   T0bt   Xtreet   I(   t   modelR<   t   AR   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   __init__;   s
    	(   t   __name__t
   __module__R?   (    (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyR6   9   s   t   simulation_mixinc           B   sA   e  Z d  „  Z d „  Z d „  Z d „  Z d d „ Z d „  Z RS(   c         C   s>   |  j  j |  j j |  j  j } g  |  j D] } | | ^ q* S(   N(   R>   t   Tb0R   t   TspR:   t   sites_F0(   t   Ot   AJAR4   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   sites_movedD   s     c      	   C   s  |  j  j d  k r |  j } n |  j  j |  j } t d |  j d | ƒ |  _ t j d |  j	 d |  j
 d |  j d |  j  ƒ |  _ t j d |  j	 d |  j
 d |  j d |  j  ƒ } |  j |  j |  _ t j d |  j	 d |  j
 d |  j d |  j  ƒ } t | |  j ƒ s
t ‚ t j d |  j	 d |  j
 d |  j d |  j  ƒ } t d |  j d |  j d |  j  ƒ } d  g } |  j g } d  } d d d g }	 t j | | | | | g |	 d	 t ƒ}
 t j | | | | | g |	 d	 t ƒ} t | |
 ƒ sët ‚ | |  _ | d |  _ d  S(
   Nt	   I_spatialt	   v_spatialR2   t   wellsR>   R   R<   i    t   f_ext_in_ff(   R   t   St   Nonet   qdR   RI   t   e_kinR   t   potential_energyRE   RK   R>   t   e_pott   potential_f_ext_fft   e_tott   potential_energy_bfR
   R   t   potential_f_ext_bfR6   R   t   FDabt   Truet   Falset   f_ext_bft   qdd(   RF   RJ   t   f_ext_fft   e_pot_bfRZ   R=   t   qRO   t   taut	   grav_accnt   qdd_using_f_ext_fft   qdd_using_f_ext_bf(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt!   energies_and_accelerations_updateH   s4    	*	'	'	'$	!!	c         C   sY   |  j  j d |  j d |  j d | ƒ |  _ |  j  j d |  j d | ƒ |  _  |  j ƒ  d  S(   NRO   R[   t   delta_t(   R   t   time_step_velocityRO   R[   t   time_step_positionRc   (   RF   Rd   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   dynamics_steph   s    *!c         C   sº   t  d |  j d |  j d |  j ƒ } d  g } t j d d ƒ g } t j d d ƒ g } d d d g } t j | | | | |  j	 g | ƒ d } t
 | |  j	 ƒ s§ t ‚ |  j j d | ƒ S(   NR<   R>   R   t   ni   i    R_   (   R6   RI   R>   R   RN   R	   t   zerosR   t   IDRZ   R
   R   t   tau_as_d_pot_d_q(   RF   R=   R^   RO   R[   R`   R_   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt	   d_pot_d_qm   s    $	(g�íµ ÷Æ°>c         C   s`  g  } xJ|  j  j |  j  j g D]0} x't t | ƒ ƒ D]} g  } xã | | g D]Ô } t | ƒ } | | c | 7<t j | ƒ } | |  j  j k rª | } |  j  j }	 n |  j  j } | }	 t j	 d |  j  j
 d | d |	 d |  j  j ƒ }
 t j d |  j d |  j d |  j d |
 ƒ } | j | ƒ qR W| j | d	 | d
 d | ƒ q8 Wq Wt j | ƒ S(   NR   R   R   t   r_is_qrR2   RK   R>   R   i    i   i   (   R   R   R   R   t   lent   listR	   R   R   R   R   Rm   R   RQ   RE   RK   R>   t   append(   RF   t   epst   resultR^   t   it   fst
   signed_epst   q_epsR   R   R   RR   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt    d_pot_d_q_via_finite_differencesw   s(       	'	$)c         C   s1   |  j  ƒ  } |  j ƒ  } t | | ƒ s- t ‚ d  S(   N(   Rl   Rw   R
   R   (   RF   t   anat   fin(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   check_d_pot_d_qŠ   s    (   R@   RA   RH   Rc   Rg   Rl   Rw   Rz   (    (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyRB   B   s   		 		
t   six_dof_simulationc           B   s   e  Z d  „  Z RS(   c      	   C   s&  t  d | ƒ |  _ t j d |  j ƒ |  _ t d |  j d |  j j ƒ |  _ t j	 d |  j d | ƒ |  _
 t j | j d d ƒ ƒ j ƒ  } t j | j d d ƒ d ƒ } | rË t j | ƒ j ƒ  | } n  t j d | d	 | d
 | d | ƒ |  _ t j | j d d ƒ d d ƒ |  _ |  j ƒ  d  S(   NR   R2   t   alignment_TR   i   i   g      à?R   R   R   Rm   i   i   i   (   R5   RE   R   t   six_dof_alignmentR>   R   R:   RI   R   t   create_wellsRK   R	   R   R   R   t   RBDA_Eq_4_12t	   transposeR   R   RO   Rc   (   RF   t   six_dof_typeRm   R   R   R   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyR?   ‘   s    	!'&(   R@   RA   R?   (    (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyR{   �   s   i    c         C   sÝ  t  d | d | d | ƒ } d | t | j j ƒ f } | j ƒ  | j ƒ  g } t j | j g ƒ }	 t j | j	 g ƒ }
 xW t
 | ƒ D]I } | j d | ƒ | j | j ƒ  ƒ |	 j | j ƒ |
 j | j	 ƒ q‡ W|	 |
 } | j ƒ  |  | IJ|  d It |	 ƒ It |	 ƒ IJ|  d It |
 ƒ It |
 ƒ IJ|  d It | ƒ It | ƒ IJ|  d	 I| d
 IJ|  d I| d IJt j | ƒ | j ƒ  } t j | ƒ t j | ƒ } | | } |  d I| IJ|  d I| IJ|  d I| IJ|  J|  j ƒ  |  t j k rÊ| j d d ƒ j d d ƒ j d d ƒ j d d ƒ j d d ƒ } t d t t d
 | f d ƒ } x< |	 |
 | g D]+ } x | D] } | | IJq‹W| d IJq~W| j ƒ  t d
 c d 7<n  | | | | | f S(   NR�   Rm   R   s   six_dof(type="%s", r_is_qr=%s)Rd   s   e_pot min, max:s   e_kin min, max:s   e_tot min, max:s   start e_tot:i    s   final e_tot:iÿÿÿÿs   ave:s   range:s   relative range:t    t    t   "t   (t   _t   )t   ,s   tmp_%02d_%02d_%s.xyt   wt   &i   (   R{   t   strR   Rm   Rz   RH   R   t   doubleRR   RP   R   Rg   Rp   t   mint   maxt   sumR   t   flusht   syst   stdoutt   replacet   opent   plot_prefixt   plot_numbert   close(   t   outR�   Rm   R   t   n_dynamics_stepsRd   t   simt	   sim_labelRH   t   e_potst   e_kinst   i_stept   e_totst   avet   ranget   relative_ranget   lt   ft   est   e(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   run_simulation§   s\    	



"""

 
c         C   sÑ  | j  ƒ  } g  } g  } g  } g  } x¨ t t g D]š }	 x‘ d d g D]ƒ }
 | j | ƒ t d |  d |
 d |	 d | d | d | ƒ \ } } } } } | j | ƒ | j | ƒ | j | ƒ | j | ƒ qD Wq1 Wt j ƒ  } xÙ t | d	 | d	 ƒ D]À \ } } |  d
 | d IJ|  d | IJt j ƒ  } xO t | d | ƒ D]: \ } } t j	 | ƒ } | j | j
 t j	 | ƒ ƒ ƒ q?W| j ƒ  j d |  d d ƒ | j t j | ƒ ƒ |  Jqó W|  j ƒ  | | | | f S(   NR   R   R˜   R�   Rm   R   R™   Rd   i   s   rms joints %si    s          vs. %s:t   prefixs     (   t   getstateRX   RY   t   setstateR§   Rp   R   RŒ   t   zipt   vec3_doublet   rms_differencet   min_max_meant   showRŽ   R�   (   R˜   R   R™   Rd   t   mt_statet
   sim_labelst   sites_moved_accut   e_tots_listt   relative_rangesRm   R�   Rš   R›   RH   RŸ   R¢   t   rms_max_listt   othert   rmst	   sites_reft   sites_other(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   run_simulationsÝ   s@    $ #
g-Cëâ6?c      
   C   s   t  j d | ƒ } d  } d  } d  } x?t | ƒ D]1}	 t d |  d | d | d | ƒ \ }
 } } } | d  k ry |
 } n | |
 k s‹ t ‚ | d  k rÅ g  t t | ƒ ƒ D] } t  j ƒ  ^ qª } n t | ƒ t | ƒ k sã t ‚ x* t | | ƒ D] \ } } | j	 | ƒ qó W| d  k rJg  t t | ƒ ƒ D] } t  j ƒ  ^ q/} n t | ƒ t | ƒ k sht ‚ x* t | | ƒ D] \ } } | j	 | ƒ qxW|  t
 j k r1 t d t |	 f d ƒ } | d IJxU t | ƒ D]G \ } } | | j d	 ƒ d
 j d	 d ƒ d  } | d | | f IJqÔWx3 | D]+ } x | D] } | | IJq3W| d IJq&W| j ƒ  q1 q1 W|  d IJ|  JxK t | | ƒ D]: \ } } |  d | IJ| j ƒ  j d |  d d ƒ |  Jq„Wxa t | d
 | ƒ D]L \ } } |  d | d IJ|  d | IJ| j ƒ  j d |  d d ƒ |  JqÖW|  t
 j k	 rœx) | D]! } t  j | ƒ d k  s<t ‚ q<Wx8 t | ƒ D]' \ } } t  j | ƒ d k  snt ‚ qnWn  d  S(   Nt   seedR˜   R   R™   Rd   s   tmp_e_tots_%02d_%02d.xyR‰   s   @with g0R„   i   Rƒ   iÿÿÿÿs   @ s%d legend "%s"RŠ   s   Accumulated results:s   relative ranges %s:R¨   s     s
   rms max %si    s       vs. %s:g-Cëâ6?(   R   R   RN   R   Rº   R   Rn   RŒ   R«   Rp   R‘   R’   R”   R•   t	   enumeratet   findR“   R—   R®   R¯   RŽ   (   R˜   t   n_trialsR™   Rd   t   random_seedR   R±   t   relative_ranges_accut   rms_max_list_accuR   t   sim_labels_newR³   R´   Rµ   Rs   R   t   aR¤   R£   R¥   R¦   R›   t   accu(    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   exercise_simulation  sd    	..
'
 c      	   C   sô   t  |  ƒ d k s t ‚ t  |  ƒ d k rH d } d } d } t ƒ  } nK t d t |  d ƒ ƒ } t d t |  d ƒ ƒ } t |  d ƒ } t j } t ƒ  t j	 d d ƒ } t
 d | ƒ t d | ƒ | a t d | d	 | d
 | d | ƒ d GHd  S(   Ni    i   i   i   i   R»   R   R˜   R¾   R™   R¿   t   OK(   i    i   (   Rn   R   R   RŽ   t   intR‘   R’   R   R   R   R    R1   R•   RÅ   (   t   argsR¾   R™   R¿   R˜   R   (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   run<  s(    	t   __main__i   (%   t
   __future__R    t   scitbx.rigid_body.protoR   R   t   scitbx.rigid_body.proto.utilsR   R   R   R   t2   scitbx.rigid_body.proto.free_motion_reference_implR   t   scitbx.array_familyR   t   scitbxR	   t   libtbx.test_utilsR
   t   libtbx.utilsR   R   R‘   R    R1   R5   t   objectR6   RB   R{   R•   R–   R§   Rº   RÅ   RÉ   R@   t   argv(    (    (    sZ   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion_hard.pyt   <module>   s0   				M		6	'8	