1import numpy as np
2
3from .. import util
4
5from ..constants import tol_path as tol
6
7
8def line_line(origins,
9              directions,
10              plane_normal=None):
11    """
12    Find the intersection between two lines.
13    Uses terminology from:
14    http://geomalgorithms.com/a05-_intersect-1.html
15
16    line 1:    P(s) = p_0 + sU
17    line 2:    Q(t) = q_0 + tV
18
19    Parameters
20    ---------
21    origins:      (2, d) float, points on lines (d in [2,3])
22    directions:   (2, d) float, direction vectors
23    plane_normal: (3, ) float, if not passed computed from cross
24
25    Returns
26    ---------
27    intersects:   boolean, whether the lines intersect.
28                  In 2D, false if the lines are parallel
29                  In 3D, false if lines are not coplanar
30    intersection: if intersects: (d) length point of intersection
31                  else:          None
32    """
33    # check so we can accept 2D or 3D points
34    origins, is_2D = util.stack_3D(origins, return_2D=True)
35    directions, is_2D = util.stack_3D(directions, return_2D=True)
36
37    # unitize direction vectors
38    directions /= util.row_norm(directions).reshape((-1, 1))
39
40    # exit if values are parallel
41    if np.sum(np.abs(np.diff(directions,
42                             axis=0))) < tol.zero:
43        return False, None
44
45    # using notation from docstring
46    q_0, p_0 = origins
47    v, u = directions
48    w = p_0 - q_0
49
50    # recompute plane normal if not passed
51    if plane_normal is None:
52        # the normal of the plane given by the two direction vectors
53        plane_normal = np.cross(u, v)
54        plane_normal /= np.linalg.norm(plane_normal)
55
56    # vectors perpendicular to the two lines
57    v_perp = np.cross(v, plane_normal)
58    v_perp /= np.linalg.norm(v_perp)
59
60    # if the vector from origin to origin is on the plane given by
61    # the direction vector, the dot product with the plane normal
62    # should be within floating point error of zero
63    coplanar = abs(np.dot(plane_normal, w)) < tol.zero
64    if not coplanar:
65        return False, None
66
67    # value of parameter s where intersection occurs
68    s_I = (np.dot(-v_perp, w) /
69           np.dot(v_perp, u))
70    # plug back into the equation of the line to find the point
71    intersection = p_0 + s_I * u
72
73    return True, intersection[:(3 - is_2D)]
74