
    ^jX                     v   d dl Z d dlZd dlmZmZmZ d dlZd dlm	Z	m
Z
 d dlmZmZ d dlmZmZ ddlmZmZmZmZmZ ded	ed
efdZded	ed
efdZ G d d      Z e j4                  d      de	fd       Z G d d      Zde	d
efdZ G d d      Z G d d      Z  G d de      Z!de	fdZ"de	de	ded
e#e   fd Z$d!e#e   d"e#e   d#ee#e   eege%f   d$eeegef   fd%Z&d&ed'ed
efd(Z'	 d7d)ed*ed+ed,ed
e(ee%f   f
d-Z)	 d7d.ed/ed0ed1ed,ed
e(eee%f   fd2Z* G d3 d4e      Z+	 d8de	de	ded
e#e+   fd5Z,e-d6k(  ryy)9    N)AnyCallable	TypedDict)Geom_BSplineCurve
Geom_Curve)gp_Pntgp_Vec)OptimizeResultminimize_scalar   )Standard_Realclone_bspline	math_BFGS$math_MultipleVarFunctionWithGradientmath_Vectorp1p2returnc           	          t        t        | j                         |j                               t        | j                         |j                               t        | j	                         |j	                                     S N)r   minXYZr   r   s     _/opt/ringagent/.cad-venv/lib/python3.12/site-packages/ocp_gordon/internal/intersect_bsplines.py	minCoordsr      J    #bddfbddf%s24462446':C<OPP    c           	          t        t        | j                         |j                               t        | j                         |j                               t        | j	                         |j	                                     S r   )r   maxr   r   r   r   s     r   	maxCoordsr"   !   r   r   c                   "    e Zd ZdedefdZd Zy)	Intervallmminmmaxc                      || _         || _        y r   )r   r!   )selfr%   r&   s      r   __init__zIntervall.__init__&   s    r   c                     d}t        | j                  |j                  z
        |k  xr% t        | j                  |j                  z
        |k  S )NV瞯<)absr   r!   )r(   otherEPSs      r   __eq__zIntervall.__eq__*   s@    488eii'(3.R3txx%))7K3Ls3RRr   N)__name__
__module____qualname__floatr)   r/    r   r   r$   r$   %   s    U % Sr   r$   i   )maxsizecurvec                    t        j                  | j                         dft         j                        }t	        d| j                         dz         D ]H  }| j                  |      }|j                         |j                         |j                         g||dz
  <   J t        j                  |d      }t        |d   |d   |d         }t        j                  |d      }t        |d   |d   |d         }||fS )N   )dtyper   r   )axis   )npzerosNbPolesfloat64rangePoler   r   r   r   r   r!   )r6   np_arrayiptemplowhighs          r   _get_low_high_from_bsplinerH   /   s    xx!,BJJ?H1emmo)* 
JJqMCCECCECCE
Q
 66(#D
a$q'47
+C66(#D$q'47DG,D9r   c                   6    e Zd Zd	dZdd dedefdZd
dZd Zy)BoundingBoxc                    t        |t              rH|}t        |j                         |j	                               | _        t        |      \  | _        | _        y |}t        |j
                  j                  |j
                  j                        | _        t        |j                  j                               | _        t        |j                  j                               | _        y r   )
isinstancer   r$   FirstParameterLastParameterr@   rH   rF   rG   r   r!   r   XYZ)r(   curve_or_otherr6   r-   s       r   r)   zBoundingBox.__init__A   s    n&78"E"5#7#7#95;N;N;PQDJ"<U"CDHdi"E"5;;??EKKOODDJeiimmo.DHuzz~~/0DIr   r-   epsr   c                 b   t        | j                  |j                        }t        | j                  |j                        }|j	                         |j	                         |z   k  xrJ |j                         |j                         |z   k  xr$ |j                         |j                         |z   k  S r   )r"   rF   r   rG   r   r   r   )r(   r-   rQ   
min_coords
max_coordss        r   
IntersectszBoundingBox.IntersectsL   s    txx3
tyy%**5
\\^jllns22 8*,,.3"668*,,.3"66	
r   c                     |j                   j                  | j                   _        t        | j                  |j                        | _        t	        | j
                  |j
                        | _        | S r   )r@   r!   r"   rG   r   rF   r(   r-   s     r   MergezBoundingBox.MergeU   sF    

