Skip to content

Commit 65ec09d

Browse files
committed
doco
1 parent bdcbc60 commit 65ec09d

1 file changed

Lines changed: 49 additions & 16 deletions

File tree

‎spatialmath/geom3d.py‎

Lines changed: 49 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -436,12 +436,20 @@ def skew(self):
436436
``M = line.skew()`` is the Plucker matrix, a 4x4 skew-symmetric matrix
437437
representation of the line.
438438
439-
Notes:
439+
.. math::
440+
441+
\sk{L} = \begin{bmatrix} 0 & v_z & -v_y & \omega_x \\
442+
-v_z & 0 & v_x & \omega_y \\
443+
v_y & -v_x & 0 & \omega_z \\
444+
-\omega_x & -\omega_y & -\omega_z & 0 \end{bmatrix}
445+
446+
.. note::
440447
441-
- For two homogeneous points P and Q on the line, :math:`PQ^T-QP^T` is also skew
442-
symmetric.
443-
- The projection of Plucker line by a perspective camera is a homogeneous line (3x1)
444-
given by :math:`\vee C M C^T` where :math:`C \in \mathbf{R}^{3 \times 4}` is the camera matrix.
448+
- For two homogeneous points P and Q on the line, :math:`PQ^T-QP^T` is
449+
also skew symmetric.
450+
- The projection of Plucker line by a perspective camera is a
451+
homogeneous line (3x1) given by :math:`\vee C M C^T` where :math:`C
452+
\in \mathbf{R}^{3 \times 4}` is the camera matrix.
445453
"""
446454

447455
v = self.v
@@ -707,6 +715,31 @@ def distance(self, l2): # pylint: disable=no-self-argument
707715
return l
708716

709717
def closest_to_line(self, line):
718+
"""
719+
Closest point between two lines
720+
721+
:param line: second line
722+
:type line: Plucker
723+
:return: nearest points and distance between lines at those points
724+
:rtype: ndarray(3,N), ndarray(N)
725+
726+
Finds the point on the first line closest to the second line, as well
727+
as the minimum distance between the lines.
728+
729+
For two sets of lines, of equal size, return an array of closest points
730+
and distances.
731+
732+
Example:
733+
734+
.. runblock:: pycon
735+
736+
>>> from spatialmath import Plucker
737+
>>> line1 = Plucker.TwoPoints([1, 1, 0], [1, 1, 1])
738+
>>> line2 = Plucker.TwoPoints([0, 0, 0], [2, 3, 5])
739+
>>> line1.closest_to_line(line2)
740+
741+
:reference: `Plucker coordinates <https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf>`_
742+
"""
710743
# point on line closest to another line
711744
# https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf
712745
# but (20) (21) is the negative of correct answer
@@ -741,18 +774,18 @@ def closest_to_point(self, x):
741774
:param l2: An arbitrary 3D point
742775
:type l2: 3-element array_like
743776
:return: Point on the line and distance to line
744-
:rtype: collections.namedtuple
777+
:rtype: ndarray(3), float
745778
746-
- ``line.closest(x).p`` is the coordinate of a point on the line that is
747-
closest to ``x``.
779+
Find the point on the line closest to ``x`` as well as the distance
780+
at that closest point.
748781
749-
- ``line.closest(x).d`` is the distance between the point on the line and ``x``.
750-
751-
The return value is a named tuple with elements:
752-
753-
- ``.p`` for the point on the line as a numpy.ndarray, shape=(3,)
754-
- ``.d`` for the distance to the point from ``x``
755-
- ``.lam`` the `lambda` value for the point on the line.
782+
Example:
783+
784+
.. runblock:: pycon
785+
786+
>>> from spatialmath import Plucker
787+
>>> line1 = Plucker.TwoPoints([0, 0, 0], [2, 2, 3])
788+
>>> line1.closest_to_point([1, 1, 1])
756789
757790
:seealso: Plucker.point
758791
"""
@@ -818,7 +851,7 @@ def __mul__(self, right): # pylint: disable=no-self-argument
818851
:seealso: Plucker.__rmul__
819852
"""
820853
left = self
821-
if isinstance(right, Plucker):
854+
if isinstance(right, Line3):
822855
# reciprocal product
823856
return np.dot(left.uw, right.v) + np.dot(right.uw, left.v)
824857
else:

0 commit comments

Comments
 (0)