Skip to content

Commit df4cf92

Browse files
authored
Make Line3 relation tests invariant to Plücker scale (#232)
## Summary `Line3.isparallel()` compared the raw cross product of Plücker direction vectors to an absolute tolerance. Scaling either representation therefore changed whether perpendicular lines were reported as parallel. The same classification feeds `|`, `isintersecting()` / `^`, and `commonperp()`. - Compare unit directions in `isparallel()` so the threshold represents angular separation. - Normalize both Plücker moments in `isintersecting()` so its reciprocal-product tolerance is independent of coordinate scale. - Build `commonperp()` from the nearest point and the cross product of unit directions. The previous expression could also produce invalid Plücker coordinates for nonorthogonal skew lines at unit scale. - Add regressions for small, large, and sign-reversed scalings, a near-parallel tolerance, crossing and skew lines, and the nearest-point geometry. For example, lines through `(0, 0, 0)` along `(1e-8, 0, 0)` and through `(0, 0, 1)` along `(0, 1e-8, 0)` are perpendicular; before this change `isparallel()` returned `True`. This branch is based on current `master` and does not contain the separate `distance()` fix in #224. I checked `closest_to_line()` on the included scaled nonparallel cases; it needs no change for this issue. ## Validation - New regression tests failed against the original implementation, then passed with the change. - `pytest -q --disable-warnings`: **350 passed, 3 skipped** (Python 3.10). - Black and `git diff --check`: passed. - Additional local geometric sampling across valid scaled line pairs agreed with the analytical common-perpendicular location to floating-point precision. Fixes #231. Implementation and tests were prepared and validated with OpenAI Codex assistance. --------- Signed-off-by: 李永祺 <doribelove@gmail.com>
1 parent a6a2f1d commit df4cf92

2 files changed

Lines changed: 74 additions & 11 deletions

File tree

‎spatialmath/geom3d.py‎

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -693,7 +693,14 @@ def isparallel(
693693
694694
:seealso: :meth:`__or__` :meth:`intersects`
695695
"""
696-
return bool(np.linalg.norm(np.cross(l1.w, l2.w)) < tol * _eps)
696+
n1 = np.linalg.norm(l1.w)
697+
n2 = np.linalg.norm(l2.w)
698+
if n1 == 0 or n2 == 0:
699+
return True
700+
701+
# Plucker coordinates are homogeneous: compare the angle, not the
702+
# magnitude of the direction vectors.
703+
return bool(np.linalg.norm(np.cross(l1.w / n1, l2.w / n2)) < tol * _eps)
697704

698705
def isintersecting(
699706
l1, l2: Line3, tol: float = 20 # type: ignore
@@ -715,7 +722,15 @@ def isintersecting(
715722
716723
:seealso: :meth:`__xor__` :meth:`intersects` :meth:`isparallel`
717724
"""
718-
return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < tol * _eps)
725+
if l1.isparallel(l2, tol=tol):
726+
return False
727+
728+
n1 = np.linalg.norm(l1.w)
729+
n2 = np.linalg.norm(l2.w)
730+
# Normalize both moments as well as directions before applying the
731+
# intersection tolerance; each pair (v, w) may be rescaled freely.
732+
reciprocal = np.dot(l1.w / n1, l2.v / n2) + np.dot(l2.w / n2, l1.v / n1)
733+
return bool(abs(reciprocal) < tol * _eps)
719734

720735
def __eq__(l1, l2: Line3) -> bool: # type: ignore pylint: disable=no-self-argument
721736
"""
@@ -1005,16 +1020,12 @@ def commonperp(
10051020
if l1 | l2:
10061021
# no common perpendicular if lines are parallel
10071022
return None
1008-
else:
1009-
# lines are skew or intersecting
1010-
w = np.cross(l1.w, l2.w)
1011-
v = (
1012-
np.cross(l1.v, l2.w)
1013-
- np.cross(l2.v, l1.w)
1014-
+ (l1 * l2) * np.dot(l1.w, l2.w) * base.unitvec(np.cross(l1.w, l2.w))
1015-
)
10161023

1017-
return l1.__class__(v, w)
1024+
# The nearest point lies on both the common perpendicular and l1.
1025+
# Unit directions keep its direction independent of Plucker scaling.
1026+
point, _ = l1.closest_to_line(l2)
1027+
w = np.cross(l1.w / np.linalg.norm(l1.w), l2.w / np.linalg.norm(l2.w))
1028+
return l1.__class__.PointDir(point, w)
10181029

10191030
def __mul__(
10201031
left, right: Line3

‎tests/test_geom3d.py‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -237,6 +237,26 @@ def test_parallel(self):
237237
self.assertFalse(L1.isparallel(L3))
238238
self.assertFalse(L1 | L3)
239239

240+
def test_parallel_plucker_scale(self):
241+
for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e-8)):
242+
l1 = Line3.PointDir([0, 0, 0], [scale1, 0, 0])
243+
perpendicular = Line3.PointDir([0, 0, 1], [0, scale2, 0])
244+
parallel = Line3.PointDir([0, 0, 1], [scale2, 0, 0])
245+
246+
self.assertFalse(l1.isparallel(perpendicular))
247+
self.assertFalse(l1 | perpendicular)
248+
self.assertTrue(l1.isparallel(parallel))
249+
self.assertTrue(l1 | parallel)
250+
251+
# A loose angular tolerance must not shrink when Plucker coordinates grow.
252+
l1 = Line3.PointDir([0, 0, 0], [1, 0, 0])
253+
# Only the directions matter here. A zero moment also avoids
254+
# round-off in the constructor's Plucker orthogonality check.
255+
almost_parallel = Line3.PointDir([0, 0, 0], [1, 1e-9, 0])
256+
scaled = Line3.PointDir([0, 0, 0], [1e8, 0.1, 0])
257+
self.assertTrue(l1.isparallel(almost_parallel, tol=1e8))
258+
self.assertTrue(l1.isparallel(scaled, tol=1e8))
259+
240260
def test_intersect(self):
241261
L1 = Line3.PointDir([4, 5, 6], [1, 2, 3])
242262
L2 = Line3.PointDir([5, 5, 6], [1, 2, 3])
@@ -253,6 +273,22 @@ def test_intersect(self):
253273
L3 ^ L4,
254274
)
255275

276+
def test_intersect_plucker_scale(self):
277+
for scale in (1, 1e-8, 1e8):
278+
l1 = Line3.PointDir([0, 0, 0], [scale, 0, 0])
279+
crossing = Line3.PointDir([0, 0, 0], [0, scale, 0])
280+
skew = Line3.PointDir([0, 0, 1e-7], [0, scale, 0])
281+
282+
self.assertTrue(l1.isintersecting(crossing))
283+
self.assertTrue(l1 ^ crossing)
284+
self.assertFalse(l1.isintersecting(skew))
285+
self.assertFalse(l1 ^ skew)
286+
287+
# Tolerance on the miss distance is independent of direction magnitude.
288+
l1 = Line3.PointDir([0, 0, 0], [1e8, 0, 0])
289+
near = Line3.PointDir([0, 0, 1e-8], [0, 1e8, 0])
290+
self.assertTrue(l1.isintersecting(near, tol=1e9))
291+
256292
def test_commonperp(self):
257293
L1 = Line3.PointDir([4, 5, 6], [0, 0, 1])
258294
L2 = Line3.PointDir([6, 5, 6], [0, 1, 0])
@@ -267,6 +303,22 @@ def test_commonperp(self):
267303
self.assertTrue(L ^ L1)
268304
self.assertTrue(L ^ L2)
269305

306+
def test_commonperp_plucker_scale(self):
307+
for scale1, scale2 in ((1, 1), (1e-8, 1e-8), (-1e-8, 1e8)):
308+
l1 = Line3.PointDir([1, 2, 3], [scale1, 0, 0])
309+
l2 = Line3.PointDir([0, 2, 4], [scale2, scale2, 0])
310+
311+
common = l1.commonperp(l2)
312+
self.assertIsNotNone(common)
313+
nt.assert_allclose(common.pp, [0, 2, 0], atol=1e-12)
314+
self.assertAlmostEqual(abs(common.uw[2]), 1)
315+
self.assertTrue(common.isintersecting(l1))
316+
self.assertTrue(common.isintersecting(l2))
317+
318+
nearest, distance = l1.closest_to_line(l2)
319+
nt.assert_allclose(nearest, [0, 2, 3], atol=1e-12)
320+
self.assertAlmostEqual(distance, 1)
321+
270322
def test_line(self):
271323
# mindist
272324
# intersect

0 commit comments

Comments
 (0)