
    ,jF                     2   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?          ZQdS )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          |           nd S Nr   xs    R/DATA/AppData/hermes/venv/lib/python3.11/site-packages/cadquery/occ_impl/solver.py<lambda>r3   B   s    '!***4     r.   c                 F    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          |           nd dfS )Nr   r/   r0   s    r2   r3   r3   U   s    !-GAJJJTST+U r4   .CompoundConstraints      g      Y@g|=-q=i  c                   F   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dS )ConstraintSpeczQ
    Geometrical constraint specification between two shapes of an assembly.
    .objectsargssublocskindparamNc                    t          |t                    st          d| d          |t          v rGt          |         \  }}t	          | ||                    D ]\  }}	|                     |||	           n9|                     |||           t          |         d         }
|
r |
|          n|}|| _        || _        || _	        || _
        || _        dS )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0 	<:4:::;;;&&&&9$&?#E#E#3#3E#:#:;; + +1tQ****+ NN4u--- +404G&-8GGENNN5E 		


r4   c           
         t           |         \  }}}}|t          |          k    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          $ r t          d|	 d| d          w xY wt!          ||          s%|#t          d	t#          |           d| d          	 ||r ||           d S d S d S # t$          $ r}t          d
| d          d }~ww xY w)Nz*Invalid number of entities for constraint z. Provided z, required rE   c                     dS )NT r0   s    r2   r3   z*ConstraintSpec._validate.<locals>.<lambda>   s    D r4   .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   s   5I$5O2|Z CIIlTllcRViilldilll   DLDMDLDL..8
	 eE$/?*@#*E$FUUVV 	S 	SDAqS	!Q S S S !Qq!Q!Q$!Q!Q!QRRRS 5*-- 	%2CYj.?.?YYJYYY  
	S Y 	%      !    	S 	S 	SQ!QQQRRR	Ss$   C!C5.E 
E%E  E%argreturnc                    t          |t                    r|                                }nt          |t                    r-|                                dk    r|                                }nTt          |t                    r-|                                dk    r|                                }nt          d|           |                                S )NCIRCLEzCannot construct Axis for )	
isinstancer   normalAtr    geomType	tangentAtnormalrH   toDir)rL   rf   rvs      r2   rX   zConstraintSpec._getAxis   s    c4   	ABBT"" 	As||~~'A'ABBT"" 	As||~~'A'ABB?#??@@@xxzzr4   c                    t          |t                    rHt          |                     |          |                                                                          }n}t          |t          t          f          rN|                                }|	                                }t          ||          }|                                }nt          d| d          |S )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   	ES))3<<>>+?+?+A+ABBBBdD\** 	EZZ\\FZZ\\F&000EBBCSCCCDDD	r4   c                 H   t          |t                    rft          d t          j        |j                  D                       r6|                                }t          |                                          }n|	                                }|
                                S )Nc              3   >   K   | ]}t          j        |          V  d S r.   )r   IsInfinite_s).0r1   s     r2   	<genexpr>z)ConstraintSpec._getPnt.<locals>.<genexpr>   s>       )
 )
*+I"1%%)
 )
 )
 )
 )
 )
r4   )rj   r   anyr   
UVBounds_swrappedrt   r   r   rs   toPnt)rL   rf   plncenters       r2   rW   zConstraintSpec._getPnt   s     c4   	"S )
 )
/8/CCK/P/P)
 )
 )
 &
 &
 	" ))++CCLLNN++FFZZ\\F||~~r4   c                    t          |t          t          f          r)|                                }|                                }nt          d| d          t          |                                |                                          S )Nrr   rE   )	rj   r    r!   rs   rm   rH   r   r   ro   )rL   rf   r   tangents       r2   rZ   zConstraintSpec._getLin   so    cD$<(( 	EZZ\\FmmooGGCSCCCDDDfllnngmmoo666r4   c                 d   t          d t          | j        | j                  D                       }| j        dk    r9|                     |d                   |                     |d                   fg}n| j        dk    r9|                     |d                   |                     |d                   fg}n| j        dk    rn|                     |d                   |                     |d                   f|                     |d                   |                     |d                   fg}n| j        dk    r8|                     |d                   |                     |d                   fg}n| j        dk    r8|                     |d                   |                     |d                   fg}n| j        d	k    rd