dii4	TXXuyy1r   c                 4    | j                   |j                   k(  S r   )r@   rW   s     r   r/   zBoundingBox.__eq__[   s    zzU[[((r   N)rP   zGeom_BSplineCurve | BoundingBox)r-   rJ   r   rJ   )	r0   r1   r2   r)   r3   boolrU   rX   r/   r4   r   r   rJ   rJ   @   s+    	1
 
E 
d 
)r   rJ   c                    | j                  d      j                  | j                  | j                                     }d}t        d| j                               D ]=  }| j                  |      }| j                  |dz         }|j                  |      }||z  }? t	        |      dk  r	|dk  rdS dS t        ||z  d      S )Nr           r+         ?g    eA)rA   Distancer>   r@   r,   r!   )r6   	len_curvetotalrC   r   r   dists          r   	curvaturerb   _   s    

1&&uzz%--/'BCIE1emmo& ZZ]ZZA{{2	 9~ems,,uy #&&r   c                       e Zd ZdedefdZy)BoundingBoxPairb1b2c                      || _         || _        y r   )re   rf   )r(   re   rf   s      r   r)   zBoundingBoxPair.__init__n   s    r   N)r0   r1   r2   rJ   r)   r4   r   r   rd   rd   m   s    ; K r   rd   c            	       .    e Zd Zededededefd       Zy)BSplineAlgorithmsr6   firstlastr   c                 @    t        |       }|j                  ||       |S r   )r   Segment)r6   rj   rk   	new_curves       r   	trimCurvezBSplineAlgorithms.trimCurvet   s$     "%(	%&r   N)r0   r1   r2   staticmethodr   r3   ro   r4   r   r   ri   ri   s   s4     ).6;	 r   ri   c                        e Zd Zdedef fdZdefdZdedede	fdZ
ded	ede	fd
Zededefd       Ze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ded	ede	fdZ xZS )CurveCurveDistanceObjectivec1c2c                 >    t         |           || _        || _        y r   )superr)   m_c1m_c2)r(   rs   rt   	__class__s      r   r)   z$CurveCurveDistanceObjective.__init__   s    		r   r   c                      y)Nr;   r4   )r(   s    r   NbVariablesz'CurveCurveDistanceObjective.NbVariables   s    r   r   Fc                 B    t        dd      }| j                  |||       y)Nr   r;   T)r   Values)r(   r   r|   Gs       r   Valuez!CurveCurveDistanceObjective.Value   s!    1Aq!r   r   c                 @    t        d      }| j                  |||       y)Nr\   T)r   r~   )r(   r   r   F_vals       r   Gradientz$CurveCurveDistanceObjective.Gradient   s!     c"Aua r   zc                     | S r   r4   r   s    r   activatez$CurveCurveDistanceObjective.activate   s    r   c                      y)Nr   r4   r   s    r   
d_activatez&CurveCurveDistanceObjective.d_activate   s    r   x0c                     | j                   j                         }| j                   j                         }| j                  |      ||z
  z  |z   S r   )rw   rM   rN   r   r(   r   uminumaxs       r   	getUParamz%CurveCurveDistanceObjective.getUParam   C    yy'')yy&&(}}R D4K0477r   x1c                     | j                   j                         }| j                   j                         }| j                  |      ||z
  z  |z   S r   )rx   rM   rN   r   r(   r   vminvmaxs       r   	getVParamz%CurveCurveDistanceObjective.getVParam   r   r   c                     | j                   j                         }| j                   j                         }| j                  |      ||z
  z  S r   )rw   rM   rN   r   r   s       r   d_getUParamz'CurveCurveDistanceObjective.d_getUParam   >    yy'')yy&&(r"dTk22r   c                     | j                   j                         }| j                   j                         }| j                  |      ||z
  z  S r   )rx   rM   rN   r   r   s       r   d_getVParamz'CurveCurveDistanceObjective.d_getVParam   r   r   c           	         | j                  |j                  d            }| j                  |j                  d            }t        ddd      }t        ddd      }t	        ddd      }t	        ddd      }	| j
                  j                  |||       | j                  j                  |||	       t	        ||      }
