def intersect_segment_with_plane(start_points, segment_vectors,
points_on_plane, plane_normals):
"""
Check for intersections between a line segment and a plane, or pairwise
between a stack of line segments and a stack of planes.
"""
orig_shape = start_points.shape
start_points, _, transform_result = columnize(start_points, (-1, 3),
name="start_points")
vg.shape.check(locals(), "segment_vectors", orig_shape)
vg.shape.check(locals(), "points_on_plane", orig_shape)
vg.shape.check(locals(), "plane_normals", orig_shape)
# Compute t values such that
# `result = reference_point + t * segment_vectors`.
t = np.nan_to_num(
vg.dot(points_on_plane - start_points, plane_normals) /
vg.dot(segment_vectors, plane_normals))
intersection_points = start_points + t.reshape(-1, 1) * segment_vectors
# Discard points which lie past the ends of the segment.
intersection_points[t < 0] = np.nan
intersection_points[t > 1] = np.nan
return transform_result(intersection_points)
def closest_point_of_line_segment(points, start_points, segment_vectors):
# Adapted from public domain algorithm
# https://gdbooks.gitbooks.io/3dcollisions/content/Chapter1/closest_point_on_line.html
k = vg.shape.check(locals(), "points", (-1, 3))
vg.shape.check(locals(), "start_points", (k, 3))
vg.shape.check(locals(), "segment_vectors", (k, 3))
# Compute t values such that
# `result = reference_point + t * vector_along_line`.
square_of_segment_lengths = vg.dot(segment_vectors, segment_vectors)
# Degenerate segments will cause a division by zero, so handle that.
t = np.nan_to_num(
vg.dot(points - start_points, segment_vectors) /
square_of_segment_lengths)
# When `0 <= t <= 1`, the point is on the segment. When `t < 0`, the
# closest point is the segment start. When `t > 1`, the closest point is
# the segment end.
#
# Start with the `0 <= t <= 1 case`, then use masks to apply the clamp.
result = start_points + t.reshape(-1, 1) * segment_vectors
clamped_to_start_point = t < 0
result[clamped_to_start_point] = start_points[clamped_to_start_point]
clamped_to_end_point = t > 1
result[clamped_to_end_point] = (start_points[clamped_to_end_point] +
segment_vectors[clamped_to_end_point])
return result
def test_dot_mixed():
v1 = np.array([[1.0, 0.0, -1.0], [1.0, 2.0, 3.0]])
v2 = np.array([4.0, 5.0, 6.0])
expected = np.array([-2.0, 32.0])
np.testing.assert_array_almost_equal(vg.dot(v1, v2), expected)
np.testing.assert_array_almost_equal(vg.dot(v2, v1), expected)
def test_dot():
v1 = np.array([1.0, 2.0, 3.0])
v2 = np.array([4.0, 5.0, 6.0])
expected = 32.0
np.testing.assert_array_almost_equal(vg.dot(v1, v2), expected)
np.testing.assert_array_almost_equal(vg.dot(v2, v1), expected)
def test_dot_error():
v1 = np.array([[1.0, 0.0, -1.0], [1.0, 2.0, 3.0]])
v2 = np.array([[4.0, 5.0, 6.0]])
with pytest.raises(
ValueError,
match="v2 must be an array with shape \\(2, 3\\); got \\(1, 3\\)"):
vg.dot(v1, v2)
v1 = np.array([[1.0, 0.0, -1.0], [1.0, 2.0, 3.0]])
v2 = np.array([[[4.0, 5.0, 6.0]]])
with pytest.raises(
ValueError,
match="Not sure what to do with 2 dimensions and 3 dimensions"):
vg.dot(v1, v2)
def coplanar_points_are_on_same_side_of_line(a, b, p1, p2, atol=1e-8):
"""
Using "same-side technique" from http://blackpawn.com/texts/pointinpoly/default.html
"""
along_line = b - a
return vg.dot(vg.cross(along_line, p1 - a), vg.cross(along_line,
p2 - a)) >= -atol
def signed_distance_to_plane(points, plane_equations):
"""
Return the signed distances from each point to the corresponding plane.
For convenience, can also be called with a single point and a single
plane.
"""
k = check_shape_any(points, (3, ), (-1, 3), name="points")
check_shape_any(plane_equations, (4, ), (-1 if k is None else k, 4),
name="plane_equations")
normals, offsets = normal_and_offset_from_plane_equations(plane_equations)
return vg.dot(points, normals) + offsets
def plane_equation_from_points(points):
"""
Given many sets of three points, return a stack of plane equations
[`A`, `B`, `C`, `D`] which satisfy `Ax + By + Cz + D = 0`. Also
works on three points to return a single plane equation.
These coefficients can be decomposed into the plane normal vector
which is `[A, B, C]` and the offset `D`, either by the caller or
by using `normal_and_offset_from_plane_equations()`.
"""
points, _, transform_result = columnize(points, (-1, 3, 3), name="points")
p1s = points[:, 0]
unit_normals = plane_normal_from_points(points)
D = -vg.dot(p1s, unit_normals)
return transform_result(np.hstack([unit_normals, D.reshape(-1, 1)]))
def coplanar_points_are_on_same_side_of_line(a, b, p1, p2):
"""
Test if the given points are on the same side of the given line.
Args:
a (np.arraylike): The first 3D point of interest.
b (np.arraylike): The second 3D point of interest.
p1 (np.arraylike): A first point which lies on the line of interest.
p2 (np.arraylike): A second point which lies on the line of interest.
Returns:
bool: `True` when `a` and `b` are on the same side of the line defined
by `p1` and `p2`.
"""
check_shape_any(a, (3,), (-1, 3), name="a")
vg.shape.check(locals(), "b", a.shape)
vg.shape.check(locals(), "p1", a.shape)
vg.shape.check(locals(), "p2", a.shape)
# Uses "same-side technique" from http://blackpawn.com/texts/pointinpoly/default.html
along_line = b - a
return vg.dot(vg.cross(along_line, p1 - a), vg.cross(along_line, p2 - a)) >= 0
