
    MieJ                        S r SSKrSSKJr  SSKJrJr  SSKJ	r	  SSK
Jr  SSK
Jr  SS	KJr   SS
KJr  S rS rS rS r " S S5      r " S S5      rS r SS jrSS jrg! \ a  rSSKJr  \" \5      r SrCNHSrCff = f)z<
proximity.py
---------------

Query mesh- point proximity.
    N   )util)log_timetol)	group_min)closest_point)points_to_barycentric)diagonal_dot)cKDTree)ExceptionWrapperc                    [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      eU R                  n[        U R                  U R                     5      nUR                  U5      S   R                  S5      nU[        R                  -  n[         R                  " X-
  X-   45      nU Vs/ s H  n[        UR!                  U5      5      PM     nnU$ s  snf )a*  
For each point find nearby faces relatively quickly.

The closest point on the mesh to the queried point is guaranteed to be
on one of the faces listed.

Does this by finding the nearest vertex on the mesh to each point, and
then returns all the faces that intersect the axis aligned bounding box
centered at the queried point and extending to the nearest vertex.

Parameters
----------
mesh : trimesh.Trimesh
  Mesh to query.
points : (n, 3) float
   Points in space

Returns
-----------
candidates : (points,) int
  Sequence of indexes for mesh.faces
dtype   points must be (n,3)!r   )r   r   )np
asanyarrayfloat64r   is_shape
ValueErrortriangles_treer   verticesreferenced_verticesqueryreshaper   mergecolumn_stacklistintersection)meshpointsrtreekdtreedistance_vertexboundsb
candidatess           k/var/www/eduai.edurigo.com/storigo/production/storigo_env/lib/python3.13/site-packages/trimesh/proximity.pynearby_facesr+      s    . ]]64F==))011 ET]]4#;#;<=F ll6*1-55g>Osyy O __f68PQRF 8>>v!$u))!,-vJ> ?s   $C7c                    U R                   R                  [        R                  5      n[        R                  " U[        R
                  S9n[        R                  " US5      (       d  [        S5      e[        R                  " US5      (       d  [        S5      e[        R                  " US[        U5      45      n[        R                  " U Vs/ s H  n[        X$R                  S5      5      PM     sn5      n[        XQ5       VVs/ s H  u  pFXF-
  S-  R                  SS9PM     nnn[        R                  " U Vs/ s H  oDR!                  5       PM     sn5      n[        R                  " [        X5       VV	s/ s H	  u  pIX   PM     sn	n5      n
[        R                  " [        X5       VV	s/ s H	  u  pIX   PM     sn	n5      S	-  nXU4$ s  snf s  snnf s  snf s  sn	nf s  sn	nf )
a  
Given a mesh and a list of points find the closest point
on any triangle.

Does this by constructing a very large intermediate array and
comparing every point to every triangle.

Parameters
----------
mesh : Trimesh
  Takes mesh to have same interfaces as `closest_point`
points : (m, 3) float
  Points in space

Returns
----------
closest : (m, 3) float
  Closest point on triangles for each point
distance : (m,) float
  Distances between point and triangle
triangle_id : (m,) int
  Index of triangle containing closest point
r   )r   r   r   ztriangles shape incorrectr   zpoints must be (n,3)r      axis      ?)	trianglesviewr   ndarrayr   r   r   r   r   tilelenarray_correspondingr   zipsumargmin)r"   r#   r1   points_tiledion_triangleq
distance_2triangle_idgclosestdistances               r*   closest_point_naiverD   F   s   2 ##BJJ/I]]64F==J//455==))/00 776As9~#67L((@LM1	99W#5	6MK
 :=[9QR9QAEa<$$!$,9QJR((
;
1HHJ
;<K hh[)FG)F)FGHGxx#k*FG*F$!*FGHCOHk)) 	N S; HGs   $G
 GG?G$
5G*
c                    [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      e[        X5      nU R                  R                  [         R                  5      n[         R                  " U5      n[        [        [        U5      5      n[         R                  " [         R                  " [        U5      5      U5      nXSS24   nX4   n[!        X5      n	[         R"                  " U5      SS n
Xy-
  n[        R$                  " X5      n[         R&                  " X5      n[         R(                  " U Vs/ s H)  n[        U5      S:  a  UR+                  5       SS OSS/PM+     sn5      nUSS=== U
R-                  SS5      -  sss& X   nX   nXO   nXSS2S4      nUSS2S4   nUSS2S4   n[         R.                  " USS	9[0        R2                  :  n[         R4                  " [         R6                  " U5      [0        R2                  :  SS	9n[         R8                  " UU5      nU R:                  UU      nXU      UU   R-                  SSS5      S
-  -  nUU-  R=                  SS	9nUR?                  SS	9nUUU4   UU'   UUU4   UU'   UUU4   UU'   US
-  nUUU4$ s  snf )aw  
Given a mesh and a list of points find the closest point
on any triangle.

Parameters
----------
mesh : trimesh.Trimesh
  Mesh to query
points : (m, 3) float
  Points in space

Returns
----------
closest : (m, 3) float
  Closest point on triangles for each point
distance : (m,)  float
  Distance to mesh.
triangle_id : (m,) int
  Index of triangle containing closest point
r   r   r   Nr   r   r-   r   r.   r0   ) r   r   r   r   r   r   r+   r1   r2   r3   concatenater    mapr5   repeataranger7   cumsumr
   array_splitint32argsortr   ptpr   r   allabsbitwise_andface_normalsr9   argmax)r"   r#   r)   r1   all_candidatesnum_candidates	tile_idxsquery_point	query_triquery_closequery_groupquery_vectorquery_distanceqdsqdidxs
two_points	two_diststwo_candidatesresult_close
result_tidresult_distancecheck_distancecheck_magnitudec_masknormalsvectorsdotsc_idxss                                r*   r   r   y   s   * ]]64F==))011 d+J ##BJJ/I ^^J/N#c:./N		"))CK0.AIA&K)I !8K))N+CR0K ,L&&|BN ..
5C883O3RR1RZZ\"1%1a&@3OPDH##B**H "J$I#)N AqDz*L1%J1oO VVIA.:NffRVVI.:CO ^^NO<F v 67G<(9V+<+D+DRA+NRU+UUGg"""*D [[a[ F (7Jv'7OF%ffn5L O*44U Ps   0Kc                 4   [         R                  " U[         R                  S9n[        X5      u  p#nU[        R
                  :  nUR                  5       (       d  U$ [         R                  " U5      S   nU R                  U   nX   Xe   R                  [        X   X%   -
  Xe   5      -  R                  -
  n[        U R                  XE      U5      nU[        R
                  * :  US[        R
                  -   :  -  R                  SS9) n	[         R                  " [        XeU	      XU	      Xy   -
  5      5      n
X5U	   ==   SU
-  -  ss'   U R                  R                  XU	)       5      nUR!                  ["        5      S-  S-
  n
X5U	)    ==   U
-  ss'   U$ )a  
Find the signed distance from a mesh to a list of points.

* Points OUTSIDE the mesh will have NEGATIVE distance
* Points within tol.merge of the surface will have POSITIVE distance
* Points INSIDE the mesh will have POSITIVE distance

Parameters
-----------
mesh : trimesh.Trimesh
  Mesh to query.
points : (n, 3) float
  Points in space

Returns
----------
signed_distance : (n,) float
  Signed distance from point to mesh
r   r   r   r.   g      r-   g      ?)r   r   r   r   r   r   anywhererR   Tr
   r	   r1   signraycontains_pointsastypeint)r"   r#   rB   rC   r@   nonzerori   
projectionbarycentric
ontrianglerq   insides               r*   signed_distancer{      s   * ]]64F &34%@"G{ "G;;== hhw"G,G6?W-==w?OPQ
!		  ({7K(LjYK


"{Q]'B	CHHaHPJ 77J'(:&'**@@	
D Z !TD[0! XX%%fj[-A&BCFMM#"c)D j[!"d*"O    c                   $    \ rS rSrSrS rS rSrg)NearestQueryResulti   zF
Stores the nearest points and attributes for nearest points queries.
c                 f    S U l         S U l        S U l        S U l        S U l        S U l        S U l        g N)nearest	distancesri   triangle_indicesbarycentric_coordinatesinterpolated_normalsvertex_indicesselfs    r*   __init__NearestQueryResult.__init__%  s7     $'+$$(!"r|   c                 H    U R                   S L=(       d    U R                  S L$ r   )ri   r   r   s    r*   has_normalsNearestQueryResult.has_normals.  s!    ||4'P4+D+DD+PPr|   )r   r   r   r   ri   r   r   N)__name__
__module____qualname____firstlineno____doc__r   r   __static_attributes__ r|   r*   r~   r~      s    #Qr|   r~   c                   :    \ rS rSrSrS r\S 5       rS rS r	Sr
g)	ProximityQueryi2  z)
Proximity queries for the current mesh.
c                     Xl         g r   _mesh)r   r"   s     r*   r   ProximityQuery.__init__7  s    
r|   c                 *    [        U R                  US9$ )a]  
Given list of points, for each point find the closest point
on any triangle of the mesh.

Parameters
----------
points : (m,3) float, points in space

Returns
----------
closest : (m, 3) float
  Closest point on triangles for each point
distance : (m,) float
  Distance to surface
triangle_id : (m,) int
  Index of closest triangle for each point.
)r"   r#   )r   r   r   r#   s     r*   
on_surfaceProximityQuery.on_surface:  s    & $**V<<r|   c                 P    U R                   R                  nUR                  U5      $ )a  
Given a set of points, return the closest vertex index to each point

Parameters
----------
points : (n, 3) float
  Points in space

Returns
----------
distance : (n,) float
   Distance from source point to vertex.
vertex_id : (n,) int
  Index of mesh.vertices for closest vertex.
)r   r%   r   )r   r#   trees      r*   vertexProximityQuery.vertexO  s"      zz  zz&!!r|   c                 .    [        U R                  U5      $ )a  
Find the signed distance from a mesh to a list of points.

* Points OUTSIDE the mesh will have NEGATIVE distance
* Points within tol.merge of the surface will have POSITIVE distance
* Points INSIDE the mesh will have POSITIVE distance

Parameters
-----------
points : (n, 3) float
  Points in space

Returns
----------
signed_distance : (n,) float
  Signed distance from point to mesh.
)r{   r   r   s     r*   r{   ProximityQuery.signed_distanceb  s    $ tzz622r|   r   N)r   r   r   r   r   r   r   r   r   r{   r   r   r|   r*   r   r   2  s*     = =("&3r|   r   c           	         [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      e[         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      e[        U5      [        U5      :w  a  [        S5      eU R                  R                  XSSS9u  p4n[        U5      S:  a$  [         R                  R                  XQU   -
  S	S
9nO[         R                  " / 5      nXF[        R                  :     nXf[        R                  :     n[         R                  " [         R                  " [        U5      5      U5      n[         R                   " XG45      n[         R                   " U[         R"                  " [         R$                  [        U5      5      45      n['        XF5      $ )a1  
Find the lengths of the longest rays which do not intersect the mesh
cast from a list of points in the provided directions.

Parameters
-----------
points : (n, 3) float
  Points in space.
directions : (n, 3) float
  Directions of rays.

Returns
----------
signed_distance : (n,) float
  Length of rays.
r   r   r   zdirections must be (n,3)!z1number of points must equal number of directions!T)return_locationsmultiple_hitsr   r   r.   )r   r   r   r   r   r   r5   rr   intersects_idlinalgnormr6   r   planar	setdiff1drI   rF   rH   infr   )r"   r#   
directions_facesrays	locationsr   no_intersectionss           r*   longest_rayr   w  sf   " ]]64F==))011z<J==W--455
6{c*o%LMM"hh44T 5 F) 4y1}IINN9d|#;!ND	HHRL	 CJJ&'Dcjj01I ||BIIc&k$:DA>>423D	299RVVSAQ=R+STUIT%%r|   c                 $   [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      eUbn  [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      e[        U5      [        U5      :w  a  [        S5      eOU R                  [        X5      S      nU(       a  U* n[        XU5      nUS-  n[         R                  " [        U5      [        S9n[         R                  " [         R                  " U5      5      S    H  n	[         R                  " U R                  X   -
  X9   5      n
U
R!                  5       ["        R$                  :  a  [         R&                  Xy'   S	X'   Me  U R                  U
R)                  5          n[         R                  " XU	   -
  XU	   -
  5      S[         R                  " XU	   -
  X9   5      -  -  Xy'   M     X[         R*                  " UR-                  S
S5      5      -  -   n[         R.                  [         R.                  [         R.                  /U[         R                  " U5      '   [         R0                  R3                  U R4                  S   U R4                  S   -
  5      nXM-  nSnUR7                  5       S:  Ga;  X:  Ga5  US-  nU R8                  R;                  X   5      u  nnn[         R<                  " U[         R0                  R3                  X   X   -
  SS9-
  5      ["        R$                  :  nS	U[         R                  " U5      S   U   '   UU)    X   -
  nXx   R?                  5       n[A        UU5      S[A        UX8   5      -  -  Xx'   X   X8   UU   R-                  S
S5      -  -   X'   UXx   -
  U:  nS	U[         R                  " U5      S   U   '   UR7                  5       S:  a  X:  a  GM5  X4$ )a  
Find the center and radius of the sphere which is tangent to
the mesh at the given point and at least one more point with no
non-tangential intersections with the mesh.

Masatomo Inui, Nobuyuki Umezu & Ryohei Shimane (2016)
Shrinking sphere:
A parallel algorithm for computing the thickness of 3D objects,
Computer-Aided Design and Applications, 13:2, 199-207,
DOI: 10.1080/16864360.2015.1084186

Parameters
----------
points : (n, 3) float
  Points in space.
inwards : bool
  Whether to have the sphere inside or outside the mesh.
normals : (n, 3) float or None
  Normals of the mesh at the given points
  if is None computed automatically.

Returns
----------
centers : (n,3) float
  Centers of spheres
radii : (n,) float
  Radii of spheres
r   r   r   normals must be (n,3)!.number of points must equal number of normals!r-   r0   r   Fr   r   r.   )!r   r   r   r   r   r   r5   rR   r   r   onesboolro   isinfdotr   maxr   r   r   rS   
nan_to_numr   nanr   r   r'   r9   r   r   rP   copyr
   )r"   r#   inwardsri   	thresholdmax_iterr   radiinot_convergedr<   projectionsr   centersDconvergence_thresholdn_itern_pointsn_dists_n_facesdonediff	old_radiicvgeds                          r*   max_tangent_spherer     s   > ]]64F==))011--rzz:}}Wg..566v;#g,&MNN ' ##M$$?$BC( D'2IOEGGCKt4M XXbhhy)*1-ffT]]VY6
C
 ??szz)vvEH$M]];#5#5#78Fvvfay0&!92DEBFF61I-wz::EH .  u}}R/C!DDDG "7GBHHUO 			t{{1~A67A%MF




!f&7!&*ll&=&=g>T&U#'8
 FF))..!7&:O!OVW.XY jj	 	 ;@bhh}-a067 !66(--/	+D$7T7#9:: 
 "(!69ORWS

'"a.: "
 E003HH;@bhh}-a0789 



!f&7< >r|   c                 N   [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      eUbn  [         R                  " U[         R                  S9n[        R                  " US5      (       d  [        S5      e[        U5      [        U5      :w  a  [        S5      eOU R                  [        X5      S      nUS:X  a  [        XU(       + US9u  pVUS-  nU$ US	:X  a   U(       a  [        XU5      $ [        XU* 5      $ [        S
5      e)a  
Find the thickness of the mesh at the given points.

Parameters
----------
points : (n, 3) float
  Points in space
exterior : bool
  Whether to compute the exterior thickness
  (a.k.a. reach)
normals : (n, 3) float
  Normals of the mesh at the given points
  If is None computed automatically.
method : string
  One of 'max_sphere' or 'ray'

Returns
----------
thickness : (n,) float
  Thickness at given points.
r   r   r   r   r   r-   
max_sphere)r"   r#   r   ri   rr   z)Invalid method, use "max_sphere" or "ray")r   r   r   r   r   r   r5   rR   r   r   r   )r"   r#   exteriorri   method_centersradius	thicknesss           r*   r   r     s
   , ]]64F==))011--rzz:}}Wg..566v;#g,&MNN ' ##M$$?$BC-(lG
 QJ		5tW55tgX66DEEr|   )TNgư>d   )FNr   )r   numpyr    r   	constantsr   r   groupingr   r1   r   r7   r	   r
   scipy.spatialr   BaseExceptionE
exceptionsr   r+   rD   r{   r~   r   r   r   r   r   r|   r*   <module>r      s      $  6 , "%*Z0*f^5BCLQ Q$B3 B3J,&` HKpf1FM  ",q!G"s   A# #B)A<<B