@@ -716,7 +716,7 @@ def distance(self, l2): # pylint: disable=no-self-argument
716716 l = abs (l1 * l2 ) / np .linalg .norm (np .cross (l1 .w , l2 .w ))** 2
717717 return l
718718
719- def closest_to_line (self , line ):
719+ def closest_to_line (self , other ):
720720 """
721721 Closest point between two lines
722722
@@ -725,8 +725,21 @@ def closest_to_line(self, line):
725725 :return: nearest points and distance between lines at those points
726726 :rtype: ndarray(3,N), ndarray(N)
727727
728- Finds the point on the first line closest to the second line, as well
729- as the minimum distance between the lines.
728+ There are four cases:
729+
730+ * ``len(self) == len(line) == 1`` find the point on the first line closest to the second line, as well
731+ as the minimum distance between the lines.
732+ * ``len(self) == 1, len(line) == N`` find the point of intersection between the first
733+ line and the ``N`` other lines, returning ``N`` intersection points and distances.
734+ * ``len(self) == N, len(line) == 1`` find the point of intersection between the ``N`` first
735+ lines and the other line, returning ``N`` intersection points and distances.
736+ * ``len(self) == N, len(line) == M`` for each of the ``N`` first
737+ lines find the closest intersection with each of the ``M`` other lines, returning ``N``
738+ intersection points and distances.
739+
740+ ** this last one should be an option, default behavior would be to
741+ test self[i] against line[i]
742+ ** maybe different function
730743
731744 For two sets of lines, of equal size, return an array of closest points
732745 and distances.
@@ -746,26 +759,50 @@ def closest_to_line(self, line):
746759 # https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf
747760 # but (20) (21) is the negative of correct answer
748761
749- p = []
750- dist = []
751- for line1 , line2 in product (self , line ):
752- v1 = line1 .v
753- w1 = line1 .w
754- v2 = line2 .v
755- w2 = line2 .w
762+ points = []
763+ dists = []
764+
765+ def intersection (line1 , line2 ):
766+
756767 with np .errstate (divide = 'ignore' , invalid = 'ignore' ):
768+ # compute the distance between all pairs of lines
769+ v1 = line1 .v
770+ w1 = line1 .w
771+ v2 = line2 .v
772+ w2 = line2 .w
773+
757774 p1 = (np .cross (v1 , np .cross (w2 , np .cross (w1 , w2 ))) - np .dot (v2 , np .cross (w1 , w2 )) * w1 ) \
758775 / np .sum (np .cross (w1 , w2 ) ** 2 )
759776 p2 = (np .cross (- v2 , np .cross (w1 , np .cross (w1 , w2 ))) + np .dot (v1 , np .cross (w1 , w2 )) * w2 ) \
760777 / np .sum (np .cross (w1 , w2 ) ** 2 )
761778
762- p .append (p1 )
763- dist .append (np .linalg .norm (p1 - p2 ))
764-
765- if len (p ) == 1 :
766- return p [0 ], dist [0 ]
779+ return p1 , np .linalg .norm (p1 - p2 )
780+
781+
782+ if len (self ) == len (other ):
783+ # two sets of lines of equal length
784+ for line1 , line2 in zip (self , other ):
785+ point , dist = intersection (line1 , line2 )
786+ points .append (point )
787+ dists .append (dist )
788+
789+ elif len (self ) == 1 and len (other ) > 1 :
790+ for line in other :
791+ point , dist = intersection (self , line )
792+ points .append (point )
793+ dists .append (dist )
794+
795+ elif len (self ) > 1 and len (other ) == 1 :
796+ for line in self :
797+ point , dist = intersection (line , other )
798+ points .append (point )
799+ dists .append (dist )
800+
801+ if len (points ) == 1 :
802+ # 1D case for self or line
803+ return points [0 ], dists [0 ]
767804 else :
768- return np .array (p ).T , np .array (dist )
805+ return np .array (points ).T , np .array (dists )
769806
770807 def closest_to_point (self , x ):
771808 """
0 commit comments