
    Rie+                        d dl mZ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mZmZmZmZmZmZmZmZmZ d dlmZmZmZ d dlmZ  ed          \  ZZ Z!Z"Z# ed          Z$e$%                    dd	ee$j&        g          Z'e'%                    d
d	e e'j(        g          Z)e)%                    dd	e!e)j*        g          Z+d Z,d Z-d Z.d Z/d Z0d Z1d Z2d Z3d Z4d Z5d Z6d Z7dS )    )sincostanpisymbolsMatrixSFunction)ParticlePointReferenceFrame	RigidBody)angular_momentumdynamicsymbolsinertiainertia_of_point_masskinetic_energylinear_momentumouterpotential_energymsubsfind_dynamicsymbols
Lagrangian)gravitycenter_of_mass_validate_coordinates)raiseszq1 q2 q3 q4 q5NAAxisBCc            	         t          d          } t          d          \  }}}t          d          \  }}}t          | |||          || j        | j        z  z  || j        | j        z  z  z   || j        | j        z  z  z   k    sJ t          | ddd          d| j        | j        z  z  k    sJ t          t          d            t          | ||||||          || j        | j        z  z  || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   || j        | j        z  z  z   k    sJ d S )Nr   zixx iyy izzzixy iyz izxr   c                  &    t          dddd          S )Nr   )r        Llib/python3.11/site-packages/sympy/physics/mechanics/tests/test_functions.py<lambda>ztest_inertia.<locals>.<lambda>   s    gaAq11 r&   )r   r   r   xyzr   	TypeError)r   ixxiyyizzixyiyzizxs          r'   test_inertiar3      s   sAM**MCcM**MCc1c3$$ac	):SS13Y> *qs+*, - - - -1aA!qsQSy/1111
9112221c3S#s33qsQSy8I1399 #qsQSy 192471394E9FHK	
qsI9QS13Y'9(*-qs*;9<>AQSCF ?9 !#)$9% & & & & & &r&   c                  Z   t          d          \  } }}}t          d          }| |j        z  }t          |||          }||| dz  z  |j        |j        z  z  || dz  z  |j        |j        z  z  z   k    sJ ||j        z  }t          |||          }|||dz  z  |j        |j        z  z  ||dz  z  |j        |j        z  z  z   k    sJ ||j        z  }t          |||          }|||dz  z  |j        |j        z  z  ||dz  z  |j        |j        z  z  z   k    sJ ||z   |z   }	t          ||	|          }|||dz  |dz  z   z  |j        |j        z  z  || z  |z  |j        |j        z  z  z
  || z  |z  |j        |j        z  z  z
  || z  |z  |j        |j        z  z  z
  || dz  |dz  z   z  |j        |j        z  z  z   ||z  |z  |j        |j        z  z  z
  || z  |z  |j        |j        z  z  z
  ||z  |z  |j        |j        z  z  z
  || dz  |dz  z   z  |j        |j        z  z  z   k    sJ d S )Nzr s t mr      )r   r   r)   r   r*   r+   )
rstmr   pxIpypzps
             r'   test_inertia_of_point_massr?   $   s   ##JAq!QsA	
QSBaQ''AAqDAC!#I&QTQS13Y)??????	
QSBaQ''AAqDAC!#I&QTQS13Y)??????	
QSBaQ''AAqDAC!#I&QTQS13Y)??????
R"AaA&&AadQTk"acACi0QacACi()QacACi() QacACi() adQTk"acACi0	1
 QacACi() QacACi() QacACi() adQTk"acACi01 2 2 2 2 2 2r&   c                      t          d          t          d          } |                     dj        z             t	          j        j                  }t          d| d|| f          t          d          }t          d|d          j                            d	j        z             t          t          fd
           t          t          fd           t                    d	j        z  dj        z  z   k    sJ d S )Nr   Ac   r      PPa   
   c                  &    t                      S Nr   r   rE   s   r'   r(   z&test_linear_momentum.<locals>.<lambda>J       oaB77 r&   c                  &    t                      S rI   rJ   r   rE   s   r'   r(   z&test_linear_momentum.<locals>.<lambda>K   rL   r&   i  )r   r   set_velr*   r   r)   r   r   pointr   r,   r   )rA   r;   rD   r   r   rE   s      @@@r'   test_linear_momentumrQ   A   s	   sA	tBJJq"qs(ac13A#r1b1b'**Ac

A	$1		BHQQS!!!
977777888
9777778881a$$QS39(<<<<<<<r&   c                    	
 t          d          \  } }}}t          d          }t          d          
