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