
    Rie+                         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"dS )    )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        d           | j        }| j        }|                    |          |                    |          k    sJ | j        t          d          k    sJ t          d          \  }}}t          d          \  }}}	| j        t          | j        ||||||	          | j        fk    sJ d S )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
             Glib/python3.11/site-packages/sympy/physics/mechanics/tests/test_body.pytest_defaultr%      s   <<D9:#$$E	MM$*a   
/CJE775>>UYYu------9{++++++899MCc899MCc<GDJS#sCMM O- - - - - - -    c                     t          d          } t          d          }t          d          }t          |ddd          }t	          d| |||          }|j        }|j        }|                     |d           |                    |          |                     |          k    s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 d S )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      sr    !788,--N$%677O?Aq!44L&(<n%|5 5J

CE  !444775>>155e<<<<<<<< 4 = =c B BBBBB?n,,,,,0D!EEEEE"""":|,,,,,:v&&&&&:w''''':y)))))))r&   c                  N   t          d          } t          d          }t          d          }t          d| ||          }|j        }|j        }|                     |d           |                    |          |                     |          k    s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                            d|j                  }|t          |j        dd
d
          z  }||f|_        |j        ||fk    sJ |j        t          d          k    sJ |j        sJ d S )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   sj    566?++M#$455N*=}') )M

"CE222775>>044U;;;;;;<< 3 < <S A AAAAA....}j11111='*****=,/////=&))))) VAYY0H$IIIII(F1II5555))))$+NAq!$D$DM!(GNAq!,L,LLLLL%%%%}===M))))$..wHHE!GM,?Aq$I$IIM*E2M ]E$:::::(F1II5555%%%%%%r&   c                     t          d          } t          d          }t          d          }t          d| ||          }t          d          }||j        j        z  }|                    ||j                   t          |j	                  dk    sJ |j        
                    |j        dz   d          }|                    |j        d           |j	        d         d         }|j        }|                    |          |                    |          k    sJ |                    |          |                    |          k    sJ |j	        d         d         |k    sJ d S )	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   se    566?++M#$455N*=}') )M 	sA}*,,LlM,DEEE}"##q(((($..i', ,E	MM-%q)))%a(+KE??5!!UYYu%5%55555,,{0K0KKKKKq!!$444444r&   c                    	
 t          d          } t          d          }t          d          }t          |ddd          }t	          d| |||          
t          d          }t          d          }
j                            d	|
j        j        z            }|	                    
j        d           |
j        j
        z  	
                    	|           t          
j                  dk    sJ 
j        d         d         }
j        }|                    |          |                    |          k    sJ |                    |          |                    |          k    sJ 
j        d         d         	k    sJ 
                    	           t          
j                  d
k    sJ 
j        d         d         	k    sJ t!          t"          	
fd           t!          t"          
fd           d S )Nr(   r)   r*   r+   r   r,   lFarigidbody_body_point0   c                  0                          d          S Nr   rA   )rD   r1   s   r$   <lambda>z%test_body_add_force.<locals>.<lambda>   s    j44\AFF r&   c                  .                          d          S rM   rN   )r1   s   r$   rO   z%test_body_add_force.<locals>.<lambda>   s    j44Q7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8,--N$%677O?Aq!44L&(<n%|5 5J 	sA	B!++	J   E 
MM*"A&&&
(**L<///z  A%%%%"1%a(KE??5!!UYYu%5%55555,,{0K0KKKKKAq!\1111<(((z  A%%%%Aq!\1111
9FFFFFGGG
9777788888r&   c                      t          d          j        j        }                     |            t	          j                  dk    sJ j        d         j        | fk    sJ t          t          fd           d S )Nr   r+   r   c                  .                          d          S rM   )apply_torque)r   s   r$   rO   z&test_body_add_torque.<locals>.<lambda>   s    d//22 r&   )r   r   r;   rV   rB   r   r   rR   )torque_vectorr   s    @r$   test_body_add_torquerX      s    <<DJLMm$$$tz??a:a=TZ77777
9222233333r&   c                  $   t          d          } t          d          }t          d|          }| j                            ||j                   |                     |          |j        k    sJ |                     |          |j        k    sJ d S NANBr   )r   r   r   r   rQ   masscenter_velr[   r\   r]   s      r$   test_body_masscenter_velra      s    S		AsASALAC   A!#%%%%A!#%%%%%%r&   c                  f   t          d          } t          d          }t          d|          }| j                            ||j                   |                     |          |j        k    sJ |                    |           |j         k    sJ |                     |          |j        k    sJ d S rZ   )r   r   r   set_ang_vely
ang_vel_inr`   s      r$   test_body_ang_velrf      s    S		AsASAG13<<??ac!!!!<<??qsd""""<<??ac!!!!!!r&   c                  B   t          d          } t          d          }| j                            |j        |j        j        d           |                     |          t          t          d          t          d          dgt          d           t          d          dgg dg          k    sJ |                     |j                  t          t          d          t          d          dgt          d           t          d          dgg dg          k    sJ d S )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GB///5588vBR!4BxR!6LiiiXYYYYYY55>>Vc"ggs2ww%:c"ggXs2wwPQ<RT]T]T]$^________r&   c                      t          d          } t          d|           }|j        | j        k    sJ |j        | j        k    sJ |j        | j        k    sJ d S )Nr\   r]   r^   )r   r   r;   rd   rQ   )r\   r]   s     r$   test_body_axisrm      sX    sASA3!#::::3!#::::3!#::::::r&   c                  ,   t          d          \  } }t          d          }t          d          }| |j        z  }||j        z  }|                    ||           |j        ||fgk    sJ |                    ||           |j        |||z   fgk    sJ d S )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	
13B	
13BMM"a72wiMM"a72b5zl""""""r&   c                     t          d          \  } }t          d          \  }}}}t          d          }t          d          }t          d          }t          d          }	t	          d          }
|                    |j        ||j        z             |                    |	j        ||	j        z             | |z  |
j        z  }|	                    |||	|           |j
        ||fgk    sJ |	j
        || fgk    sJ |j        |z  |
