o ॵiiF@sddlmZddlmZmZmZmZmZmZddl Z ddl Z e j e ddfd>ddZe jde jdZgdgdgdged<gdgdgdged<gdgdgdged<gdgdgdged<gdgdgd ged!<gd"gd gdged#<gd gd"gdged$<gd gd%gdged&<gd"gd gd'ged(<gd)gdgd ged*<ed+d+d,Ze eae d-Zgd.gd/gd0gd1gedddddf<gd2gd.gd3gd0geddddd4f<gd5gd1gd.gd2geddddd6f<gd3gd5gd/gd.geddddd7f<eddd4dddfZe eaGd8d9d9ZGd:d;d;ZGdZd`dAdBZedadEdFZdYdGdHZdbdIdJZdcdKdLZdYdMdNZdOS)dRotationmatrcCs,|jdddkrtd|j||_dS)Nr!r zincorrect rotation shape: )shape ValueError_mat)selfr$rrr__init__>s zRotation.__init__r Fr r rrrrrcCsRtjd|||d}|jgdt|ddR}|jg|ddR}t|S)Nrrrr)reyeviewlenexpandr#)r%r rrr$rrridentityFs  zRotation.identityabcCs|||jSN)floattyper )r0r1rrr mat_mul_matSszRotation.mat_mul_matrtcCs$||dd|jSNr)r3 unsqueezesqueezer4r )r6r7rrr mat_mul_vecWs$zRotation.mat_mul_vecindexrcCs0t|ts|f}t|j|tdtdfdSNr$) isinstancetupler#r'slicer(r<rrr __getitem__[s  zRotation.__getitem__rightcCsRt|ttfrt|j|dSt|tjrt|j|ddStdt|d)Nr>).NNz/multiplicand must be a tensor or a number, got .) r?intr3r#r'rTensor TypeErrorr4)r(rDrrr__mul__`s zRotation.__mul__leftcC ||Sr2rIr(rJrrr__rmul__js zRotation.__rmul__othercCst|j|j}t|dSr=)r#r5rot_mat)r(rOnew_matrrr __matmul__ms zRotation.__matmul__cCs|jddS)Nrr!)r' transposer(rrr_inv_matqszRotation._inv_matcC|jSr2)r'rTrrrrPuszRotation.rot_matcCs t|jdSr=)r#rUrTrrrinvertys zRotation.invertptscCt|j|Sr2)r#r;r'r(rXrrrapply|zRotation.applycCrYr2)r#r;rUrZrrr invert_applyr\zRotation.invert_apply torch.SizecC|jjdd}|S)Nr!)r'r%r(srrrr%zRotation.shape torch.dtypecC|jjSr2)r'r rTrrrr zRotation.dtype torch.devicecCrdr2)r'rrTrrrrrezRotation.devicecCrdr2)r'rrTrrrrrezRotation.requires_graddimrFcCs<|t|jkr td|j|dkr|n|d}t|dS)NInvalid dimensionrrr>)r-r%r&r'r9r#)r(rgrot_matsrrrr9s zRotation.unsqueezefn&Callable[[torch.Tensor], torch.Tensor]cCs`|j|jjddd}tjtt|tj|dddd}||jddd}t|dS)Nr!)r"rrgr r>) r'r,r%rstacklistmapunbindr#)r(rjr$rrr map_tensor_fns" zRotation.map_tensor_fnrsSequence[Rotation]cCs6dd|D}tj||dkr|n|dd}t|dS)NcSg|]}|jqSr)rP).0r6rrr z Rotation.cat..rrrlr>)rcatr#)rrrgrirrrrxs z Rotation.catcCt|jdSr=)r#r'cudarTrrrrzz Rotation.cudacCst|jj||ddS)N)rr r>)r#r'to)r(rr rrrr|sz Rotation.tocCst|j|dSr=)r#r'r4r(r rrrr4z Rotation.typecCryr=)r#r'detachrTrrrrr{zRotation.detachN)r$r)r r rrrrrr#)r0rr1rrr)r6rr7rrr)r<rrr#)rDrrr#)rJrrr#)rOr#rr#rrrr#rXrrrrr^rrcrrf)rr)rgrFrr#)rjrkrr#)rrrsrgrFrr#)rrr r rr#)r r rr#)__name__ __module__ __qualname__r) staticmethodrr3rr/r5r;rCrIrNrRpropertyrUrPrWr[r]r%r rr9rqrxrzr|r4rrrrrr#<sL                     r#c@sNeZdZdbddZeejeddfdcddZddddZ deddZ dfddZ e dgd d!Z e dhd#d$Zdid&d'Zdjd(d)Zdkd+d,Zdld.d/Zdld0d1Zdmd2d3Zdnd6d7Zdjd8d9Zedod;d<Ze =dpdqdCdDZdrdGdHZedsdKdLZdtdNdOZdndPdQZdudSdTZdmdUdVZedvdXdYZdmdZd[Ze dwd]d^Z dmd_d`Z!daS)xFramerotationOptional[Rotation] translationOptional[torch.Tensor]cCs|dur|durtd}td}n$|dur"t|j|j|j|j}n|dur6t|jdd|j|j|j}|j|jddksF|j|jkrJtd||_||_ dS)N)rrz+RotationMatrix and translation incompatible) r#r/rr%r rrr&_r_t)r(rrrrrr)s*     zFrame.__init__r Fr% Iterable[int]r r rrrrrcCs tt||||t||||Sr2)rr#r/r)r%r rrrrrr/s zFrame.identityr<rcCs2t|tkr |f}t|j||j|tdfSr2)r4r@rrrrArBrrrrCs zFrame.__getitem__rDrcCs6t|tjs td|j|}|j|d}t||S)Nz'The other multiplicand must be a Tensor).N)r?rrGrHrrr)r(rDnew_rots new_transrrrrIs   z Frame.__mul__rJcCrKr2rLrMrrrrNs zFrame.__rmul__r^cCr_r8)rr%r`rrrr%rbz Frame.shaperfcCrdr2)rrrTrrrrrez Frame.devicer#cCrVr2rrTrrrget_rots zFrame.get_rotscCrVr2rrTrrr get_trans rzFrame.get_transrOcCs*|j|j}|j|j|j}t||Sr2)rr[rr)r(rOnew_rotrrrrcomposes  z Frame.composerXcCs|j|}||jSr2)rr[r)r(rXrotatedrrrr[s  z Frame.applycCs||j}|j|Sr2)rrr]rZrrrr]s  zFrame.invert_applycCs$|j}||j}t|d|Sr8)rrWr[rr)r(rot_invtrn_invrrrrW"s  z Frame.invertrjrkcCs:|j|}tjtt|tj|jdddd}t||S)Nrrl) rrqrrmrnrorprr)r(rjrrrrrrq(s  zFrame.map_tensor_fncCsV|jg|jddR}|jj|dddddf<|j|ddddf<d|d<|S)Nr.rr).rr)r new_zerosr%rrP)r(tensorrrr to_tensor_4x40s zFrame.to_tensor_4x4r7cCsR|jdddkr tdt|dddddfd}|ddddf}t||S)Nr!)rrzIncorrectly shaped input tensor.rr>)r%r&r#r)r7rotsrrrrfrom_tensor_4x47s  zFrame.from_tensor_4x4:0yE> p_neg_x_axisorigin p_xy_planeepsr3c stj|dd}tj|dd}tj|dd}ddt||D}ddt||D}ttdd|D|fdd|D}td dt||Dfd dt||D}ttd d|D|fd d|D}|d |d|d|d |d|d|d|d|d|d |d |dg}tjddt|||Ddd}||jddd}t|d}t |tj|ddS)NrrlcSg|]\}}||qSrrruc1c2rrrrvLz'Frame.from_3_points..cSrrrrrrrrvMrcs|]}||VqdSr2rrucrrr Oz&Frame.from_3_points..cg|]}|qSrrrdenomrrrvPcss|] \}}||VqdSr2rrrrrrQscsg|] \}}||qSrrr)dotrrrvRscsrr2rrrrrrSrcrrrrrrrrvTrrrrcSsg|] }|D]}|qqSrr)rutuprrrrrv[sr r>) rrpzipsqrtsumrmreshaper%r#r) rrrre0e1e2rrot_objr)rrr from_3_pointsAs&  zFrame.from_3_pointsrgrFcCsH|t|jkr td|j|}|j|dkr|n|d}t||S)Nrhrr)r-r%r&rr9rr)r(rgrrrrrr9bs   zFrame.unsqueezeTsSequence[Frame]cCsHtdd|D|}tjdd|D|dkr|n|dd}t||S)NcSrtrrruTrrrrvrrwzFrame.cat..cSrtrrrrrrrvsrwrrrl)r#rxrr)rrgrrrrrrxms( z Frame.catCallable[[Rotation], Rotation]cCst||j|jSr2rrrr(rjrrr apply_rot_fnwr~zFrame.apply_rot_fncCst|j||jSr2rrrrrapply_trans_fnzszFrame.apply_trans_fntrans_scale_factorcsfdd}||S)Ncs|Sr2r)r7rrrrjz#Frame.scale_translation..fn)r)r(rrjrrrscale_translation~s  zFrame.scale_translationcCsdd}||S)NcSs|Sr2)r)r6rrrrjrz#Frame.stop_rot_gradient..fn)rrrrrstop_rot_gradients zFrame.stop_rot_gradient#B ;cs$|j}|}||fddtdD\}}}t||d|d}| |} ||} | g| jddR} | | d<d| | d<| | d<| | d <d | d <t||d|d|d}||} t|d|d|} | g| jddR}| |d<| |d <d |d <d| |d <| |d <t|| }t |fddtdD\}}}t||d|d}| |}||}| g| jddR}d |d<||d <d||d<||d<||d <t||}| dd}t| |d}t || |S)Ncg|]}d|fqS.rrui)c_xyzrrrvrz7Frame.make_transform_from_reference..rr).rrr).rr).rr).rrr).rr).rr).rrcrrrr)n_xyzrrrvr).rr).rrr!r>) r r3rangerrrr%r#r5r;rSr4r)rca_xyzrr input_dtypec_xc_yd_pairnormsin_c1cos_c1c1_rotssin_c2cos_c2c2_rotsc_rots_n_yn_zsin_ncos_nn_rotsrrr)rrrmake_transform_from_referencesR  "       z#Frame.make_transform_from_referencecCst|j|jSr2)rrrzrrTrrrrzsz Frame.cudarccCs|jj|jjks J|jjSr2)rr rrTrrrr sz Frame.dtypecCst|j||j|Sr2)rrr4rr}rrrr4sz Frame.typeN)rrrr) r%rr r rrrrrr)r<rrr)rDrrr)rJrrrrrrr)rOrrrr)rr)rjrkrr)r7rrr)r) rrrrrrrr3rr)rgrFrr)rrrgrFrr)rjrrr)rr3rr)rr)"rrrr)rrr3rr/rCrIrNrr%rrrr[r]rWrqrrrr9rxrrrrrrzr r4rrrrrsP                     5 rc@seZdZd,ddZeejeddfd-ddZddZ ddZ ddZ eddZ eddZ eddZ d.d/d"d#Zd0d'd(Zd1d)d*Zd+S)2 Quaternion quaternionrrcCs.|jddkrtd|j||_||_dS)Nrrzincorrect quaternion shape: )r%r&_qr)r(rrrrrr)s zQuaternion.__init__r Fr%rr r rrrrrcCsdt||||}tjg|dR|||d}t d|d<Wdn1s(wYt||S)Nrrr).r)rrrno_gradr)r%r rrrquatsrrrr/s   zQuaternion.identitycCrVr2)rrTrrr get_quatsrzQuaternion.get_quatscCrVr2rrTrrrrrzQuaternion.get_transcCs|}t|}|Sr2)rr quat_to_rot)r(rrirrr get_rot_matss zQuaternion.get_rot_matscCs|j}|}tj|jkrt|jatjt|dddddf|dddddfdd}||}|jg|j ddddR}|S)N.r!rlrr) r r3_QUAT_TO_ROT_tensorrr|rrr4r,r%)normalized_quatr rot_tensorrrrrs  "zQuaternion.quat_to_rotcCs2|j}|}|tjj|ddd}||}|S)NrT)rgkeepdim)r r3rlinalgrr4)rr rrrnormalize_quats  zQuaternion.normalize_quatcCs|j}|}|}tj|jkrt|jat}|dt|jdd|j}tj ||dddddf|dddddfdd |S)Nr*r.rrl) r r3_QUAT_MULTIPLY_BY_VEC_tensorrr|r,r-r%rrr4)quatvecr r$ reshaped_matrrrquat_multiply_by_vec s "*zQuaternion.quat_multiply_by_vecT q_update_vecnormalize_quatscCs*|}|t||}|rt|}|Sr2)rrrr)r(rrr new_quatsrrrcompose_q_update_vecs zQuaternion.compose_q_update_vec update_vec pre_rot_matr#cCsJ|dddf|dddf}}||}||}|j|}t||S)N.r)rr[rr)r(rrq_vect_vecr trans_updaterrrrcompose_update_vec$s "    zQuaternion.compose_update_veccCst|j|jSr2)rrrrrTrrrr1r~zQuaternion.stop_rot_gradientN)rrrr) r%rr r rrrrrr)T)rrrrrr)rrrr#rr)rr)rrrr)rrr3rr/rrrrrrrr rrrrrrs(      r) r r r r rrrrrr) __future__rtypingrrrrrrnumpynprr3rrrfloat32 _QUAT_TO_ROTr from_numpyr_QUAT_MULTIPLY_QUAT_MULTIPLY_BY_VECrr#rrrrrrsR      {