Fix error in lamba return value from intersect_plane, make this computation its own method

petercorke · petercorke · commit 2cb0e2c2785d · 2021-04-18T11:58:23.000+10:00
diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py
@@ -451,7 +451,8 @@ def pp(self):
         :seealso: Plucker.ppd, Plucker.point
         """
         
-        return np.cross(self.v, self.w) / np.dot(self.w, self.w)    
+        return np.cross(self.v, self.w) / np.dot(self.w, self.w)
+
     @property
     def ppd(self):
         """
@@ -487,6 +488,9 @@ def point(self, lam):
         lam = base.getvector(lam, out='row')
         return self.pp.reshape((3,1)) + self.uw.reshape((3,1)) * lam
 
+    def lam(self, point):
+        return np.dot( point.flatten() - self.pp, self.uw)
+
     # ------------------------------------------------------------------------- #
     #  TESTS ON PLUCKER OBJECTS
     # ------------------------------------------------------------------------- #
@@ -596,7 +600,6 @@ def __or__(self, l2):  # pylint: disable=no-self-argument
         l1 = self
         return l1.isparallel(l2)
 
-    
     def __xor__(self, l2):  # pylint: disable=no-self-argument
         
         """
@@ -853,7 +856,7 @@ def intersect_plane(self, plane):  # pylint: disable=no-self-argument
             # P = -(np.cross(line.v, plane.n) + plane.d * line.w) / den
             p = (np.cross(self.v, plane.n) - plane.d * self.w) / den
             
-            t = np.dot( self.pp - p, plane.n)
+            t = self.lam(p)
             return namedtuple('intersect_plane', 'p lam')(p, t)
         else:
             return None
diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py
@@ -239,15 +239,24 @@ def test_line(self):
         # side
         pass
     
-    def test_point(self):
+    def test_contains(self):
         P = [2, 3, 7]
         Q = [2, 1, 0]
         L = Plucker.PQ(P, Q)
         
         self.assertTrue( L.contains(L.point(0)) )
         self.assertTrue( L.contains(L.point(1)) )
         self.assertTrue( L.contains(L.point(-1)) )
-    
+
+    def test_point(self):
+        P = [2, 3, 7]
+        Q = [2, 1, 0]
+        L = Plucker.PQ(P, Q)
+        
+        nt.assert_array_almost_equal(L.point(0).flatten(), L.pp)
+
+        for x in (-2, 0, 3):
+            nt.assert_array_almost_equal(L.lam(L.point(x)), x)
     
     def test_char(self):
         P = [2, 3, 7]
@@ -257,6 +266,7 @@ def test_char(self):
         s = str(L)
         self.assertIsInstance(s, str)
 
+
     def test_plane(self):
         
         xyplane = [0, 0, 1, 0]
