# coding=utf-8
"""Utility functions for computing intersections between geometry in 2D space.
Taken mostly from the euclid package available at
https://pypi.org/project/euclid/
"""
from __future__ import division
import math
from .geometry2d.pointvector import Point2D, Vector2D
def _isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
"""Implementation of the math.isclose method from Python 3.5 onward."""
return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)
[docs]def intersect_line2d(line_ray_a, line_ray_b):
"""Get the intersection between any Ray2D or LineSegment2D objects as a Point2D.
This function calculates scaling parameters for ua and ub where:
A.p + ua * A.v = B.p + ub * B.v
Which represents the intersection point between line A and line B.
The derivation of ua is achieved by crossing both sides of the above equation
with the direction vector of B, and rearranging the formula:
.. code-block:: python
A.p + ua * A.v = B.p + ub * B.v
(A.p + ua * A.v) x B.v = (B.p + ub * B.v) x B.v # Cross both sides with B.v
(A.p x B.v) + (ua * A.v x B.v) = (B.p x B.v) + (ub * B.v x B.v) # B.v x B.v = 0
ua = (B.p - A.p) x B.v / (A.v x B.v)
Args:
line_ray_a: A LineSegment2D or Ray2D object.
line_ray_b: Another LineSegment2D or Ray2D to intersect.
Returns:
Point2D of intersection if it exists. None if no intersection exists.
"""
# d is the determinant between lines, if 0 lines are collinear
d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
if d == 0:
return None
# (dx, dy) = A.p - B.p
dy = line_ray_a.p.y - line_ray_b.p.y
dx = line_ray_a.p.x - line_ray_b.p.x
# Find parameters ua and ub for intersection between two lines
# Calculate scaling parameter for line_ray_b
ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
# Checks the bounds of ua to ensure it obeys ray/line behavior
if not line_ray_a._u_in(ua):
return None
# Calculate scaling parameter for line_ray_b
ub = (line_ray_a.v.x * dy - line_ray_a.v.y * dx) / d
# Checks the bounds of ub to ensure it obeys ray/line behavior
if not line_ray_b._u_in(ub):
return None
return Point2D(line_ray_a.p.x + ua * line_ray_a.v.x,
line_ray_a.p.y + ua * line_ray_a.v.y)
[docs]def intersect_line_segment2d(line_a, line_b):
"""Get the intersection between two LineSegment2D objects as a Point2D.
This function is identical to intersect_line2d but has some extra checks to
avoid certain cases of floating point tolerance issues. It is only intended
to work with LineSegment2D and not Ray2D.
Args:
line_a: A LineSegment2D object.
line_b: Another LineSegment2D intersect.
Returns:
Point2D of intersection if it exists. None if no intersection exists.
"""
# d is the determinant between lines, if 0 lines are collinear
d = line_b.v.y * line_a.v.x - line_b.v.x * line_a.v.y
if d == 0:
return None
# (dx, dy) = A.p - B.p
dy = line_a.p.y - line_b.p.y
dx = line_a.p.x - line_b.p.x
# Find parameters ua and ub for intersection between two lines
# Calculate scaling parameter for line_b
ua = (line_b.v.x * dy - line_b.v.y * dx) / d
# Checks the bounds of ua to ensure it obeys ray/line behavior
if not line_a._u_in(ua):
return None
# Calculate scaling parameter for line_b
ub = (line_a.v.x * dy - line_a.v.y * dx) / d
# Checks the bounds of ub to ensure it obeys ray/line behavior
if not line_b._u_in(ub):
return None
# compute the intersection point
int_pta = Point2D(line_a.p.x + ua * line_a.v.x, line_a.p.y + ua * line_a.v.y)
int_ptb = Point2D(line_b.p.x + ub * line_b.v.x, line_b.p.y + ub * line_b.v.y)
# if the two points are unequal, there's a floating point tolerance issue
if _isclose(int_pta.x, int_ptb.x) and _isclose(int_pta.y, int_ptb.y):
return int_pta
return None
[docs]def intersect_line2d_infinite(line_ray_a, line_ray_b):
"""Get intersection between a Ray2D/LineSegment2D and another extended infinitely.
Args:
line_ray_a: A LineSegment2D or Ray2D object.
line_ray_b: A LineSegment2D or Ray2D that will be extended infinitely
for intersection.
Returns:
Point2D of intersection if it exists. None if no intersection exists.
"""
d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
if d == 0:
return None
dy = line_ray_a.p.y - line_ray_b.p.y
dx = line_ray_a.p.x - line_ray_b.p.x
ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
if not line_ray_a._u_in(ua):
return None
return Point2D(line_ray_a.p.x + ua * line_ray_a.v.x,
line_ray_a.p.y + ua * line_ray_a.v.y)
[docs]def does_intersection_exist_line2d(line_ray_a, line_ray_b):
"""Boolean denoting whether an intersection exists between Ray2D or LineSegment2D.
This is slightly faster than actually computing the intersection but should only be
used in cases where the actual point of intersection is not needed.
Args:
line_ray_a: A LineSegment2D or Ray2D object.
line_ray_b: Another LineSegment2D or Ray2D to intersect.
Returns:
True if an intersection exists. False if no intersection exists.
"""
d = line_ray_b.v.y * line_ray_a.v.x - line_ray_b.v.x * line_ray_a.v.y
if d == 0:
return False
dy = line_ray_a.p.y - line_ray_b.p.y
dx = line_ray_a.p.x - line_ray_b.p.x
ua = (line_ray_b.v.x * dy - line_ray_b.v.y * dx) / d
if not line_ray_a._u_in(ua):
return False
ub = (line_ray_a.v.x * dy - line_ray_a.v.y * dx) / d
if not line_ray_b._u_in(ub):
return False
return True
[docs]def intersect_line2d_arc2d(line_ray, arc):
"""Get the intersection between any Ray2D/LineSegment2D and an Arc2D.
Args:
line_ray: A LineSegment2D or Ray2D object.
arc: An Arc2D object along which the closest point will be determined.
Returns:
A list of 2 Point2D objects if a full intersection exists.
A list with a single Point2D object if the line is tangent or intersects
only once. None if no intersection exists.
"""
a = line_ray.v.magnitude_squared
b = 2 * (line_ray.v.x * (line_ray.p.x - arc.c.x) +
line_ray.v.y * (line_ray.p.y - arc.c.y))
c = arc.c.magnitude_squared + line_ray.p.magnitude_squared - \
2 * arc.c.dot(line_ray.p) - arc.r ** 2
det = b ** 2 - 4 * a * c
if det < 0:
return None
sq = math.sqrt(det)
u1 = (-b + sq) / (2 * a)
u2 = (-b - sq) / (2 * a)
pt1 = Point2D(line_ray.p.x + u1 * line_ray.v.x,
line_ray.p.y + u1 * line_ray.v.y) if line_ray._u_in(u1) else None
pt2 = Point2D(line_ray.p.x + u2 * line_ray.v.x,
line_ray.p.y + u2 * line_ray.v.y) if line_ray._u_in(u2) else None
if u1 == u2: # Tangent
pt = Point2D(line_ray.p.x + u1 * line_ray.v.x,
line_ray.p.y + u1 * line_ray.v.y)
return pt if arc._pt_in(pt) else None
pts = [p for p in (pt1, pt2) if p is not None and arc._pt_in(p)]
return pts if len(pts) != 0 else None
[docs]def intersect_line2d_infinite_arc2d(line_ray, arc):
"""Get intersection between an Arc2D and a Ray2D/LineSegment2D extended infinitely.
Args:
line_ray: A LineSegment2D or Ray2D that will be extended infinitely
for intersection.
arc: An Arc2D object along which the closest point will be determined.
Returns:
A list of 2 Point2D objects if a full intersection exists.
A list with a single Point2D object if the line is tangent or intersects
only once. None if no intersection exists.
"""
a = line_ray.v.magnitude_squared
b = 2 * (line_ray.v.x * (line_ray.p.x - arc.c.x) +
line_ray.v.y * (line_ray.p.y - arc.c.y))
c = arc.c.magnitude_squared + line_ray.p.magnitude_squared - \
2 * arc.c.dot(line_ray.p) - arc.r ** 2
det = b ** 2 - 4 * a * c
if det < 0:
return None
sq = math.sqrt(det)
u1 = (-b + sq) / (2 * a)
u2 = (-b - sq) / (2 * a)
if u1 == u2: # Tangent
pt = Point2D(line_ray.p.x + u1 * line_ray.v.x, line_ray.p.y + u1 * line_ray.v.y)
return pt if arc._pt_in(pt) else None
pt1 = Point2D(line_ray.p.x + u1 * line_ray.v.x, line_ray.p.y + u1 * line_ray.v.y)
pt2 = Point2D(line_ray.p.x + u2 * line_ray.v.x, line_ray.p.y + u2 * line_ray.v.y)
pts = [p for p in (pt1, pt2) if arc._pt_in(p)]
return pts if len(pts) != 0 else None
[docs]def closest_point2d_on_line2d(point, line_ray):
"""Get the closest Point2D on a LineSegment2D or Ray2D to the input point.
Args:
point: A Point2D object.
line_ray: A LineSegment2D or Ray2D object along which the closest point
will be determined.
Returns:
Point2D for the closest point on the line_ray to point.
"""
d = line_ray.v.magnitude_squared
if d == 0: # zero-length segment; just return the end point
return line_ray.p
u = ((point.x - line_ray.p.x) * line_ray.v.x +
(point.y - line_ray.p.y) * line_ray.v.y) / d
if not line_ray._u_in(u):
u = max(min(u, 1.0), 0.0)
return Point2D(line_ray.p.x + u * line_ray.v.x, line_ray.p.y + u * line_ray.v.y)
[docs]def closest_point2d_on_line2d_infinite(point, line_ray):
"""Get the closest Point2D on a Ray2D/LineSegment2D extended infinitely.
Args:
point: A Point2D object.
line_ray: A LineSegment2D or Ray2D object that will be extended infinitely
to determine where the closest point lies.
Returns:
Point2D for the closest point on the line_ray to point.
"""
d = line_ray.v.magnitude_squared
if d == 0: # zero-length segment; just return the end point
return line_ray.p
u = ((point.x - line_ray.p.x) * line_ray.v.x +
(point.y - line_ray.p.y) * line_ray.v.y) / d
return Point2D(line_ray.p.x + u * line_ray.v.x, line_ray.p.y + u * line_ray.v.y)
[docs]def closest_point2d_between_line2d(line_ray_a, line_ray_b):
"""Get the two closest Point2D between two LineSegment2D objects.
When the closest point on one of the segments lies in the middle of the
segment, this will be accounted for. Also note that the line segments
should not intersect for the result to be valid.
Args:
line_ray_a: A LineSegment2D object.
line_ray_b: Another LineSegment2D to which closest points will
be determined.
Returns:
A tuple with two elements
- dists[0]: The distance between the two LineSegment2D objects.
- pts[0]: A tuple of two Point2D objects representing:
1) The point on line_ray_a that is closest to line_ray_b
2) The point on line_ray_b that is closest to line_ray_a
"""
# one of the 4 endpoints must be a closest point
pt_1 = closest_point2d_on_line2d(line_ray_a.p, line_ray_b)
dist_1 = pt_1.distance_to_point(line_ray_a.p)
a_p2 = line_ray_a.p2
pt_2 = closest_point2d_on_line2d(a_p2, line_ray_b)
dist_2 = pt_2.distance_to_point(a_p2)
pt_3 = closest_point2d_on_line2d(line_ray_b.p, line_ray_a)
dist_3 = pt_3.distance_to_point(line_ray_b.p)
b_p2 = line_ray_b.p2
pt_4 = closest_point2d_on_line2d(b_p2, line_ray_a)
dist_4 = pt_4.distance_to_point(b_p2)
# sort the closest points based on their distance
dists = [dist_1, dist_2, dist_3, dist_4]
pts = [(line_ray_a.p, pt_1), (a_p2, pt_2), (pt_3, line_ray_b.p), (pt_4, b_p2)]
dists, i = zip(*sorted(zip(dists, range(len(pts)))))
return dists[0], pts[i[0]]
[docs]def closest_end_point2d_between_line2d(line_a, line_b):
"""Get the two closest end Point2Ds between two LineSegment2D objects.
The result will always be composed of endpoints of the segments and will not
account for cases where the closest point lies in the middle of a segment.
For cases where such middle points are important, the
closest_point2d_between_line2d method should be used.
Args:
line_a: A LineSegment2D object.
line_b: Another LineSegment2D to which closest points will
be determined.
Returns:
A tuple with two elements
- dist: The distance between the two endpoints objects.
- pts: A tuple of two Point2D objects representing:
1) The end point on line_a that is closest to line_b
2) The end point on line_b that is closest to line_a
"""
# one of the 4 endpoints must be a closest point
pts = [
(line_a.p1, line_b.p1), (line_a.p1, line_b.p2),
(line_a.p2, line_b.p1), (line_a.p2, line_b.p2)
]
dists = [p1.distance_to_point(p2) for p1, p2 in pts]
# sort the closest points based on their distance
dists, i = zip(*sorted(zip(dists, range(len(pts)))))
return dists[0], pts[i[0]]
[docs]def closest_point2d_on_arc2d(point, arc):
"""Get the closest Point2D on a Arc2D to the input point.
Args:
point: A Point2D object.
arc: An Arc2D object along which the closest point will be determined.
Returns:
Point2D for the closest point on arc to point.
"""
v = point - arc.c
v = v.normalize() * arc.r
if arc.is_circle:
return Point2D(arc.c.x + v.x, arc.c.y + v.y)
else:
a = Vector2D(1, 0).angle_counterclockwise(v)
if (not arc.is_inverted and arc.a1 < a < arc.a2) or \
(arc.is_inverted and arc.a1 > a > arc.a2):
return Point2D(arc.c.x + v.x, arc.c.y + v.y)
else:
if arc.p1.distance_to_point(point) <= arc.p2.distance_to_point(point):
return arc.p1
return arc.p2