U
    L?h,                     @   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ftd }| jdks*t| jtdks<t| jjdksLt| j|| jfks`t| j|ksnt| jjdks~t| 	 dkst| 
 dkstd S )NBz#B_ixx B_iyy B_izz B_ixy B_iyz B_izxZB_massZB_masscenterZ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AssertionErrormass
masscentercentral_inertia__str____repr__)bI r   ^/var/www/html/venv/lib/python3.8/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sft|j kstt|jkst|j	|jfkst| |_||_||_||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"t|j| ks2t|j|ksBt|j|ksRt|j	||jfkshtt|j	tszttd}	||	||	j ||	j  ||	j   ||	| ||	j ||	j  ||	j   kst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   r   	TypeErrorr   
isinstancer	   set_velxyzZlinear_momentum)
m2Zv1Zv2Zv3omegar    r"   ZI2r   r)   r   r$   r   test_rigidbody   s:    &r2   c                  C   s>  t d\} }}}}}td}td}||||j  td}t|j|j}	|	|f}
td||| |
}||||j  |||||j kst	td}||||j  |
|||j  |||||j | | | |j  kst	| | | |_|j| | | kst	td|| |d | |d   ks:t	d S )NzM v r omega g hr)   r   r!   r   O   )r   r   set_ang_velr-   r   r   r   r,   angular_momentumr   Zset_posr.   r/   Zpotential_energyr   Zkinetic_energy)Mvrr1   ghr)   r   r!   r   ZInertia_tupler   r3   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st||
|||
|kstd S )Nzq1:5zp1:4r%   r   r   Zaxisr3   r!   rb1rb2)r   r   r   	orientnewr-   r   r,   r.   r/   	locatenewv2pt_theoryr   r   r
   Zpos_fromr   r   r6   )q1q2Zq3Zq4p1Zp2Zp3r%   r   r   r3   r!   r   r=   r>   r   r   r   test_rigidbody3T   s     &&
rE   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t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   r4      r3   r   GS   N)r   r   r   r?   r/   r5   diffr   r   r@   r-   r   r,   rA   r6   Zexpressr   )
r%   arF   rG   rH   r   r3   r   rJ   rK   r   r   r   test_pendulum_angular_momentumj   s$    & rO   c               	   C   sN  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||	f}
t| ||d |  ||d |  |||d |d    || | }t|
jt	st
|
j||	fkst
|
j|kst
||
_|
j||fkst
|
j|kst
||	f|
_|
j||	fkst
|
j|ks*t
t	|||
_|
j||fksJt
d S )Nr)   m, I_x, I_y, I_z, a, boprG   r4   )r   r   r   r   r@   r-   r.   r   r+   r	   r   r   )r)   r%   IxIyIzrN   r   IorQ   rR   rG   ZI_checkr   r   r   test_rigidbody_inertia   s*      

rW   c               	   C   s  t d} td\}}}}}}t| |||}td}|d|| j || j  }	td|| |||f}
|
|	}t| |||d   |||d   |||d |d    | | | d}||kst	t d}|
| | jd	 t|
|	|| |td
d
kst	d S )Nr)   rP   rQ   rR   rG   r4   )Zixyr      rI   )r   r   r   r   r@   r-   r.   r   Zparallel_axisr   Zorient_axisr/   r   Z	to_matrixr   )r)   r%   rS   rT   rU   rN   r   rV   rQ   rR   rG   ZIpZIp_expectedr   r   r   r   test_parallel_axis   s&    
  rY   c               	   C   sb   t d\} }}td}td}td}td||| ||f}t  || | |  W 5 Q R X d S )Nzm g hr   r!   r   r   )r   r   r   r   r   r   Zset_potential_energy)r%   r:   r;   r   r!   r   r   r   r   r   $test_deprecated_set_potential_energy   s    rZ   N)Zsympy.physics.mechanicsr   r   r   r   r   r   r   r	   r
   Zsympyr   r   r   r   Zsympy.testing.pytestr   r   r   r2   r<   rE   rO   rW   rY   rZ   r   r   r   r   <module>   s   #