
    vjF                     L   U d dl mZmZmZmZmZmZmZmZm	Z
mZmZ d dlmZmZ d dlZd dl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 m!Z!m"Z" dd	l#m$Z$m%Z%m&Z&m'Z' d
dl(m)Z) d
dl*m+Z+  e,d      Z-eee.e.e.f   ee.e.e.f   f   Z/eeeeedf   Z0ed   Z1ed   Z2ed   Z3d
eefe)dfd
eefe)d fd
eefe)dfd
eefe)dfdded   dfdefee)e)e)f   dfdefee)e)e)f   dfddee)e)e)f   d fdZ4ddd fiZ5ee3eee3df   eegeedf   f   f   f   e6d<   eee0df   e3ee   f   Z7dZ8dZ9dZ:dZ;dZ<dZ=dZ> G d d e?      Z@d! ZAd" ZBd# ZC	 	 d7d$ed%ed&ee.   d'e.d(e.f
d)ZD	 	 d7d$ed%ed&ee.   d'e.d(e.f
d*ZE	 	 d7d$ed%ed&ee.   d'e.d(e.f
d+ZF	 	 d7d$ed%ed&ee.   d'e.d(e.f
d,ZG	 	 d7d$ed   d&eed      d'e.fd-ZH	 d8d$ed&ee.e.e.f   d'e.fd.ZI	 d8d$ed&ee.e.e.f   d'e.fd/ZJ	 d8d$ed   d&ee.e.e.f   d'e.fd0ZK eLeDeEeFeGeHeIeJeK      ZMeeNede.f   f   e6d1<    eLd2d3d2d2d3d2d3d3      ZOeeNePf   e6d4<    G d5 d6e?      ZQy)9    )ListTupleUnionAnyCallableOptionalDictLiteralcastType
get_origin)radianspiN)gp_Vecgp_Plngp_Dirgp_Pntgp_Trsfgp_Quaterniongp_Lingp_Extrinsic_XYZ)	BRepTools)	Precision   )LocationVectorPlane)ShapeFaceEdgeWire   )Real)instance_of)Fixed
FixedPoint	FixedAxisFixedRotation)r   PointAxisPointInPlanePointOnLine)	r   r)   r*   r+   r%   r&   r'   r,   r(   c                      | t        |       S d S Nr   xs    E/DATA/.local/lib/python3.12/site-packages/cadquery/occ_impl/solver.py<lambda>r3   B   s    '!*747    r.   c                 4    t        t        t        |             S r.   )tuplemapr   r0   s    r2   r3   r3   M   s    %GQ(r4   )r)   r*   r+   r,   r%   r&   r'   r(   r   )r*   r)   c                 (    | t        |       dfS d dfS )Nr   r/   r0   s    r2   r3   r3   U   s    !-GAJST+UTST+Ur4   .CompoundConstraints      g      Y@g|=-q=i  c                      e Zd ZU dZeedf   ed<   eedf   ed<   eedf   ed<   e	ed<   e
ed<   	 ddeedf   deedf   deedf   de	de
f
d	Zdeedf   de	de
fd
ZdedefdZdedefdZdedefdZdedefdZdeedf   fdZy)ConstraintSpeczQ
    Geometrical constraint specification between two shapes of an assembly.
    .objectsargssublocskindparamNc                 b   t        |t              st        d| d      |t        v r:t        |   \  }}t	        | ||            D ]  \  }}	| j                  |||	        n+| j                  |||       t        |   d   }
|
r |
|      n|}|| _        || _        || _	        || _
        || _        y)a  
        Construct a constraint.

        :param objects: object names referenced in the constraint
        :param args: subshapes (e.g. faces or edges) of the objects
        :param sublocs: locations of the objects (only relevant if the objects are nested in a sub-assembly)
        :param kind: constraint kind
        :param param: optional arbitrary parameter passed to the solver
        zUnknown constraint .N)r$   ConstraintKind
ValueErrorr9   zip	_validateConstraintInvariantsr?   r@   rA   rB   rC   )selfr?   r@   rA   rB   rC   kindsconvert_compoundkpconverts              r2   __init__zConstraintSpec.__init__s   s    & 4024&:;;&&&9$&?#E#E#3E#:;1tQ* < NN4u- +404G&-GEN5E 		
r4   c           
      l   t         |   \  }}}}|t        |      k7  rt        d| dt        |       d| d      t        | j                  t
        | j                  t        | j                  t        | j                  d d i}t        |t        t        t        t           df   |            D ]  \  }	}
	  ||
   |	        t!        ||      s|t        d	t#        |       d| d      	 ||r	 ||       y y y # t        $ r t        d|	 d| d      w xY w# t$        $ r}t        d