t          d          }t          d                              d|
j        z            }|                    d|
j        z            }                    
d
j        z             |                    
|
j        z             |	                    
|           |	                    
|           t          d	||           t          d
||||t          
j        
j                  z  |f          	d| z  |z  |z  
j        z  ||z  |z  
j        z  z   }t          
	          |k    sJ t          t           	
fd           t          t           	fd           t          t           
fd           |||dz  z  z   d| z  |dz  z  z   |z  
j        z  }t#          
	          |k    sJ dS )a   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.z
m, M, l, Iomegar   aOrA   rD   r   rE   r   r5   c                  (    t                     S rI   r   )r   r   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>c       .q!Q;; r&   c                  (    t                     S rI   rW   )r   rU   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>d   rX   r&   c                  (    t                     S rI   rW   )r   rU   rE   s   r'   r(   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>e   rX   r&      N)r   r   r   r   	locatenewr)   rO   set_ang_velr+   v2pt_theoryr   r   r   r*   r   r   r,   r   )r9   Mlr;   rS   rT   rA   rD   expectedr   r   rU   rE   s            @@@@r'   )test_angular_momentum_and_linear_momentumrb   O   s   
 &&JAq!Q7##EsAsAc

A	
T1qs7	#	#B
S!ac'""AIIaQSMM!UQS[!!!NN1aMM!Q	$1		B#r1a!eACoo"5r!:;;A1uu}q 13&Q)<<H1a$$0000
9;;;;;;<<<
9;;;;;;<<<
9;;;;;;<<<A1Hq1uq!t|+u4qs:HAq!R((H444444r&   c                    	
 t          d          \  } }}t          d          }t          d          
t          d          }|                    
d
j        z             |                    d|
j        z            }|                    d|
j        z            }t          d          }|                    
|
j        z             |	                    |
|           |	                    |
|           t          d	||           t          
j        
j                  }t          d
|||||f          	t          t          	fd           t          t          	
fd           dt          
	          ||dz  z  |dz  z  dz  d|dz  z  | z  |dz  z  z   |dz  dz  z   z
                                  k    sJ d S )Nzm M l1rS   r   rU   r   rA   rD   rT   rE   r   c                  &    t                     S rI   r   rK   s   r'   r(   z%test_kinetic_energy.<locals>.<lambda>y   s    nRQ77 r&   c                  &    t                     S rI   re   )r   r   s   r'   r(   z%test_kinetic_energy.<locals>.<lambda>z   s    nQ155 r&   r5   )r   r   r   r   rO   r)   r\   r]   r+   r^   r   r   r   r   r,   r   expand)r9   r_   l1rS   rU   rA   rD   rT   r;   r   r   rE   s            @@@r'   test_kinetic_energyri   j   s   x  HAq"7##EsAc

AIIaQS	
T28	$	$B
S"qs(##AsAMM!UQS[!!!NN1aMM!Q	$1		Bac13A#r1a!R))A
977777888
9555556662q))Qr1uWUAX-=a-?Agaiq .!#(!8A:.. /069 9 9 9 9 9r&   c                     t          d          \  } }}}}}t          d          }t          d          }t          d          }|                    |d|j        z             |                    d||j        z            }	|	                    d||j        z            }
t          d          }|                    |||j        z             |		                    |||           |
	                    |||           t          d	|
|           }t          |j        |j                  }t          d
|	||||	f          }| |z  |z  |_        ||z  |z  |_        t          ||          | |z  |z  ||z  |z  z   k    sJ d S )Nzm M l1 g h HrS   r   rU   r   rA   rD   rT   rE   r   )r   r   r   r   rO   r)   r\   r]   r+   r^   r   r   r   r   )r9   r_   rh   ghHrS   r   rU   rA   rD   rT   rE   r;   r   s                  r'   test_potential_energyrn      sq   //Aq"aA7##EsAc

AIIaQS	
T28	$	$B
S"qs(##AsAMM!UQS[!!!NN1aMM!Q	$1		Bac13A#r1a!R))Aa%!)BQAAr""a!eai!a%!)&;;;;;;;r&   c                    	
 t          d          \  } }}}t          d          
t          d          }|                    
d
j        z             |                    dd
j        z            }|                    
d
j        z             t          d|d          |                    d	d