|
j                         |_	        |j                  dd|
j                  |      z  | j                  |j                  d            z         |j                  dd|
j                  |	      z  | j                  |j                  d            z         y)Nr   r;   r   g       @g       T)r   r   r   r   r	   rw   D1rx   SquareMagnitudevalueSetValueDotr   r   )r(   r   r|   r   uvr   r   d1_vecd2_vecdiffs              r   r~   z"CurveCurveDistanceObjective.Values   s   NN1771:&NN1771:&Aq!_Aq!_1a1a		QF#		QF#b"~&&(	

$((6""T%5%5aggaj%AA	
 	


488F##d&6&6qwwqz&BB	

 r   )r0   r1   r2   r   r)   intr{   r   r   rZ   r   r   rp   r3   r   r   r   r   r   r   r~   __classcell__)ry   s   @r   rr   rr      s
   : : S { }  
!,	 E e   e   &8E 8e 8
8E 8e 8
3e 3 3
3e 3 3
  + $ r   rr   c                 n   d| j                         | j                         z   z  }| j                  | j                         dz
        r|S d}t	        d| j                               D ](  }| j                  |      | j                  |      kD  s'|}* | j                  |      dkD  r| j                  |      S |S )zv
    Find the max inner multiplicity. If it is higher than 1, return the knot.
    otherwise return the mid point
          ?r   r;   r8   )rM   rN   IsCNDegreer@   NbKnotsMultiplicityKnot)r6   	mid_pointmax_inner_mult_indexrC   s       r   find_split_pointr      s    
 u++-0C0C0EEFIzz%,,.1$%1emmo& %a 5#5#56J#KK#$ % ./!3zz.//r   curve1curve2	tolerancec                 @   t        |       }t        |      }|j                  ||      sg S t        |       }t        |      }d}||k  r||k  rt        ||      gS t	        |       }t	        |      }	g }
||kD  r||kD  rt
        j                  | | j                         |      }t
        j                  | || j                               }t
        j                  ||j                         |	      }t
        j                  ||	|j                               }|
j                  t        |||             |
j                  t        |||             |
j                  t        |||             |
j                  t        |||             |
S ||k  r||k  rt
        j                  ||j                         |	      }t
        j                  ||	|j                               }|
j                  t        | ||             |
j                  t        | ||             |
S ||k  r||k  rt
        j                  | | j                         |      }t
        j                  | || j                               }|
j                  t        |||             |
j                  t        |||             |
S )N5^I?)rJ   rU   rb   rd   r   ri   ro   rM   rN   extendgetRangesOfIntersection)r   r   r   h1h2c1_curvaturec2_curvaturemax_curvaturecurve1MidParmcurve2MidParmresultsc11c12c21c22s                  r   r   r      sl    
V	B	V	B==Y'	V$LV$LM}$)FB'(($V,M$V,MGm#}(D))F))+]
  ))&-AUAUAWX))F))+]
  ))&-AUAUAWX.sCCD.sCCD.sCCD.sCCD( N% 
	&=<+G))F))+]
  ))&-AUAUAWX.vsIFG.vsIFG N 
	&=<+G))F))+]
  ))&-AUAUAWX.sFIFG.sFIFGNr   avoid_u_valueslist_objis_adjacent_func
merge_funcc                    d}|t        |      k  rz|dz   }|t        |      k\  ry  || ||   ||         rD |||   ||         }|j                  |       |j                  |       |j                  ||       n|}|t        |      k  ryy y )Nr   r   )lenpopinsert)r   r   r   r   rC   next_i
merged_vals          r   replace_adjacent_in_listr   /  s     	
A
c(m
QS]"NHQK&9IJ#HQK&1ABJLLOLLOOOAz*A c(m
r   re   rf   c                 >    t        |       }|j                  |       |S r   )rJ   rX   )re   rf   new_boxs      r   merge_boxesr   D  s    "oG MM"Nr   pointsegment_startsegment_endr   c                    t        ||      }t        ||       }|j                         }|dk  rd|j                         dk  fS |j                  |      ||z  z  }d}dt	        j
                  |dz
        z  }	d|	z  |z  }
||z  |
 k  s||z  ||
z   kD  rd}t        |j                               }|j                  d|z
  ||       | j                  |      }||
kD  rd}||fS )z
    Checks if a 3D point lies on a 3D line segment which
    is the chord of a B-spline with the max_curvature.
    t is the parameter for the line segment in the range of [0, 1].
    g&.>r   T# ?r   333333?F)
r	   	Magnituder   r   mathsqrtr   rO   
BaryCenterr^   )r   r   r   r   vec_segmentvec_point_from_startline_lengthtpossible_intersectrel_sagittamax_distanceclosest_point_on_linedistance_to_lines                r   is_point_on_line_segmentr   Q  s    4K!-7 '')KT&6684???  -{1JKA dii(9::K${2L 	;,&!k/K,<V*V" #=#4#4#67$$QUK;~~&;<,&"   r   p1_startp1_endp2_startp2_endc                 Z   t        | |      }t        ||      }t        | |      }|j                  |      }|j                  |      }	|j                  |      }
|j                  |      }|j                  |      }||
z  |	|	z  z
  }t        |      dk  rd}d}n|