j        z  }|	j        |z  |
j        z  }|	                    |           |		                    |           |j
        ||f|j        |fgk    sJ |	j
        || f|	j        |fgk    sJ ||
j        z  }|	                    ||	           |j
        ||f|j        ||z   fgk    sJ |	j
        || f|	j        | |z   fgk    sJ d S )	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JJrxBD!!!JJrxBD!!!aCGENN5"b"%%%8U}$$$$8eV~%%%%	13B	13BNN2NN28E
R]B$7888888eVr}b&9:::::qsUFNN6N,,,8UbmVBY%?@@@@@8eVr}vgbj&ABBBBBBBr&   c                     t          d          } t          d          }t          d          }t          d          }t          d          }| |z  |j        z  }|                    ||           |j        |j        |fgk    sJ |j        |j        | fgk    sJ | |j        z  }|                    |           |j        |j        ||z   fgk    sJ d S )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86*+++++8F7+,,,,,eGOOG86'>23333333r&   c                     t          d          } t          d          }t          d          }| |j        z  }|                    ||           |j        ||fgk    sJ |                                 |j        g k    sJ d S )Nr?   ro   r]   )r   r
   r   rQ   rA   r   clear_loads)r?   ro   r]   r   s       r$   test_clear_loadr      s{    Ac

AS		AacEEMM%75zl""""MMOOO7b======r&   c                  2   t          d          } t          d          }t          d          }|j        }|j        }|                    ||            |                    ||           |j        | |f||fgk    sJ |                    |           |j        | |fgk    sJ |                    |                    |                     |j        | |f|j	        |                    |          fgk    sJ |                                 |j        | |fgk    sJ d S )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7Bx"b*****MM"7Bxj    NN288B<<   7Bx!'288B<<!899999MMOOO7Bxj      r&   c                     t          d          } t          d          }t          d          }t          d          }t          d          \  }}t          d          \  }}}}	t          d          \  }
}|                    |
| j        z             |                     ||z  | j        z  |           |                     ||                                z  | j        z  |           |                    |j        |z  | j        z             |                    |j        |z  | j        z             |                    |	|z  | j	        z  |           |                    || j	        z             |j
        |j        |
||z  z
  ||                                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        ||                                z  ||z  z   | j        z  fgk    sJ d	S )
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     s7   S		AS		AS		AS		AG$$FB*%%KAq!R%  DAq MM!AC%MM!B$qs(!M,,,MM!BGGII+ac/M333MM!&(13,MM!&(13, NN2b59AN...NN1QS57q1R4x!BGGII+'=qs&BCDDDDD7afQhqsl3agBrE	135OPPPPP7afQhqsl3ags2vacz5JKKKKK7q{QrT'913&>?@@@@@@@r&   c            
         t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    d|| j        z  || j        z  z             }	t          d|| ||          }
|
                    |	          }t          | |||dz  z  z   |||dz  z  z   |||dz  |dz  z   z  z   | |z  |z            }||k    sJ t          d	          }|	                    | | j
        d
           t          |
                    |	|          |z
                      |                    t          dd          k    sJ t          d          }|                    d|| j        z  || j        z  z             }	t          d|||           }|                    |	|           }t          | ||dz  z  ||dz  z  ||dz  |dz  z   z  | |z  |z            }|j        rJ ||k    sJ d S )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   &  sB   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++ASQaaDDDA	
		B!R!ad(]BQTMqAqD1a4K00qb1fqjB B BKsAMM!QS!	
A			,77::< <?DQ{{K K K K 	c

A	CQS1qs7*++ASQQa00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 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 N N N N N N N N N N N N 1 1 1 1 1 1 1 1 ' ' ' ' ' '- - - * * *2!& !& !&H5 5 509 9 9@4 4 4& & &" " "` ` `  	# 	# 	#C C CB4 4 4   ! ! ! A A A6    r&   