
    g,                         d dl mZmZmZmZ d dl mZmZmZmZ d dl m	Z	 d dl
mZmZ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S )    )PointReferenceFrameDyadic	RigidBody)dynamicsymbolsouterinertiaInertia)inertia_of_point_mass)expandzerossimplifysymbols)raiseswarns_deprecated_sympyc                     t          d          } t          | j        gt          d          R  }| j        dk    sJ | j        t          d          k    sJ | j        j        dk    sJ | j        || j        fk    sJ | j        |k    sJ | j        j        dk    sJ |                                 dk    sJ | 	                                dk    sJ d S )NBz#B_ixx B_iyy B_izz B_ixy B_iyz B_izxB_massB_masscenterB_framea|  RigidBody('B', masscenter=B_masscenter, frame=B_frame, mass=B_mass, inertia=Inertia(dyadic=B_ixx*(B_frame.x|B_frame.x) + B_ixy*(B_frame.x|B_frame.y) + B_izx*(B_frame.x|B_frame.z) + B_ixy*(B_frame.y|B_frame.x) + B_iyy*(B_frame.y|B_frame.y) + B_iyz*(B_frame.y|B_frame.z) + B_izx*(B_frame.z|B_frame.x) + B_iyz*(B_frame.z|B_frame.y) + B_izz*(B_frame.z|B_frame.z), point=B_masscenter)))
r   r	   framer   namemass
masscentercentral_inertia__str____repr__)bIs     h/var/www/html/ai-engine/env/lib/python3.11/site-packages/sympy/physics/mechanics/tests/test_rigidbody.pytest_rigidbody_defaultr!      s    #AI'"GHHIIIA6S====6WX&&&&&&<....9AL)))))!!!!7<9$$$$99;;#::<<	               c                    
 t          d          \  } }}}}t          d          
t          d          }t          d          t          d          }t          d          t          d          }t	          d
f          }|j        k    sJ |j        
k    sJ |j        k    sJ |j        |j        fk    sJ | |_        ||_        ||_        ||j        f|_        t          t          
fd           t          t          fd	           t          t          
fd
           t          t          
fd           |                                dk    sJ |j        | k    sJ |j        |k    sJ |j        |k    sJ |j        ||j        fk    sJ t          |j        t                    sJ t          d          }	|                    |	||	j        z  ||	j        z  z   ||	j        z  z              |                    |	          | ||	j        z  ||	j        z  z   ||	j        z  z   z  k    sJ d S )Nzm m2 v1 v2 v3 omegaAA2PP2r   r   c                  .    t           f          S )Nr   r$   r   r&   ms   r    <lambda>z test_rigidbody.<locals>.<lambda>/   s    i1aQF;; r"   c                  .    t          d f          S Nr   r)   )r   r&   r+   s   r    r,   z test_rigidbody.<locals>.<lambda>0       iQ1q!f== r"   c                  .    t          d f          S r.   r)   )r$   r&   r+   s   r    r,   z test_rigidbody.<locals>.<lambda>1   r/   r"   c                  .    t          d f          S r.   r)   r*   s   r    r,   z test_rigidbody.<locals>.<lambda>2   r/   r"   N)r   r   r   r   r   r   r   r   r	   r   	TypeErrorr   
isinstancer
   set_velxyzlinear_momentum)m2v1v2v3omegar%   r'   I2r   r2   r$   r   r&   r+   s             @@@@r    test_rigidbodyr@      su   &'<==Ar2r2usA			Bc

A	tBq		A	B#q!QA''A6Q;;;;7a<<<<<19AL)))))AFAGALQ\"AI
9;;;;;;;<<<
9======>>>
9======>>>
9=======>>>99;;#6R<<<<7b====<29Q\*****ai))))) 	sAJJq"qs(R!#X%QS0111Q2acBH)<rACx)G#HHHHHHHr"   c                  \   t          d          \  } }}}}}t          d          }t          d          }|                    |||j        z             t	          d          }t          |j        |j                  }	|	|f}
t          d||| |
          }|                    |||j        z             |                    ||          ||j        z  k    sJ t	          d          }|                    |||j        z             |	                    |||j
        z             |                    ||          ||j        z  | |z  |z  |j        z  z
  k    sJ | |z  |z  |_        |j        | |z  |z  k    sJ t          d|                    |          z            |dz  | |dz  z  z   k    sJ d S )NzM v r omega g hr2   r   r&   r   O   )r   r   set_ang_velr6   r   r   r   r5   angular_momentumset_posr7   r8   potential_energyr   kinetic_energy)Mvrr>   ghr2   r   r&   r   Inertia_tupler   rB   s                r    test_rigidbody2rO   @   s   )*;<<Aq!UAqsAsAMM!UQS[!!!c

Aac13AFM#q!Q..AIIaQSa##uqs{2222c

AIIaQSIIaQSa##uqs{QqSU13Y'>>>>>QAQ****!a&&q)))**eQhQT.AAAAAAAr"   c                  >   t          d          \  } }}}t          d          \  }}}t          d          }t          d          }|                    dd| |j        g          }	t          d          }
|
                    |||j        z  ||j        z  z   ||j        z  z              |
	                    d||	j        z  ||	j        z  z   ||	j        z  z             }|
                    |