|z  |	|z  z
  |z  }|	|z  ||z  z
  |z  }d}t        j                  |      }t        j                  |
      }dt        j                  |dz
        z  }d|z  ||z   z  }||z  | k  s||z  ||z   kD  rd}||z  | k  s||z  ||z   kD  rd}t        | j                               }|j                  d|z
  ||       t        |j                               }|j                  d|z
  ||       |j                  |      }||kD  rd}|||fS )z
    Finds the parameters (t, s) for the closest points on two 3D line segments which
    are the chords of possibly intersected two B-splines with the max_curvature.
    t is the parameter for line 1, s for line 2. Both are in the range of [0, 1].
    g-q=r   Tr   r   r   F)	r	   r   r,   r   r   r   rO   r   r^   )r   r   r   r   r   r   r   wabcdedenomr   sr   p1_magp2_magr   r   
closest_p1
closest_p2distances                           r   line_line_intersection_3dr   z  s    	x Ax Ax"A	aA	aA	aA	aA	aAEAEME
5zEUQU]e#UQU]e# YYq\FYYq\Fdii(9::K$8L 	6z\M!QZ&<2G%G"6z\M!QZ&<2G%G" 'J!a%+'J!a%+"":.H,"a###r   c                   ,    e Zd ZU eed<   eed<   eed<   y)IntersectTypeparmOnCurve1parmOnCurve2r   N)r0   r1   r2   r3   __annotations__r   r4   r   r   r   r     s    Mr   r   c                   "#$ t        | |      }g }g }|D ]8  }|j                  |j                         |j                  |j                         : |j	                  d        |j	                  d        g }|rQ|j                  |d          t        dt        |            D ]%  }||   ||dz
     k(  r|j                  ||          ' |}g }	|rQ|	j                  |d          t        dt        |            D ]%  }||   ||dz
     k(  r|	j                  ||          ' |	}dt        t           dt        dt        d	t        fd
}
dt        fd}t         ||       ||
t               t         ||      ||
t               g }|D ]7  }|D ]0  }|j                  |      s|j                  t        ||             2 9 g }dt        t            dt         ffd}t#        dt%        d            "|D ]  }t&        j)                  | |j                  j
                  j$                  |j                  j
                  j"                        #t&        j)                  ||j                  j
                  j$                  |j                  j
                  j"                        $dt        dt        f#$fd}dt        dt        dt        f"fd} |##j+                         $      \  }}|||| |||      d} |||        |##j-                         $      \  }}|||| |||      d} |||       C |$$j+                         #      \  }}|||| |||      d} |||       z |$$j-                         #      \  }}|||| |||      d} |||       t/        #$      }#j1                  #j+                               }#j1                  #j-                               }$j1                  $j+                               }$j1                  $j-                               }t3        ||||t#        t5        #      t5        $                  \  }}}|slt7        dd      }|j9                  d|       |j9                  d|       t        d      D ]  }t;        ||""z        } | s n |j=                  |j1                  d            }|j?                  |j1                  d            }|t%        #j+                         #j-                               "z
  k  s.|t#        #j+                         #j-                               "z   kD  r^|t%        $j+                         $j-                               "z
  k  s.|t#        $j+                         $j-                               "z   kD  r#j1                  |      jA                  $j1                  |            }!|!k  s|| |||      d} |||       
 |S )Nc                 .    | j                   j                  S r   r@   r   r   s    r   <lambda>z#IntersectBSplines.<locals>.<lambda>      177;; r   )keyc                 .    | j                   j                  S r   r  r  s    r   r  z#IntersectBSplines.<locals>.<lambda>  r  r   r   r   r   re   rf   r   c                 l   d}t        |       dkD  rgt        | D cg c]N  }t        t        |j                  j                  |z
        t        |j                  j                  |z
              P c}      |k  ryt        |j                  j                  |j                  j                  z
        |k  S c c}w )Nr+   r   F)r   r   r,   r@   r!   )r   re   rf   r.   r   s        r   is_adjacentz&IntersectBSplines.<locals>.is_adjacent  s     !# , BHHLL1,-s288<<!3C/DE  288<<"((,,./#55s   AB1r6   c                     g }t        d| j                               D ]7  }| j                  |      dkD  s|j                  | j	                  |             9 |S )zS
        We want to avoid merge adjacent boxes if the merge creates a kink
        r;   r   )r@   r   r   appendr   )r6   r   rC   s      r   create_avoid_u_valuesz0IntersectBSplines.<locals>.create_avoid_u_values  sV     ')q%--/* 	5A!!!$q(%%ejjm4	5 r   r   resultc                     | D ]3  }t        |d   |d   z
        k  st        |d   |d   z
        k  s3 y  | j                  |       y )Nr   r   )r,   r  )r   r  rr   s      r   append_resultz(IntersectBSplines.<locals>.append_result  s[     	AAn%~(>>?)K.)F>,BBCiO	 	vr   g|=h㈵>r   r   c                 r    j                  |       }j                  |      }|j                  d|d       |S )Nr]   )r   r   )r   r   r   r   rs   rt   s       r   get_mid_pointz(IntersectBSplines.<locals>.get_mid_point  s2    !B!BMM#r3'Ir   cx1param1cx2c                 v    j                         }j                  |      } j                        j                  |      	k  r|fS j                         }j                  |      } j                        j                  |      	k  r|fS t	         j                        ||t        t               t                          \  }}|syt         fdj                         j                         fddd	z  	z  i      }|j                  r |j                  		z  k  r|j                  fS y)N)NNc                 b    j                        j                  j                  |             S r   )r   SquareDistance)r   r  r  r  s    r   r  zBIntersectBSplines.<locals>.check_point_on_curve2.<locals>.<lambda>4  s#    #))F+::399Q<H r   Boundedxatolg?)boundsmethodoptions)rM   r   r^   rN   r   r!   rb   r   successfunx)
r  r  r  r   cx2_first_pointcx2_last_pointr   r   res_tolerance_distances
   ```      r   check_point_on_curve2z0IntersectBSplines.<locals>.check_point_on_curve2  s/    ""$A!iilOyy ))/:=PPqy !!#A YYq\Nyy )).9<OOqy $<		&!IcNIcN3	%!A! &!!H**,c.?.?.AB  #(;";>Q"QR	C {{sww)<?R)RRsuu}$!r   )r   r   r   r;   )!r   r  re   rf   sortr@   r   listr3   rJ   rZ   r   r   r   rU   rd   r   r!   r   ri   ro   rM   rN   rr   r   r   rb   r   r   r   r   r   r^   )%r   r   r   hullscurve1_intscurve2_intshullunique_curve1_intsrC   unique_curve2_intsr	  r  intersectionCandidatesre   rf   r   r  boxesr  r&  r   r   r  objr   r   r   r   	t_closest	s_closestr   guess	convergedr   r%  rs   rt   s%     `                               @@@r   IntersectBSplinesr6    s    $FFI>E%'K%'K $477#477#$ ././,.!!+a.1q#k*+ 	:Aq>[Q%77"))+a.9	: %K,.!!+a.1q#k*+ 	:Aq>[Q%77"))+a.9	: %K6U6)46:E6	6"%6  f%{K f%{K 57 G 	GB}}R+&--ob".EF	GG
 $&GtM2 M  eSy%9:' S+((1C1CUXX^^EWEWX((1C1CUXX^^EWEWX	U 	u 		""	",1	"8I	"B %R):):)<bA1=Q] ! !&q!,%F
 '6*$R)9)9);R@1=Q] ! !&q!,%F
 '6*$R):):)<bA1=Q] ! !&q!,%F
 '6*$R)9)9);R@1=Q] ! !&q!,%F
 '6* *"b1 88B--/0"**,-88B--/0"**,-3L	"y}-4
0	90 " Aq!q)$q)$ q 	A!#u.ADW.WXI	 MM%++a.)MM%++a.) B%%')9)9);<?RRR3r((*B,<,<,>?BUUU B%%')9)9);<?RRR3r((*B,<,<,>?BUUU88A;''4 i ! !&q!,%F
 '6*gS+j Nr   __main__)r   )r  ).	functoolsr   typingr   r   r   numpyr<   OCP.Geomr   r   OCP.gpr   r	   scipy.optimizer
   r   miscr   r   r   r   r   r   r"   r$   	lru_cacherH   rJ   r3   rb   rd   ri   rr   r   r(  r   rZ   r   r   tupler   r   r   r6  r0   r4   r   r   <module>rA     sh     + +  2 ! : &Q& Qf Q QQ& Qf Q QS S T"&7  # ) )>'& '5 '  ]"F ]@- (77'87EJ7	/7tK; U[+FLM +{3[@A	*K [ [ " "	&!&!&! &! 	&!
 5$;&!\ "8$8$8$ 8$ 	8$
 8$ 5%8$vI  NRhh'8hEJh	-hX z r   