o
    *ήc#                     @   sP  d dl mZmZmZmZmZmZmZ d dlm	Z	m
Z
mZmZ d dlmZmZmZmZmZmZmZmZmZmZmZ d dlmZmZ d dlmZ d dlmZ de_ed\Z Z!Z"Z#Z$ed	Z%e%&d
de e%j'gZ(e(&dde!e(j)gZ*e*&dde"e*j+gZ,dd Z-dd Z.dd Z/dd Z0dd Z1dd Z2dd Z3dd Z4dd Z5d d! Z6d"d# Z7d$S )%    )sincostanpisymbolsMatrixS)ParticlePointReferenceFrame	RigidBody)angular_momentumdynamicsymbolsinertiainertia_of_point_masskinetic_energylinear_momentumouterpotential_energymsubsfind_dynamicsymbols
Lagrangian)gravitycenter_of_mass)Vector)raisesTzq1 q2 q3 q4 q5NAAxisBCc                  C   sB  t d} td\}}}td\}}}t| ||||| j| jB  || j| jB   || j| jB   ks3J t| dddd| j| jB  ksDJ ttdd  t| |||||||| j| jB  || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   || j| jB   ksJ d S )Nr   zixx iyy izzzixy iyz izxr   c                   S   s   t ddddS )Nr   )r    r!   r!   S/tmp/pip-target-vg8gfxp4/lib/python/sympy/physics/mechanics/tests/test_functions.py<lambda>       ztest_inertia.<locals>.<lambda>)r   r   r   xyzr   	TypeError)r   ixxiyyizzixyiyzizxr!   r!   r"   test_inertia   s>   

" 
r/   c            
      C   s  t d\} }}}td}| |j }t|||}||| d  |j|jB  || d  |j|jB   ks3J ||j }t|||}|||d  |j|jB  ||d  |j|jB   ksZJ ||j }t|||}|||d  |j|jB  ||d  |j|jB   ksJ || | }	t||	|}|||d |d   |j|jB  ||  | |j|jB   ||  | |j|jB   ||  | |j|jB   || d |d   |j|jB   || | |j|jB   ||  | |j|jB   || | |j|jB   || d |d   |j|jB   ks
J d S )Nzr s t mr      )r   r   r%   r   r&   r'   )
rstmr   pxIpypzpr!   r!   r"   test_inertia_of_point_mass$   s<   
8
8
8 r:   c                     s   t dtd} | dj  tjj}td| d|| f td}td|djd	j  t	t
 fd
d t	t
fdd t d	j dj  ks\J d S )Nr   Ac   r      PPa   
   c                         t   S Nr   r!   r   r?   r!   r"   r#   J       z&test_linear_momentum.<locals>.<lambda>c                      rB   rC   rD   r!   r   r?   r!   r"   r#   K   rF   i  )r   r
   set_velr&   r   r%   r   r	   pointr   r(   r   )r;   r6   r>   r!   r   r   r?   r"   test_linear_momentumA   s   (rK   c            	   	      s  t d\} }}}td}tdtd}tdd|j }|d|j }dj  ||j  |	| |	| t
d	|| td
||||tjj |f d|  | | j || | j  }t |ksJ tt fdd tt fdd ttfdd |||d   d|  |d   | j }t |ksJ dS )a   A rod with length 2l, centroidal inertia I, and mass M along with a
    particle of mass m fixed to the end of the rod rotate with an angular rate
    of omega about point O which is fixed to the non-particle end of the rod.
    The rod's reference frame is A and the inertial frame is N.z
m, M, l, Iomegar   aOr;   r>   r   r?   r   r0   c                         t  S rC   r   r!   rJ   r!   r"   r#   c   r$   z;test_angular_momentum_and_linear_momentum.<locals>.<lambda>c                      rO   rC   rP   r!   )r   rN   r?   r!   r"   r#   d   r$   c                      s   t  S rC   rP   r!   )r   rN   r?   r!   r"   r#   e   r$      N)r   r   r   r
   	locatenewr%   rH   set_ang_velr'   v2pt_theoryr	   r   r   r&   r   r   r(   r   )	r4   Mlr6   rL   rM   r;   r>   expectedr!   )r   r   rN   r?   r"   )test_angular_momentum_and_linear_momentumO   s(   "(*rX   c            	         s@  t d\} }}td}tdtd}|dj  |d|j }|d|j }td}||j  |	|| |	|| t
d	|| tjj}td
|||||f tt fdd tt fdd dt ||d  |d  d d|d  |  |d   |d d    ksJ d S )Nzm M l1rL   r   rN   r   r;   r>   rM   r?   r   c                         t  S rC   r   r!   rE   r!   r"   r#   y   rF   z%test_kinetic_energy.<locals>.<lambda>c                      rY   rC   rZ   r!   )r   r   r!   r"   r#   z   rF   r0   )r   r   r   r
   rH   r%   rR   rS   r'   rT   r	   r   r   r   r(   r   expand)	r4   rU   l1rL   rN   r;   r>   rM   r6   r!   rJ   r"   test_kinetic_energyj   s.   "
