@@ -1182,58 +1182,90 @@ def pprint(value: float | ArrayLike) -> None:
11821182 print (pretty (value ))
11831183
11841184
1185- def ray_line_intersect (
1186- a1 : VectorLike ,
1187- a2 : VectorLike ,
1188- b1 : VectorLike ,
1189- b2 : VectorLike ,
1185+ def point_on_segment (a : VectorLike , b : VectorLike , p : VectorLike , abs_tol : float = ATOL ) -> bool :
1186+ """Point on line segment."""
1187+
1188+ l = len (p )
1189+
1190+ ab = [b [i ] - a [i ] for i in range (l )]
1191+ ap = [p [i ] - a [i ] for i in range (l )]
1192+
1193+ # Check if `ap` is parallel to `ab` (cross product is zero)
1194+ cp = cross (ab , ap )
1195+ if _any (abs (c ) > abs_tol for c in cp ):
1196+ return False
1197+
1198+ # Check if the point is between a and b
1199+ for i in range (l ):
1200+ if abs (ab [i ]) < abs_tol :
1201+ continue
1202+ t = ap [i ] / ab [i ]
1203+ break
1204+
1205+ # See if `ab` is a point and `p` is that point
1206+ else :
1207+ return _all (abs (i - j ) < abs_tol for i , j in zip (a , p ))
1208+
1209+ return 0 <= t <= 1
1210+
1211+
1212+ def line_interesect (
1213+ s1 : VectorLike ,
1214+ e1 : VectorLike ,
1215+ s2 : VectorLike ,
1216+ e2 : VectorLike ,
1217+ rel_tol : float = RTOL ,
11901218 abs_tol : float = ATOL
11911219) -> Vector | None :
1192- """Find the intersection of a ray and a line."""
1193-
1194- da = [a - b for a , b in zip (a2 , a1 )]
1195- db = [a - b for a , b in zip (b2 , b1 )]
1196- dp = [a - b for a , b in zip (a1 , b1 )]
1197- dap = [- da [1 ], da [0 ]]
1198- denom = dot (dap , db , dims = D1 )
1199- # Perpendicular cases
1200- if abs (denom ) < abs_tol : # pragma: no cover
1220+ """
1221+ Find intersection of two lines.
1222+
1223+ This was designed particularly for 3D intersection, but can be used for either 2D or 3D,
1224+ but 2D line intersection could be calculated with less work using other methods if performance
1225+ was of importance.
1226+
1227+ 3D lines rarely intersect, but often the shortest line between can be found.
1228+ If the shortest line is has no length (a point) then it is an actual intersection.
1229+ Our cases are constructed such that an intersection is expected, and a line is not sufficient.
1230+ We can verify closeness of the points (to account for floating point errors) to verify that within
1231+ some expected threshold, the two line points are essentially a point and an intersection is found.
1232+ """
1233+
1234+ # Line segment difference
1235+ l1 = [a - b for a , b in zip (e1 , s1 )]
1236+ l2 = [a - b for a , b in zip (e2 , s2 )]
1237+
1238+ # Magnitude
1239+ m1 = math .sqrt (sum (a ** 2 for a in l1 ))
1240+ m2 = math .sqrt (sum (a ** 2 for a in l2 ))
1241+
1242+ # One of the lines is a point.
1243+ if m1 < abs_tol :
1244+ return list (s1 ) if point_on_segment (e2 , s2 , s1 , abs_tol = abs_tol ) else None
1245+ elif m2 < abs_tol :
1246+ return list (s2 ) if point_on_segment (e1 , s1 , s2 , abs_tol = abs_tol ) else None
1247+
1248+ # Unit vector
1249+ u1 = [a / m1 for a in l1 ]
1250+ u2 = [a / m2 for a in l2 ]
1251+ # Direction projection
1252+ u = vdot (u1 , u2 )
1253+ if u == 1 : # pragma: no cover
12011254 return None
1202- t = dot (dap , dp , dims = D1 ) / denom
1203- # Check if intersection is within bounds
1204- if 0 <= t <= 1 :
1205- # Intersect
1206- return add (b1 , multiply (t , db , dims = SC_D1 ), dims = D1 )
1207- return None # pragma: no cover
1208-
1209-
1210- def line_intersect (
1211- a1 : VectorLike ,
1212- a2 : VectorLike ,
1213- b1 : VectorLike ,
1214- b2 : VectorLike ,
1215- abs_tol : float = ATOL
1216- ) -> Vector | None : # pragma: no cover
1217- """Find the intersection of of lines."""
1218-
1219- da = [a - b for a , b in zip (a2 , a1 )]
1220- db = [a - b for a , b in zip (b2 , b1 )]
1221- dp = [a - b for a , b in zip (a1 , b1 )]
1222- dap = [- da [1 ], da [0 ]]
1223- denom = dot (dap , db , dims = D1 )
1224- # Perpendicular cases
1225- if abs (denom ) < abs_tol : # pragma: no cover
1255+ # Separation projection
1256+ sp = [a - b for a , b in zip (s2 , s1 )]
1257+ sp1 = vdot (sp , u1 )
1258+ sp2 = vdot (sp , u2 )
1259+ # Distance along lines
1260+ d1 = (sp1 - u * sp2 ) / (1 - u * u )
1261+ d2 = (sp2 - u * sp1 ) / (u * u - 1 )
1262+ # Calculate points of closest line
1263+ p1 = [a + b for a , b in zip (s1 , [x * d1 for x in u1 ])]
1264+ p2 = [a + b for a , b in zip (s2 , [x * d2 for x in u2 ])]
1265+ # If points are close enough, assume intersect, otherwise raise error
1266+ if not _all (math .isclose (i , j , rel_tol = rel_tol , abs_tol = abs_tol ) for i , j in zip (p1 , p2 )): # pragma: no cover
12261267 return None
1227- t = dot (dap , dp , dims = D1 ) / denom
1228- # Intersect
1229- i = add (b1 , multiply (t , db , dims = SC_D1 ), dims = D1 )
1230- # Check if intersection is within bounds
1231- if (
1232- 0 <= t <= 1 and
1233- 0 <= divide (dot (subtract (i , a1 , dims = D1 ), da , dims = D1 ), dot (da , da , dims = D1 ), dims = SC ) <= 1
1234- ):
1235- return i
1236- return None
1268+ return p1
12371269
12381270
12391271def all (a : float | ArrayLike ) -> bool : # noqa: A001
0 commit comments