
    ,jv$                        U d dl mZmZmZmZmZmZmZmZm	Z	 d dl m
Z
 d dlmZ d dlmZ d dlmZmZ d dl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Zd d
lm Z  ddl!m"Z" ddl#m$Z$  e%d	          Z&ee'e'e'e'f         Z(ee'e'e'e'e'f         Z)ee(e)f         Z*e
d         Z+dde&d	fddee$         d	fdde&d	fdde$efdde$d	fddeee$         ee$         e$f         d	fdde$d	fddee$e$f         d	fdde$efd	Z,eee-ee-         f         e+ee         f         Z.dZ/dZ0d Z1d Z2d Z3d Z4d Z5d Z6d Z7d Z8d Z9d Z:d Z;d Z<d  Z=d! Z>d" Z?d# Z@d$ ZA eBe9e:e;e<e=e>e?e@eA	  	        ZCeeDed%e'f         f         eEd&<    G d' d(eF          ZGd	S ))    )	TupleUnionAnyCallableListOptionalIterableDictSequence)Literal)NDArray)float64)
accumulatechain)sincosradians)arrayfullinfsignnormN)gp_Vec2d   )Geoms   )Real)	Fixed
FixedPoint
CoincidentAngleLengthDistanceRadiusOrientationArcAngle)CIRCLELINE)r(   )r)   g|=g&.>c                       t          d          S )NzInvalid argument types {t})
ValueError)ts    Y/DATA/AppData/hermes/venv/lib/python3.11/site-packages/cadquery/occ_impl/sketch_solver.pyinvalid_argsr.   ;   s    2333    c           	          t          | d         | d         t          | d                   z  z   | d         | d         t          | d                   z  z   f          S )Nr   r      r   r   r   r   xs    r-   	arc_firstr5   @   sM    !A$1AaD		))1Q4!A$QqT2B+BCDDDr/   c           
          t          | d         | d         t          | d         | d         z             z  z   | d         | d         t          | d         | d         z             z  z   f          S )Nr   r   r1      r   r2   r3   s    r-   arc_lastr8   E   sa    !A$1AaD1Q4K 0 000!A$1AaD1Q4K@P@P9P2PQRRRr/   c                     || d d         }nc| d         || d         z  z   }t          | d         | d         t          |          z  z   | d         | d         t          |          z  z   f          }|S )Nr   r1   r7   r   r   r2   )r4   valrvas       r-   	arc_pointr=   J   sq    
{rrUaD31:AaD1Q4#a&&=(!A$1A*>?@@Ir/   c                 H    | d d         || dd          | d d         z
  z  z   S )Nr    )r4   r:   s     r-   
line_pointr@   U   s.    RaR53!ABB%!BQB%-(((r/   c                     t          t          | d                   t          | d                   z  t          | d                    t          | d                   z            S Nr7   r1   r   r   r   r   r3   s    r-   arc_first_tangentrD   Z   sF    D1JJQqT*T!A$ZZK#ad)),CDDDr/   c           	          t          t          | d                   t          | d         | d         z             z  t          | d                    t          | d         | d         z             z            S rB   rC   r3   s    r-   arc_last_tangentrF   _   sZ    D1JJQqTAaD[!1!11D1JJ;QqTAaD[AQAQ3QRRRr/   c                 &    t          | |z
            S Nr   )r4   r,   x0r:   s       r-   
fixed_costrJ   d   s    B<<r/   c                     |dk    r/t          t          | |          t          ||          z
            }nD|dk    r/t          t          | |          t          ||          z
            }nt          |          |S Nr)   r(   )r   r@   r=   r.   r4   r,   rI   r:   r;   s        r-   fixed_point_costrN   i   su    F{{*Q$$z"c':'::;;	
h)As##iC&8&88991ooIr/   c                 v   |dk    r|dk    r| dd          }|d d         }n|dk    r |dk    r| dd          }t          |          }na|dk    r |dk    rt          |           }|d d         }n;|dk    r%|dk    rt          |           }t          |          }nt          ||          t          ||z
            S Nr)   r   r(   )r5   r8   r.   r   	x1t1x10x2t2x20r:   v1v2s	            r-   coincident_costrZ   u   s    	V||fVV	v"..Vr]]	xB&LLb\\V	xB(NNb\\r]]2r"""R==r/   c                    |dk    r?|dk    r9t          | dd          | d d         z
   }t          |dd          |d d         z
   }n|dk    r2|dk    r,t          | dd          | d d         z
   }t          |          }ns|dk    r2|dk    r,t          |           }t          |dd          |d d         z
   }n;|dk    r%|dk    rt          |           }t          |          }nt          ||          |                    |          |z
  S rP   )r   rD   rF   r.   r"   rQ   s	            r-   
