
    g(                        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 d dlmZmZmZ d dlmZ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 Z/d Z0d Z1d Z2d Z3d Z4d Z5dS )    )sincostanpisymbolsMatrixSFunction)ParticlePointReferenceFrame	RigidBody)	angular_momentumdynamicsymbolskinetic_energylinear_momentumouterpotential_energymsubsfind_dynamicsymbols
Lagrangian)center_of_mass_validate_coordinates_parse_linear_solver)raiseswarns_deprecated_sympyzq1 q2 q3 q4 q5NAAxisBCc                      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   r'   s   h/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_functions.py<lambda>z&test_linear_momentum.<locals>.<lambda>       oaB77     c                  &    t                      S r+   r,   r   r'   s   r.   r/   z&test_linear_momentum.<locals>.<lambda>   r0   r1   i  )r   r   set_velyr   xr   r   pointr   	TypeErrorr   )r#   Ir&   r   r   r'   s      @@@r.   test_linear_momentumr:      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(<<<<<<<r1   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Or#   r&   r   r'   r      c                  (    t                     S r+   r   )r   r   r'   s   r.   r/   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>7       .q!Q;; r1   c                  (    t                     S r+   rA   )r   r>   r'   s   r.   r/   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>8   rB   r1   c                  (    t                     S r+   rA   )r   r>   r'   s   r.   r/   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>9   rB   r1      N)r   r   r   r   	locatenewr6   r4   set_ang_velzv2pt_theoryr   r   r   r5   r   r   r8   r   )mMlr9   r<   r=   r#   r&   expectedr   r   r>   r'   s            @@@@r.   )test_angular_momentum_and_linear_momentumrN   #   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4r1   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 l1r<   r   r>   r   r#   r&   r=   r'   r   c                  &    t                     S r+   r   r-   s   r.   r/   z%test_kinetic_energy.<locals>.<lambda>M   s    nRQ77 r1   c                  &    t                     S r+   rQ   )r   r   s   r.   r/   z%test_kinetic_energy.<locals>.<lambda>N   s    nQ155 r1   r?   )r   r   r   r   r4   r6   rF   rG   rH   rI   r   r   r   r   r8   r   expand)rJ   rK   l1r<   r>   r#   r&   r=   r9   r   r   r'   s            @@@r.   test_kinetic_energyrU   >   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 9r1   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 Hr<   r   r>   r   r#   r&   r=   r'   r   )r   r   r   r   r4   r6   rF   rG   rH   rI   r   r   r   r   )rJ   rK   rT   ghHr<   r   r>   r#   r&   r=   r'   r9   r   s                  r.   test_potential_energyrZ   S   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%!)&;;;;;;;r1   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   r>   r   r&   r(   r)   r'   r#   r?      r=   r   r%   c                  &    t                      S r+   r   r-   s   r.   r/   z!test_Lagrangian.<locals>.<lambda>w       jAr22 r1   c                  &    t                      S r+   r^   r3   s   r.   r/   z!test_Lagrangian.<locals>.<lambda>x   r_   r1   )r   r   r   r4   r6   rF   r   r5   rG   rH   r   r   r   r   r8   )rK   rJ   rW   rX   r>   r&   r#   r=   r9   r   r   r'   s            @@@r.   test_Lagrangianra   g   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3r1   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, zr(   r   r?   T)smartr   )r   r   r   diffr   r   r   r   r   r   r6   r5   )r=   br6   r5   rH   exprsolsdr   vdv_sold_sols                r.   
test_msubsro   {   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      r1   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 )Nrc   rd   )excludezd, e, fr   )reference_framec                  "    t                     S r+   )r   )rk   s   r.   r/   z*test_find_dynamicsymbols.<locals>.<lambda>   s    2155 r1   )r   r   r   rf   r   r   r   r6   r5   rH   r   
ValueError)r=   rg   r6   r5   rH   rh   ri   exclude_listrl   efr   rk   s               @r.   test_find_dynamicsymbolsrx      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6r1   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 )Nr=   rJ   T)realp1p1_ptp2p2_ptr?   p3p3_pt   p4p4_ptb_fb_cmmbrg   or\      r   )r   r   r   r   r	   Oner   r   r6   r7   set_posr5   
masscenterrH   r   pos_from)r=   rJ   r{   r}   r   r   r   r   r   rg   point_orh   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//////r1   c                  n   	
 t          d          \	  
t          d          \  t          g
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          g
gd
g
           t          t          
