
    sg,                         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y)    )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       }| 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 | j                         dk(  sJ | j                         dk(  sJ y )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     _/var/www/html/venv/lib/python3.12/site-packages/sympy/physics/mechanics/tests/test_rigidbody.pytest_rigidbody_defaultr!      s    #AI'"GHIA66S==66WX&&&&<<...99ALL))))!!!77<<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       |j                         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                  |	||	j                  z  ||	j                   z  z   ||	j"                  z  z          |j%                  |	      | ||	j                  z  ||	j                   z  z   ||	j"                  z  z   z  k(  sJ y )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@      s   &'<=Ar2r2usA		Bc
A	tBq	A	B#q!QA'A66Q;;77a<<<<199ALL))))AFAGALQ\\"AI
9;<
9=>
9=>
9=>99;#66R<<77b==<<299Q\\****aii))) 	sAJJq"qss(R!##X%QSS01Q2accBH)<rACCx)G#HHHHr"   c                  >   t        d      \  } }}}}}t        d      }t        d      }|j                  |||j                  z         t	        d      }t        |j                  |j                        }	|	|f}
t        d||| |
      }|j                  |||j                  z         |j                  ||      ||j                  z  k(  sJ t	        d      }|j                  |||j                  z         |j                  |||j                  z         |j                  ||      ||j                  z  | |z  |z  |j                  z  z
  k(  sJ | |z  |z  |_        |j                  | |z  |z  k(  sJ t        d|j                  |      z        |dz  | |dz  z  z   k(  sJ y )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!UQSS[!c
Aacc133AFM#q!Q.AIIaQSSa#uqss{222c
AIIaQSSIIaQSSa#uqss{QqSU133Y'>>>>QAQ***!a&&q))*eQhQT.AAAAr"   c                  &   t        d      \  } }}}t        d      \  }}}t        d      }t        d      }|j                  dd| |j                  g      }	t        d      }
|
j                  |||j                  z  ||j                  z  z   ||j                  z  z          |
j                  d||	j                  z  ||	j                  z  z   ||	j                  z  z         }|j                  |
||	       t        |	j                  |	j                        }t        d	||	|||f      }t        d
||	||t        ||j                  |
      |	      z   |
f      }|j                  |j                  k(  sJ |j!                  |
|      |j!                  |
|      k(  sJ y )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   sh   #F+NBBJBBAsA	C"acc+Ac
AIIaACC"QSS&2acc6)*	CACC"QSS&2acc612AMM!Qacc133A
E1aQF
+C
E1a.q!**Q-CCQGIC #"5"55551%)=)=a)CCCCr"   c                     t        d      \  } }t        d      }t        d      }|j                  dd||j                  g      }|j                  ||j                         |j                  z         t        |d| |dz  z  dz  | |dz  z  dz        }t        d	      }|j                  d
d|z  |j                  z        }|j                  d||j                  z        }t        d||| ||f      }	|j                  |d       |j                  |||       |j                  |||       d| z  |dz  z  dz  |j                         z  |j                  z  |	j                  ||      j                  |      z
  dk(  sJ y)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	
T6Aqss8	,BNN1affhn%Aq1a4x!|QAX\2Ac
A	C1rtt$A	CRTT"A#q"a!Q(AIIaOMM!QMM!QEAqDL1qvvx'!##-q!$,,Q/0456 6 6r"   c            
         t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  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 y )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QSS1qss7*+A#q!QQ(Aaa1fqj"qAvz/1Qa00!a%!)=Gaii)))99Q'''A99Q"""QAI99Q'''AAI99Qr"   c            
      X   t        d      } t        d      \  }}}}}}t        | |||      }t        d      }|j	                  d|| j
                  z  || j                  z  z         }	t        d|| |||f      }
|
j                  |	      }t        | |||dz  z  z   |||dz  z  z   |||dz  |dz  z   z  z   | |z  |z        }||k(  sJ t        d      }|j                  | | j                  d	       t        |
j                  |	|      |z
  j                  |            t        d
d
      k(  sJ y )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      sA   sA!":;Ar2r1a	BB	Bc
A	CQSS1qss7*+A#q!QQ(A	
	B!R!ad(]BQTMqAqD1a4K00qb1fqjBKsAMM!QSS!	
A		,77:<?DQ{K K Kr"   c                      t        d      \  } }}t        d      }t        d      }t        d      }t	        d||| ||f      }t               5  |j                  | |z  |z         d d d        y # 1 sw Y   y xY w)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      su    gGAq!sAc
Aq	A#q!QA'A		! &	qs1u%& & &s   A--A6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      sJ    L L K K 9 2 2 ? * IFB(D,6< .K$&r"   