angle_costr\      s   	V||f122BQB)122BQB)	v"..122BQB)r""	xB&LLb!!122BQB)	xB(NNb!!r""2r"""88B<<#r/   c                     d}|dk    r&t          | dd          | d d         z
            |z
  }n7|dk    r"t          | d         | d         z            |z
  }nt          |          |S )Nr   r)   r   r(   r7   )r   r.   rM   s        r-   length_costr^      sr    	
BF{{!ABB%!BQB%-  3&	
h!A$1+$1ooIr/   c                    |\  }}}	|dk    r'|dk    r!t          | |          }
t          ||          }n|dk    r'|dk    r!t          | |          }
t          ||          }nj|dk    r'|dk    r!t          | |          }
t          ||          }n=|dk    r'|dk    r!t          | |          }
t          ||          }nt          ||          t          |
|z
            |	z
  S rL   )r@   r=   r.   r   )rR   rS   rT   rU   rV   rW   r:   val1val2drX   rY   s               r-   distance_costrc      s    MD$	V||fD!!D!!	v"..D!!r4  	xB&LLr4  D!!	xB(NNr4  r4  2r"""R==1r/   c                 H    |dk    r| d         |z
  }nt          |          |S )Nr(   r   r.   rM   s        r-   radius_costrf      +    H}}qTCZ1ooIr/   c                     |dk    r7t          | dd          | d d         z
                       t          |           }nt          |          |S )Nr)   r   )r   r"   r.   rM   s        r-   orientation_costri      sN    F{{!"""1"'--hn==1ooIr/   c                 H    |dk    r| d         |z
  }nt          |          |S )Nr(   r7   re   rM   s        r-   arc_angle_costrk      rg   r/   .costsc            	          e Zd ZU ee         ed<   ee         ed<   ee         ed<   eed<   eed<   ee         ed<   de	e         de	e         de	e         fdZ
dee         d	eeee         gef         eee         ee         gd
f         ee         ee         f         fdZd	eeee                  eeef         f         fdZd
S )SketchConstraintSolverentitiesconstraintsgeomsnencixsc                 J   t          |          | _        t          |          | _        t          |          | _        t	          | j                  | _        t	          | j                  | _        dgt          t          d | j        D                                 z   | _        d S )Nr   c              3   4   K   | ]}t          |          V  d S rH   )len).0es     r-   	<genexpr>z2SketchConstraintSolver.__init__.<locals>.<genexpr>  s(      (G(GAQ(G(G(G(G(G(Gr/   )	listro   rp   rq   rw   rr   rs   r   rt   )selfro   rp   rq   s       r-   __init__zSketchConstraintSolver.__init__   s     X,,%[[
dm$$d&''
 3j(G(G(G(G(GGGHHHr/   rI   returnNc                 x  	
 | j         
| j        | j        	
fdt          | j                  D             dt
          f	
fd}d		
fd}t          
d         t                     }t          
d         t          
           }t          	          D ]\  }}|dk    rd|
|         dz   <   ||||fS )
Nc                 B    g | ]}|         |d z                     S )r   r?   )rx   ry   rt   rI   s     r-   
<listcomp>z0SketchConstraintSolver._cost.<locals>.<listcomp>  s/    ???1r#a&3q1u:%&???r/   r~   c                 (   d}t          	          D ]~\  }\  \  }}}}t          |         }| |         |dz                     
|         |         g}|+|| |         |dz                     
|         |         gz  }| |g ||R  dz  z  }|S )z7
            Cost function to be minimized
            g        r   Nr   )	enumeraterl   )r4   r;   ie1e2kindr:   costargsrp   rq   rt   x0ss            r-   fz'SketchConstraintSolver._cost.<locals>.f  s    
 B,5k,B,B 
, 
,((HRdCT{ #b'CQK/0%)SWE>Qs2wR!V45uRy#b'JJD dd&D&#&&&!++Ir/   c           	         d|dd<   t                    D ]\  }\  \  }}}}t          |         }| |         |dz                     }|                                |         |         g}	|?| |         |dz                     }
|	|
                                |         |         gz  }	 |g |	|R  }t          t          |         |dz                                D ]\\  }}|	d         |xx         t          z  cc<    |g |	|R  }||xx         d|z  ||z
  z  t          z  z  cc<   ||         |	d         |<   ]|t          t          |         |dz                                D ]\\  }}|	d         |xx         t          z  cc<    |g |	|R  }||xx         d|z  ||z
  z  t          z  z  cc<   |
