a
    {d                     @   s   d dl mZ d dlmZmZmZmZ d dlmZmZm	Z	 d dlm
Z
 d dl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S )    )symbols)PointReferenceFrameDyadic	RigidBody)dynamicsymbolsouterinertia)inertia_of_point_mass)expandzeros_simplify_matrix)raiseswarns_deprecated_sympyc            
         s  t d\} }}}}td td}tdtd}tdtd}td f}|jksfJ |j kstJ |jksJ |j|jfksJ | |_||_||_||jf|_t	t
 fdd	 t	t
fd
d	 t	t
 fdd	 t	t
 fdd	 | dks"J |j| ks2J |j|ksBJ |j|ksRJ |j||jfkshJ |j|ksxJ |j||jfksJ td}	||	||	j ||	j  ||	j   ||	| ||	j ||	j  ||	j   ksJ d S )Nzm m2 v1 v2 v3 omegaAA2PP2r   Bc                      s   t  fS )Nr    r   Ir   mr   u/var/www/html/stable-diffusion-webui/venv/lib/python3.9/site-packages/sympy/physics/mechanics/tests/test_rigidbody.py<lambda>       z test_rigidbody.<locals>.<lambda>c                      s   t d fS Nr   r   r   )r   r   r   r   r   r      r   c                      s   t d fS r   r   r   )r   r   r   r   r   r      r   c                      s   t d fS r   r   r   r   r   r   r      r   N)r   r   r   r   r   ZmassframeZ
masscenterr	   r   	TypeError__str__set_velxyzZlinear_momentum)
m2Zv1Zv2Zv3omegar   r   ZI2r   r   r   r   r   test_rigidbody
   s<    &r(   c                  C   s>  t d\} }}}}}td}td}||||j  td}t|j|j}	|	|f}
td||| |
}||||j  |||||j ksJ td}||||j  |	|||j
  |||||j | | | |j  ksJ | | | |_|j| | | ksJ td|| |d | |d   ks:J d S )NzM v r omega g hr   br   r   O   )r   r   set_ang_velr#   r   r   r   r"   angular_momentumZset_posr$   r%   Zpotential_energyr   Zkinetic_energy)Mvrr'   ghr   r)   r   r   ZInertia_tupler   r*   r   r   r   test_rigidbody2.   s"    ,r3   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sJ ||
|||
|ksJ d S )Nzq1:5zp1:4r   r   r   Zaxisr*   r   rb1rb2)r   r   r   	orientnewr#   r   r"   r$   r%   	locatenewv2pt_theoryr   r   r
   Zpos_fromcentral_inertiar-   )Zq1Zq2Zq3Zq4p1p2Zp3r   r   r   r*   r   r   r4   r5   r   r   r   test_rigidbody3A   s     &&
r<   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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R1ZAxisr   r+      r*   r   GS   N)r   r   r   r6   r%   r,   diffr	   r   r7   r#   r   r"   r8   r-   Zexpress)
r   ar=   r>   r?   r   r*   r   rA   rB   r   r   r   test_pendulum_angular_momentumW   s$    & rF   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||	f}
t| ||d |  ||d |  |||d |d    || | }|
j||	fksJ |
j|ksJ ||
_|
j||fksJ |
j|ksJ ||	f|
_|
j||	fks
J |
j|ksJ d S )Nr   m, I_x, I_y, I_z, a, bopr>   r+   )	r   r   r	   r   r7   r#   r$   r   r9   )r   r   IxIyIzrE   r)   IorH   rI   r>   ZI_checkr   r   r   test_rigidbody_inertiau   s"      
rN   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||f}
|
|	}t| |||d   |||d   |||d |d    | | | d}||ksJ t d}|	| | j
d	 t|
|	|| |td
d
ksJ d S )Nr   rG   rH   rI   r>   r+   )Zixyr      r@   )r   r   r	   r   r7   r#   r$   r   Zparallel_axisZorient_axisr%   r   Z	to_matrixr   )r   r   rJ   rK   rL   rE   r)   rM   rH   rI   r>   ZIpZIp_expectedr   r   r   r   test_parallel_axis   s$    
 "rP   c                  C   sv   t d\} }}td}td}td}td||| ||f}t " || | |  W d    n1 sh0    Y  d S )Nzm g hr   r   r   r   )r   r   r   r   r   r   Zset_potential_energy)r   r1   r2   r   r   r   r   r   r   r   $test_deprecated_set_potential_energy   s    rQ   N)Zsympy.core.symbolr   Zsympy.physics.mechanicsr   r   r   r   r   r   r	   r
   Zsympy.core.backendr   r   r   Zsympy.testing.pytestr   r   r(   r3   r<   rF   rN   rP   rQ   r   r   r   r   <module>   s   $