
    g#/                         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#dS )    )SymbolsymbolssincosMatrixzerossimplify)PointReferenceFramedynamicsymbolsDyadic)inertiaBody)raiseswarns_deprecated_sympyc            
      H   t                      5  t          d          } d d d            n# 1 swxY w Y   | 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   r   nameloadsr
   set_velframe
masscentervelmassr   r   r   )
r   pointcomr   ixxiyyizzixyiyzizxs
             c/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_body.pytest_defaultr&      su   		!	!  F||              9:#$$E	MM$*a   
/CJE775>>UYYu------9{++++++899MCc899MCc<GDJS#sCMM O- - - - - - -s   +//c                     t          d          } t          d          }t          d          }t          |ddd          }t	                      5  t          d| |||          }d d d            n# 1 swxY w Y   |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   r   pos_fromr   is_rigidbodyhasattr)r(   r)   r*   body_inertia
rigid_bodyr   r   s          r%   test_custom_rigid_bodyr2      s    !788,--N$%677O?Aq!44L		!	! I I*,@(/<I I
I I I I I I I I I I I I I I I 
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)))))))s   A..A25A2c                     t          d          } t          d          }t          d          }t                      5  t	          d| ||          }d d d            n# 1 swxY w Y   |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                      5  t	          d|          }d d d            n# 1 swxY w Y   |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   r   central_inertiar.   	locatenewx)r4   r5   r6   r7   r   r   r   point_inertias           r%   test_particle_bodyr=   3   s?    566?++M#$455N		!	! < <_.A*N< << < < < < < < < < < < < < < < 
"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%%%%		!	! B B_=AAAB B B B B B B B B B B B B B B))))$..wHHE!GM,?Aq$I$IIM*E2M ]E$:::::(F1II5555%%%%%%s#   AA"A
G((G,/G,c                     t          d          } t          d          }t          d          }t                      5  t	          d| ||          }d d d            n# 1 swxY w Y   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   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6?++M#$455N		!	! < <_.A*N< << < < < < < < < < < < < < < < 	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4s   AA"Ac                  J  	
 t          d          } t          d          }t          d          }t          |ddd          }t	                      5  t          d| |||          