| d      d }~ww xY w)Nz*Invalid number of entities for constraint z. Provided z, required rE   c                      y)NT r0   s    r2   r3   z*ConstraintSpec._validate.<locals>.<lambda>   s    Dr4   .zUnsupported entity z for constraint zUnsupported argument types z
Exception z$ occured in the parameter conversion)rK   lenrH   r   _getPntr   _getAxisr   _getPlnr   _getLinrI   tcastr   r   ConstraintMarkerr$   r   	Exception)rL   r@   rB   rC   aritymarker_types
param_type	converter	arg_checkates               r2   rJ   zConstraintSpec._validate   sq   5I$5O2|Z CI<TF+cRVi[Xcdicjjkl  DLLDMMDLLDLL.8
	 eE$/?*@#*E$FUVDAqS	!Q W 5*-%2C-j.?-@J<WXY 
	S Y%  &/   S #6qc9I$q!QRRS  	Sz!,PQRR	Ss$   1C7(D 7D	D3D..D3argreturnc                    t        |t              r |j                         }|j                         S t        |t              r3|j	                         dk7  r |j                         }|j                         S t        |t              r3|j	                         dk(  r |j                         }|j                         S t        d|       )NCIRCLEzCannot construct Axis for )	
isinstancer   normalAtr    geomType	tangentAtnormalrH   toDir)rL   rf   rvs      r2   rX   zConstraintSpec._getAxis   s    c4 B xxz T"s||~'AB xxz T"s||~'AB xxz 9#?@@r4   c                 \   t        |t              r9t        | j                  |      |j	                         j                               }|S t        |t        t        f      r?|j                         }|j                         }t        ||      }|j                         }|S t        d| d      )N)rn   Cannot construct a plane for rE   )rj   r   r   rW   rk   ro   r    r!   rn   Centerr   toPlnrH   )rL   rf   rp   rn   originplanes         r2   rY   zConstraintSpec._getPln   s    c4 S)3<<>+?+?+ABB 	 dD\*ZZ\FZZ\F&0EB 	 <SECDDr4   c                 2   t        |t              rht        d t        j                  |j
                        D              r9|j                         }t        |j                               }|j                         S |j                         }|j                         S )Nc              3   F   K   | ]  }t        j                  |        y wr.   )r   IsInfinite_s).0r1   s     r2   	<genexpr>z)ConstraintSpec._getPnt.<locals>.<genexpr>   s       )
/P!I""1%/Ps   !)rj   r   anyr   
UVBounds_swrappedrt   r   r   rs   toPnt)rL   rf   plncenters       r2   rW   zConstraintSpec._getPnt   su     c4 S )
/8/C/CCKK/P)
 &
 ))+CCLLN+F ||~ ZZ\F||~r4   c                     t        |t        t        f      r!|j                         }|j	                         }nt        d| d      t        |j                         |j                               S )Nrr   rE   )	rj   r    r!   rs   rm   rH   r   r   ro   )rL   rf   r   tangents       r2   rZ   zConstraintSpec._getLin   sT    cD$<(ZZ\FmmoG<SECDDfllngmmo66r4   c                    t        d t        | j                  | j                        D              }| j                  dk(  r+| j                  |d         | j                  |d         fg}n| j                  dk(  r+| j                  |d         | j                  |d         fg}nq| j                  dk(  rR| j                  |d         | j                  |d         f| j                  |d         | j                  |d         fg}n| j                  dk(  r*| j                  |d         | j                  |d         fg}n| j                  dk(  r*| j                  |d         | j                  |d         fg}n| j                  d	k(  rd
g}n| j                  dk(  r| j                  |d         fg}ne| j                  dk(  r| j                  |d         fg}n?| j                  dk(  rg d}n+| j                  dk(  rd
g}nt        d| j                         | j                  t        v r)t        | j                     \  }} || j                        }n| j                  f}| j                  f}t        t        |||            S )z
        Convert the constraint to a representation used by the solver.

        NB: Compound constraints are decomposed into simple ones.
        c              3   f   K   | ])  \  }}|j                  ||j                         z         + y wr.   )locatedlocation)rz   rf   locs      r2   r{   z(ConstraintSpec.toPODs.<locals>.<genexpr>   s0      
8S KKclln,-8s   /1r*   r   r   r)   r   r+   r,   r%   r.   r&   r'   r(   )r.   r.   r.   FixedRotationAxiszUnknown constraint kind )r6   rI   r@   rA   rB   rX   rW   rY   rZ   rH   r9   rC   )rL   r@   markersrM   ra   paramss         r2   toPODszConstraintSpec.toPODs   sF     
		4<<8
 
 99d1g.d1g0FHIGYY'!T!W-t||DG/DEFGYY'!tAw'tAw)?Ad1g&T!W(=>G
 YY.(T!W-t||DG/DEFGYY-'T!W-t||DG/DEFGYY'!iGYY,&T!W-/0GYY+%d1g.01GYY/)1GYY--iG 7		{CDD 99++2499=E9tzz+FYYLEjj]F S%011r4   r.   )__name__
