U
    L?hU0                     @   s   d dl mZmZmZ d dlmZmZ d dl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d 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&d' Zd(d) Zd*d+ Zd,d- Zd.d/ ZdS )0    )dynamicsymbolsPointReferenceFrame)raisesignore_warningsNc            
      C   s   t d\} }t dd\}}t dd\}}td}td}||||j  td}|d|j}	|	|d ||d |	|||||j	 kst
|||j |	||||j||j	  kst
|	||j |	||||j|j ||j	  kst
d S 	Nq q2      NBOPr   )r   r   set_ang_velzr   	locatenewxset_velv1pt_theoryyAssertionError
qq2qdZq2dqddZq2ddr   r   r   r    r   W/var/www/html/venv/lib/python3.8/site-packages/sympy/physics/vector/tests/test_point.pytest_point_v1pt_theorys   s    "r   c            
      C   sD  t d\} }t dd\}}t dd\}}td}td}||||j  td}|d|j}	|	|d ||d |	||||d  |j ||j	  kst
|	|||j  |	||||d  |j ||j	  ||j  kst
||||j  |	|||||d  |j || | |j	  ||j  ks@t
d S r   )r   r   r   r   r   r   r   r   a1pt_theoryr   r   r   r   r   r   test_point_a1pt_theorys   s"    ,6.r    c                  C   s   t d} t dd}td}|dd| |jg}td}|dd}||d ||||dksdt|d|j	}||||||j |j	A kst|||j	 |||||j	||j
  kstd S )	Nr   r	   r   r   Axisr   r   r   )r   r   	orientnewr   r   r   r   v2pt_theoryr   r   r   )r   r   r   r   r   r   r   r   r   test_point_v2pt_theorys*   s    
"r$   c                  C   s   t d} t dd}t dd}td}|dd| |jg}td}|dd	}||d	 ||||d	ksnt|	||j
 |||||d  |j
 ||j  kstd S )
Nr   r	   r
   r   r   r!   r   r   r   )r   r   r"   r   r   r   r   a2pt_theoryr   set_posr   r   )r   r   r   r   r   r   r   r   r   r   test_point_a2pt_theorys9   s    

r'   c            
      C   sP  t d\} }t dd\}}t dd\}}td}td}||d|j  td}|d| |j ||j  }	|	|| |j ||j  kst|		|||j ||j   |	
|||j ||j  kst|	|d	 |
|d	kst|	|||d
|  | |j ||j  d| |j  ks*t|dd| |jg}td}|dd|j }	|	|d|j  |
|d|j kst|	|||d|d  |j d| |j  kst||d|j  td}|d| |j ||j  }	|		|||j ||j   |	|d	 |	|||||j ||j  d|  |j  ksLtd S )Nr   r	   r
   r   r      r   r   r   ir!   
   )r   r   r   r   r   r   r   pos_fromr   r   velr   r   r"   r%   r   r   r   r   r   test_point_funcsG   s8    ""&4r-   c                  C   s  t d} td}|dd| |jg}td}|dd|j d|j  }||d|j d|j  ksjt|d	d|j	 d|j	  }||d|j	 d|j	  kst||d|j d|j	  d|j  d|j	  kst||d
|j d|j	  d|j  d|j	  kstd S )Nr   r   r   r!   r   r   r*   r(   Qr)   )
r   r   r"   r   r   r   r   r+   r   r   )r   r   r   r   r   r.   r   r   r   test_point_posg   s    ""6r/   c                     s   t d} t d tdtd\}|  j || j   |  jksVt| | j| jfkstttt	 fdd d S )Nr   Apzu1, u2c                      s     S N)partial_velocityr   r0   r1   u1r   r   <lambda>       z-test_point_partial_velocity.<locals>.<lambda>)
r   r   r   r   r   r   r3   r   r   
ValueError)r   u2r   r4   r   test_point_partial_velocitys   s    r:   c                     s   t d\} }tdtd tdtd| j  ttfdd |j  	|j ks|t
tt fdd d S )	Nq1 q2r   r   r.   r   c                      s
     S r2   r,   r   )r   r.   r   r   r6      r7   z test_point_vel.<locals>.<lambda>c                      s
     S r2   r<   r   )r   r   r   r   r6      r7   )r   r   r   r&   r   r   r8   r   r   r,   r   )q1r   r   )r   r   r   r.   r   test_point_vel   s    r>   c                     s   t j} t d\}}tdtd td}td}|||j  ||j  ||	| j |j  ks~t