j        z            }|                    
d
j        z             t          d          }|                    
d
j	        z             t          
j	        
j	                  }t          d||d||f          	||z  |z  _        | |z  |z  	_        t          t          	fd           t          t          
fd           d S )NzM m g hr   rU   r   rD   rF   rG   rE   rA   r5      rT   r   rC   c                  &    t                      S rI   r   rK   s   r'   r(   z!test_Lagrangian.<locals>.<lambda>       jAr22 r&   c                  &    t                      S rI   rr   rN   s   r'   r(   z!test_Lagrangian.<locals>.<lambda>   rs   r&   )r   r   r   rO   r)   r\   r   r*   r]   r+   r   r   r   r   r,   )r_   r9   rk   rl   rU   rD   rA   rT   r;   r   r   rE   s            @@@r'   test_Lagrangianru      st   ##JAq!QsAc

AIIaQS	CQS!!AIIaac	$1		B	
T1qs7	#	#BJJq!ac'sAMM!R!#Xac13A#r1b1b'**Aa%!)BQA
922222333
92222233333r&   c            	      d   t          d          \  } }t          d          \  }}}t          | |z  |z   ||                                z  |z   g|                                                                |t	          |                                          z   gg          }t          | |z   |g|                                                                dgg          }|d|d|                                d|                                di}t          ||          |k    sJ t          ||z             t          ||z             z  ||                                z  z   }|d|t          dz  |                                di}t          ||d          |dz   k    sJ t          d          }||j
        z  ||j        z  z   }	||j
        |j
        z  z  ||j        |j        z  z  z   }
d|j        z  }d|j        |j        z  z  }|d|di}t          |	|          |k    sJ t          |
|          |k    sJ d S )	Na, bx, y, zrF   r   r5   T)smartr   )r   r   r   diffr   r   r   r   r   r   r)   r*   )rT   br)   r*   r+   exprsolsdr   vdv_sold_sols                r'   
test_msubsr      s   6??DAqY''GAq!AaC!GQqvvxxZ!^,FFHHMMOOQQVVXX%679 : :D
1q5!*6688==??A&( ) )C
Q1affhh16688Q	/Br??c!!!!q1u::c!a%jj 1QVVXX:-D
Q2a41	%Br&&&!a%////sA	!#!#A	13qs7aQSk!AacEEqs13wKE
Q1BB<<5    B<<5      r&   c            	         t          d          \  } }t          d          \  }}}t          | |z  |z   ||                                z  |z   g|                                                                |t	          |                                          z   gg          }||                                ||                                                                ||                                h}t          |          |k    sJ |||g}|                                |                                                                |                                h}t          ||          |k    sJ t          d          \  }}	}
t          d          }||j        z  |	|j        z  z   |
|j	        z  z   ||	|
h}t          |          |k    sJ t          t          fd           d S )Nrw   rx   )excludezd, e, fr   )reference_framec                  "    t                     S rI   )r   )r   s   r'   r(   z*test_find_dynamicsymbols.<locals>.<lambda>   s    2155 r&   )r   r   r   rz   r   r   r   r)   r*   r+   r   
ValueError)rT   r{   r)   r*   r+   r|   r}   exclude_listr   efr   r   s               @r'   test_find_dynamicsymbolsr      s   6??DAqY''GAq!AaC!GQqvvxxZ!^,FFHHMMOOQQVVXX%679 : :D affhh16688==??Aqvvxx
8Ct$$++++q!9L6688QVVXX]]__affhh
/Ct\:::cAAAAY''GAq!sA	AC!ac'AG#Aa)Cq!444;;;;
:555566666r&   c                     t          d          } t          d          \  }}}t          d          \  }}t          d          }t	          d||          }t          d          }t          d          }	t          |j        |j                  }
t          d|	|||
|	f          }||f|	|fg}|                    t          || j
        z  ||                     ||f|	|f|||z  | j
        z  f|	||z  | j
        z  fg}t          t          |                    D ]G}t          t          ||                             D ]"}||         |         ||         |         k    sJ #Hd S )	Nr   zm M gzF1 F2popar   rD   r!   )r   r   r   r   r   r   r)   r   extendr   r*   rangelen)r   r9   r_   rk   F1F2r   r   r   rD   r;   r!   	forceListr`   ijs                   r'   test_gravityr      sj   sAgGAq!G$$FB	tB	$A		BsAc

Aac13A#q!QA''AbAr7#IWQqsUB**+++
bAr7R1QSMAqs13w<8A3q66]] . .s1Q4yy!! 	. 	.AQ<?ad1g-----	.. .r&   c                     t          d          } t          dd          }t          dt          d          t          j                  }t          dt          d          t	          d	                    }t          d
t          d          t	          d                    }t          dt          d          |          }t          d          }t          d          }t          d          }t          d|||t          |j        |j                  |f          }	|j	        
                    |j	        | j                   |j	        
                    |j	        | j        | j        z              |j	        
                    |j	        | j                   |	j        
                    |j	        | j        | j        z              t          d          }
