
    g!&h#/                         d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
mZmZ d dlmZmZ d dlmZmZ d Zd Zd Zd Zd	 Zd
 Zd Zd Zd Zd Zd Zd Zd Zd Z d Z!d Z"d Z#y)    )SymbolsymbolssincosMatrixzerossimplify)PointReferenceFramedynamicsymbolsDyadic)inertiaBody)raiseswarns_deprecated_sympyc            
      @   t               5  t        d      } d d d         j                  dk(  sJ | j                  g k(  sJ t	        d      }|j                  | j                  d       | j                  }| j                  }|j                  |      |j                  |      k(  sJ | j                  t        d      k(  sJ t        d      \  }}}t        d      \  }}}	| j                  t        | j                  ||||||	      | j                  fk(  sJ y # 1 sw Y   xY w)Nbodybody_masscenterr   	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   r   nameloadsr
   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ixxiyyizzixyiyzizxs
             }/var/www/pru.catia.catastroantioquia-mas.com/valormas/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_body.pytest_defaultr&      s
   		! F|99::#$E	MM$**a 
//CJJE775>UYYu----99{++++89MCc89MCc<<GDJJS#sCM OO- - - - s   DDc                  z   t        d      } t        d      }t        d      }t        |ddd      }t	               5  t        d| |||      }d d d        j                  }|j                  }| j                  |d       |j                  |      | j                  |      k(  sJ |j                  |      | j                  |      k(  sJ |j                  |k(  sJ |j                  || fk(  sJ |j                  sJ t        |d      sJ t        |d      sJ t        |d	      sJ t        |d
      sJ y # 1 sw Y   xY w)Nrigidbody_masscenterrigidbody_massrigidbody_frame   r   rigidbody_bodyr   r   r   r   )r
   r   r   r   r   r   r   r   r   r   pos_fromr   is_rigidbodyhasattr)r(   r)   r*   body_inertia