td}||| j  td|| j  tt fdd	 ttfd
d	 d S )Nr;   r   r   r   r.   P1P2c                      s
     S r2   r<   r   )r   r@   r   r   r6      r7   z%test_auto_point_vel.<locals>.<lambda>c                      s
     S r2   r<   r   r   r@   r   r   r6      r7   )r   _tr   r   r&   r   r   r   r,   diffr   r   r   r8   )tr=   r   r   r.   r?   r   )r   r   r@   r   test_auto_point_vel   s    (rE   c                  C   s   t j} t d\}}td}td}||||j  td}||||j  ||||j  td}||||j  td}||d| |j  |	|d|
|  |j ||
|  |j  kstd S )Nr;   r   r   r?   r@   P3r*   )r   rB   r   r   r   r   r&   r   r   r,   rC   r   )rD   r=   r   r   r   r?   r@   rF   r   r   r   'test_auto_point_vel_multiple_point_path   s    rG   c                  C   s   t j} t d\}}}td}td}||||j  td}||||j  |||	| |j ||j  ksxt
||||j kst
||||j  ||||j kst
d S )Nz
q1, q2, u1r   r?   )r   rB   r   r   r   r   r&   r   r,   rC   r   r   )rD   r=   r   r5   r   r   r?   r   r   r   test_auto_vel_dont_overwrite   s    (rH   c                     sx   t d\} }td tdtd}| |  j  td|j tt fdd ttfdd d S )	Nr;   r   Sr   r?   c                      s
     S r2   r<   r   )r   r?   r   r   r6      r7   zRtest_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector.<locals>.<lambda>c                      s
     S r2   r<   r   )r?   rI   r   r   r6      r7   )	r   r   r   r   r   r&   r   r   r8   )r=   r   r   r   )r   r?   rI   r   @test_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector   s    rJ   c               
   C   sz  t j} t d\}}}}td}td}||||j  td}||||j  ||||j  td}||||j  td}	|	|d| |j  td}
|
|	||j  td	}||||j  td
}||||j  |
|||j ||j   t	
 b t	d ttD |
||| |j ||j  d||  |j  ksbtW 5 Q R X W 5 Q R X d S )Nzq1 q2 u1 u2r   r   r?   r@   rF   r*   P4r   O1errorr
   )r   rB   r   r   r   r   r&   r   r   warningscatch_warningssimplefilterr   UserWarningr,   rC   r   )rD   r=   r   r5   r9   r   r   r?   r@   rF   rK   r   rL   r   r   r   !test_auto_point_vel_shortest_path   s.    


rR   c                     s   t j} t d\}}}}td td}td}| | j  td|| j ||j   tt	 fdd  
|d||jf  |||   j || |j  |||  |j  kstd S )	Nz	q q1 q2 ur   r   r   r   c                      s
     S r2   r<   r   r   r   r   r   r6      r7   z6test_auto_point_vel_connected_frames.<locals>.<lambda>r!   )r   rB   r   r   r   r   r&   r   r   r8   Zorientr,   rC   r   r   )rD   r   r=   r   ur   r   r   rS   r   $test_auto_point_vel_connected_frames   s    rU   c               	      s   t d\} }td tdtd}td}td}| | j  | | j  | | j  ||  j  ||  j  ||  j  t	 $ t
d tt fdd	 W 5 Q R X d S )
Nzq ur   r   r   r.   RrM   c                      s
     S r2   r<   r   r   r   r   r   r6      r7   zCtest_auto_point_vel_multiple_paths_warning_arises.<locals>.<lambda>)r   r   r   r   r   r   r   r&   rN   rO   rP   r   rQ   )r   rT   r   r.   rV   r   rW   r   1test_auto_point_vel_multiple_paths_warning_arises   s    