d d d            n# 1 swxY w Y   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     c                  .                          d          S rM   rN   )r1   s   r%   rO   z%test_body_add_force.<locals>.<lambda>   s    j44Q7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8,--N$%677O?Aq!44L		!	! I I*,@(/<I I
I I I I I I I I I I I I I I I 	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8s   A00A47A4c                  L   t                      5  t          d          d d d            n# 1 swxY w Y   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 rP   )	r   r   r   r;   rW   rB   r   r   rS   )torque_vectorr   s    @r%   test_body_add_torquerY      s    		!	!  F||              JLMm$$$tz??a:a=TZ77777
9222233333s   ,00c                     t                      5  t          d          } d d d            n# 1 swxY w Y   t          d          }t                      5  t          d|          }d d d            n# 1 swxY w Y   | j                            ||j                   |                     |          |j        k    sJ |                     |          |j        k    sJ d S NANBr   )r   r   r   r   r   rR   masscenter_velr\   r]   r^   s      r%   test_body_masscenter_velrb      sP   		!	!  II              sA		!	!  A              LAC   A!#%%%%A!#%%%%%%   +//A11A58A5c                     t                      5  t          d          } d d d            n# 1 swxY w Y   t          d          }t                      5  t          d|          }d d d            n# 1 swxY w Y   | j                            ||j                   |                     |          |j        k    sJ |                    |           |j         k    sJ |                     |          |j        k    sJ d S r[   )r   r   r   r   set_ang_vely
ang_vel_inra   s      r%   test_body_ang_velrh      sg   		!	!  II              sA		!	!  A              G13<<??ac!!!!<<??qsd""""<<??ac!!!!!!rc   c                     t                      5  t          d          } t          d          }d d d            n# 1 swxY w Y   | 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   r   orient_axisrR   dcmr   r   r   )r\   r^   s     r%   test_body_dcmrm      sS   		!	!  IIII               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]$^________s   :>>c                      t          d          } t                      5  t          d|           }d d d            n# 1 swxY w Y   |j        | j        k    sJ |j        | j        k    sJ |j        | j        k    sJ d S )Nr]   r^   r_   )r   r   r   r;   rf   rR   )r]   r^   s     r%   test_body_axisro      s    sA		!	!  A              3!#::::3!#::::3!#::::::s   <A A c                  x   t          d          \  } }t          d          }t                      5  t          d          }d d d            n# 1 swxY w Y   | |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   r;   rf   rA   r   )r?   brq   r^   f1f2s         r%   #test_apply_force_multiple_one_pointru      s   5>>DAqc

A		!	!  II              	
13B	
13BMM"a72wiMM"a72b5zl""""""s   AAAc                     t          d          \  } }t          d          \  }}}}t          d          }t          d          }t                      5  t	          d          }t	          d          }	d d d            n# 1 swxY w Y   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   r;   rR   rA   r   r   rf   r   )fgqr;   v1v2rw   rx   ry   rz   r]   forceg1g2force2s                  r%   test_apply_forcer      s\   5>>DAq!+..LAq"b	tB	tB		!	!  $ZZ$ZZ               	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Bs   A>>BBc                     t          d          } t          d          }t                      5  t          d          }t          d          }d d d            n# 1 swxY w Y   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   ry   rz   r]   )
r   r   r   r   r   r;   rW   r   r   rf   )r   r   ry   rz   r]   torquetorque2s          r%   test_apply_torquer      sC   AsA		!	!  $ZZ$ZZ               	sAqSWFOOFB86*+++++8F7+,,,,,eGOOG86'>23333333s   AAAc                  N   t          d          } t          d          }t                      5  t          d          }d d d            n# 1 swxY w Y   | |j        z  }|                    ||           |j        ||fgk    sJ |                                 |j        g k    sJ d S )Nr?   rq   r^   )r   r
   r   r   rR   rA   r   clear_loads)r?   rq   r^   r   s       r%   test_clear_loadr     s    Ac

A		!	!  II              acEEMM%75zl""""MMOOO7b======   A		AAc                  ~   t          d          } t          d          }t                      5  t          d          }d d d            n# 1 swxY w Y   |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 )Nrw   rx   r^   )r
   r   r   r;   rf   rA   r   remove_loadrW   crossr   )rw   rx   r^   rs   rt   s        r%   test_remove_loadr     sz   	tB	tB		!	!  II              	
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                      5  t          d          } t          d          }t          d          }t          d          }ddd           n# 1 swxY w Y   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^   rq   rr   Nzq1 q2zk c g kTzF Tr{   )r   r   r   r   rA   r;   diffr   rf   rW   rR   r   r   r   )r   r^   rq   rr   q1q2kcr~   kTFTs               r%   9test_apply_loads_on_multi_degree_freedom_holonomic_systemr     s   		!	!  IIIIIIII	              
 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&>?@@@@@@@s   =AAAc            
         t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    d|| j        z  || j        z  z             }	t                      5  t          d|| ||          }
d d d            n# 1 swxY w Y   |
	                    |	          }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                      5  t          d|||           }d d d            n# 1 swxY w Y   |	                    |	|           }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+      rq   )r   r   r   )r   r   r   r
   r:   r;   rf   r   r   parallel_axisrk   rR   r	   	to_matrixr   r.   )r]   mIxIyIzr?   rr   Ior   r   r   IpIp_expectedr\   rq   s                  r%   test_parallel_axisr   9  s   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++A		!	! I I!!RHHHI I I I I I I I I I I I I I I	
		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		!	! 5 5!4445 5 5 5 5 5 5 5 5 5 5 5 5 5 5	
A		B!QaZQ!VQ!q&16/5J b1fqj* * *K~s$   >BB#&B#G!!G%(G%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   rh   rm   ro   ru   r   r   r   r   r   r    rP   r%   <module>r      s  * * * * * * * * * * * * * * * * * * N N N N N N N N N N N N 1 1 1 1 1 1 1 1 ? ? ? ? ? ? ? ?- - -"* * *4#& #& #&L5 5 529 9 9B4 4 4& & &	" 	" 	"` ` `  
# 
# 
# C  C  CD4 4 4"	 	 	! ! !"A A A8    rP   