rigid_bodyr   r   s          r%   test_custom_rigid_bodyr2      sS    !78,-N$%67O?Aq!4L		! I*,@(/<I
I 

CE  !4775>155e<<<<<< 4 = =c BBBB??n,,,,0D!EEEE"""":|,,,:v&&&:w''':y)))#I Is   D11D:c                     t        d      } t        d      }t        d      }t               5  t	        d| ||      }d d d        j
                  }|j                  }| j                  |d       |j                  |      | j                  |      k(  sJ |j                  |      | j                  |      k(  sJ |j                  |k(  sJ t        |d      rJ t        |d      sJ t        |d      sJ t        |d	      sJ |j                  t        d      |j
                  fk(  sJ |j                  t        d      k(  sJ |j                  rJ t        |d
d
d
      |_        |j                  t        |d
d
d
      k(  sJ |j                  sJ t               5  t	        d|      }d d d        |j                  rJ |j
                  j!                  d|j"                        }|t        |j                  dd
d
      z  }||f|_        |j                  ||fk(  sJ |j                  t        d      k(  sJ |j                  sJ y # 1 sw Y   xY w# 1 sw Y   xY w)Nparticle_masscenterparticle_massparticle_frameparticle_bodyr   _inertiar   r   r   r+   )r   r   )r
   r   r   r   r   r   r   r   r   r-   r   r/   r   r   central_inertiar.   	locatenewx)r4   r5   r6   r7   r   r   r   point_inertias           r%   test_particle_bodyr=   3   s\    56?+M#$45N		! <_.A*N<< 
"
"CE2775>044U;;;;<< 3 < <S AAAA...}j111='***=,///=&)))  VAY0H0H$IIII((F1I555))))$+NAq!$DM!((GNAq!,LLLL%%%%		! B_=AB))))$$..wHE!GM,?,?Aq$IIM*E2M  ]E$::::((F1I555%%%%=< <,B Bs   I I I
Ic                     t        d      } t        d      }t        d      }t               5  t	        d| ||      }d d d        t        d      }|j
                  j                  z  }|j                  ||j                         t        |j                        dk(  sJ |j                  j                  |j                  dz   d      }|j                  |j
                  d       |j                  d   d   }|j
                  }|j                  |      |j                  |      k(  sJ |j                  |      |j                  |      k(  sJ |j                  d   d   |k(  sJ y # 1 sw Y   )xY w)	Nr4   r5   r6   r7   ar+   _point0r   )r
   r   r   r   r   r   r;   apply_forcer   lenr   r:   _namer   r   r-   )	r4   r5   r6   r7   r?   force_vectorr   force_pointr   s	            r%   test_particle_body_add_forcerF   Y   s\    56?+M#$45N		! <_.A*N<< 	sA}**,,,LlM,D,DE}""#q((($$..i',E	MM-%%q)%%a(+KE??5!UYYu%5555,{0KKKKq!!$444#< <s   E""E,c                    	
 t        d      } t        d      }t        d      }t        |ddd      }t	               5  t        d| |||      
d d d        t        d      }t        d      }
j                  j                  d	|
j                  j                  z        }|j                  
j                  d       |
j                  j                  z  	
j                  	|       t        
j                        dk(  sJ 
j                  d   d   }
j                  }|j                  |      |j                  |      k(  sJ |j!                  |      |j!                  |      k(  sJ 
j                  d   d   	k(  sJ 
j                  	       t        
j                        d
k(  sJ 
j                  d   d   	k(  sJ t#        t$        	
fd       t#        t$        
fd       y # 1 sw Y   xY w)Nr(   r)   r*   r+   r   r,   lFarigidbody_body_point0   c                  (    j                   d      S Nr   rA   )rD   r1   s   r%   <lambda>z%test_body_add_force.<locals>.<lambda>   s    j44\AF     c                  &     j                  d      S rM   rN   )r1   s   r%   rO   z%test_body_add_force.<locals>.<lambda>   s    j44Q7 rP   )r
   r   r   r   r   r   r   r:   r   r;   r   zrA   rB   r   r   r-   r   	TypeError)r(   r)   r*   r0   rH   rI   r   rE   r   rD   r1   s            @@r%   test_body_add_forcerT   r   s    !78,-N$%67O?Aq!4L		! I*,@(/<I
I 	sA	B!!++	J E 
MM*""A&
((***L</z A%%%""1%a(KE??5!UYYu%5555,{0KKKKAq!\111<(z A%%%Aq!\111
9FG
9783I Is   G''G1c                  B   t               5  t        d      d d d        j                  j                  } j	                  |        t        j                        dk(  sJ j                  d   j                  | fk(  sJ t        t        fd       y # 1 sw Y   ~xY w)Nr   r+   r   c                  &     j                  d      S rM   )apply_torque)r   s   r%   rO   z&test_body_add_torque.<locals>.<lambda>   s    d//2 rP   )	r   r   r   r;   rW   rB   r   r   rS   )torque_vectorr   s    @r%   test_body_add_torquerY      s    		! F|JJLLMm$tzz?a::a=TZZ7777
923 s   BBc                     t               5  t        d      } d d d        t        d      }t               5  t        d|      }d d d         j                  j	                  ||j
                         | j                        |j
                  k(  sJ | j                  |      |j
                  k(  sJ y # 1 sw Y   xY w# 1 sw Y   |xY wNANBr   )r   r   r   r   r   rR   masscenter_velr\   r]   r^   s      r%   test_body_masscenter_velrb      s    		! IsA		! ALLACC A!##%%%A!##%%%  s   B1B=1B:=Cc                     t               5  t        d      } d d d        t        d      }t               5  t        d|      }d d d         j                  j	                  ||j
                         | j                        |j
                  k(  sJ |j                  |       |j
                   k(  sJ | j                  |      |j
                  k(  sJ y # 1 sw Y   xY w# 1 sw Y   xY wr[   )r   r   r   r   set_ang_vely
ang_vel_inra   s      r%   test_body_ang_velrg      s    		! IsA		! AGG133<<?acc!!!<<?qssd"""<<?acc!!!  s   CCCC'c                  4   t               5  t        d      } t        d      }d d d         j                  j                  j                  |j                  j                  d       | j                  |      t        t        d      t        d      dgt        d       t        d      dgg dg      k(  sJ | j                  |j                        t        t        d      t        d      dgt        d       t        d      dgg dg      k(  sJ y # 1 sw Y   xY w)Nr\   r^   
   r   )r   r   r+   )	r   r   r   orient_axisrR   dcmr   r   r   )r\   r^   s     r%   test_body_dcmrl      s    		! II GGB/558vBR!4BxR!6LiXYYYY55>Vc"gs2w%:c"gXs2wPQ<RT]$^____ s   DDc                     t        d      } t               5  t        d|       }d d d        j                  | j                  k(  sJ |j                  | j                  k(  sJ |j
                  | j
                  k(  sJ y # 1 sw Y   [xY w)Nr]   r^   r_   )r   r   r   r;   re   rR   )r]   r^   s     r%   test_body_axisrn      so    sA		! A33!##::33!##::33!##::	 s   A>>Bc                  d   t        d      \  } }t        d      }t               5  t        d      }d d d        | j                  z  }||j
                  z  }|j                  ||       |j                  ||fgk(  sJ |j                  ||       |j                  |||z   fgk(  sJ y # 1 sw Y   wxY w)Nza bPr^   )r   r
   r   r   r;   re   rA   r   )r?   brp   r^   f1f2s         r%   #test_apply_force_multiple_one_pointrt      s    5>DAqc
A		! I	
133B	
133BMM"a772wiMM"a772b5zl""" s   B&&B/c                  B   t        d      \  } }t        d      \  }}}}t        d      }t        d      }t               5  t	        d      }t	        d      }	d d d        t        d      }
|j                  j                  ||j                  z         |j                  	j                  ||	j                  z         | |z  |
j                  z  }|j                  |||	|       |j                  ||fgk(  sJ |	j                  || fgk(  sJ |j                  |z  |
j                  z  }|	j                  |z  |
j                  z  }|j                  |       |	j                  |       |j                  ||f|j                  |fgk(  sJ |	j                  || f|	j                  |fgk(  sJ ||
j                  z  }|j                  ||	       |j                  ||f|j                  ||z   fgk(  sJ |	j                  || f|	j                  | |z   fgk(  sJ y # 1 sw Y   xY w)	Nzf gz	q x v1 v2P1P2B1B2r]   reaction_body)r   r   r
   r   r   r   r   r   r;   rR   rA   r   r   re   r   )fgqr;   v1v2rv   rw   rx   ry   r]   forceg1g2force2s                  r%   test_apply_forcer      s   5>DAq!+.LAq"b	tB	tB		! $Z$Z 	sAJJrxxBDD!JJrxxBDD!aCGENN5"b"%88U}$$$88eV~%%%	133B	133BNN2NN288E
R]]B$7888888eVr}}b&9::::qssUFNN6N,88UbmmVBY%?@@@@88eVr}}vgbj&ABBBB7 s   HHc                     t        d      } t        d      }t               5  t        d      }t        d      }d d d        t	        d      }| |z  |j
                  z  }j                  |       |j                  |j                  |fgk(  sJ |j                  |j                  | fgk(  sJ | |j                  z  }|j                  |       |j                  |j                  ||z   fgk(  sJ y # 1 sw Y   xY w)Ntr~   rx   ry   r]   )
r   r   r   r   r   r;   rW   r   r   re   )r   r~   rx   ry   r]   torquetorque2s          r%   test_apply_torquer      s    AsA		! $Z$Z 	sAqSWFOOFB886*++++88F7+,,,,eGOOG886'>23333 s   C..C7c                  0   t        d      } t        d      }t               5  t        d      }d d d        | j                  z  }|j                  ||       |j                  ||fgk(  sJ |j                          |j                  g k(  sJ y # 1 sw Y   `xY w)Nr?   rp   r^   )r   r
   r   r   rR   rA   r   clear_loads)r?   rp   r^   r   s       r%   test_clear_loadr     s    Ac
A		! IaccEEMM%775zl"""MMO77b== s   BBc                  \   t        d      } t        d      }t               5  t        d      }d d d        j                  }|j                  }|j                  ||        |j                  ||       |j                  | |f||fgk(  sJ |j                  |       |j                  | |fgk(  sJ |j                  |j                  |             |j                  | |f|j                  |j                  |      fgk(  sJ |j                          |j                  | |fgk(  sJ y # 1 sw Y   xY w)Nrv   rw   r^   )r
   r   r   r;   re   rA   r   remove_loadrW   crossr   )rv   rw   r^   rr   rs   s        r%   test_remove_loadr     s
   	tB	tB		! I	
B	
BMM"bMM"b77Bx"b****MM"77Bxj   NN288B< 77Bx!''288B<!89999MMO77Bxj    s   D""D+c                  p   t               5  t        d      } t        d      }t        d      }t        d      }ddd       t        d      \  }}t        d      \  }}}}	t        d      \  }
}j	                  |
 j
                  z         | j	                  ||z  | j
                  z  |	       | j	                  ||j                         z  | j
                  z  |	       j	                  |j                  |z  | j                  z         j	                  |j                  |z  | j                  z         |j                  |	|z  | j                  z  |	       |j                  || j                  z         |j                  |j                  |
