
    v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      ZGy	))    )	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    L/DATA/.local/lib/python3.12/site-packages/cadquery/occ_impl/sketch_solver.pyinvalid_argsr.   ;   s    2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   @   sF    !A$1AaD	))1Q4!A$QqT2B+BC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   sX    !A$1AaD1Q4K 000!A$1AaD1Q4K@P9P2PQRRr/   c                     || d d }|S | 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   so    
{rU
 I aD31:AaD1Q4#a&=(!A$1A*>?@Ir/   c                 *    | d d || dd  | d d z
  z  z   S )Nr    )r4   r:   s     r-   
line_pointr@   U   s(    Ra53!AB%!BQ%-(((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   s;    D1JQqT*T!A$ZK#ad),C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   _   sM    D1JQqTAaD[!11D1J;QqTAaD[AQ3Q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
        }|S |dk(  r$t        t        | |      t        ||      z
        }|S t        |      Nr)   r(   )r   r@   r=   r.   r4   r,   rI   r:   r;   s        r-   fixed_point_costrN   i   sf    F{*Q$z"c'::; I 
h)As#iC&889 I 1or/   c                    |dk(  r|dk(  r| dd  }|d d }nc|dk(  r|dk(  r| dd  }t        |      }nH|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&Lb\V	xB(Nb\r]2r""R=r/   c                 z   |dk(  r(|dk(  r#t        | dd  | d d z
   }t        |dd  |d d z
   }n{|dk(  r"|dk(  rt        | dd  | d d z
   }t        |      }nT|dk(  r"|dk(  rt        |       }t        |dd  |d d z
   }n-|dk(  r|dk(  rt        |       }t        |      }nt        ||      |j	                  |      |z
  S rP   )r   rD   rF   r.   r"   rQ   s	            r-   
angle_costr\      s    	V|f12BQ)12BQ)	v".12BQ)r"	xB&Lb!12BQ)	xB(Nb!r"2r""88B<#r/   c                     d}|dk(  rt        | dd  | d d z
        |z
  }|S |dk(  rt        | d   | d   z        |z
  }|S t        |      )Nr   r)   r   r(   r7   )r   r.   rM   s        r-   length_costr^      sk    	
BF{!AB%!BQ%- 3& I 
h!A$1+$ I 1or/   c                 `   |\  }}}	|dk(  r|dk(  rt        | |      }
t        ||      }nu|dk(  r|dk(  rt        | |      }
t        ||      }nR|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&Lr4 D!	xB(Nr4 r4 2r""R=1r/   c                 6    |dk(  r
| d   |z
  }|S t        |      )Nr(   r   r.   rM   s        r-   radius_costrf      )    H}qTCZ I 1or/   c                 r    |dk(  r(t        | dd  | d d z
   j                  t        |       }|S t        |      )Nr)   r   )r   r"   r.   rM   s        r-   orientation_costri      sE    F{!""1'--hn= I 1or/   c                 6    |dk(  r
| d   |z
  }|S t        |      )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y
)SketchConstraintSolverentitiesconstraintsgeomsnencixsc                 (   t        |      | _        t        |      | _        t        |      | _        t	        | j                        | _        t	        | j                        | _        dgt        t        d | j                  D                    z   | _        y )Nr   c              3   2   K   | ]  }t        |        y wrH   )len).0es     r-   	<genexpr>z2SketchConstraintSolver.__init__.<locals>.<genexpr>  s     (GAQs   )	listro   rp   rq   rw   rr   rs   r   rt   )selfro   rp   rq   s       r-   __init__zSketchConstraintSolver.__init__   sm     X,%[
dmm$d&&'
 3j(G(GGHHr/   rI   returnNc                   	
 | j                   | j                  	| j                  
t        | j                        D cg c]  }||   |dz        c}dt
        f	
fd}d		
fd}t        d   t               }t        d   t              }t        
      D ]  \  }}|dk(  sd||   dz   <    ||||fS c c}w )
Nr   r~   c                     d}t        	      D ]X  \  }\  \  }}}}t        |   }| |   |dz       
|   |   g}||| |   |dz       
|   |   gz  }| |g || dz  z  }Z |S )z7
            Cost function to be minimized
            g        r   r   )	enumeraterl   )r4   r;   ie1e2kindr:   costargsrp   rq   rt   x0ss            r-   fz'SketchConstraintSolver._cost.<locals>.f  s    
 B,5k,B((HRdCT{ #b'CQK0%)SWE>Qs2wR!V5uRy#b'JJD d&D&#&!++ -C Ir/   c           	         d|dd t              D ]N  \  }\  \  }}}}t        |   }| |   |dz       }|j                         |   |   g}	|*| |   |dz       }
|	|
j                         |   |   gz  }	 |g |	| }t        t        |   |dz                  D ]K  \  }}|	d   |xx   t        z  cc<    |g |	| }||xx   d|z  ||z
  z  t        z  z  cc<   ||   |	d   |<   M |t        t        |   |dz                  D ]K  \  }}|	d   |xx   t        z  cc<    |g |	| }||xx   d|z  ||z
  z  t        z  z  cc<   
|   |	d   |<   M Q y)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   
 BqE,5k,B((HRdCT{ s2wR!V-	59c"g6>3r7Sa[1BRWWYb	3r7;;D &D&#&%eCGSa[&ABDAqGAJ(*J++s+DqEQWs
3h>>E!#ADGAJ	 C > )%BR!V*E F1Q
h.
#/T/3/1SD3J!7(!BB%'UQ
	 !G+ -Cr/   r(   r   r   )r~   N)	rt   rp   rq   r   rr   floatr   r   r   )r|   rI   ry   r   r   lbubr   grp   rq   rt   r   s            @@@@r-   _costzSketchConstraintSolver._cost	  s     hh&&

 16dgg?1r#a&3q1u:&?	E 	 	* 	+  	+F #b'C4 #b'C4 e$DAqH}!"3q6A: % $BC @s    Cc                 N   t        t        t        j                  | j                                    j                         }| j                  |      \  }}fd}t        j                  t        j                  t        |            }|j                  |       |j                  |       |j                  |       |j                  d       |j                  d       |j!                  t"               |j%                  t"        dz         |j'                  t(               |j+                  |      }| j                  |j-                         |j/                         |j1                         d}| j2                  }t5        ||dd        D 	
cg c]
  \  }	}
||	|
  c}
}	|fS c c}
}	w )Nc                 D    |j                   dkD  r	 | |        |       S )Nr   )size)r4   r   r   r   s     r-   funcz*SketchConstraintSolver.solve.<locals>.func_  s     vvzQ
Q4Kr/   r   gMbP?)ro   r   itersstatusr   )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   r4   r   rt   i1i2r   r   s              @@r-   solvezSketchConstraintSolver.solveZ  sQ   4++DMM:;<BBD**R.4R	 iiB0d#R R t$ LL**,%%'..0	
 hh'*3AB'89'8VR"R'896AA9s   F!)__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!!;GG	cNI3-I j)I 	I&O,O	%,&'%,e-t34ee	
Ob!BuXhuo6S#XFG !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  ) " ' " " ( (     :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
E
S
)
E
S

	((, *.

 
*tC#u*%%& 
NBV NBr/   