
    MZd+                         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 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_matrix)PointReferenceFramedynamicsymbolsDyadic)inertiaBody)raisesc            
         t        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 )Nbodybody_masscenterr   	body_masszbody_ixx body_iyy body_izzzbody_ixy body_iyz body_izx)r   nameloadsr
   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ixxiyyizzixyiyzizxs
             I/usr/lib/python3/dist-packages/sympy/physics/mechanics/tests/test_body.pytest_defaultr%      s    <D99::#$E	MM$**a 
//CJJE775>UYYu----99{++++89MCc89MCc<<GDJJS#sCM OO- - - -    c                  <   t        d      } t        d      }t        d      }t        |ddd      }t	        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 )Nrigidbody_masscenterrigidbody_massrigidbody_frame   r   rigidbody_bodyr   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      s1    !78,-N$%67O?Aq!4L&(<n%|5J


CE  !4775>155e<<<<<< 4 = =c BBBB??n,,,,0D!EEEE"""":|,,,:v&&&:w''':y)))r&   c                  4   t        d      } t        d      }t        d      }t        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        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 )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   central_inertiar.   	locatenewx)r4   r5   r6   r7   r   r   r   point_inertias           r$   test_particle_bodyr=   1   s     56?+M#$45N*=}')M

"
"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%%%%}=M))))$$..wHE!GM,?,?Aq$IIM*E2M  ]E$::::((F1I555%%%%r&   c                     t        d      } t        d      }t        d      }t        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 )	Nr4   r5   r6   r7   ar+   _point0r   )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   U   sA    56?+M#$45N*=}')M 	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4r&   c                    	
 t        d      } t        d      }t        d      }t        |ddd      }t	        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 )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 r&   c                  &     j                  d      S rM   rN   )r1   s   r$   rO   z%test_body_add_force.<locals>.<lambda>   s    j44Q7 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_forcerS   m   s    !78,-N$%67O?Aq!4L&(<n%|5J 	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8r&   c                     t        d      j                  j                  } j                  |        t	        j
                        dk(  sJ j
                  d   j                  | fk(  sJ t        t        fd       y )Nr   r+   r   c                  &     j                  d      S rM   )apply_torque)r   s   r$   rO   z&test_body_add_torque.<locals>.<lambda>   s    d//2 r&   )r   r   r;   rV   rB   r   r   rR   )torque_vectorr   s    @r$   test_body_add_torquerX      sg    <DJJLLMm$tzz?a::a=TZZ7777
923r&   c                     t        d      } t        d      }t        d|      }| j                  j                  ||j                         | j                  |      |j                  k(  sJ | j                  |      |j                  k(  sJ y NANBr   )r   r   r   r   rQ   masscenter_velr[   r\   r]   s      r$   test_body_masscenter_velra      ss    S	AsASALLACC A!##%%%A!##%%%r&   c                  X   t        d      } t        d      }t        d|      }| j                  j                  ||j                         | j                  |      |j                  k(  sJ |j                  |       |j                   k(  sJ | j                  |      |j                  k(  sJ y rZ   )r   r   r   set_ang_vely
ang_vel_inr`   s      r$   test_body_ang_velrf      s    S	AsASAGG133<<?acc!!!<<?qssd"""<<?acc!!!r&   c                     t        d      } t        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 )Nr[   r]   
   r   )r   r   r+   )r   r   orient_axisrQ   dcmr   r   r   )r[   r]   s     r$   test_body_dcmrk      s    S	AS	AGGB/558vBR!4BxR!6LiXYYYY55>Vc"gs2w%:c"gXs2wPQ<RT]$^____r&   c                      t        d      } t        d|       }|j                  | j                  k(  sJ |j                  | j                  k(  sJ |j                  | j                  k(  sJ y )Nr\   r]   r^   )r   r   r;   rd   rQ   )r\   r]   s     r$   test_body_axisrm      sT    sASA33!##::33!##::33!##::r&   c                  &   t        d      \  } }t        d      }t        d      }| |j                  z  }||j                  z  }|j                  ||       |j                  ||fgk(  sJ |j                  ||       |j                  |||z   fgk(  sJ y )Nza bPr]   )r   r
   r   r;   rd   rA   r   )r?   bro   r]   f1f2s         r$   #test_apply_force_multiple_one_pointrs      s    5>DAqc
