ó s™xTc@ s™dZddlmZyddlZWnek rEddlZnXddlmZd„Zd„Zd„Z d„Z d „Z d „Z d „Z dS( sBased in part on Roy Featherstone's spatial_v1 matlab code: http://axiom.anu.edu.au/~roy/spatial/ Version 1: January 2008 (latest bug fix: 7 October 2008) See also: RBDA: Rigid Body Dynamics Algorithms. Roy Featherstone, Springer, New York, 2007. ISBN-10: 0387743146 iÿÿÿÿ(tdivisionN(tmatrixc %C sš|\ }}}}}}}}}tj|||ddd|||ddd|||dddddd|||ddd|||ddd|||f$ƒS(sßRBDA Tab. 2.2, p. 23: Spatial coordinate transform (rotation around origin). Calculates the coordinate transform matrix from A to B coordinates for spatial motion vectors, in which frame B is rotated relative to frame A. i(Rtsqr( tetatbtctdtftgthti((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytxrots!c%C s‹|\}}}tjddddddddddddddddddd|| ddd| d|ddd|| ddddf$ƒS(süRBDA Tab. 2.2, p. 23: Spatial coordinate transform (translation of origin). Calculates the coordinate transform matrix from A to B coordinates for spatial motion vectors, in which frame B is translated by an amount r (3D vector) relative to frame A. ii(RR(trtr1tr2tr3((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytxtrans$scC s(t|jƒt|jjƒ |jƒS(sNRBDA Eq. 2.28, p. 22: Conversion of matrix.rt object cb to spatial transform. (R R Rt transposett(tcb((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytcb_as_spatial_transform4sc%C sš|\}}}}}}tjd| |ddd|d| ddd| |ddddd| |d| ||d| |d| | |d| |df$ƒS(s¹RBDA Eq. 2.31, p. 25: Spatial cross-product operator (motion). Calculates the 6x6 matrix such that the expression crm(v)*m is the cross product of the spatial motion vectors v and m. i(RR(tvtv1tv2tv3tv4tv5tv6((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytcrm:scC st|ƒjƒ S(sÑRBDA Eq. 2.32, p. 25: Spatial cross-product operator (force). Calculates the 6x6 matrix such that the expression crf(v)*f is the cross product of the spatial motion vector v with the spatial force vector f. (RR(R((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytcrfIscC s[tj|ƒ}tj||||jƒ||||jƒ|tjdƒfƒjƒS(søRBDA Eq. 2.63, p. 33: Spatial rigid-body inertia from mass, CoM and rotational inertia. Calculates the spatial inertia matrix of a rigid body from its mass, centre of mass (3D vector) and rotational inertia (3x3 matrix) about its centre of mass. i(Rtcross_product_matrixRRtidentitytresolve_partitions(tmRR tcx((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytmciRscC sd|j||ƒS(sRBDA Eq. 2.67, p. 35gà?(tdot(t i_spatialt v_spatial((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pytkinetic_energy^s(t__doc__t __future__Rtscitbxt ImportErrort scitbx_matrixRR RRRRR$R((((sS/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/essence/spatial_lib.pyt s