o
    *ήc                     @   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 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S )    )symbols)PointReferenceFrameDyadic	RigidBody)dynamicsymbolsouterinertia)inertia_of_point_mass)expand)raiseswarns_deprecated_sympyc            
         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 |j|ksJ |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   S/tmp/pip-target-vg8gfxp4/lib/python/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   massframe
masscenterr	   r   	TypeError__str__set_velxyzlinear_momentum)
m2v1v2v3omegar   r   I2r   r   r   r   r   test_rigidbody
   s<   &4r-   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   br   r   O   )r   r   set_ang_velr#   r   r   r   r"   angular_momentumset_posr$   r%   potential_energyr   kinetic_energy)Mvrr+   ghr   r.   r   r   Inertia_tupler   r/   r   r   r   test_rigidbody2.   s"   ,.r<   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"   r$   r%   	locatenewv2pt_theoryr   r   r
   pos_fromcentral_inertiar2   )q1q2q3q4p1p2p3r   r   r   r/   r   r   r>   r?   r   r   r   test_rigidbody3A   s    &&
 rL   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   r0      r/   r   GS   N)r   r   r   r@   r%   r1   diffr	   r   rA   r#   r   r"   rB   r2   express)
r   arM   rN   rO   r   r/   r   rR   rS   r   r   r   test_pendulum_angular_momentumW   s$   & rX   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 d S )Nr   zm, I_x, I_y, I_z, a, boprN   r0   )ixy)	r   r   r	   r   rA   r#   r$   r   parallel_axis)r   r   IxIyIzrW   r.   IorY   rZ   rN   IpIp_expectedr   r   r   test_parallel_axisu   s   
 "rc   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   r9   r:   r   r   r   r   r   r   r   $test_deprecated_set_potential_energy   s   "re   N)sympy.core.symbolr   sympy.physics.mechanicsr   r   r   r   r   r   r	   r
   sympy.core.backendr   sympy.testing.pytestr   r   r-   r<   rL   rX   rc   re   r   r   r   r   <module>   s    $