o
    GZhU0                     @   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sJJ |||j |	||||j||j	  ksbJ |	||j |	||||j|j ||j	  ks}J d S 	Nq q2      NBOPr   )
r   r   set_ang_velzr   	locatenewxset_velv1pt_theoryy
qq2qdZq2dqddZq2ddr   r   r   r    r   T/var/www/auris/lib/python3.10/site-packages/sympy/physics/vector/tests/test_point.pytest_point_v1pt_theorys   s   ",r   c            
      C   sB  t d\} }t dd\}}t dd\}}td}td}||||j  td}|d|j}	|	|d ||d |	||||d  |j ||j	  ksRJ |	|||j  |	||||d  |j ||j	  ||j  ksvJ ||||j  |	|||||d  |j || | |j	  ||j  ksJ d S r   )
r   r   r   r   r   r   r   r   a1pt_theoryr   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s2J |d|j}||||||j |jA ksJJ |||j |||||j||j	  ksbJ 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   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s7J |||j	 |||||d  |j	 ||j
  ksTJ d S )
Nr   r	   r
   r   r   r    r   r   r   )r   r   r!   r   r   r   r   a2pt_theoryset_posr   r   )r   r   r   r   r   r   r   r   r   r   test_point_a2pt_theorys9   s   

0r&   c            
      C   sJ  t d\} }t dd\}}t dd\}}td}td}||d|j  td}|d| |j ||j  }	|	|| |j ||j  ksHJ |	|||j ||j   |		|||j ||j  ksgJ ||d	 |	|d	ksvJ |	
|||d
|  | |j ||j  d| |j  ksJ |dd| |jg}td}|dd|j }	||d|j  |	|d|j ksJ |	|||d|d  |j d| |j  ksJ ||d|j  td}|d| |j ||j  }	|	|||j ||j   ||d	 |	|||||j ||j  d|  |j  ks#J d S )Nr   r	   r
   r   r      r   r   r   ir    
   )r   r   r   r   r   r   r   pos_fromr   velr   r   r!   r$   r   r   r   r   r   test_point_funcsG   s8   ""&
2:r,   c                  C   s  t d} td}|dd| |jg}td}|dd|j d|j  }||d|j d|j  ks5J |d	d|j d|j  }||d|j d|j  ksTJ ||d|j d|j  d|j  d|j  ksoJ ||d
|j d|j  d|j  d|j  ksJ 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   test_point_posg   s   ""6:r.   c                     s   t d} t d tdtd\}|  j || j   |  jks+J | | j| jfks:J tt fdd d S )Nr   Apzu1, u2c                      s     S N)partial_velocityr   r/   r0   u1r   r   <lambda>   s    z-test_point_partial_velocity.<locals>.<lambda>)	r   r   r   r   r   r   r2   r   
ValueError)r   u2r   r3   r   test_point_partial_velocitys   s   r8   c                     s   t d\} }tdtd tdtd| j  ttfdd |j  	|j ks>J tt fdd d S )	Nq1 q2r   r   r-   r   c                      
     S r1   r+   r   )r   r-   r   r   r5         
 z test_point_vel.<locals>.<lambda>c                      r:   r1   r;   r   )r   r   r   r   r5      r<   )
r   r   r   r%   r   r   r6   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?J td}||| j  td|| j
  tt fdd	 ttfd
d	 d S )Nr9   r   r   r   r-   P1P2c                      r:   r1   r;   r   )r   r@   r   r   r5      r<   z%test_auto_point_vel.<locals>.<lambda>c                      r:   r1   r;   r   r   r@   r   r   r5      r<   )r   _tr   r   r%   r   r   r   r+   diffr   r   r6   )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sgJ d S )Nr9   r   r   r?   r@   P3r)   )r   rB   r   r   r   r   r%   r   r   r+   rC   )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s<J ||||j ksHJ ||||j
  ||||j
 ks]J d S )Nz
q1, q2, u1r   r?   )r   rB   r   r   r   r   r%   r   r+   rC   r   )rD   r=   r   r4   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 )	Nr9   r   Sr   r?   c                      r:   r1   r;   r   )r   r?   r   r   r5      r<   zRtest_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector.<locals>.<lambda>c                      s
     S r1   r;   r   )r?   rI   r   r   r5      r<   )	r   r   r   r   r   r%   r   r   r6   )r=   r   r   r   )r   r?   rI   r   @test_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector   s   rJ   c               	   C   s  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	
 H t	d tt& |
||| |j ||j  d||  |j  ksJ W d    n1 sw   Y  W d    d S W d    d S 1 sw   Y  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   )rD   r=   r   r4   r7   r   r   r?   r@   rF   rK   r   rL   r   r   r   !test_auto_point_vel_shortest_path   s6   


>"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siJ d S )	Nz	q q1 q2 ur   r   r   r   c                      r:   r1   r;   r   r   r   r   r   r5      r<   z6test_auto_point_vel_connected_frames.<locals>.<lambda>r    )r   rB   r   r   r   r   r%   r   r   r6   Zorientr+   rC   r   )rD   r   r=   r   ur   r   r   rS   r   $test_auto_point_vel_connected_frames   s   J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 d    d S 1 sow   Y  d S )
Nzq ur   r   r   r-   RrM   c                      r:   r1   r;   r   r   r   r   r   r5      r<   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 d    d S 1 sYw   Y  d S )	Nr   r?   r@   rF   r   rM   c                      r:   r1   r;   r   rA   r   r   r5     r<   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.}t	d |
| t|d	 jd
d}t|d	 jts`J d|v sfJ W d    d S 1 sqw   Y  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@   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:}t	d |
|  t|d jd	d
}t|d jtsRJ d|v sXJ d|v s^J d|v sdJ W d    d S 1 sow   Y  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-   re   rf   r   r   r   'test_auto_vel_multiple_path_warning_msg  s"   

"ri   c                  C   sH  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sJ |
|	_	|	||| |	|||j || |j  ksJ d S )Nq1:3zu1:3r	   r/   r   CAmr   BmCm)r   r   orient_axisr   r   r   r   r%   r   	_vel_dictcopyr+   r   r"   )r=   r   r4   r7   r/   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sJ d S )Nr   r   r   )r   r   r   accr   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sGJ d S )Nr=   r   r/   r   r   r   r
   )r   rB   r   ro   r   r   r   r%   r   rt   rC   r   )rD   r=   r   r/   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sJ |||
_|_|||
_
|_
|
|	|| ||
|| |
|	|| ||
|||ksJ d S )Nrj   r	   r
   r/   r   rk   rl   r   rm   rn   )r   r   ro   r   r   r   r%   r   rp   rq   Z	_acc_dictr   rt   r"   r$   )r=   r   r4   r7   v1v2r/   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   :rz   )Zsympy.physics.vectorr   r   r   Zsympy.testing.pytestr   r   rN   r   r   r#   r&   r,   r.   r8   r>   rE   rG   rH   rJ   rR   rU   rX   rY   rg   ri   rs   ru   rv   rz   r   r   r   r   <module>   s2     