city_retrofit/venv/lib/python3.7/site-packages/trimesh/proximity.py

516 lines
17 KiB
Python
Raw Normal View History

"""
proximity.py
---------------
Query mesh- point proximity.
"""
import numpy as np
from . import util
from .grouping import group_min
from .constants import tol, log_time
from .triangles import closest_point as closest_point_corresponding
def nearby_faces(mesh, points):
"""
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 object
points : (n,3) float , points in space
Returns
-----------
candidates : (points,) int, sequence of indexes for mesh.faces
"""
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)!')
# an r-tree containing the axis aligned bounding box for every triangle
rtree = mesh.triangles_tree
# a kd-tree containing every vertex of the mesh
kdtree = mesh.kdtree
# query the distance to the nearest vertex to get AABB of a sphere
distance_vertex = kdtree.query(points)[0].reshape((-1, 1))
distance_vertex += tol.merge
# axis aligned bounds
bounds = np.column_stack((points - distance_vertex,
points + distance_vertex))
# faces that intersect axis aligned bounding box
candidates = [list(rtree.intersection(b)) for b in bounds]
return candidates
def closest_point_naive(mesh, points):
"""
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
"""
# get triangles from mesh
triangles = mesh.triangles.view(np.ndarray)
# establish that input points are sane
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(triangles, (-1, 3, 3)):
raise ValueError('triangles shape incorrect')
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)')
# create a giant tiled array of each point tiled len(triangles) times
points_tiled = np.tile(points, (1, len(triangles)))
on_triangle = np.array([closest_point_corresponding(
triangles, i.reshape((-1, 3))) for i in points_tiled])
# distance squared
distance_2 = [((i - q)**2).sum(axis=1)
for i, q in zip(on_triangle, points)]
triangle_id = np.array([i.argmin() for i in distance_2])
# closest cartesian point
closest = np.array([g[i] for i, g in zip(triangle_id, on_triangle)])
distance = np.array([g[i] for i, g in zip(triangle_id, distance_2)]) ** .5
return closest, distance, triangle_id
def closest_point(mesh, points):
"""
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
triangle_id : (m,) int
Index of triangle containing closest point
"""
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)!')
# do a tree- based query for faces near each point
candidates = nearby_faces(mesh, points)
# view triangles as an ndarray so we don't have to recompute
# the MD5 during all of the subsequent advanced indexing
triangles = mesh.triangles.view(np.ndarray)
# create the corresponding list of triangles
# and query points to send to the closest_point function
all_candidates = np.concatenate(candidates)
num_candidates = list(map(len, candidates))
tile_idxs = np.repeat(np.arange(len(points)), num_candidates)
query_point = points[tile_idxs, :]
query_tri = triangles[all_candidates]
# do the computation for closest point
query_close = closest_point_corresponding(query_tri, query_point)
query_group = np.cumsum(num_candidates)[:-1]
# vectors and distances for
# closest point to query point
query_vector = query_point - query_close
query_distance = util.diagonal_dot(query_vector, query_vector)
# get best two candidate indices by arg-sorting the per-query_distances
qds = np.array_split(query_distance, query_group)
idxs = np.int32([qd.argsort()[:2] if len(qd) > 1 else [0, 0] for qd in qds])
idxs[1:] += query_group.reshape(-1, 1)
# distances and traingle ids for best two candidates
two_dists = query_distance[idxs]
two_candidates = all_candidates[idxs]
# the first candidate is the best result for unambiguous cases
result_close = query_close[idxs[:, 0]]
result_tid = two_candidates[:, 0]
result_distance = two_dists[:, 0]
# however: same closest point on two different faces
# find the best one and correct triangle ids if necessary
check_distance = two_dists.ptp(axis=1) < tol.merge
check_magnitude = np.all(np.abs(two_dists) > tol.merge, axis=1)
# mask results where corrections may be apply
c_mask = np.bitwise_and(check_distance, check_magnitude)
# get two face normals for the candidate points
normals = mesh.face_normals[two_candidates[c_mask]]
# compute normalized surface-point to query-point vectors
vectors = (query_vector[idxs[c_mask]] /
two_dists[c_mask].reshape(-1, 2, 1) ** 0.5)
# compare enclosed angle for both face normals
dots = (normals * vectors).sum(axis=2)
# take the idx with the most positive angle
# allows for selecting the correct candidate triangle id
c_idxs = dots.argmax(axis=1)
# correct triangle ids where necessary
# closest point and distance remain valid
result_tid[c_mask] = two_candidates[c_mask, c_idxs]
# we were comparing the distance squared so
# now take the square root in one vectorized operation
result_distance **= .5
return result_close, result_distance, result_tid
def signed_distance(mesh, points):
"""
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 object
points : (n,3) float, list of points in space
Returns
----------
signed_distance : (n,3) float, signed distance from point to mesh
"""
# make sure we have a numpy array
points = np.asanyarray(points, dtype=np.float64)
# find the closest point on the mesh to the queried points
closest, distance, triangle_id = closest_point(mesh, points)
# we only care about nonzero distances
nonzero = distance > tol.merge
if not nonzero.any():
return distance
inside = mesh.ray.contains_points(points[nonzero])
sign = (inside.astype(int) * 2) - 1
# apply sign to previously computed distance
distance[nonzero] *= sign
return distance
class ProximityQuery(object):
"""
Proximity queries for the current mesh.
"""
def __init__(self, mesh):
self._mesh = mesh
@log_time
def on_surface(self, points):
"""
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
triangle_id : (m,) int, index of closest triangle for each point
"""
return closest_point(mesh=self._mesh,
points=points)
def vertex(self, points):
"""
Given a set of points, return the closest vertex index to each point
Parameters
----------
points : (n,3) float, list of points in space
Returns
----------
distance : (n,) float, distance from source point to vertex
vertex_id : (n,) int, index of mesh.vertices which is closest
"""
tree = self._mesh.kdtree
return tree.query(points)
def signed_distance(self, points):
"""
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, list of points in space
Returns
----------
signed_distance : (n,3) float, signed distance from point to mesh
"""
return signed_distance(self._mesh, points)
def longest_ray(mesh, points, directions):
"""
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, list of points in space
directions : (n,3) float, directions of rays
Returns
----------
signed_distance : (n,) float, length of rays
"""
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)!')
directions = np.asanyarray(directions, dtype=np.float64)
if not util.is_shape(directions, (-1, 3)):
raise ValueError('directions must be (n,3)!')
if len(points) != len(directions):
raise ValueError('number of points must equal number of directions!')
faces, rays, locations = mesh.ray.intersects_id(points, directions,
return_locations=True,
multiple_hits=True)
if len(rays) > 0:
distances = np.linalg.norm(locations - points[rays],
axis=1)
else:
distances = np.array([])
# Reject intersections at distance less than tol.planar
rays = rays[distances > tol.planar]
distances = distances[distances > tol.planar]
# Add infinite length for those with no valid intersection
no_intersections = np.setdiff1d(np.arange(len(points)), rays)
rays = np.concatenate((rays, no_intersections))
distances = np.concatenate((distances,
np.repeat(np.inf,
len(no_intersections))))
return group_min(rays, distances)
def max_tangent_sphere(mesh,
points,
inwards=True,
normals=None,
threshold=1e-6,
max_iter=100):
"""
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, list of points in space
inwards : bool, whether to have the sphere inside or outside the mesh
normals : (n,3) float, normals of the mesh at the given points
None, compute this automatically.
Returns
----------
centers : (n,3) float, centers of spheres
radii : (n,) float, radii of spheres
"""
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)!')
if normals is not None:
normals = np.asanyarray(normals, dtype=np.float64)
if not util.is_shape(normals, (-1, 3)):
raise ValueError('normals must be (n,3)!')
if len(points) != len(normals):
raise ValueError('number of points must equal number of normals!')
else:
normals = mesh.face_normals[closest_point(mesh, points)[2]]
if inwards:
normals = -normals
# Find initial tangent spheres
distances = longest_ray(mesh, points, normals)
radii = distances * 0.5
not_converged = np.ones(len(points), dtype=np.bool) # boolean mask
# If ray is infinite, find the vertex which is furthest from our point
# when projected onto the ray. I.e. find v which maximises
# (v-p).n = v.n - p.n.
# We use a loop rather a vectorised approach to reduce memory cost
# it also seems to run faster.
for i in np.where(np.isinf(distances))[0]:
projections = np.dot(mesh.vertices - points[i], normals[i])
# If no points lie outside the tangent plane, then the radius is infinite
# otherwise we have a point outside the tangent plane, take the one with maximal
# projection
if projections.max() < tol.planar:
radii[i] = np.inf
not_converged[i] = False
else:
vertex = mesh.vertices[projections.argmax()]
radii[i] = (np.dot(vertex - points[i], vertex - points[i]) /
(2 * np.dot(vertex - points[i], normals[i])))
# Compute centers
centers = points + normals * np.nan_to_num(radii.reshape(-1, 1))
centers[np.isinf(radii)] = [np.nan, np.nan, np.nan]
# Our iterative process terminates when the difference in sphere
# radius is less than threshold*D
D = np.linalg.norm(mesh.bounds[1] - mesh.bounds[0])
convergence_threshold = threshold * D
n_iter = 0
while not_converged.sum() > 0 and n_iter < max_iter:
n_iter += 1
n_points, n_dists, n_faces = mesh.nearest.on_surface(
centers[not_converged])
# If the distance to the nearest point is the same as the distance
# to the start point then we are done.
done = np.abs(
n_dists -
np.linalg.norm(
centers[not_converged] -
points[not_converged],
axis=1)) < tol.planar
not_converged[np.where(not_converged)[0][done]] = False
# Otherwise find the radius and center of the sphere tangent to the mesh
# at the point and the nearest point.
diff = n_points[~done] - points[not_converged]
old_radii = radii[not_converged].copy()
# np.einsum produces element wise dot product
radii[not_converged] = (np.einsum('ij, ij->i',
diff,
diff) /
(2 * np.einsum('ij, ij->i',
diff,
normals[not_converged])))
centers[not_converged] = points[not_converged] + \
normals[not_converged] * radii[not_converged].reshape(-1, 1)
# If change in radius is less than threshold we have converged
cvged = old_radii - radii[not_converged] < convergence_threshold
not_converged[np.where(not_converged)[0][cvged]] = False
return centers, radii
def thickness(mesh,
points,
exterior=False,
normals=None,
method='max_sphere'):
"""
Find the thickness of the mesh at the given points.
Parameters
----------
points : (n,3) float, list of 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
None, compute this automatically.
method : string, one of 'max_sphere' or 'ray'
Returns
----------
thickness : (n,) float, thickness
"""
points = np.asanyarray(points, dtype=np.float64)
if not util.is_shape(points, (-1, 3)):
raise ValueError('points must be (n,3)!')
if normals is not None:
normals = np.asanyarray(normals, dtype=np.float64)
if not util.is_shape(normals, (-1, 3)):
raise ValueError('normals must be (n,3)!')
if len(points) != len(normals):
raise ValueError('number of points must equal number of normals!')
else:
normals = mesh.face_normals[closest_point(mesh, points)[2]]
if method == 'max_sphere':
centers, radius = max_tangent_sphere(mesh=mesh,
points=points,
inwards=not exterior,
normals=normals)
thickness = radius * 2
return thickness
elif method == 'ray':
if exterior:
return longest_ray(mesh, points, normals)
else:
return longest_ray(mesh, points, -normals)
else:
raise ValueError('Invalid method, use "max_sphere" or "ray"')