||	           t          |	j        |	j                  }t          d	||	|||f          }t          d
||	||t          ||                    |
          |	          z   |
f          }|j        |j        k    sJ |                    |
|          |                    |
|          k    sJ d S )Nzq1:5zp1:4r+   r$   r   axisrB   r&   rb1rb2)r   r   r   	orientnewr6   r   r5   r7   r8   	locatenewv2pt_theoryr   r   r   pos_fromr   rE   )q1q2q3q4p1p2p3r+   r$   r   rB   r&   r   rR   rS   s                  r    test_rigidbody3r_   T   s   #F++NBBJBBAsA	C"ac++Ac

AIIaAC"QS&2ac6)***	CAC"QS&2ac6122AMM!Qac13A
E1aQF
+
+C
E1a.q!**Q--CCCQGI IC #"555551%%)=)=a)C)CCCCCCCr"   c                  N   t          d          \  } }t          d          }t          d          }|                    dd||j        g          }|                    ||                                |j        z             t          |d| |dz  z  dz  | |dz  z  dz            }t          d	          }|	                    d
d|z  |j
        z            }|	                    d||j
        z            }t          d||| ||f          }	|                    |d           |                    |||           |                    |||           d| z  |dz  z  dz  |                                z  |j        z  |	                    ||                              |          z
  dk    sJ dS )zConsider a pendulum of length OA = 2a, of mass m as a rigid body of
    center of mass G (OG = a) which turn around (O,z). The angle between the
    reference frame R and the rod is q.  The inertia of the body is I =
    (G,0,ma^2/3,ma^2/3). zm, aqRR1Axisr   rC      rB   r$   GS   N)r   r   r   rT   r8   rD   diffr	   r   rU   r6   r   r5   rV   rE   express)
r+   ara   rb   rc   r   rB   r$   rf   rg   s
             r    test_pendulum_angular_momentumrl   j   s    6??DAqsAsA	
T6Aqs8	,	,BNN1affhhn%%%Aq1a4x!|QAX\22Ac

A	C1rt$$A	CRT""A#q"a!Q((AIIaOOOMM!QMM!QEAqDL1qvvxx'!#-q!$$,,Q//0456 6 6 6 6 6r"   c            
         t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    d|| j        z  || j        z  z             }	t          d|| |||	f          }
t          | ||dz  |z  z
  ||dz  |z  z
  |||dz  |dz  z   z  z
  ||z  |z            }t          |
j        t                    sJ |
j        ||	fk    sJ |
j
        |k    sJ ||
_
        |
j        ||fk    sJ |
j
        |k    sJ ||	f|
_        |
j        ||	fk    sJ |
j
        |k    sJ t          ||          |
_        |
j        ||fk    sJ d S )Nr2   m, I_x, I_y, I_z, a, boprb   rC   )r   r   r	   r   rU   r6   r7   r   r4   r
   r   )r2   r+   IxIyIzrk   r   Ioro   rp   rb   I_checks               r    test_rigidbody_inertiarv      s   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++A#q!QQ((Aaa1fqj"qAvz/1Qa00!a%!)= =Gai)))))9Q''''A9Q""""QAI9Q''''AAI9Qr"   c            
         t          d          } t          d          \  }}}}}}t          | |||          }t          d          }|                    d|| j        z  || j        z  z             }	t          d|| |||f          }
|
                    |	          }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 d S )Nr2   rn   ro   rp   rb   rC   )ixyr$      re   )r   r   r	   r   rU   r6   r7   r   parallel_axisorient_axisr8   r   	to_matrixr   )r2   r+   rq   rr   rs   rk   r   rt   ro   rp   rb   IpIp_expectedr$   s                 r    test_parallel_axisr      s   sA!":;;Ar2r1a	BB		Bc

A	CQS1qs7*++A#q!QQ((A	
		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 K Kr"   c                  4   t          d          \  } }}t          d          }t          d          }t          d          }t	          d||| ||f          }t                      5  |                    | |z  |z             d d d            d S # 1 swxY w Y   d S )Nzm g hr$   r&   r   r   )r   r   r   r   r   r   set_potential_energy)r+   rL   rM   r$   r&   r   r   s          r    $test_deprecated_set_potential_energyr      s    gGAq!sAc

Aq		A#q!QA''A		!	! & &	qs1u%%%& & & & & & & & & & & & & & & & & &s   $BBBN)sympy.physics.mechanicsr   r   r   r   r   r   r	   r
   r   sympyr   r   r   r   sympy.testing.pytestr   r   r!   r@   rO   r_   rl   rv   r   r    r"   r    <module>r      s*   L L L L L L L L L L L L K K K K K K K K K K K K 9 9 9 9 9 9 2 2 2 2 2 2 2 2 2 2 2 2 ? ? ? ? ? ? ? ?     * I  I  IFB B B(D D D,6 6 6<     .K K K$& & & & &r"   