__module____qualname____doc__r   str__annotations__r   r   rG   r   rR   rJ   r   rX   r   rY   r   rW   r   rZ   
Constraintr   rU   r4   r2   r>   r>   h   s    38_
s

8S=!!
J &sCx& E3J& x}%	&
 & &P$SeE3J/ $S~ $Sc $SLE f 5 V 5 V 75 7V 7=2j#o. =2r4   r>   c                 `    t        j                  |       }d| z  d|z   z  }d|z
  d|z   z  }||fS Nr"   r   )casumsqr)Rmuss       r2   
Quaternionr   5  s;    
		!A	AQA	
Q1q5Aa4Kr4   c                     t        |      \  }}dt        j                  ||       z  |z  |dz  t        j                  ||      z
  | z  z   d|z  t        j                  ||       z  z   S )Nr"   )r   r   dotcross)vr   r   r   s       r2   Rotater   ?  s`    a=DAqrvva|a16BFF1aL#8A"==AQRTU@VVVr4   c                      t        | |      |z   S r.   )r   )r   Tr   s      r2   	Transformr   F  s    !Q<!r4   m1m2valscalerg   c                    |dn|}t        j                  |j                         |j                         |j	                         f      }t        j                  |j                         |j                         |j	                         f      }t        |||z   ||z         t        |||	z   ||
z         z
  |z  }|dk(  rt        j                  |      S t        j                  |      ||z  dz  z
  dz  S Nr   r"   r   DMXYZr   r   )problemr   r   T1_0R1_0T2_0R2_0T1R1T2R2r   r   m1_dmm2_dmdummys                   r2   
point_costr   K  s      {!CEE244624462446*+EEE244624462446*+E 	%D2I.5$)TTVY1WWE axyyIIee11a77r4   c                 @   |t         n|}t        j                  |j                         |j	                         |j                         f      }t        j                  |j                         |j	                         |j                         f      }t        |||z         t        |||
z         }}|dk(  r||z
  }t        j                  |      S |t         k(  r||z   }t        j                  |      S t        j                  ||      t        j                  |      z
  }|dz  S r   )
r   r   r   r   r   r   r   r   r   cos)r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   d1d2r   s                     r2   	axis_costr   j  s      "CEE244624462446*+EEE244624462446*+EUD2I&udRi(@B
axRyy	RyyFF2rNRVVC[(EA:r4   c           
         |dn|}t        j                  |j                         |j                         |j	                         f      }|j                         j                         }|j                         j                         j                  |t        |      z        }t        j                  |j                         |j                         |j	                         f      }t        j                  |j                         |j                         |j	                         f      }t        j                  t        |||
z         t        |||	z   ||
z         t        |||z   ||z         z
        |z  }|dz  S r   )r   r   r   r   r   r*   	Directionr   
Translatedr   r   r   r   )r   r   r   r   r   r   r   r   r   r   r   r   r   r   m2_dirm2_pnt	m2_dir_dm	m2_pnt_dmr   s                      r2   point_in_plane_costr     s$     {!CEE244624462446*+EWWY  "FWWY!,,S6&>-ABFvxxz688:vxxz:;Ivxxz688:vxxz:;I 	9dRi(iD2I6tby$)45	

 	 
 A:r4   c                    |dn|}t        j                  |j                         |j                         |j	                         f      }|j                         }|j                         }t        j                  |j                         |j                         |j	                         f      }t        j                  |j                         |j                         |j	                         f      }t        |||z   ||z         t        |||	z   ||
