o
    oÇh­  ã                
   @   s\  d dl m  mZ d dlZd dlZd dlZ	e 
d¡Ze 
d¡Zd Ze eddd¡Ze d¡Ze d¡Ze de d¡e d	¡¡Ze d
e d¡e d	¡¡Ze d¡\ZZZe dd¡\ZZZe d¡Ze ed ¡ e 
d¡Z e !dee e "d	¡e #e j$e j$¡ef¡Z%e &ej'eej$ ¡ de (ej'¡ eej)  Z*e +eeej, ¡ de -e¡ eej)  Z*e  +eeej, ¡ de  -e¡ eej)  Z*e .ee -e¡ /e¡¡ de 0e¡ eej)  Z*ej' eeej$ eej)  ¡ e 1eeej) ¡ e 2e 3e¡ej' 4e¡¡Z5e5Z6de6 Z7e7d Z8de e e Z9e9de 7 Z9de Z9e d¡\Z:Z;Z<Z=e dd¡\Z>Z?Z@ZAe d¡\Z$Z)e dd¡\ZBZCe dd¡\ZDZEe d¡ZFe$eBd  d ZFe Gd gg¡Zde$ ed < e HejId  e Gd gg¡¡Zde) eejId  d < ded   Ze Gg d¢¡ Jdd¡Zded< ed d Ze:ej$ ZKe;ej, ZLe:ej$ e;ej)  ZKeKd ZMdS )é    NÚnÚaé   Úpo1Úpo2Úp1Úp1_ptÚmÚp2Úp2_ptzc1 c2 c3zc1_ c2_ c3_Úr_cmÚr_fÚré   é   zq1 q2 u1 u2zq1_ q2_ u1_ u2_zx yzx_ y_Úyy)	r   r   r   é   é   é   é   é   é	   r   )r   r   )NÚsympy.physics.mechanicsÚphysicsÚ	mechanicsÚ_meÚsympyÚ_smÚmathr	   ÚnumpyÚ_npÚReferenceFrameÚframe_nÚframe_ar   ÚinertiaÚdÚPointÚ	point_po1Ú	point_po2ÚParticleÚSymbolÚparticle_p1Úparticle_p2ÚdynamicsymbolsÚc1Úc2Úc3Úc1_dÚc2_dÚc3_dÚ	body_r_cmÚset_velÚbody_r_fÚ	RigidBodyÚsymbolsÚouterÚxÚbody_rÚset_posÚpointÚpos_fromÚyÚvÚset_ang_velÚzÚ
ang_vel_inÚset_ang_accÚdtÚ
ang_acc_inÚset_accÚcrossÚaccÚvelÚv_aÚx_b_cÚx_b_dÚ	a_b_c_d_eÚa_b_cÚq1Úq2Úu1Úu2Úq1_dÚq2_dÚu1_dÚu2_dÚx_dÚy_dÚx_ddÚy_ddr   ÚMatrixÚ
row_insertÚshapeÚreshapeÚforce_roÚtorque_aÚf© rc   rc   ú/var/www/html/construction_image-detection-poc/venv/lib/python3.10/site-packages/sympy/parsing/autolev/test-examples/ruletest9.pyÚ<module>   sl    





(