AS	A	
133B	
133BMM"a772wiMM"a772b5zl"""r&   c                     t        d      \  } }t        d      \  }}}}t        d      }t        d      }t        d      }t        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 )	Nzf gz	q x v1 v2P1P2B1B2r\   reaction_body)r   r   r
   r   r   r   r   r;   rQ   rA   r   r   rd   r   )fgqr;   v1v2ru   rv   rw   rx   r\   forceg1g2force2s                  r$   test_apply_forcer      s   5>DAq!+.LAq"b	tB	tB	dB	dB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Br&   c                     t        d      } t        d      }t        d      }t        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 )Ntr}   rw   rx   r\   )	r   r   r   r   r;   rV   r   r   rd   )r   r}   rw   rx   r\   torquetorque2s          r$   test_apply_torquer      s    AsA	dB	dBsAqSWFOOFB886*++++88F7+,,,,eGOOG886'>23333r&   c                      t        d      } t        d      }t        d      }| |j                  z  }|j	                  ||       |j
                  ||fgk(  sJ |j                          |j
                  g k(  sJ y )Nr?   ro   r]   )r   r
   r   rQ   rA   r   clear_loads)r?   ro   r]   r   s       r$   test_clear_loadr      si    Ac
AS	AaccEEMM%775zl"""MMO77b==r&   c                     t        d      } t        d      }t        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 )Nru   rv   r]   )
r
   r   r;   rd   rA   r   remove_loadrV   crossr   )ru   rv   r]   rq   rr   s        r$   test_remove_loadr      s    	tB	tBS	A	
B	
BMM"bMM"b77Bx"b****MM"77Bxj   NN288B< 77Bx!''288B<!89999MMO77Bxj   r&   c                  0   t        d      } t        d      }t        d      }t        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	)
zXExample based on: https://pydy.readthedocs.io/en/latest/examples/multidof-holonomic.htmlWr]   ro   rp   zq1 q2zk c g kTzF Try   N)r   r   r   rA   r;   diffr   rd   rV   rQ   r   r   r   )r   r]   ro   rp   q1q2kcr|   kTFTs               r$   9test_apply_loads_on_multi_degree_freedom_holonomic_systemr     s   S	AS	AS	AS	A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&>?@@@@r&   c            
         t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t        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        d|||       }|j                  |	|       }t        | ||dz  z  ||dz  z  ||dz  |dz  z   z  | |z  |z        }|j                  rJ ||k(  sJ y )Nr\   zm, I_x, I_y, I_z, a, bopR)r   r   r   r9   rK   )r!   r[   r+      ro   )r   r   r   )r   r   r   r
   r:   r;   rd   r   parallel_axisri   rQ   r	   	to_matrixr   r.   )r\   mIxIyIzr?   rp   Ior   r   r   IpIp_expectedr[   ro   s                  r$   test_parallel_axisr   &  s   sA!":;Ar2r1a	BB	Bc
A	CQSS1qss7*+ASQaaDA	
	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SQQa0A	
A	B!QaZQ!VQ!q&16/5J b1fqj*K~~r&   N)#sympy.core.backendr   r   r   r   r   r   r	   sympy.physics.vectorr
   r   r   r   sympy.physics.mechanicsr   r   sympy.testing.pytestr   r%   r2   r=   rF   rS   rX   ra   rf   rk   rm   rs   r   r   r   r   r   r    r&   r$   <module>r      s{   2 2 2 N N 1 '- *2!&H509@4&"`	#CB4 ! A6r&   