Coverage for trimesh/path/intersections.py: 96%

1import numpy as np 

2 

3from .. import util 

4from ..constants import tol_path as tol 

5 

6 

7def line_line(origins, directions, plane_normal=None): 

8 """ 

9 Find the intersection between two lines. 

10 Uses terminology from: 

11 http://geomalgorithms.com/a05-_intersect-1.html 

12 

13 line 1: P(s) = p_0 + sU 

14 line 2: Q(t) = q_0 + tV 

15 

16 Parameters 

17 --------- 

18 origins : (2, d) float 

19 Points on lines (d in [2,3]) 

20 directions : (2, d) float 

21 Direction vectors 

22 plane_normal : (3, ) float 

23 If not passed computed from cross 

24 

25 Returns 

26 --------- 

27 intersects : bool 

28 Whether the lines intersect. 

29 In 2D, false if the lines are parallel 

30 In 3D, false if lines are not coplanar 

31 intersection : (d,) float or None 

32 Point of intersection 

33 """ 

34 # check so we can accept 2D or 3D points 

35 origins, is_2D = util.stack_3D(origins, return_2D=True) 

36 directions, is_2D = util.stack_3D(directions, return_2D=True) 

37 

38 # unitize direction vectors 

39 directions /= util.row_norm(directions).reshape((-1, 1)) 

40 

41 # exit if values are parallel 

42 if np.sum(np.abs(np.diff(directions, 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 w_norm = np.linalg.norm(w) 

64 if w_norm > tol.zero and abs(np.dot(plane_normal, w / w_norm)) > tol.zero: 

65 # not coplanar 

66 return False, None 

67 

68 # value of parameter s where intersection occurs 

69 s_I = np.dot(-v_perp, w) / 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)]