||z  z
  ||j                         z  z
  | j
                  z  fgk(  sJ |j                  |j                  |j                  |z  | j                  z  f|j                  ||	|z  z   | j                  z  fgk(  sJ |j                  |j                  |j                  |z  | j                  z  f|j                  |	 |z  | j                  z  fgk(  sJ | j                  | j                  ||j                         z  ||z  z   | j
                  z  fgk(  sJ y# 1 sw Y   uxY w)
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlWr^   rp   rq   Nzq1 q2zk c g kTzF Trz   )r   r   r   r   rA   r;   diffr   re   rW   rR   r   r   r   )r   r^   rp   rq   q1q2kcr}   kTFTs               r%   9test_apply_loads_on_multi_degree_freedom_holonomic_systemr     s7   		! IIII	
 G$FB*%KAq!R% DAq MM!ACC%MM!B$qss(!M,MM!BGGI+acc/M3MM!&&(133,MM!&&(133, NN2b59AN.NN1QSS577q1R4x!BGGI+'=qss&BCDDDD77affQhqssl3aggBrE	1335OPPPP77affQhqssl3aggs2vaccz5JKKKK77q{QrT'9133&>?@@@@/ s   -J++J5c            
         t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t               5  t        d|| ||      }
d d d        
j                  |	      }t        | |||dz  z  z   |||dz  z  z   |||dz  |dz  z   z  z   | |z  |z        }||k(  sJ t        d	      }|j                  | | j                  d
       t        |
j                  |	|      |z
  j                  |            t        dd      k(  sJ t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t               5  t        d|||       }d d d        j                  |	|       }t        | ||dz  z  ||dz  z  ||dz  |dz  z   z  | |z  |z        }|j                  rJ ||k(  sJ y # 1 sw Y   qxY w# 1 sw Y   mxY w)Nr]   zm, I_x, I_y, I_z, a, bopR)r   r   r   r9   rK   )r"   r\   r+      rp   )r   r   r   )r   r   r   r
   r:   r;   re   r   r   parallel_axisrj   rR   r	   	to_matrixr   r.   )r]   mIxIyIzr?   rq   Ior   r   r   IpIp_expectedr\   rp   s                  r%   test_parallel_axisr   9  s"   sA!":;Ar2r1a	BB	Bc
A	CQSS1qss7*+A		! I!!RHI	
	B!R!ad(]BQTMqAqD1a4K00qb1fqjBKsAMM!QSS!	
A		,77:<?DQ{K K K 	c
A	CQSS1qss7*+A		! 5!45	
A	B!QaZQ!VQ!q&16/5J b1fqj*K~~)I I5 5s   0G0G=0G:=HN)$sympyr   r   r   r   r   r   r	   sympy.physics.vectorr
   r   r   r   sympy.physics.mechanicsr   r   sympy.testing.pytestr   r   r&   r2   r=   rF   rT   rY   rb   rg   rl   rn   rt   r   r   r   r   r   r    rP   r%   <module>r      s{   * * * N N 1 ?-"*4#&L529B4&	"`
# CD4"	!"A8rP   