ó
u™xTc           @   s½   d  Z  d d l m Z y d d l m Z Wn e k
 rI d d l Z n Xd e f d „  ƒ  YZ d e f d „  ƒ  YZ	 d	 „  Z
 d
 „  Z d „  Z d „  Z d „  Z e d k r¹ e ƒ  n  d S(   sÅ  6-DoF joint (free motion) reference implementation using the notation of
Featherstone (2007).

Dynamics simulation of the motion of a rigid triangle attached with
springs to an arbitrary position in space.

RBDA:
  Rigid Body Dynamics Algorithms.
  Roy Featherstone,
  Springer, New York, 2007.
  ISBN-10: 0387743146

Shabana (2005):
  Dynamics of Multibody Systems
  Ahmed A. Shabana
  Cambridge University Press, 3 edition, 2005
  ISBN-10: 0521850118
iÿÿÿÿ(   t   division(   t   matrixNt
   simulationc           B   s&   e  Z d  „  Z d „  Z e d „ Z RS(   c   
      C   s  t  ƒ  } | |  _ t |  j ƒ |  _ t |  j ƒ |  _ t j d ƒ j ƒ  } t j d ƒ } t	 | | ƒ |  _
 t j d ƒ } t j d ƒ } t j | | f ƒ j ƒ  |  _ t j d ƒ j ƒ  } t j d ƒ } t	 | | ƒ } g  |  j D] }	 | j |	 ^ qå |  _ |  j ƒ  d  S(   NgÍÌÌÌÌÌì?gìQ¸…ëÁ¿gš™™™™™¹?g¸…ëQ¸®?g333333Ã¿gš™™™™™©?g{®Gáz¤?g{®Gáz´?gš™™™™™©¿g®Gáz®ï?g)\�Âõ(¼¿gìQ¸…ëÁ?g®Gáz®×¿(   gÍÌÌÌÌÌì?gìQ¸…ëÁ¿gš™™™™™¹?g¸…ëQ¸®?(   g333333Ã¿gš™™™™™¹?gš™™™™™©?(   g{®Gáz¤?g333333Ã¿gš™™™™™¹?(   gš™™™™™¹?g{®Gáz´?gš™™™™™©¿(   g®Gáz®ï?gš™™™™™©¿g{®Gáz¤?g)\�Âõ(¼¿(   g333333Ã¿gìQ¸…ëÁ?g®Gáz®×¿(   t-   create_triangle_with_center_of_mass_at_origint   sites_cart_F1t   body_inertiat   I_F1t   lent   mR   t   colt	   normalizet   six_dof_joint_euler_paramst   J1t   resolve_partitionst   qdt   Tspt   sites_cart_wells_F01t!   energies_and_accelerations_update(
   t   Ot   sites_cart_F0t   qE_J1t   qr_J1t   w_F1t   v_F1t   qE_Jpt   qr_Jpt   Jpt   xyz(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyt   __init__   s    		&c         C   s"  g  |  j  D] } |  j j | ^ q
 |  _ g  |  j D] } |  j j | ^ q3 } d |  _ x9 t |  j  | ƒ D]% \ } } |  j | | j ƒ  7_ qk Wg  t |  j  | ƒ D] \ } } d | | ^ q§ } t	 j
 d ƒ |  _ t	 j
 d ƒ |  _ xG t |  j  | ƒ D]3 \ } } |  j | 7_ |  j | j | ƒ 7_ qþ W|  j |  j |  _ t	 j
 |  j j d  ƒ t	 j
 |  j j d ƒ } } |  j j ƒ  |  j | j |  j | ƒ |  _ |  j | j | ƒ |  _ d | j |  j | ƒ |  _ d |  j | j ƒ  |  _ |  j |  j |  _ |  j |  j |  _ d  S(   Ni    iþÿÿÿi   g      à?(   i    i    i    (   i    i    i    (   R   R   R   t   sites_cart_moved_F01R   t   Tpst   e_pott   zipt   dotR   R	   t   f_F1t   nc_F1t   crossR   t   ac_F1R   t   elemsR   t   inverset   wd_F1t   as_F1t	   e_kin_angt	   e_kin_lint   e_kint   e_tot(   R   R   t   sites_cart_wells_moved_F1t   xyz_wells_movedt	   f_cart_F1t   forceR   R   (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR   =   s.    )#		'3-c         C   s’   | r* t  j |  j |  j f ƒ j ƒ  } n! t  j |  j |  j f ƒ j ƒ  } |  j j |  j | | ƒ |  _ |  j j	 |  j | ƒ |  _ |  j
 ƒ  d  S(   N(   R   R	   R(   R%   R   R)   R   t   time_step_velocityR   t   time_step_positionR   (   R   t   delta_tt   use_classical_accelt   qdd(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyt   dynamics_stepn   s    $!(   t   __name__t
   __module__R   R   t   FalseR7   (    (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR      s   	 	1R   c           B   s#   e  Z d  „  Z d „  Z d „  Z RS(   c         C   s…   | |  _  | |  _ t | ƒ |  _ |  j j ƒ  | |  _ t j |  j |  j |  j f ƒ |  _ t j |  j j ƒ  |  j f ƒ |  _	 d  S(   N(
   t   qEt   qrt   RBDA_Eq_4_12t   Et	   transposet   rR   t   rtR   R   (   R   R;   R<   (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR   y   s    		&c   	      C   sŠ   t  j | j d  | j d g ƒ \ } } t |  j j ƒ | } | | j |  j ƒ } |  j | | j ƒ  } |  j | | } t | | ƒ S(   Ni   (	   R   t   col_listR&   t   RBDA_Eq_4_13R;   R$   R<   R
   R   (	   R   R   R4   R   R   t   qEdt   qrdt   new_qEt   new_qr(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR3   †   s    )c         C   s   | | | S(   N(    (   R   R   R6   R4   (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR2   Ž   s    (   R8   R9   R   R3   R2   (    (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR   w   s   		c         C   s¶   |  \ } } } } t  j | d | d d | | | | | | | | | | | | | d | d d | | | | | | | | | | | | | d | d d f	 ƒ d S(   Ni   g      à?(   R   t   sqr(   t   qt   p0t   p1t   p2t   p3(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR=   ‘   s
    11c         C   sS   |  \ } } } } t  j | | | | | | | | | | | | f d d ƒd S(   Nt   ni   i   g      à?(   i   i   (   R   t   rec(   RI   RJ   RK   RL   RM   (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyRC   ˜   s    

c         C   sî   d g d } x§ |  D]Ÿ \ } } } | d c | | | | 7<| d c | | | | 7<| d c | | | | 7<| d c | | 8<| d c | | 8<| d c | | 8<q W| d | d <| d | d	 <| d | d
 <t  j | ƒ S(   Ni    i	   i   i   i   i   i   i   i   i   (   R   RH   (   t
   sites_cartR   t   xt   yt   z(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR       s    c          C   s¸   t  j d	 d
 d d d d f g ƒ }  t  j d ƒ } x |  D] } | | 7} q; W| t |  ƒ } g  |  D] } | | ^ qf }  t t  j d ƒ j ƒ  ƒ } g  |  D] } | | ^ qž }  |  S(   Ni    i   g      à?i   gìQ¸…ëé?g{®GázÔ¿gáz®GáÚ¿g¤p=
×£Ð¿(   i    i    i    (   i   i    i    gªLXèz¶û?(   i    i    i    (   gìQ¸…ëé?g{®GázÔ¿gáz®GáÚ¿g¤p=
×£Ð¿(   R   RB   R	   R   R=   R
   (   RP   t   comR   R>   (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyR   ®   s    c          C   sŠ   t  ƒ  }  xu t d ƒ D]g } d G|  j G|  j G|  j GHd G|  j GHd G|  j GHd G|  j j GHd G|  j	 j GHH|  j
 d d ƒ q Wd	 GHd  S(
   Ni
   s   e_kin tot ang lin:s               e_pot:s               e_tot:s   ang acc 3D:s   lin acc 3D:R4   g{®Gáz„?t   OK(   R   t   xrangeR,   R*   R+   R   R-   R(   R&   R)   R7   (   R   t   i_time_step(    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyt   run¼   s    	t   __main__(   t   __doc__t
   __future__R    t   scitbxR   t   ImportErrort   scitbx_matrixt   objectR   R   R=   RC   R   R   RX   R8   (    (    (    s`   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/free_motion_reference_impl.pyt   <module>   s   \					