g}n| j        dk    r|                     |d                   fg}n_| j        dk    r|                     |d                   fg}n6| 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   p   K   | ]1\  }}|                     ||                                z            V  2d S r.   )locatedlocation)rz   rf   locs      r2   r{   z(ConstraintSpec.toPODs.<locals>.<genexpr>   sS       
 
S KKcllnn,--
 
 
 
 
 
r4   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   s     
 
	4<88
 
 
 
 
 9d1g..d1g0F0FHIGGY'!!T!W--t||DG/D/DEFGGY'!!tAw''tAw)?)?Ad1g&&T!W(=(=>GG
 Y.((T!W--t||DG/D/DEFGGY-''T!W--t||DG/D/DEFGGY'!!iGGY,&&T!W--/0GGY+%%d1g..01GGY/))111GGY---iGG C	CCDDD 9+++249=E9Ytz++FFYLEj]F S%001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=!!!!
JJJ & &sCx& E3J& x}%	&
 & & & & &P$SeE3J/ $S~ $Sc $S $S $S $SLE f    5 V    5 V    75 7V 7 7 7 7=2j#o. =2 =2 =2 =2 =2 =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   ?  sb    a==DAqrva||a16BF1aLL#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        |                                |                                |                                f          }t          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E244662446624466*++EE244662446624466*++E 	%D2I..5$)TTVY1W1WWE axxyIee11a77r4   c                 |   |t           n|}t          j        |                                |                                |                                f          }t          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E244662446624466*++EE244662446624466*++EUD2I&&udRi(@(@B
axxRy	RyF2rNNRVC[[(EA:r4   c           
      p   |dn|}t          j        |                                |                                |                                f          }|                                                                }|                                                                                    |t          |          z            }t          j        |                                |                                |                                f          }t          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     sV     {!!CE244662446624466*++EWWYY  ""FWWYY!!,,S6&>>-ABBFvxxzz688::vxxzz:;;Ivxxzz688::vxxzz:;;I 	9dRi((iD2I66tby$)445	
 	

 	 
 A:r4   c                 T   |dn|}t          j        |                                |                                |                                f          }|                                }|                                }t          j        |                                |                                |                                f          }t          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     sT     {!!CE244662446624466*++E\\^^F[[]]Fvxxzz688::vxxzz:;;Ivxxzz688::vxxzz:;;I%D2I..4"9dRi2 2 	A 	y$)$$ARVAq\\!!U*E
axxyIes"q((r4   c                     d S r.   rU   )r   r   r   r   r   r   r   r   s           r2   
fixed_costr     s	     4r4   c                 "   t          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     sm     E244662446624466*++EudRi33beCjj@EIE9Ur4   c                 f   t          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     s~     E244662446624466*++EE#JJ25::...E5$)$$u,E9Ur4   c           	      v   t                      } |j        t          g|R   t          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%,,,,,5!##%%qssuu-..Drz:dRi#8#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dS )ConstraintSolveropti	variablesstarting_points.constraintslockednencr   r   entitiesc                    | _         t          j                    x| _        fdt	          |          D             | _        fd|D             | _        t	          t          | j        | j        |                    D ]\  }\  \  }}\  }}	}
|                     |
          \  }}	                    ||           	                    |	|           |v r-	                    |d           	                    |d           
                    |d           
                    |d           || _        t          |          | _        | _        t          | j                  | _        d S )Nc                     g | ]t\  }}|vr7                     t                    z                       t                    fn3                    t                                        t                    fuS rU   )variableNDOF_VNDOF_Q	parameter)rz   i_r   r   r   s      r2   
<listcomp>z-ConstraintSolver.__init__.<locals>.<listcomp>T  s     
 
 
 1  T]]6***DMM&,A,ABB..(($..*@*@A
 
 
r4   c                 x    g | ]6}                     t                                         t                    f7S rU   )r   r   r   )rz   r   r   s     r2   r   z-ConstraintSolver.__init__.<locals>.<listcomp>Z  sC     
 
 
ABT^^F##T^^F%;%;<
 
 
r4   )r   r   r   )        r   r   ){Gz?r   r   )r   r   Optir   	enumerater   start_pointsrI   
_locToDOF6	set_valueset_initialr   rV   r   r   r   )rL   r   r   r   r   r   r   r   T0R0r   T0valR0valr   s      ``        @r2   rR   zConstraintSolver.__init__J  s    
799$	D
 
 
 
 
 
 "(++	
 
 

 
 
 