z         z
  }t        |||
z         }||t        j                  ||      z  z
  |z  }|dk(  rt        j                  |      S t        j                  |      |z
  dz  S r   )r   r   r   r   r   r   r   r   r   r   r   )r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   dnr   s                        r2   point_on_line_costr     s*     {!CEE244624462446*+E\\^F[[]Fvxxz688:vxxz:;Ivxxz688:vxxz:;I%D2I.4"9dRi2 	A 	y$)$ARVVAq\!!U*E
axyyIIes"q((r4   c                      y r.   rU   )r   r   r   r   r   r   r   r   s           r2   
fixed_costr     s     r4   c                    t        j                  |j                         |j                         |j	                         f      }t        |||z   ||z         t        j                  |      z
  |z  }	t        j                  |	      S r.   r   )
r   r   r   r   r   r   r   r   r   r   s
             r2   fixed_point_costr     sb     EE244624462446*+EudRi3beeCj@EIE99Ur4   c                 P   t        j                  |j                         |j                         |j	                         f      }t        j                  |      t        j
                  t        j                  |            z  }	t        |||z         |	z
  }
t        j                  |
      S r.   )r   r   r   r   r   norm_2r   r   )r   r   r   r   r   r   r   r   r   m_valr   s              r2   fixed_axis_costr     sr     EE244624462446*+EEE#J255:..E5$)$u,E99Ur4   c           	      Z   t               } |j                  t        g|  t        j                  |j                         |j                         |j                         |j                         f      }	dt        j                  t        j                  t        ||z          |	      dz  z
  }