r]   c                  C   s  t d\} }}}}}td}td}td}||d|j  |d||j }	|	d||j }
td}||||j  |		||| |
	||| t
d	|
| }t|j|j}td
|	||||	f}| | | |_|| | |_t||| | | || |  ksJ d S )Nzm M l1 g h HrL   r   rN   r   r;   r>   rM   r?   r   )r   r   r   r
   rH   r%   rR   rS   r'   rT   r	   r   r   r   )r4   rU   r\   ghHrL   r   rN   r;   r>   rM   r?   r6   r   r!   r!   r"   test_potential_energy   s"   *ra   c            	         s
  t d\} }}}tdtd}|dj  |ddj }|dj  td|d|d	d
j }|dj  td}|dj	  t
j	j	}td||d||f || | _| | |  _tt fdd ttfdd d S )NzM m g hr   rN   r   r>   r@   rA   r?   r;   r0      rM   r   r=   c                      rB   rC   r   r!   rE   r!   r"   r#      rF   z!test_Lagrangian.<locals>.<lambda>c                      rB   rC   rc   r!   rG   r!   r"   r#      rF   )r   r   r
   rH   r%   rR   r	   r&   rS   r'   r   r   r   r   r(   )	rU   r4   r^   r_   rN   r>   r;   rM   r6   r!   rJ   r"   test_Lagrangian   s"   rd   c                  C   s  t d\} }td\}}}t| | | ||  | g|  |t|  gg}t| | |g|  dgg}|d|d| d| di}t|||ksRJ t|| t||  ||   }|d|td | di}t||dd|d ks}J t	d}||j
 ||j  }	||j
|j
B  ||j|jB   }
d|j }d|j|jB  }|d|di}t|	||ksJ t|
||ksJ d S )	Na, bx, y, zr@   r   r0   T)smartr   )r   r   r   diffr   r   r   r   r   r   r%   r&   )rM   br%   r&   r'   exprsolsdr   vdv_sold_solr!   r!   r"   
test_msubs   s*   $ 
rq   c                     s&  t d\} }td\}}}t| | | ||  | g|  |t|  gg}|| ||  || h}t||ksCJ |||g}| |  | h}t||d|ks_J td\}}	}
td}||j |	|j  |
|j	   ||	|
h}t |d|ksJ t
t fdd d S )	Nre   rf   )excludezd, e, fr   )reference_framec                      s   t  S rC   )r   r!   rm   r!   r"   r#      s    z*test_find_dynamicsymbols.<locals>.<lambda>)r   r   r   rh   r   r   r   r%   r&   r'   r   
ValueError)rM   ri   r%   r&   r'   rj   rk   exclude_listrn   efr   r!   rt   r"   test_find_dynamicsymbols   s     

ry   c                  C   s  t d} td\}}}td\}}td}td||}t d}td}	t|j|j}
td|	|||
|	f}||f|	|fg}|t	|| j
 || ||f|	|f||| | j
 f|	|| | j
 fg}tt|D ]}tt|| D ]}|| | || | ksJ qpqfd S )	Nr   zm M gzF1 F2popar   r>   r   )r   r   r   r
   r	   r   r%   r   extendr   r&   rangelen)r   r4   rU   r^   F1F2rz   r{   r   r>   r6   r   	forceListrV   ijr!   r!   r"   test_gravity   s"   0r   c               
   C   st  t d} tddd}tdtdtj}tdtdtd	}td
tdtd}tdtd|}t d}td}td}td|||t|j|j|f}	|j	
|j	| j |j	
|j	| j| j  |j	
|j	| j |	j
|j	| j| j  td}
|

|j	t|j	|||||	 d|| d  | j || d || d  | j  ||| d  | j  }|
|j	| dksJ d S )NrM   r4   T)realp1p1_ptp2p2_ptr0   p3p3_pt   p4p4_ptb_fb_cmmbri   orb      r   )r   r   r	   r
   r   Oner   r   r%   rI   set_posr&   
masscenterr'   r   pos_from)rM   r4   r   r   r   r   r   r   r   ri   point_orj   r!   r!   r"   test_center_of_mass   s$   Jr   N)8sympy.core.backendr   r   r   r   r   r   r   sympy.physics.mechanicsr	   r
   r   r   r   r   r   r   r   r   r   r   r   r   r   !sympy.physics.mechanics.functionsr   r   sympy.physics.vector.vectorr   sympy.testing.pytestr   simpq1q2q3q4q5r   	orientnewr'   r   r%   r   r&   r    r/   r:   rK   rX   r]   ra   rd   rq   ry   r   r   r!   r!   r!   r"   <module>   s.   $ 4