ó
u™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 m Z d  d l m Z m Z d  d l Z d	 „  Z d
 „  Z d e f d „  ƒ  YZ d e j f d „  ƒ  YZ d „  Z d „  Z e d k re e j d ƒ n  d S(   iÿÿÿÿ(   t   division(   t   free_motion_reference_impl(   t   featherstoneN(   t   flex(   t   matrix(   t   approx_equal(   t   format_cpu_timest   null_outc       	   C   s„  t  j ƒ  }  t t j |  ƒ j ƒ  d ƒ s0 t ‚ t  j d |  ƒ } t j	 d t
 j j d t j |  ƒ d d ƒ ƒ } t | | ƒ s‡ t ‚ xö t t g D]è } t  j ƒ  } t | j | j | j | j | j g d d d d	 d
 g ƒ sè t ‚ x* t d ƒ D] } | j d d d | ƒ qõ Wd d d d d g d d d d d g g t | ƒ } t | j | j | j | j | j g | ƒ s” t ‚ q” Wd  S(   Ni    t
   sites_cartt   sym_mat3t   pointst   pivotgHÚÚh}ä?gƒyùK6‰?gdÌ]KÈ�?gùF$wÑ¤?g¸ëM€Êå?id   t   delta_tg{®Gáz„?t   use_classical_accelgï“Íy0�?gt·´vÁl·?g4óè~†â?g"Š¿­ôä?gÁö=v¢Ýå?g±/ G«?gÖÜ²Á7á?gm³áY%ä?go^«äÉÙå?(   i    i    i    (   i    i    i    (   t   fmrit-   create_triangle_with_center_of_mass_at_originR   R   t   vec3_doublet   meant   AssertionErrort   body_inertiaR   t   symt   scitbxt   matht   inertia_tensort   Falset   Truet
   simulationt   e_pott	   e_kin_angt	   e_kin_lint   e_kint   e_tott   xranget   dynamics_stept   int(   R   t   inertia1t   inertia2R   t   simt   it   expected(    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   exercise_reference_impl_quick   s0    $!			!c            sT  t  j ƒ  ‰ t j ˆ j g ƒ } ˆ  d IJ‡  ‡ f d †  } t d |  d ƒ } xQ t |  ƒ D]C } ˆ j d d ƒ | j ˆ j ƒ | | d k r] | | ƒ q] q] W| |  ƒ ˆ  Jˆ  d I|  IJˆ  d	 I| d IJˆ  d
 I| d IJˆ  d It j	 | ƒ IJˆ  d It j | ƒ IJˆ  d It j | ƒ t j	 | ƒ IJˆ  Jˆ  j
 ƒ  d  S(   Ns3   i_step, [e_pot, e_kin_ang, e_kin_lin, e_kin, e_tot]c            s<   ˆ  |  Iˆ j  ˆ j ˆ j ˆ j ˆ j g IJˆ  j ƒ  d  S(   N(   R   R   R   R   R   t   flush(   t   i_step(   t   outR%   (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   show+   s    *i   i
   R   gü©ñÒMbP?i    s   number of dynamics steps:s   e_tot start:s         final:iÿÿÿÿs           min:s           max:s       max-min:(   R   R   R   t   doubleR   t   maxR    R!   t   appendt   minR)   (   t   n_dynamics_stepsR+   t   e_totsR,   t   n_showR*   (    (   R+   R%   sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   exercise_reference_impl_long'   s(    

&t   featherstone_system_modelc           B   s   e  Z d  „  Z RS(   c         C   sX   d |  _  | g |  _ d g |  _ t j d d ƒ g |  _ t j | d | ƒ g |  _ d  S(   Ni   iÿÿÿÿt   ni   i    (   i    i    i    (	   t   NBt   pitcht   parentR   t   identityt   XtreeR   t   mcIt   I(   t   modelt   mR=   t   J(    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   __init__B   s
    	(   t   __name__t
   __module__RA   (    (    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyR5   @   s   t'   six_dof_joint_euler_params_featherstonec           B   s   e  Z d  „  Z RS(   c         C   sD   | d  k s t ‚ t j |  j ƒ t j |  j ƒ } d  } | | f S(   N(   t   NoneR   R   t   Xrott   Et   Xtranst   r(   t   Ot   qt   Xjt   S(    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   Xj_SK   s
    (   RB   RC   RN   (    (    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyRD   I   s   c            sZ   ‡  ‡ ‡ f d †  } t  j ƒ  ‰ x+ t d ƒ D] ‰  | ƒ  ˆ j d d ƒ q. W| ƒ  d  S(   Nc             so  t  d ˆ j d ˆ j d t d ˆ j j d ˆ j j ƒ ƒ }  d  g } ˆ j g } d  } t	 j
 ˆ j ˆ j f ƒ j ƒ  g } d d d g } t j |  | | | | | ƒ } ˆ  d d k r%ˆ d Iˆ j j IJˆ d	 I| d j d
  IJˆ Jˆ d Iˆ j j IJˆ d	 I| d j d
 IJˆ Jn  t | d j d
  ˆ j ƒ sHt ‚ t | d j d
 ˆ j ƒ skt ‚ d  S(   NR?   R=   R@   t   qEt   qri    i
   s   ang acc 3D:s           6D:i   s   lin acc 3D:(   R5   R?   t   I_F1RD   t   J1RO   RP   RE   t   qdR   t   colt   nc_F1t   f_F1t   resolve_partitionsR   t   FDabt   wd_F1t   elemst   as_F1R   R   (   R>   RK   RS   t   taut   f_extt	   grav_accnt   qdd(   R*   R+   R%   (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   checkS   s&    		$	$#id   R   gš™™™™™¹?(   R   R   R    R!   (   R+   R`   (    (   R*   R+   R%   sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   exercise_featherstone_FDabR   s    c         C   s‘   t  |  ƒ d k s t ‚ t  |  ƒ d k r< d } t ƒ  } n" t d t |  d ƒ ƒ } t j } t ƒ  t d | ƒ t	 d | d | ƒ t
 ƒ  GHd  S(   Ni    i   R+   R1   (   i    i   (   t   lenR   R   R.   R"   t   syst   stdoutR(   Ra   R4   R   (   t   argsR1   R+   (    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   runm   s    	t   __main__i   (   t
   __future__R    t   scitbx.rigid_body.protoR   R   R   t   scitbx.mathR   t   scitbx.array_familyR   R   t   libtbx.test_utilsR   t   libtbx.utilsR   R   Rc   R(   R4   t   objectR5   t   six_dof_joint_euler_paramsRD   Ra   Rf   RB   t   argv(    (    (    sU   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_free_motion.pyt   <module>   s    						