|
S Nr   r"   )r   SetEulerAnglesr   r   r   Wr   r   r   r   vertcatr   )r   r   r   r   r   r   r   r   qq_dmr   s              r2   fixed_rotation_costr     s     	AA%,,55!##%qssu-.Drzz:dRi#894@AEEELr4   costsTFscalingc            	          e Zd ZU ej                  ed<   eeej                  ej                  f      ed<   eeej                  ej                  f      ed<   eeee	df   e
f      ed<   ee	   ed<   e	ed<   e	ed<   eed	<   g d
fdee   deeee	df   e
f      dee	   d	efdZededefd       Zdej                  dej                  defdZdde	deee   eeef   f   fdZy)ConstraintSolveropti	variablesstarting_points.constraintslockednencr   r   entitiesc                    || _         t        j                         x| _        }t	        |      D cg c]`  \  }}||vr-||j                  t              z  |j                  t              fn)|j                  t              |j                  t              fb c}}| _	        |D cg c],  }|j                  t              |j                  t              f. c}| _
        t	        t        | j                  | j                  |            D ]  \  }\  \  }}	\  }
}}| j                  |      \  }}|j                  |
|       |j                  ||       ||v r%|j                  |d       |j                  |	d       q|j                  |d       |j                  |	d        || _        t!        |      | _        || _        t!        | j                        | _        y c c}}w c c}w )N)r   r   r   )        r   r   ){Gz?r   r   )r   r   Optir   	enumeratevariableNDOF_VNDOF_Q	parameterr   start_pointsrI   
_locToDOF6	set_valueset_initialr   rV   r   r   r   )rL   r   r   r   r   r   i_r   r   T0R0r   T0valR0vals                  r2   rR   zConstraintSolver.__init__J  s    
779$	D
 "(+	
 ,1  T]]6**DMM&,AB..($..*@AB ,	
 GO
FNT^^F#T^^F%;<h

 +4 1 18<+
&A&AR#  ??3/LE5NN2u%NN2u%F{q),q),  O4  $67+
 ' h-d&&'?

s   A%G#1Gr   rg   c                    | j                   j                         }|j                         }|j                         }d|j	                         z
  d|j	                         z   z  }|dz   |j                         z  dz  }|dz   |j                         z  dz  }|dz   |j                         z  dz  }|j                         |j                         |j                         f|||ffS r   )r~   TransformationTranslationPartGetRotationr   r   r   r   )r   Trr   r   alpha_2rc   bcs           r2   r   zConstraintSolver._locToDOF6u  s     [['') NNqssu9QSSU+q[ACCE!A%q[ACCE!A%q[ACCE!A%qssuacce$q!Qi//r4   r   r   c           
      L   | j                   }t               }|j                  |      \  }}}|dz  |dz  z   |dz  z   }|j                  t	        d|z  |dz   z  d|z  |dz   z  d|z  |dz   z  d|z
  |dz   z               |j                  t        |j                  |              |S r   )r   r   valueSetRotationr   SetTranslationPartr   )	rL   r   r   r   rp   rc   r  r  r   s	            r2   _build_transformz!ConstraintSolver._build_transform  s    yyY**Q-1aFQ!VOa1f$
AQQ!a%!a%1q5/AEaRSeCT	

 	fdjjm45	r4   	verbosityc                 N   |dk(  rdnd}| j                   }| j                  }| j                  }| j                  }d}|D ]>  \  }}	|t	        j
                  t	        j                  || j                  z  |	            z  }@ d}
|D ]q  \  }\  }}}g }g }|D ]*  }|j                  ||          |j                  ||          , t        |   |g||||dt        |   r| j                  ndi}|m|
|z  }
s |j                  |
d|z  z          |j                  dd	d
iddddddd||ddd       |j                         }|j                         }||d<   t        ||      D 	cg c],  \  \  }}	\  }}t!        | j#                  ||z   |	|z               . }}}	}}||fS c c}}}	}w )Nr   yesnor   r   r   gؗҜ<ipopt
print_timeFr<   g+=exactnonemumps)acceptable_obj_change_tolacceptable_itertolhessian_approximationnlp_scaling_methodhonor_original_boundsbound_relax_factorprint_levelsbprint_timing_statisticslinear_solverr   )r   r   r   r   r   r   r   r   extendr   r   minimizesolversolve_limitedstatsrI   r   r	  )rL   r
  suppress_bannerr   r   r   r   penaltyr   r   	objectiveksmsrB   r   s_ksv_ksrO   r  solresultr   r   locss                           r2   solvezConstraintSolver.solve  s   #,>%tyy&&NN	(( DAqryyA

NA!>??G  	&1"B"T6 !#D "DLO,IaL)  d  	
  %,DMdjjqA }Q	) '2, 	i%'/12 	5!-2#$)0&,).&'(%+/!(	
"   "v %(	<$@
$@ AR T**1r61r6:;$@ 	 

 V|
s   %1F
N)r   )r   r   r   r   r   r   r   r   MXintr   floatr   rR   staticmethodDOF6r   r   r	  r	   r   r   r-  rU   r4   r2   r   r   ?  s<   
''ME"%%,'((%ruu-..eE#s(OZ7899IGGL )(x.)( %c3h ;<=)( S		)(
 )(V 0 0T 0 0"%% BEE g $Fs F5hc3h1O+P Fr4   r   )Nr   )r   )Rtypingr   r   r   r   r   r   r	   r
   r   r[   r   r   mathr   r   casadir   OCP.gpr   r   r   r   r   r   r   r   OCP.BRepToolsr   OCP.Precisionr   geomr   r   r   shapesr   r   r    r!   typesr#   utilsr$   typeNoneTyper0  r2  r\   UnaryConstraintKindBinaryConstraintKindrG   rK   r9   r   r   r   r   NDOFDIR_SCALINGDIFF_EPSTOLMAXITERobjectr>   r   r   r   r   r   r   r   r   r   r   r   dictr   r   r   boolr   rU   r4   r2   <module>rI     s       	 	 	 $ # ) ) + +   :U5%&'ueU/B)CCD=> QR VW 
 &&!4.		7	 ($5't4$t*d+vitT4'7!8$?fYdD$&6 7>	dD$(	 2 !UV TE% 34hueCQTHo?U6VVWW  	
C
 .(3-@
 
	


I2V I2ZW"  88 	8 
%8 8 8V  ## 	# 
%# # #d  ## 	# 
%# # #d  $)$) 	$) 
%$) $) $)^ !%T
 
$t*	 ,  
ueU"	# 4  
ueU"	# 6 T
 
ueU"	# * *.
	$"
%	*tC#u*%%& 	  
	
	c4i 	\v \r4   