@@ -252,7 +252,7 @@ def isvalid(x, check=False):
252252 return x .shape == (6 ,)
253253
254254 @staticmethod
255- def PQ (P = None , Q = None ):
255+ def Points2 (P = None , Q = None ):
256256 """
257257 Create Plucker line object from two 3D points
258258
@@ -273,11 +273,11 @@ def PQ(P=None, Q=None):
273273 Q = base .getvector (Q , 3 )
274274 # compute direction and moment
275275 w = P - Q
276- v = np .cross (P - Q , P )
276+ v = np .cross (w , P )
277277 return Plucker (np .r_ [v , w ])
278278
279279 @staticmethod
280- def Planes (pi1 , pi2 ):
280+ def Planes2 (pi1 , pi2 ):
281281 r"""
282282 Create Plucker line from two planes
283283
@@ -324,10 +324,10 @@ def PointDir(point, dir):
324324 :seealso: Plucker, Plucker.Planes, Plucker.PQ
325325 """
326326
327- point = base .getvector (point , 3 )
328- dir = base .getvector (dir , 3 )
329-
330- return Plucker (np .r_ [np . cross ( dir , point ), dir ])
327+ p = base .getvector (point , 3 )
328+ w = base .getvector (dir , 3 )
329+ v = np . cross ( w , p )
330+ return Plucker (np .r_ [v , w ])
331331
332332 def append (self , x ):
333333 """
@@ -688,8 +688,32 @@ def distance(self, l2): # pylint: disable=no-self-argument
688688 l = abs (l1 * l2 ) / np .linalg .norm (np .cross (l1 .w , l2 .w ))** 2
689689 return l
690690
691-
692- def closest (self , x ):
691+ def closest_to_line (self , line ):
692+ # point on line closest to another line
693+ # https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf
694+ # but (20) (21) is the negative of correct answer
695+
696+ p = []
697+ dist = []
698+ for line1 , line2 in zip (self , line ):
699+ v1 = line1 .v
700+ w1 = line1 .w
701+ v2 = line2 .v
702+ w2 = line2 .w
703+ p1 = (np .cross (v1 , np .cross (w2 , np .cross (w1 , w2 ))) - np .dot (v2 , np .cross (w1 , w2 )) * w1 ) \
704+ / np .sum (np .cross (w1 , w2 ) ** 2 )
705+ p2 = (np .cross (- v2 , np .cross (w1 , np .cross (w1 , w2 ))) + np .dot (v1 , np .cross (w1 , w2 )) * w2 ) \
706+ / np .sum (np .cross (w1 , w2 ) ** 2 )
707+
708+ p .append (p1 )
709+ dist .append (np .linalg .norm (p1 - p2 ))
710+
711+ if len (p ) == 1 :
712+ return p [0 ], dist [0 ]
713+ else :
714+ return np .array (p ).T , np .array (dist )
715+
716+ def closest_to_point (self , x ):
693717 """
694718 Point on line closest to given point
695719
0 commit comments