Coverage for trimesh/proximity.py: 85%
197 statements
« prev ^ index » next coverage.py v7.14.1, created at 2026-07-31 23:55 +0000
1"""
2proximity.py
3---------------
5Query mesh- point proximity.
6"""
8import numpy as np
10from . import util
11from .constants import log_time, tol
12from .grouping import group_min
13from .triangles import closest_point as _corresponding
14from .triangles import points_to_barycentric
15from .typed import ArrayLike
16from .util import diagonal_dot
18try:
19 from scipy.spatial import cKDTree
20except BaseException as E:
21 from .exceptions import ExceptionWrapper
23 cKDTree = ExceptionWrapper(E)
26def nearby_faces(mesh, points: ArrayLike):
27 """
28 For each point find nearby faces relatively quickly.
30 The closest point on the mesh to the queried point is guaranteed to be
31 on one of the faces listed.
33 Does this by finding the nearest vertex on the mesh to each point, and
34 then returns all the faces that intersect the axis aligned bounding box
35 centered at the queried point and extending to the nearest vertex.
37 Parameters
38 ----------
39 mesh : trimesh.Trimesh
40 Mesh to query.
41 points : (n, 3) float
42 Points in space
44 Returns
45 -----------
46 candidates : (points,) int
47 Sequence of indexes for mesh.faces
48 """
49 points = np.asanyarray(points, dtype=np.float64)
50 # empty points lists should get empty candidates
51 if len(points) == 0:
52 return []
54 # mishapen points should error
55 if not util.is_shape(points, (-1, 3)):
56 raise ValueError("points must be (n,3)!")
58 # an r-tree containing the axis aligned bounding box for every triangle
59 rtree = mesh.triangles_tree
60 # a kd-tree containing every vertex of the mesh
61 kdtree = cKDTree(mesh.vertices[mesh.referenced_vertices])
63 # query the distance to the nearest vertex to get AABB of a sphere
64 distance_vertex = kdtree.query(points)[0].reshape((-1, 1))
65 distance_vertex += tol.merge
67 # axis aligned bounds
68 bounds = np.column_stack((points - distance_vertex, points + distance_vertex))
70 try:
71 # use the batch API added in 1.4.0 and fixed to actually work in 1.4.1
72 hit_ids, hit_counts = rtree.intersection_v(bounds[:, :3], bounds[:, 3:])
73 return np.array_split(hit_ids, np.cumsum(hit_counts)[:-1])
74 except BaseException:
75 # fall back to a list comprehension
76 return [list(rtree.intersection(b)) for b in bounds]
79def closest_point_naive(mesh, points):
80 """
81 Given a mesh and a list of points find the closest point
82 on any triangle.
84 Does this by constructing a very large intermediate array and
85 comparing every point to every triangle.
87 Parameters
88 ----------
89 mesh : Trimesh
90 Takes mesh to have same interfaces as `closest_point`
91 points : (m, 3) float
92 Points in space
94 Returns
95 ----------
96 closest : (m, 3) float
97 Closest point on triangles for each point
98 distance : (m,) float
99 Distances between point and triangle
100 triangle_id : (m,) int
101 Index of triangle containing closest point
102 """
103 # get triangles from mesh
104 triangles = mesh.triangles.view(np.ndarray)
105 # establish that input points are sane
106 points = np.asanyarray(points, dtype=np.float64)
107 if not util.is_shape(triangles, (-1, 3, 3)):
108 raise ValueError("triangles shape incorrect")
109 if not util.is_shape(points, (-1, 3)):
110 raise ValueError("points must be (n,3)")
112 # create a giant tiled array of each point tiled len(triangles) times
113 points_tiled = np.tile(points, (1, len(triangles)))
114 on_triangle = np.array(
115 [_corresponding(triangles, i.reshape((-1, 3))) for i in points_tiled]
116 )
118 # distance squared
119 distance_2 = [((i - q) ** 2).sum(axis=1) for i, q in zip(on_triangle, points)]
121 triangle_id = np.array([i.argmin() for i in distance_2])
123 # closest cartesian point
124 closest = np.array([g[i] for i, g in zip(triangle_id, on_triangle)])
125 distance = np.array([g[i] for i, g in zip(triangle_id, distance_2)]) ** 0.5
127 return closest, distance, triangle_id
130def closest_point(mesh, points):
131 """
132 Given a mesh and a list of points find the closest point
133 on any triangle.
135 Parameters
136 ----------
137 mesh : trimesh.Trimesh
138 Mesh to query
139 points : (m, 3) float
140 Points in space
142 Returns
143 ----------
144 closest : (m, 3) float
145 Closest point on triangles for each point
146 distance : (m,) float
147 Distance to mesh.
148 triangle_id : (m,) int
149 Index of triangle containing closest point
150 """
151 points = np.asanyarray(points, dtype=np.float64)
152 if not util.is_shape(points, (-1, 3)):
153 raise ValueError("points must be (n,3)!")
155 # do a tree- based query for faces near each point
156 candidates = nearby_faces(mesh, points)
157 # view triangles as an ndarray so we don't have to recompute
158 # the MD5 during all of the subsequent advanced indexing
159 triangles = mesh.triangles.view(np.ndarray)
161 # create the corresponding list of triangles
162 # and query points to send to the closest_point function
163 all_candidates = np.concatenate(candidates)
165 num_candidates = list(map(len, candidates))
166 tile_idxs = np.repeat(np.arange(len(points)), num_candidates)
167 query_point = points[tile_idxs, :]
169 query_tri = triangles[all_candidates]
171 # do the computation for closest point
172 query_close = _corresponding(query_tri, query_point)
173 query_group = np.cumsum(num_candidates)[:-1]
175 # vectors and distances for
176 # closest point to query point
177 query_vector = query_point - query_close
178 query_distance = util.diagonal_dot(query_vector, query_vector)
180 # get best two candidate indices by arg-sorting the per-query_distances
181 qds = np.array_split(query_distance, query_group)
182 idxs = np.int32([qd.argsort()[:2] if len(qd) > 1 else [0, 0] for qd in qds])
183 idxs[1:] += query_group.reshape(-1, 1)
185 # points, distances and triangle ids for best two candidates
186 two_points = query_close[idxs]
187 two_dists = query_distance[idxs]
188 two_candidates = all_candidates[idxs]
190 # the first candidate is the best result for unambiguous cases
191 result_close = query_close[idxs[:, 0]]
192 result_tid = two_candidates[:, 0]
193 result_distance = two_dists[:, 0]
195 # however: same closest point on two different faces
196 # find the best one and correct triangle ids if necessary
197 check_distance = np.ptp(two_dists, axis=1) < tol.merge
198 check_magnitude = np.all(np.abs(two_dists) > tol.merge, axis=1)
200 # mask results where corrections may be apply
201 c_mask = np.bitwise_and(check_distance, check_magnitude)
203 # get two face normals for the candidate points
204 normals = mesh.face_normals[two_candidates[c_mask]]
205 # compute normalized surface-point to query-point vectors
206 vectors = query_vector[idxs[c_mask]] / two_dists[c_mask].reshape(-1, 2, 1) ** 0.5
207 # compare enclosed angle for both face normals
208 dots = (normals * vectors).sum(axis=2)
210 # take the idx with the most positive angle
211 # allows for selecting the correct candidate triangle id
212 c_idxs = dots.argmax(axis=1)
214 # correct triangle ids where necessary
215 # closest point and distance remain valid
216 result_tid[c_mask] = two_candidates[c_mask, c_idxs]
217 result_distance[c_mask] = two_dists[c_mask, c_idxs]
218 result_close[c_mask] = two_points[c_mask, c_idxs]
220 # we were comparing the distance squared so
221 # now take the square root in one vectorized operation
222 result_distance **= 0.5
224 return result_close, result_distance, result_tid
227def signed_distance(mesh, points):
228 """
229 Find the signed distance from a mesh to a list of points.
231 * Points OUTSIDE the mesh will have NEGATIVE distance
232 * Points within tol.merge of the surface will have POSITIVE distance
233 * Points INSIDE the mesh will have POSITIVE distance
235 Parameters
236 -----------
237 mesh : trimesh.Trimesh
238 Mesh to query.
239 points : (n, 3) float
240 Points in space
242 Returns
243 ----------
244 signed_distance : (n,) float
245 Signed distance from point to mesh
246 """
247 # make sure we have a numpy array
248 points = np.asanyarray(points, dtype=np.float64)
250 # find the closest point on the mesh to the queried points
251 closest, distance, triangle_id = closest_point(mesh, points)
253 # we only care about nonzero distances
254 nonzero = distance > tol.merge
256 if not nonzero.any():
257 return distance
259 # For closest points that project directly in to the triangle, compute sign from
260 # triangle normal Project each point in to the closest triangle plane
261 nonzero = np.where(nonzero)[0]
262 normals = mesh.face_normals[triangle_id]
263 projection = (
264 points[nonzero]
265 - (
266 normals[nonzero].T
267 * diagonal_dot(points[nonzero] - closest[nonzero], normals[nonzero])
268 ).T
269 )
271 # Determine if the projection lies within the closest triangle
272 barycentric = points_to_barycentric(mesh.triangles[triangle_id[nonzero]], projection)
273 ontriangle = ~(
274 ((barycentric < -tol.merge) | (barycentric > 1 + tol.merge)).any(axis=1)
275 )
277 # Where projection does lie in the triangle, compare vector to projection to the
278 # triangle normal to compute sign
279 sign = np.sign(
280 diagonal_dot(
281 normals[nonzero[ontriangle]],
282 points[nonzero[ontriangle]] - projection[ontriangle],
283 )
284 )
285 distance[nonzero[ontriangle]] *= -1.0 * sign
287 # For all other triangles, resort to raycasting against the entire mesh
288 inside = mesh.ray.contains_points(points[nonzero[~ontriangle]])
289 sign = (inside.astype(int) * 2) - 1.0
291 # apply sign to previously computed distance
292 distance[nonzero[~ontriangle]] *= sign
294 return distance
297class NearestQueryResult:
298 """
299 Stores the nearest points and attributes for nearest points queries.
300 """
302 def __init__(self):
303 self.nearest = None
304 self.distances = None
305 self.normals = None
306 self.triangle_indices = None
307 self.barycentric_coordinates = None
308 self.interpolated_normals = None
309 self.vertex_indices = None
311 def has_normals(self):
312 return self.normals is not None or self.interpolated_normals is not None
315class ProximityQuery:
316 """
317 Proximity queries for the current mesh.
318 """
320 def __init__(self, mesh):
321 self._mesh = mesh
323 @log_time
324 def on_surface(self, points):
325 """
326 Given list of points, for each point find the closest point
327 on any triangle of the mesh.
329 Parameters
330 ----------
331 points : (m,3) float, points in space
333 Returns
334 ----------
335 closest : (m, 3) float
336 Closest point on triangles for each point
337 distance : (m,) float
338 Distance to surface
339 triangle_id : (m,) int
340 Index of closest triangle for each point.
341 """
342 return closest_point(mesh=self._mesh, points=points)
344 def vertex(self, points):
345 """
346 Given a set of points, return the closest vertex index to each point
348 Parameters
349 ----------
350 points : (n, 3) float
351 Points in space
353 Returns
354 ----------
355 distance : (n,) float
356 Distance from source point to vertex.
357 vertex_id : (n,) int
358 Index of mesh.vertices for closest vertex.
359 """
360 tree = self._mesh.kdtree
361 return tree.query(points)
363 def signed_distance(self, points):
364 """
365 Find the signed distance from a mesh to a list of points.
367 * Points OUTSIDE the mesh will have NEGATIVE distance
368 * Points within tol.merge of the surface will have POSITIVE distance
369 * Points INSIDE the mesh will have POSITIVE distance
371 Parameters
372 -----------
373 points : (n, 3) float
374 Points in space
376 Returns
377 ----------
378 signed_distance : (n,) float
379 Signed distance from point to mesh.
380 """
381 return signed_distance(self._mesh, points)
384def longest_ray(mesh, points, directions):
385 """
386 Find the lengths of the longest rays which do not intersect the mesh
387 cast from a list of points in the provided directions.
389 Parameters
390 -----------
391 points : (n, 3) float
392 Points in space.
393 directions : (n, 3) float
394 Directions of rays.
396 Returns
397 ----------
398 signed_distance : (n,) float
399 Length of rays.
400 """
401 points = np.asanyarray(points, dtype=np.float64)
402 if not util.is_shape(points, (-1, 3)):
403 raise ValueError("points must be (n,3)!")
405 directions = np.asanyarray(directions, dtype=np.float64)
406 if not util.is_shape(directions, (-1, 3)):
407 raise ValueError("directions must be (n,3)!")
409 if len(points) != len(directions):
410 raise ValueError("number of points must equal number of directions!")
412 _faces, rays, locations = mesh.ray.intersects_id(
413 points, directions, return_locations=True, multiple_hits=True
414 )
415 if len(rays) > 0:
416 distances = np.linalg.norm(locations - points[rays], axis=1)
417 else:
418 distances = np.array([])
420 # Reject intersections at distance less than tol.planar
421 rays = rays[distances > tol.planar]
422 distances = distances[distances > tol.planar]
424 # Add infinite length for those with no valid intersection
425 no_intersections = np.setdiff1d(np.arange(len(points)), rays)
426 rays = np.concatenate((rays, no_intersections))
427 distances = np.concatenate((distances, np.repeat(np.inf, len(no_intersections))))
428 return group_min(rays, distances)
431def max_tangent_sphere(
432 mesh, points, inwards=True, normals=None, threshold=1e-6, max_iter=100
433):
434 """
435 Find the center and radius of the sphere which is tangent to
436 the mesh at the given point and at least one more point with no
437 non-tangential intersections with the mesh.
439 Masatomo Inui, Nobuyuki Umezu & Ryohei Shimane (2016)
440 Shrinking sphere:
441 A parallel algorithm for computing the thickness of 3D objects,
442 Computer-Aided Design and Applications, 13:2, 199-207,
443 DOI: 10.1080/16864360.2015.1084186
445 Parameters
446 ----------
447 points : (n, 3) float
448 Points in space.
449 inwards : bool
450 Whether to have the sphere inside or outside the mesh.
451 normals : (n, 3) float or None
452 Normals of the mesh at the given points
453 if is None computed automatically.
455 Returns
456 ----------
457 centers : (n,3) float
458 Centers of spheres
459 radii : (n,) float
460 Radii of spheres
461 """
462 points = np.asanyarray(points, dtype=np.float64)
463 if not util.is_shape(points, (-1, 3)):
464 raise ValueError("points must be (n,3)!")
466 if normals is not None:
467 normals = np.asanyarray(normals, dtype=np.float64)
468 if not util.is_shape(normals, (-1, 3)):
469 raise ValueError("normals must be (n,3)!")
471 if len(points) != len(normals):
472 raise ValueError("number of points must equal number of normals!")
473 else:
474 normals = mesh.face_normals[closest_point(mesh, points)[2]]
476 if inwards:
477 normals = -normals
479 # Find initial tangent spheres
480 distances = longest_ray(mesh, points, normals)
481 radii = distances * 0.5
482 not_converged = np.ones(len(points), dtype=bool) # boolean mask
484 # If ray is infinite, find the vertex which is furthest from our point
485 # when projected onto the ray. I.e. find v which maximises
486 # (v-p).n = v.n - p.n.
487 # We use a loop rather a vectorised approach to reduce memory cost
488 # it also seems to run faster.
489 for i in np.where(np.isinf(distances))[0]:
490 projections = np.dot(mesh.vertices - points[i], normals[i])
492 # If no points lie outside the tangent plane, then the radius is infinite
493 # otherwise we have a point outside the tangent plane, take the one with maximal
494 # projection
495 if projections.max() < tol.planar:
496 radii[i] = np.inf
497 not_converged[i] = False
498 else:
499 vertex = mesh.vertices[projections.argmax()]
500 radii[i] = np.dot(vertex - points[i], vertex - points[i]) / (
501 2 * np.dot(vertex - points[i], normals[i])
502 )
504 # Compute centers
505 centers = points + normals * np.nan_to_num(radii.reshape(-1, 1))
506 centers[np.isinf(radii)] = [np.nan, np.nan, np.nan]
508 # Our iterative process terminates when the difference in sphere
509 # radius is less than threshold*D
510 D = np.linalg.norm(mesh.bounds[1] - mesh.bounds[0])
511 convergence_threshold = threshold * D
512 n_iter = 0
513 while not_converged.sum() > 0 and n_iter < max_iter:
514 n_iter += 1
515 n_points, n_dists, _n_faces = mesh.nearest.on_surface(centers[not_converged])
517 # If the distance to the nearest point is the same as the distance
518 # to the start point then we are done.
519 done = (
520 np.abs(
521 n_dists
522 - np.linalg.norm(centers[not_converged] - points[not_converged], axis=1)
523 )
524 < tol.planar
525 )
526 not_converged[np.where(not_converged)[0][done]] = False
528 # Otherwise find the radius and center of the sphere tangent to the mesh
529 # at the point and the nearest point.
530 diff = n_points[~done] - points[not_converged]
531 old_radii = radii[not_converged].copy()
532 radii[not_converged] = diagonal_dot(diff, diff) / (
533 2 * diagonal_dot(diff, normals[not_converged])
534 )
535 centers[not_converged] = points[not_converged] + normals[not_converged] * radii[
536 not_converged
537 ].reshape(-1, 1)
539 # If change in radius is less than threshold we have converged
540 cvged = old_radii - radii[not_converged] < convergence_threshold
541 not_converged[np.where(not_converged)[0][cvged]] = False
543 return centers, radii
546def thickness(mesh, points, exterior=False, normals=None, method="max_sphere"):
547 """
548 Find the thickness of the mesh at the given points.
550 Parameters
551 ----------
552 points : (n, 3) float
553 Points in space
554 exterior : bool
555 Whether to compute the exterior thickness
556 (a.k.a. reach)
557 normals : (n, 3) float
558 Normals of the mesh at the given points
559 If is None computed automatically.
560 method : string
561 One of 'max_sphere' or 'ray'
563 Returns
564 ----------
565 thickness : (n,) float
566 Thickness at given points.
567 """
568 points = np.asanyarray(points, dtype=np.float64)
569 if not util.is_shape(points, (-1, 3)):
570 raise ValueError("points must be (n,3)!")
572 if normals is not None:
573 normals = np.asanyarray(normals, dtype=np.float64)
574 if not util.is_shape(normals, (-1, 3)):
575 raise ValueError("normals must be (n,3)!")
577 if len(points) != len(normals):
578 raise ValueError("number of points must equal number of normals!")
579 else:
580 normals = mesh.face_normals[closest_point(mesh, points)[2]]
582 if method == "max_sphere":
583 _centers, radius = max_tangent_sphere(
584 mesh=mesh, points=points, inwards=not exterior, normals=normals
585 )
586 thickness = radius * 2
587 return thickness
589 elif method == "ray":
590 if exterior:
591 return longest_ray(mesh, points, normals)
592 else:
593 return longest_ray(mesh, points, -normals)
594 else:
595 raise ValueError('Invalid method, use "max_sphere" or "ray"')