|

                    |j	        t          |j	        |||||	                     d||z   dz   z  | j        z  ||z   dz   ||z   dz   z  | j        z  z   |||z   dz   z  | j        z  z   }|
                    |j	                  |z
  dk    sJ d S )NrT   r9   T)realp1p1_ptp2p2_ptr5   p3p3_pt   p4p4_ptb_fb_cmmbr{   orp      r   )r   r   r   r   r	   Oner   r   r)   rP   set_posr*   
masscenterr+   r   pos_from)rT   r9   r   r   r   r   r   r   r   r{   point_or|   s               r'   test_center_of_massr      s   sA$A	$g	.	.B	$g!	-	-B	$g!	-	-B	$g	*	*B


C==D	B#tS"uSUCE':':D&ABBAHRXqs###HRXqsQSy)))HRXqs###L139---#JJGOOBHnRXr2r2qIIJJJa"fqj>!#R!a"fqj 9!# ==AFQJPQPS@SSDBH%%d*a//////r&   c                     	
 t          d          \  
t          d          \  t          g
g           t          g           t          g
g           t          
g           t          g
gd           t          t          
fd           t          g
gd           t          t          
fd           t          t          fd           t          z   gd	           t          t          fd
           t          gd
gd	           t          t          
fd           t          z   z   gd
gd	           t          t          
fd           t           j        	t          d           t          dt                    \  t                                  gd	           t          t           fd           t          t           fd            t           _        t                                  g           t          t          	fd           	t           _        d S )Nz	q1:4 u1:4zs1:4speedsFcheck_duplicatesc                  0    t           gg          S rI   r   )q1q2u1u2u3s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>
  s#    4	Rr2rl $  $ r&   c                  4    t           ggd          S NTr   r   )r   r   q3r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  *    4	Rr2rlT ;  ;  ; r&   c                  4    t           g gd          S r   r   )r   r   r   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  r   r&   is_dynamicsymbolsc                  ,    t           z   g          S rI   r   )r   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s    4b2gr]CC r&   r   c                  4    t           gdgd          S Nr   Tr   r   )r   r   s1r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s*    4	Rq"bkT ;  ;  ; r&   c                  <    t          z   z    gdgd          S r   r   )r   r   s2s3r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  s/    4	b2rQGt =  =  = r&   rT   zf1:3)clsc                  J    t                                  g          S rI   r   rT   f1f2s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>  $    4bbeeRRUU^DD r&   c                  L    t                                  g          S )Nr   r   r   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>   s'    4RRUUBBqEENKKK r&   c                  J    t                                 g          S rI   r   )r   r   r8   s   r'   r(   z+test_validate_coordinates.<locals>.<lambda>#  r   r&   )r   r   r   r   r   _tr
   )rT   r   r   r   r   r   r   r   r   r8   r   r   r   s   @@@@@@@@@@@@@r'   test_validate_coordinatesr      so   +K88BBBJBB2r2,R5552r(###2r(RD)))"b****2r2,RuMMMM
: $ $ $ $ $ $ $ $ % % %2r2,RuMMMM
: ; ; ; ; ; ; ; ; < < <
: ; ; ; ; ; ; ; ; < < < 27B-5AAAA
:CCCCCCDDD2r2,BuMMMM
: ; ; ; ; ; ; ; ; < < <27R<,q"gOOOO
: = = = = = = = = > > > 	AAV***FB22a55""Q%%.EBBBB
:DDDDDDEEE
:KKKKKKLLLN22a55""Q%%.)))
:DDDDDDEEENr&   N)8sympy.core.backendr   r   r   r   r   r   r	   r
   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   r   r   r   r   !sympy.physics.mechanics.functionsr   r   r   sympy.testing.pytestr   r   r   r   q4q5r   	orientnewr+   r   r)   r!   r*   r"   r3   r?   rQ   rb   ri   rn   ru   r   r   r   r   r   r%   r&   r'   <module>r      s   N N N N N N N N N N N N N N N N N N N N0 0 0 0 0 0 0 0 0 0 0 0F F F F F F F F F F F F F F F F F F F F F F F F F FF F F F F F F F F F ' ' ' ' ' ' W-.. BBN3KKVb!#Y''KKVb!#Y''KKVb!#Y''& & &2 2 2:= = =5 5 569 9 9*< < <(4 4 4(! ! !07 7 7,. . .*0 0 0*% % % % %r&   