ó
u™xTc           @   s’   d  d l  m Z d  d l m Z d  d l m Z d „  Z d „  Z d d d d „ Z	 d „  Z
 d „  Z d	 e f d
 „  ƒ  YZ d „  Z d „  Z d S(   iÿÿÿÿ(   t   division(   t   featherstone(   t   matrixc         C   sV   t  |  ƒ d k s t ‚ t j d ƒ } x |  D] } | | 7} q. W| t  |  ƒ } | S(   Ni    (   i    i    i    (   t   lent   AssertionErrorR   t   col(   t   sitest   resultt   site(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   center_of_mass_from_sites   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   t   sqr(   R   t   pivott   mR   t   xt   yt   z(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   inertia_from_sites   s    c         C   s�   | d  k r t |  ƒ } n  | d  k r9 t d |  ƒ } n  t d |  d | ƒ } | d  k	 r� | | } | j | | j j ƒ  } n  t j d | d | d | ƒ S(   NR   R   R   t   ct   I(   t   NoneR   R	   R   t   rt	   transposeR   t   mcI(   R   t   masst   center_of_masst   alignment_Tt   inertia(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   spatial_inertia_from_sites   s    
c         C   s   d | j  |  | ƒ S(   s   RBDA Eq. 2.67g      à?(   t   dot(   t	   I_spatialt	   v_spatial(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   kinetic_energy+   s    c         C   s.   t  j |  j ƒ t  j |  j j ƒ  |  j ƒ S(   N(   R   t   XrotR   t   XtransR   t   t(   t   Tps(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   T_as_X/   s    t   featherstone_system_modelc           B   s   e  Z d  „  Z RS(   c         C   së   t  | ƒ |  _ g  |  _ g  |  _ g  |  _ g  |  _ g  |  _ x¨ | D]  } |  j j | j ƒ |  j j | j ƒ | j d k r� | j	 j
 } n | j	 j
 | | j j	 j } |  j j | ƒ |  j j t | ƒ ƒ |  j j | j ƒ qC Wd  S(   Niÿÿÿÿ(   R   t   NBt   pitcht   parentt   Ttreet   XtreeR   t   appendt   Jt   At   T0bt   Tb0R$   (   t   modelt   bodiest   BR)   (    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   __init__5   s    					(   t   __name__t
   __module__R3   (    (    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyR%   3   s   c   	      C   sÞ   d  g |  j } d  g |  j } x· t |  j ƒ D]¦ } t j |  j | | | ƒ \ } } | d  k rr | | } n | | | } | |  j | | | <|  j | d k rµ | | | <q0 | | | |  j | | | | <q0 W| S(   Niÿÿÿÿ(   R   R&   t   xrangeR   t   jcalcR'   R*   R(   (	   R0   t   qt   qdR   t   Xupt   it   XJt   St   vJ(    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   spatial_velocities_from_modelG   s    #%c         C   sR   d } xE t  |  j t |  | | ƒ ƒ D]% \ } } | t d | d | ƒ 7} q% W| S(   Ni    R   R   (   t   zipR   R?   R   (   R0   R8   R9   R   R   R   (    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   e_kin_from_modelW   s
    %N(   t
   __future__R    t   scitbx.rigid_body.protoR   t   scitbxR   R	   R   R   R   R   R$   t   objectR%   R?   RA   (    (    (    sK   /public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/utils.pyt   <module>   s   					