o
    g,                     @   s   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d Zdd	 Zd
d Zdd Zdd Zdd Zdd Zdd ZdS )    )PointReferenceFrameDyadic	RigidBody)dynamicsymbolsouterinertiaInertia)inertia_of_point_mass)expandzerossimplifysymbols)raiseswarns_deprecated_sympyc                  C   s   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s1J | j|ks8J | jjdks@J |  dksHJ | 	 dksPJ 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I r   c/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/mechanics/tests/test_rigidbody.pytest_rigidbody_default   s   r    c            
         s  t d\} }}}}td td}tdtd}tdtd}td f}|jks3J |j ks:J |jksAJ |j|jfksKJ | |_||_||_||jf|_t	t
 fdd	 t	t
fd
d	 t	t
 fdd	 t	t
 fdd	 | dksJ |j| ksJ |j|ksJ |j|ksJ |j||jfksJ t|jtsJ td}	||	||	j ||	j  ||	j   ||	| ||	j ||	j  ||	j   ksJ d S )Nzm m2 v1 v2 v3 omegaAA2PP2r   r   c                      s   t  fS )Nr   r   r!   r   r#   mr   r   <lambda>/       z test_rigidbody.<locals>.<lambda>c                      s   t d fS Nr   r%   r   )r   r#   r'   r   r   r(   0   r)   c                      s   t d fS r*   r%   r   )r!   r#   r'   r   r   r(   1   r)   c                      s   t d fS r*   r%   r   r&   r   r   r(   2   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   r+   r   r&   r   test_rigidbody   s:   &4r9   c                  C   s:  t d\} }}}}}td}td}||||j  td}t|j|j}	|	|f}
td||| |
}||||j  |||||j ksHJ td}||||j  |	|||j
  |||||j | | | |j  kstJ | | | |_|j| | | ksJ td|| |d | |d   ksJ d S )NzM v r omega g hr+   r   r#   r   O   )r   r   set_ang_velr/   r   r   r   r.   angular_momentumset_posr0   r1   potential_energyr   kinetic_energy)Mvrr7   ghr+   r   r#   r   Inertia_tupler   r:   r   r   r   test_rigidbody2@   s"   ,.rG   c                  C   s  t d\} }}}td\}}}td}td}|dd| |jg}	td}
|
|||j ||j  ||j   |
	d||	j ||	j  ||	j  }|
|
||	 t|	j|	j}td	||	|||f}td
||	||t|||
|	 |
f}|j|jks~J ||
|||
|ksJ d S )Nzq1:5zp1:4r'   r!   r   axisr:   r#   rb1rb2)r   r   r   	orientnewr/   r   r.   r0   r1   	locatenewv2pt_theoryr   r   r
   pos_fromr   r=   )q1q2q3q4p1p2p3r'   r!   r   r:   r#   r   rI   rJ   r   r   r   test_rigidbody3T   s    &&
 rV   c            
      C   s  t d\} }td}td}|dd||jg}||| |j  t|d| |d  d | |d  d }td	}|	d
d| |j
 }|	d||j
 }td||| ||f}	||d |||| |||| d|  |d  d |  |j |	||| dksJ 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   r;      r:   r!   GS   N)r   r   r   rK   r1   r<   diffr   r   rL   r/   r   r.   rM   r=   express)
r'   arW   rX   rY   r   r:   r!   r\   r]   r   r   r   test_pendulum_angular_momentumj   s$   & rb   c               	   C   sH  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||	f}
t| ||d |  ||d |  |||d |d    || | }t|
jt	s[J |
j||	fksdJ |
j
|kskJ ||
_
|
j||fkswJ |
j
|ks~J ||	f|
_|
j||	fksJ |
j
|ksJ t	|||
_|
j||fksJ d S )Nr+   m, I_x, I_y, I_z, a, boprX   r;   )r   r   r   r   rL   r/   r0   r   r-   r	   r   )r+   r'   IxIyIzra   r   Iord   re   rX   I_checkr   r   r   test_rigidbody_inertia   s(     
rk   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||f}
|
|	}t| |||d   |||d   |||d |d    | | | d}||ks`J t d}|	| | j
d	 t|
|	|| |td
d
ksJ d S )Nr+   rc   rd   re   rX   r;   )ixyr!      r[   )r   r   r   r   rL   r/   r0   r   parallel_axisorient_axisr1   r   	to_matrixr   )r+   r'   rf   rg   rh   ra   r   ri   rd   re   rX   IpIp_expectedr!   r   r   r   test_parallel_axis   s$   
 "rs   c                  C   sx   t d\} }}td}td}td}td||| ||f}t  || | |  W d    d S 1 s5w   Y  d S )Nzm g hr!   r#   r   r   )r   r   r   r   r   r   set_potential_energy)r'   rD   rE   r!   r#   r   r   r   r   r   $test_deprecated_set_potential_energy   s   "ru   N)sympy.physics.mechanicsr   r   r   r   r   r   r   r	   r
   sympyr   r   r   r   sympy.testing.pytestr   r   r    r9   rG   rV   rb   rk   rs   ru   r   r   r   r   <module>   s    #