o
    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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qdq2dqddq2ddr   r   r   r    r   Y/var/www/vscode/kcb/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:r0   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   r1   r2   u1r   r   <lambda>   s    z-test_point_partial_velocity.<locals>.<lambda>)	r   r   r   r   r   r   r4   r   
ValueError)r   u2r   r5   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>J tt fdd d S )	Nq1 q2r   r   r/   r   c                      
     S r3   r-   r   )r   r/   r   r   r7         
 z test_point_vel.<locals>.<lambda>c                      r<   r3   r=   r   )r   r   r   r   r7      r>   )
r   r   r   r'   r   r   r8   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 )Nr;   r   r   r   r/   P1P2c                      r<   r3   r=   r   )r   rB   r   r   r7      r>   z%test_auto_point_vel.<locals>.<lambda>c                      r<   r3   r=   r   r   rB   r   r   r7      r>   )r   _tr   r   r'   r   r   r   r-   diffr   r   r8   )tr?   r   r   r/   rA   r   )r   r   rB   r   test_auto_point_vel   s   (rG   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 )Nr;   r   r   rA   rB   P3r+   )r   rD   r   r   r   r   r'   r   r   r-   rE   )rF   r?   r   r   r   rA   rB   rH   r   r   r   'test_auto_point_vel_multiple_point_path   s   :rI   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   rA   )r   rD   r   r   r   r   r'   r   r-   rE   r   )rF   r?   r   r6   r   r   rA   r   r   r   test_auto_vel_dont_overwrite   s   (rJ   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   rA   c                      r<   r3   r=   r   )r   rA   r   r   r7      r>   zRtest_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector.<locals>.<lambda>c                      s
     S r3   r=   r   )rA   rK   r   r   r7      r>   )	r   r   r   r   r   r'   r   r   r8   )r?   r   r   r   )r   rA   rK   r   @test_auto_point_vel_if_tree_has_vel_but_inappropriate_pos_vector   s   rL   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   rA   rB   rH   r+   P4r   O1errorr
   )r   rD   r   r   r   r   r'   r   r   warningscatch_warningssimplefilterr   UserWarningr-   rE   )rF   r?   r   r6   r9   r   r   rA   rB   rH   rM   r   rN   r   r   r   !test_auto_point_vel_shortest_path   s6   


>"rT   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<   r3   r=   r   r   r   r   r   r7      r>   z6test_auto_point_vel_connected_frames.<locals>.<lambda>r"   )r   rD   r   r   r   r   r'   r   r   r8   orientr-   rE   r   )rF   r   r?   r   ur   r   r   rU   r   $test_auto_point_vel_connected_frames   s   JrX   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/   RrO   c                      r<   r3   r=   r   r   r   r   r   r7      r>   zCtest_auto_point_vel_multiple_paths_warning_arises.<locals>.<lambda>)r   r   r   r   r   r   r   r'   rP   rQ   rR   r   rS   )r   rW   r   r/   rY   r   rZ   r   1test_auto_point_vel_multiple_paths_warning_arises   s    

"r[   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   rA   rB   rH   r   rO   c                      r<   r3   r=   r   rC   r   r   r7     r>   z5test_auto_vel_cyclic_warning_arises.<locals>.<lambda>)r   r   r   r   r'   r   r   rP   rQ   rR   r   rS   )r   rA   rH   r   rC   r   #test_auto_vel_cyclic_warning_arises   s   

"r\   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   rA   rB   rH   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   rP   rQ   rR   r-   strmessagereplace
issubclasscategoryrS   )r   rA   rB   rH   r   wmsgr   r   r    test_auto_vel_cyclic_warning_msg  s"   

"rj   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/   Tr]   r_   r`   ra   rb   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   rP   rQ   rR   r-   rc   rd   re   rf   rg   rS   )r   r   r   r/   rh   ri   r   r   r   'test_auto_vel_multiple_path_warning_msg  s"   

"rl   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	   r1   r   CAmr   BmCm)r   r   orient_axisr   r   r   r   r'   r   	_vel_dictcopyr-   r   r$   )r?   r   r6   r9   r1   r   rn   ro   rp   rq   tempr   r   r   test_auto_vel_derivative1  s.   
&*rv   c                  C   s8   t d} td}|| d || d| j ksJ d S )Nr   r   r   )r   r   r   accr   rZ   r   r   r   test_auto_point_acc_zero_velK  s   rx   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   r1   r   r   r   r
   )r   rD   r   rr   r   r   r   r'   r   rw   rE   r   )rF   r?   r   r1   r   r   r   r   r   test_auto_point_acc_compute_velQ  s   :ry   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 )Nrm   r	   r
   r1   r   rn   ro   r   rp   rq   )r   r   rr   r   r   r   r'   r   rs   rt   	_acc_dictr   rw   r$   r&   )r?   r   r6   r9   v1v2r1   r   rn   ro   rp   rq   Bm_vel_dictCm_vel_dictBm_acc_dictCm_acc_dictcheckr   r   r   test_auto_acc_derivative^  s6   :r   )sympy.physics.vectorr   r   r   sympy.testing.pytestr   r   rP   r   r!   r%   r(   r.   r0   r:   r@   rG   rI   rJ   rL   rT   rX   r[   r\   rj   rl   rv   rx   ry   r   r   r   r   r   <module>   s2     