FN
 
 

 +4 18<<+
 +
 	8 	8&A&AR#  ??3//LE5NN2u%%%NN2u%%%F{{q),,,q),,,,  O444  $67777& h--d&''r4   r   rg   c                    | j                                         }|                                }|                                }d|                                z
  d|                                z   z  }|dz   |                                z  dz  }|dz   |                                z  dz  }|dz   |                                z  dz  }|                                |                                |                                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qssuu9QSSUU+q[ACCEE!A%q[ACCEE!A%q[ACCEE!A%qssuuaccee$q!Qi//r4   r   r   c           
      t   | j         }t                      }|                    |          \  }}}|dz  |dz  z   |dz  z   }|                    t	          d|z  |dz   z  d|z  |dz   z  d|z  |dz   z  d|z
  |dz   z                       |                    t          |                    |                      |S r   )r   r   valueSetRotationr   SetTranslationPartr   )	rL   r   r   r   rp   rc   r  r  r   s	            r2   _build_transformz!ConstraintSolver._build_transform  s    yYY**Q--1aFQ!VOa1f$
AQQ!a%!a%1q5/AEaRSeCT 	
 	
 	

 	fdjjmm4555	r4   r   	verbosityc                     |dk    rdnd} j         } j        } j        } j        }d}|D ]7\  }}	|t	          j        t	          j        | j        z  |	                    z  }8d}
|D ]\  }\  }}}g }g }|D ]8}|                    ||                    |                    ||                    9t          |         |g||||R dt          |         r j        ndi}||
|z  }
|                    |
d|z  z              |                    dd	d
iddddddd||ddd           |                                }|                                }||d<    fdt          ||          D             }||fS )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   c           	      t    g | ]4\  \  }}\  }}t                              ||z   ||z                       5S rU   )r   r  )rz   r   r   r   r   rL   s        r2   r   z*ConstraintSolver.solve.<locals>.<listcomp>  sU     
 
 
 AR T**1r61r6::;;
 
 
r4   )r   r   r   r   r   r   r   r   extendr   r   minimizesolversolve_limitedstatsrI   )rL   r  suppress_bannerr   r   r   r   penaltyr   r   	objectiveksmsrB   r   s_ksv_ksrO   r  solresultlocss   `                     r2   solvezConstraintSolver.solve  s"   #,>>%%ty&N	(  	@ 	@DAqryA
NA!>!>???GG 	&1 	 	"B"T6 !#D "D * *LO,,,IaL))))d  	
    %,DM8djjq A }Q	i%'/1222 	5!-2#$)0&,).&'(%+/!( 	
 	
 	
"   ""v
 
 
 
$'	<$@$@
 
 

 V|r4   N)r   )r   r   r   r   r   r   r   r   MXintr   floatr   rR   staticmethodDOF6r   r   r  r	   r   r   r1  rU   r4   r2   r   r   ?  s        
'MMME"%,'((((%ru-....eE#s(OZ789999IGGGGGGLLL )( )(x.)( %c3h ;<=)( S		)(
 )( )( )( )(V 0 0T 0 0 0 \0"% BE g    $F Fs F5hc3h1O+P F F F F F 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NoneTyper4  r6  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>rM     s8                                        	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 $ # # # # # # # # # # # ) ) ) ) ) ) ) ) ) ) + + + + + + + + + + + +             4::U5%&'ueU/B)CCD=> QR VW 
 &&!4.		77	 ($5't4$t*d+vitT4'7!8$?fYdD$&6 7>	dD$((	  2 !U!UV TE% 34hueCQTHo?U6VVWW    	
C
 .(3-@
 
	


I2 I2 I2 I2 I2V I2 I2 I2Z  W W W  "  8 88 	8 
%8 8 8 8 8 8V  # ## 	# 
%# # # # # #d  # ## 	# 
%# # # # # #d  $) $)$) 	$) 
%$) $) $) $) $) $)^ !% T
 
$t*	    ,   
ueU"	#    4   
ueU"	#    6  T
 
ueU"	#    * *.
	$"
%	* 	* 	*tC#u*%%& 	 	 	  4
	
	 	 	c4i 	 	 	\ \ \ \ \v \ \ \ \ \r4   