rX   c               	      s   t d} t d}t dt d}td |   j ||  j | j | j || j j  t $ t	d t
t fdd W 5 Q R X d S )	Nr   r?   r@   rF   r   rM   c                      s
     S r2   r<   r   rA   r   r   r6     r7   z5test_auto_vel_cyclic_warning_arises.<locals>.<lambda>)r   r   r   r   r&   r   r   rN   rO   rP   r   rQ   )r   r?   rF   r   rA   r   #test_auto_vel_cyclic_warning_arises   s    

rY   c               	   C   s   t d} t d}t d}t d}td}| ||j || |j |||j |||j |||j|j  tjddP}t	d |
| t|d	 jd
d}t|d	 jtstd|kstW 5 Q R X d S )Nr   r?   r@   rF   r   Trecordalways
 zKinematic loops are defined among the positions of points. This is likely not desired and may cause errors in your calculations.)r   r   r   r   r&   r   r   rN   rO   rP   r,   strmessagereplace
issubclasscategoryrQ   r   )r   r?   r@   rF   r   wmsgr   r   r    test_auto_vel_cyclic_warning_msg  s     

rg   c               	   C   s   t d} td}td}td}|| | j || | j ||| j ||| j tjddh}t	d |
|  t|d jd	d
}t|d jtstd|kstd|kstd|kstW 5 Q R X d S )Nr   r   r   r.   TrZ   r\   r]   r^   r_   Velocityz'automatically calculated based on pointzfVelocities from these points are not necessarily the same. This may cause errors in your calculations.)r   r   r   r   r   r&   r   rN   rO   rP   r,   r`   ra   rb   rc   rd   rQ   r   )r   r   r   r.   re   rf   r   r   r   'test_auto_vel_multiple_path_warning_msg  s     

ri   c                  C   sL  t d\} }t dd\}}td}td}td}|||j|  ||||j  |||j| ||||j  td}||d td	}|||j ||d ||d td
}	|	||j |	|d |	j	
 }
|	|||j || |j  kst|
|	_	|	||| |	|||j || |j  ksHtd S )Nq1:3zu1:3r	   r0   r   CAmr   BmCm)r   r   orient_axisr   r   r   r   r&   r   	_vel_dictcopyr,   r   r   r#   )r=   r   r5   r9   r0   r   rk   rl   rm   rn   tempr   r   r   test_auto_vel_derivative1  s.    
(rs   c                  C   s8   t d} td}|| d || d| j ks4td S )Nr   r   r   )r   r   r   accr   r   rW   r   r   r   test_auto_point_acc_zero_velK  s    ru   c                  C   s   t j} t d}td}td}|||j| td}||d td}|||j |	||
| d  |j |
| d|j  kstd S )Nr=   r   r0   r   r   r   r
   )r   rB   r   ro   r   r   r   r&   r   rt   rC   r   r   )rD   r=   r   r0   r   r   r   r   r   test_auto_point_acc_compute_velQ  s    rv   c                  C   s  t d\} }t dd\}}t dd\}}td}td}td}|||j|  |||j| td}	|	|d td	}
|
|	|j |
|d |
|d td
}||
|j ||d |
j	 |j	  }}|
j
	 |j
	  }}|d  |j ||j  || d |j  || |j  }|||ks>t|| |
_|_|| |
_
|_
|
|	|| ||
|| |
|	|| ||
|||kstd S )Nrj   r	   r
   r0   r   rk   rl   r   rm   rn   )r   r   ro   r   r   r   r&   r   rp   rq   Z	_acc_dictr   rt   r   r#   r%   )r=   r   r5   r9   Zv1Zv2r0   r   rk   rl   rm   rn   ZBm_vel_dictZCm_vel_dictZBm_acc_dictZCm_acc_dictcheckr   r   r   test_auto_acc_derivative^  s6    :rx   )Zsympy.physics.vectorr   r   r   Zsympy.testing.pytestr   r   rN   r   r    r$   r'   r-   r/   r:   r>   rE   rG   rH   rJ   rR   rU   rX   rY   rg   ri   rs   ru   rv   rx   r   r   r   r   <module>   s0    