|         |	d         |<   ]dS )z7
            Gradient of the cost function
            r   Nr   r   r1   )r   rl   copyrangeDIFF_EPS)r4   r;   r   r   r   r   r:   r   rR   r   rU   tmpjktmp1tmp2rp   rq   rt   r   s                   r-   gradz*SketchConstraintSolver._cost.<locals>.grad.  s(   
 BqqqE,5k,B,B + +((HRdCT{ s2wR!V,-		59c"g6>3r7Sa[01BRWWYYb	3r7;;D d&D&#&&&%eCGSa[&A&ABB ' 'DAqGAJJJ(*JJJ4++s+++DqEEEQWs
3h>>EEE!#ADGAJJ> )%BR!V*E*E F F + +1Q


h.


#t/T/3///1SD3J!7(!BB%'UQ

3+ +r/   r(   r   r   )r~   N)	rt   rp   rq   r   rr   floatr   r   r   )r|   rI   r   r   lbubr   grp   rq   rt   r   s    `      @@@@r-   _costzSketchConstraintSolver._cost	  s    h&
 @????dg???	E 	 	 	 	 	 	 	 	 	* 	+  	+  	+  	+  	+  	+  	+  	+  	+F #b'C4  #b'C4  e$$ 	# 	#DAqH}}!"3q6A:$Br/   c                   	
 t          t          t          j        | j                                                                      }|                     |          \  	}}	fd}t          j        t          j	        t          |                    }|                    |           |                    |           |                    |           |                    d           |                    d           |                    t"                     |                    t"          dz             |                    t(                     |                    |          
| j        |                                |                                |                                d}| j        }
fdt5          ||dd                    D             |fS )Nc                 H    |j         dk    r | |            |           S )Nr   )size)r4   r   r   r   s     r-   funcz*SketchConstraintSolver.solve.<locals>.func_  s*    vzzQ


1Q44Kr/   r   gMbP?)ro   r   itersstatusc                 *    g | ]\  }}||         S r?   r?   )rx   i1i2r4   s      r-   r   z0SketchConstraintSolver.solve.<locals>.<listcomp>{  s%    999VR"R%999r/   r   )r   r{   r   from_iterablero   ravelr   nloptoptLD_SLSQPrw   set_min_objectiveset_lower_boundsset_upper_boundsset_ftol_absset_ftol_relset_xtol_relTOLset_xtol_absset_maxevalMAXITERoptimizelast_optimum_valueget_numevalslast_optimize_resultrt   zip)r|   rI   r   r   r   r   r   rt   r   r   r4   s           @@@r-   solvezSketchConstraintSolver.solveZ  s   4+DM::;;<<BBDD**R..4R	 	 	 	 	 	 iB00d###R   R   t$$$   LL**,,%%''..00	
 
 h9999s3ABB'8'89996AAr/   )__name__
__module____qualname__r   DOF__annotations__
Constraintr   intr	   r}   ArrayFloatr   r   r   r   r   r
   strr   r   r?   r/   r-   rn   rn      s[        3ij!!!!;GGGGGG	cNNNI3-I j)I 	I I I I&O,O	%,&'%,e-t34ee	
O O O Ob!BuXhuo6S#XFG !B !B !B !B !B !Br/   rn   )Htypingr   r   r   r   r   r   r	   r
   r   r   numpy.typingr   r   numpyr   r   	itertoolsr   r   mathr   r   r   r   r   r   r   numpy.linalgr   r   OCP.gpr   shapesr   typesr   typeNoneTyper   
SegmentDOFArcDOFr   ConstraintKindConstraintInvariantsr   r   r   r   r   r.   r5   r8   r=   r@   rD   rF   rJ   rN   rZ   r\   r^   rc   rf   ri   rk   dictrl   r   r   objectrn   r?   r/   r-   <module>r      s   X X X X X X X X X X X X X X X X X X X X X X X       ) ) ) ) ) ) " " " " " " ' ' ' ' ' ' ' ' " " " " " " " " " " ( ( ( ( ( ( ( ( ( ( ( (                         4::5%-.
	ueUE50	1J
 #Xt4((4.$?((D9#T73$dD1	htnhtnd23	 +tT*y%d
"3T:Kw/  " 5hsm+,nhsmKL


4 4 4
E E E
S S S
  ) ) )
E E E
S S S
  
	 	 	  (  (    ,       *.

 
* 
* 
*tC#u*%%& 
 
 
NB NB NB NB NBV NB NB NB NB NBr/   