f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          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:4 ua1:4zs1:4u_auxiliaryspeedsFcheck_duplicatesc                  0    t           gg          S r+   r   )q1q2u1u2u3s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   s#    4	Rr2rl $  $ r1   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 ;  ;  ; r1   c                  4    t           g gd          S r   r   )r   r   r   r   r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   r   r1   )r   r   c                  :    t           ggg          S Nr   r   r   r   r   r   r   r   ua2ua3s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   3    4	Rr2rlS# @  @  @ r1   c                  :    t           gg g          S r   r   r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   r   r1   c                  :    t           ggg          S r   r   )r   r   r   r   r   r   ua1r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   s3    4	Rr2rlc3 A  A  A r1   is_dynamicsymbolsc                  ,    t           z   g          S r+   r   )r   r   r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   s    4b2gr]CC r1   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 ;  ;  ; r1   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 =  =  = r1   )r   r   c                  (    t           g          S r   r   )r   r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   s    4"cKKK r1   r=   zf1:3)clsc                  J    t                                  g          S r+   r   r=   f1f2s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   $    4bbeeRRUU^DD r1   c                  L    t                                  g          S )Nr   r   r   s   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   s'    4RRUUBBqEENKKK r1   c                  J    t                                 g          S r+   r   )r   r   ts   r.   r/   z+test_validate_coordinates.<locals>.<lambda>   r   r1   )r   r   r   r   rt   _tr
   )r=   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   s   @@@@@@@@@@@@@@@@r.   test_validate_coordinatesr      s   ,:;L,M,M)BBBCcJBB2r2,R'*Co7 7 7 7 2r(###2r(RD)))"b****2r2,RuMMMM
: $ $ $ $ $ $ $ $ % % %2r2,RuMMMM
: ; ; ; ; ; ; ; ; < < <
: ; ; ; ; ; ; ; ; < < <2r2,Ru')3n6 6 6 6
: @ @ @ @ @ @ @ @ @ @ @ A A A
: @ @ @ @ @ @ @ @ @ @ @ A A A
: A A A A A A A A A A A B B B 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
: = = = = = = = = > > >r3i5IIII
:KKKKKLLL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r1   c                  4   t          ddt          d                    t          ddt          d                    }} t          t           j                  t           j        k    sJ  t          d          | |          t          j        | |          k    sJ d S )Nr   za:9r?   zb:6LU)r   r   r   LUsolve)r   rg   s     r.   test_parse_linear_solverr      s    !Q''1genn)E)EqA//6>AAAA%%%a++v~a/C/CCCCCCCr1   c                  
   ddl m} m}m} t	          d          }t                      5   | |dddd          |j        |j        z  |j        |j        z  z   |j        |j        z  z   k    sJ 	 d d d            n# 1 swxY w Y   t                      5   |d|j        |j        z   |          |j        |j        z  |j        |j        z  z   d|j        |j        z  z  z   |j        |j        z  z
  |j        |j        z  z
  k    sJ 	 d d d            n# 1 swxY w Y   t          d          }t                      5   |d|j        z  |          |j
        d|j        z  |j        z  fgk    sJ 	 d d d            d S # 1 swxY w Y   d S )Nr   )inertiainertia_of_point_massgravityr   r(   r?   r&   )!sympy.physics.mechanics.functionsr   r   r   r   r   r6   r5   rH   r   r   mass)r   r   r   r   ps        r.   test_deprecated_moved_functionsr      s   1 1 1 1 1 1 1 1 1 1sA		!	! Q Qwq!Q1%%!#)ac	)BacACi)PPPPPPQ Q Q Q Q Q Q Q Q Q Q Q Q Q Q		!	! ' '$$Qac	155S13Y139%QS13Y7S13Y39&' ' ' ' '' ' ' ' ' ' ' ' ' ' ' ' ' ' ' 	A		!	! K KwrACx##rAF{QS7H(I'JJJJJJK K K K K K K K K K K K K K K K K Ks7   AA88A<?A<A1DDD73E88E<?E<N)6sympyr   r   r   r   r   r   r	   r
   sympy.physics.mechanicsr   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   sympy.testing.pytestr   r   r   r   r   q4q5r   	orientnewrH   r   r6   r    r5   r!   r:   rN   rU   rZ   ra   ro   rx   r   r   r   r    r1   r.   <module>r      s   A A A A A A A A A A A A A A A A A A A A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
A A A A A A A A A A ? ? ? ? ? ? ? ? W-.. BBN3KKVb!#Y''KKVb!#Y''KKVb!#Y''= = =5 5 569 9 9*< < <(4 4 4(! ! !07 7 720 0 0*0 0 0fD D DK K K K Kr1   