# Part of Spatial Math Toolbox for Python
# Copyright (c) 2000 Peter Corke
# MIT Licence, see details in top-level file: LICENCE
from __future__ import annotations
import numpy as np
import math
from collections import namedtuple
import matplotlib.pyplot as plt
import spatialmath.base as base
from spatialmath.base.types import *
from spatialmath.baseposelist import BasePoseList
import warnings
_eps = np.finfo(np.float64).eps
# ======================================================================== #
[docs]class Plane3:
r"""
Create a plane object from linear coefficients
:param c: Plane coefficients
:type c: array_like(4)
:return: a Plane object
:rtype: Plane
Planes are represented by the 4-vector :math:`[a, b, c, d]` which describes
the plane :math:`\pi: ax + by + cz + d=0`.
"""
[docs] def __init__(self, c: ArrayLike4):
self.plane = base.getvector(c, 4)
# point and normal
[docs] @classmethod
def PointNormal(cls, p: ArrayLike3, n: ArrayLike3) -> Self:
"""
Create a plane object from point and normal
:param p: Point in the plane
:type p: array_like(3)
:param n: Normal vector to the plane
:type n: array_like(3)
:return: a Plane object
:rtype: Plane
:seealso: :meth:`ThreePoints` :meth:`LinePoint`
"""
n = base.getvector(n, 3) # normal to the plane
p = base.getvector(p, 3) # point on the plane
return cls(np.r_[n, -np.dot(n, p)])
# point and normal
[docs] @classmethod
def ThreePoints(cls, p: R3x3) -> Self:
"""
Create a plane object from three points
:param p: Three points in the plane
:type p: ndarray(3,3)
:return: a Plane object
:rtype: Plane
The points in ``p`` are arranged as columns.
:seealso: :meth:`PointNormal` :meth:`LinePoint`
"""
p = base.getmatrix(p, (3, 3))
v1 = p[:, 0]
v2 = p[:, 1]
v3 = p[:, 2]
# compute a normal
n = np.cross(v2 - v1, v3 - v1)
return cls(np.r_[n, -np.dot(n, v1)])
[docs] @classmethod
def LinePoint(cls, l: Line3, p: ArrayLike3) -> Self:
"""
Create a plane object from a line and point
:param l: 3D line
:type l: Line3
:param p: Points in the plane
:type p: ndarray(3)
:return: a Plane object
:rtype: Plane
:seealso: :meth:`PointNormal` :meth:`ThreePoints`
"""
n = np.cross(l.w, p)
d = np.dot(l.v, p)
return cls(np.r_[n, d])
[docs] @classmethod
def TwoLines(cls, l1: Line3, l2: Line3) -> Self:
"""
Create a plane object from two line
:param l1: 3D line
:type l1: Line3
:param l2: 3D line
:type l2: Line3
:return: a Plane object
:rtype: Plane
.. warning:: This algorithm fails if the lines are parallel.
:seealso: :meth:`LinePoint` :meth:`PointNormal` :meth:`ThreePoints`
"""
n = np.cross(l1.w, l2.w)
d = np.dot(l1.v, l2.w)
return cls(np.r_[n, d])
[docs] @staticmethod
def intersection(pi1: Plane3, pi2: Plane3, pi3: Plane3) -> R3:
"""
Intersection point of three planes
:param pi1: plane 1
:type pi1: Plane
:param pi2: plane 2
:type pi2: Plane
:param pi3: plane 3
:type pi3: Plane
:return: coordinates of intersection point
:rtype: ndarray(3)
This static method computes the intersection point of the three planes
given as arguments.
.. warning:: This algorithm fails if the planes do not intersect, or
intersect along a line.
:seealso: :meth:`Plane`
"""
A = np.vstack([pi1.n, pi2.n, pi3.n])
b = np.array([pi1.d, pi2.d, pi3.d])
return np.linalg.det(A) @ b
@property
def n(self) -> R3:
r"""
Normal to the plane
:return: Normal to the plane
:rtype: ndarray(3)
For a plane :math:`\pi: ax + by + cz + d=0` this is the vector
:math:`[a,b,c]`.
:seealso: :meth:`d`
"""
# normal
return self.plane[:3]
@property
def d(self) -> float:
r"""
Plane offset
:return: Offset of the plane
:rtype: float
For a plane :math:`\pi: ax + by + cz + d=0` this is the scalar
:math:`d`.
:seealso: :meth:`n`
"""
return self.plane[3]
[docs] def contains(self, p: ArrayLike3, tol: float = 20) -> bool:
"""
Test if point in plane
:param p: A 3D point
:type p: array_like(3)
:param tol: tolerance in units of eps, defaults to 20
:type tol: float
:return: if the point is in the plane
:rtype: bool
"""
return abs(np.dot(self.n, p) - self.d) < tol * _eps
[docs] def plot(
self,
bounds: Optional[ArrayLike] = None,
ax: Optional[plt.Axes] = None,
**kwargs,
):
"""
Plot plane
:param bounds: bounds of plot volume, defaults to None
:type bounds: array_like(2|4|6), optional
:param ax: 3D axes to plot into, defaults to None
:type ax: Axes, optional
:param kwargs: optional arguments passed to ``plot_surface``
The ``bounds`` of the 3D plot volume is [xmin, xmax, ymin, ymax, zmin, zmax]
and a 3D plot is created if not already existing. If ``bounds`` is not
provided it is taken from current 3D axes.
The plane is drawn using ``plot_surface``.
:seealso: :func:`axes_logic`
"""
ax = base.axes_logic(ax, 3)
if bounds is None:
bounds = np.r_[ax.get_xlim(), ax.get_ylim(), ax.get_zlim()]
# X, Y = np.meshgrid(bounds[0: 2], bounds[2: 4])
# Z = -(X * self.plane[0] + Y * self.plane[1] + self.plane[3]) / self.plane[2]
X, Y = np.meshgrid(
np.linspace(bounds[0], bounds[1], 50), np.linspace(bounds[2], bounds[3], 50)
)
Z = -(X * self.plane[0] + Y * self.plane[1] + self.plane[3]) / self.plane[2]
Z[Z < bounds[4]] = np.nan
Z[Z > bounds[5]] = np.nan
ax.plot_surface(X, Y, Z, **kwargs)
def __str__(self) -> str:
"""
Convert plane to string representation
:return: Compact string representation of plane
:rtype: str
"""
return str(self.plane)
def __repr__(self) -> str:
"""
Display parameters of plane
:return: Compact string representation of plane
:rtype: str
"""
return str(self)
# ======================================================================== #
[docs]class Line3(BasePoseList):
__array_ufunc__ = None # allow pose matrices operators with NumPy values
@overload
def __init__(self, v: ArrayLike3, w: ArrayLike3):
...
@overload
def __init__(self, v: ArrayLike6):
...
[docs] def __init__(self, v=None, w=None, check=True):
"""
Create a Line3 object
:param v: Plucker coordinate vector, or Plucker moment vector
:type v: array_like(6) or array_like(3)
:param w: Plucker direction vector, optional
:type w: array_like(3), optional
:param check: check that the parameters are valid, defaults to True
:type check: bool
:raises ValueError: bad arguments
:return: 3D line
:rtype: ``Line3`` instance
A representation of a 3D line using Plucker coordinates.
- ``Line3(p)`` creates a 3D line from a Plucker coordinate vector ``p=[v, w]``
where ``v`` (3,) is the moment and ``w`` (3,) is the line direction.
- ``Line3(v, w)`` as above but the components ``v`` and ``w`` are
provided separately.
- ``Line3(L)`` creates a copy of the ``Line3`` object ``L``.
:notes:
- The ``Line3`` object inherits from ``collections.UserList`` and has list-like
behaviours.
- A single ``Line3`` object contains a 1D-array of Plucker coordinates.
- The elements of the array are guaranteed to be Plucker coordinates.
- The number of elements is given by ``len(L)``
- The elements can be accessed using index and slice notation, eg. ``L[1]`` or
``L[2:3]``
- The ``Line3`` instance can be used as an iterator in a for loop or list comprehension.
- Some methods support operations on the internal list.
:seealso: :meth:`Join` :meth:`TwoPlanes` :meth:`PointDir`
"""
from spatialmath.pose3d import SE3
super().__init__() # enable list powers
if w is None:
# zero or one arguments passed
if super().arghandler(v, convertfrom=(SE3,)):
if check and not base.iszero(np.dot(self.v, self.w)):
raise ValueError("invalid Plucker coordinates")
return
if base.isvector(v, 3) and base.isvector(w, 3):
if check and not base.iszero(np.dot(v, w)):
raise ValueError("invalid Plucker coordinates")
self.data = [np.r_[v, w]]
else:
raise ValueError("invalid argument to Line3 constructor")
# needed to allow __rmul__ to work if left multiplied by ndarray
# self.__array_priority__ = 100
@property
def shape(self) -> Tuple[int]:
return (6,)
@staticmethod
def _identity() -> R6:
return np.zeros((6,))
[docs] @staticmethod
def isvalid(x: NDArray, check: bool = False) -> bool:
return x.shape == (6,)
[docs] @classmethod
def Join(cls, P: ArrayLike3, Q: ArrayLike3) -> Self:
"""
Create 3D line from two 3D points
:param P: First 3D point
:type P: array_like(3)
:param Q: Second 3D point
:type Q: array_like(3)
:return: 3D line
:rtype: ``Line3`` instance
``Line3.Join(P, Q)`` create a ``Line3`` object that represents
the line joining the 3D points ``P`` (3,) and ``Q`` (3,). The direction
is from ``Q`` to ``P``.
:seealso: :meth:`IntersectingPlanes` :meth:`PointDir`
"""
P = base.getvector(P, 3)
Q = base.getvector(Q, 3)
# compute direction and moment
w = P - Q
v = np.cross(w, P)
return cls(np.r_[v, w])
[docs] @classmethod
def TwoPlanes(cls, pi1: Plane3, pi2: Plane3) -> Self:
r"""
Create 3D line from intersection of two planes
:param pi1: First plane
:type pi1: array_like(4), or ``Plane``
:param pi2: Second plane
:type pi2: array_like(4), or ``Plane``
:return: 3D line
:rtype: ``Line3`` instance
``L = Line3.TwoPlanes(π1, π2)`` is a ``Line3`` object that represents
the line formed by the intersection of two planes ``π1`` and ``π3``.
Planes are represented by the 4-vector :math:`[a, b, c, d]` which describes
the plane :math:`\pi: ax + by + cz + d=0`.
:seealso: :meth:`Join` :meth:`PointDir`
"""
# TODO inefficient to create 2 temporary planes
if not isinstance(pi1, Plane3):
pi1 = Plane3(base.getvector(pi1, 4))
if not isinstance(pi2, Plane3):
pi2 = Plane3(base.getvector(pi2, 4))
w = np.cross(pi1.n, pi2.n)
v = pi2.d * pi1.n - pi1.d * pi2.n
return cls(np.r_[v, w])
[docs] @classmethod
def IntersectingPlanes(cls, pi1: Plane3, pi2: Plane3) -> Self:
warnings.warn("use TwoPlanes method instead", DeprecationWarning)
return cls.TwoPlanes(pi1, pi2)
[docs] @classmethod
def PointDir(cls, point: ArrayLike3, dir: ArrayLike3) -> Self:
"""
Create 3D line from a point and direction
:param point: A 3D point
:type point: array_like(3)
:param dir: Direction vector
:type dir: array_like(3)
:return: 3D line
:rtype: ``Line3`` instance
``Line3.PointDir(P, W)`` is a `Line3`` object that represents the
line containing the point ``P`` and parallel to the direction vector ``W``.
:seealso: :meth:`Join` :meth:`IntersectingPlanes`
"""
p = base.getvector(point, 3)
w = base.getvector(dir, 3)
v = np.cross(w, p)
return cls(np.r_[v, w])
[docs] def append(self, x: Line3):
"""
Append a line
:param x: line object
:type x: Line3
:raises ValueError: Attempt to append a non Plucker object
:return: Line3 object with new line appended
:rtype: Line3 instance
"""
# print('in append method')
if not type(self) == type(x):
raise ValueError("can only append Line3 object")
if len(x) > 1:
raise ValueError("cant append a Line3 sequence - use extend")
super().append(x.A)
@property
def A(self) -> R6:
# get the underlying numpy array
if len(self.data) == 1:
return self.data[0]
else:
return self.data
def __getitem__(self, i):
# print('getitem', i, 'class', self.__class__)
return self.__class__(self.data[i])
@property
def v(self) -> R3:
r"""
Moment vector
:return: the moment vector
:rtype: ndarray(3)
The line is represented by a vector :math:`(\vec{v}, \vec{w}) \in \mathbb{R}^6`.
:seealso: :meth:`w`
"""
return self.data[0][0:3]
@property
def w(self) -> R3:
r"""
Direction vector
:return: the direction vector
:rtype: ndarray(3)
The line is represented by a vector :math:`(\vec{v}, \vec{w}) \in \mathbb{R}^6`.
:seealso: :meth:`v` :meth:`uw`
"""
return self.data[0][3:6]
@property
def uw(self) -> R3:
r"""
Line direction as a unit vector
:return: Line direction as a unit vector
:rtype: ndarray(3,)
``line.uw`` is a unit-vector parallel to the line.
The line is represented by a vector :math:`(\vec{v}, \vec{w}) \in \mathbb{R}^6`.
:seealso: :meth:`w`
"""
return base.unitvec(self.w)
@property
def vec(self) -> R6:
r"""
Line as a Plucker coordinate vector
:return: Plucker coordinate vector
:rtype: ndarray(6,)
``line.vec`` is the Plucker coordinate vector :math:`(\vec{v}, \vec{w}) \in \mathbb{R}^6`.
"""
return np.r_[self.v, self.w]
[docs] def skew(self) -> R4x4:
r"""
Line as a Plucker skew-symmetric matrix
:return: Skew-symmetric matrix form of Plucker coordinates
:rtype: ndarray(4,4)
``line.skew()`` is the Plucker matrix, a 4x4 skew-symmetric matrix
representation of the line whose six unique elements are the
Plucker coordinates of the line.
.. math::
\sk{L} = \begin{bmatrix} 0 & v_z & -v_y & \omega_x \\
-v_z & 0 & v_x & \omega_y \\
v_y & -v_x & 0 & \omega_z \\
-\omega_x & -\omega_y & -\omega_z & 0 \end{bmatrix}
.. note::
- For two homogeneous points P and Q on the line, :math:`PQ^T-QP^T` is
also skew symmetric.
- The projection of Plucker line by a perspective camera is a
homogeneous line (3x1) given by :math:`\vee C M C^T` where :math:`C
\in \mathbf{R}^{3 \times 4}` is the camera matrix.
"""
v = self.v
w = self.w
# the following matrix is at odds with H&Z pg. 72
return np.array(
[
[0, v[2], -v[1], w[0]],
[-v[2], 0, v[0], w[1]],
[v[1], -v[0], 0, w[2]],
[-w[0], -w[1], -w[2], 0],
] # type: ignore
)
@property
def pp(self) -> R3:
"""
Principal point of the 3D line
:return: Principal point of the line
:rtype: ndarray(3)
``line.pp`` is the point on the line that is closest to the origin.
Notes:
- Same as Plucker.point(0)
:seealso: :meth:`ppd` :meth`point`
"""
return np.cross(self.v, self.w) / np.dot(self.w, self.w)
@property
def ppd(self) -> float:
"""
Distance from principal point to the origin
:return: Distance from principal point to the origin
:rtype: float
``line.ppd`` is the distance from the principal point to the origin.
This is the smallest distance of any point on the line
to the origin.
:seealso: :meth:`pp`
"""
return math.sqrt(np.dot(self.v, self.v) / np.dot(self.w, self.w))
[docs] def point(self, lam: Union[float, ArrayLike]) -> Points3:
r"""
Generate point on line
:param lam: Scalar distance from principal point
:type lam: float
:return: Distance from principal point to the origin
:rtype: float
``line.point(λ)`` is a point on the line, where ``λ`` is the parametric
distance along the line from the principal point of the line such
that :math:`P = P_p + \lambda \hat{d}` and :math:`\hat{d}` is the line
direction given by ``line.uw``.
:seealso: :meth:`pp` :meth:`closest` :meth:`uw` :meth:`lam`
"""
lam = base.getvector(lam, out="row")
return cast(Points3, self.pp.reshape((3, 1)) + self.uw.reshape((3, 1)) * lam)
[docs] def lam(self, point: ArrayLike3) -> float:
r"""
Parametric distance from principal point
:param point: 3D point
:type point: array_like(3)
:return: parametric distance λ
:rtype: float
``line.lam(P)`` is the value of :math:`\lambda` such that
:math:`Q = P_p + \lambda \hat{d}` is closest to ``P``.
:seealso: :meth:`point`
"""
return np.dot(base.getvector(point, 3, out="row") - self.pp, self.uw)
# ------------------------------------------------------------------------- #
# TESTS ON PLUCKER OBJECTS
# ------------------------------------------------------------------------- #
[docs] def contains(
self, x: Union[R3, Points3], tol: float = 20
) -> Union[bool, List[bool]]:
"""
Test if points are on the line
:param x: 3D point
:type x: 3-element array_like, or ndarray(3,N)
:param tol: Tolerance in units of eps, defaults to 20
:type tol: float, optional
:raises ValueError: Bad argument
:return: Whether point is on the line
:rtype: bool or numpy.ndarray(N) of bool
``line.contains(X)`` is true if the point ``X`` lies on the line defined by
the Line3 object self.
If ``X`` is an array with 3 rows, the test is performed on every column and
an array of booleans is returned.
"""
if base.isvector(x, 3):
x = cast(R3, base.getvector(x))
return bool(np.linalg.norm(np.cross(x - self.pp, self.w)) < tol * _eps)
elif base.ismatrix(x, (3, None)):
return [
bool(np.linalg.norm(np.cross(p - self.pp, self.w)) < tol * _eps)
for p in x.T
]
else:
raise ValueError("bad argument")
[docs] def isequal(
l1, l2: Line3, tol: float = 20 # type: ignore
) -> bool: # pylint: disable=no-self-argument
"""
Test if two lines are equivalent
:param l2: Second line
:type l2: ``Line3``
:param tol: Tolerance in multiples of eps, defaults to 20
:type tol: float, optional
:return: lines are equivalent
:rtype: bool
``L1 == L2`` is True if the ``Line3`` objects describe the same line in
space. Note that because of the over parameterization, lines can be
equivalent even if their coordinate vectors are different.
:seealso: :meth:`__eq__`
"""
return bool(
abs(1 - np.dot(base.unitvec(l1.vec), base.unitvec(l2.vec))) < tol * _eps
)
[docs] def isparallel(
l1, l2: Line3, tol: float = 20 # type: ignore
) -> bool: # pylint: disable=no-self-argument
"""
Test if lines are parallel
:param l2: Second line
:type l2: ``Line3``
:param tol: Tolerance in multiples of eps, defaults to 20
:type tol: float, optional
:return: lines are parallel
:rtype: bool
``l1.isparallel(l2)`` is true if the two lines are parallel.
``l1 | l2`` as above but in binary operator form
:seealso: :meth:`__or__` :meth:`intersects`
"""
return bool(np.linalg.norm(np.cross(l1.w, l2.w)) < tol * _eps)
[docs] def isintersecting(
l1, l2: Line3, tol: float = 20 # type: ignore
) -> bool: # pylint: disable=no-self-argument
"""
Test if lines are intersecting
:param l2: Second line
:type l2: Line3
:param tol: Tolerance in multiples of eps, defaults to 20
:type tol: float, optional
:return: lines intersect
:rtype: bool
``l1.isintersecting(l2)`` is true if the two lines intersect.
.. note:: Is ``False`` if the lines are equivalent since they would intersect at
an infinite number of points.
:seealso: :meth:`__xor__` :meth:`intersects` :meth:`isparallel`
"""
return not l1.isparallel(l2, tol=tol) and bool(abs(l1 * l2) < tol * _eps)
[docs] def __eq__(l1, l2: Line3) -> bool: # type: ignore pylint: disable=no-self-argument
"""
Test if two lines are equivalent
:param l2: Second line
:type l2: ``Line3``
:return: lines are equivalent
:rtype: bool
``L1 == L2`` is True if the ``Line3`` objects describe the same line in
space. Note that because of the over parameterization, lines can be
equivalent even if their coordinate vectors are different.
.. note:: There is a hardwired tolerance of 10eps.
:seealso: :meth:`isequal` :meth:`__ne__`
"""
return l1.isequal(l2)
[docs] def __ne__(l1, l2: Line3) -> bool: # type:ignore pylint: disable=no-self-argument
"""
Test if two lines are not equivalent
:param l2: Second line
:type l2: ``Line3``
:return: lines are not equivalent
:rtype: bool
``L1 != L2`` is True if the Line3 objects describe different lines in
space. Note that because of the over parameterization, lines can be
equivalent even if their coordinate vectors are different.
.. note:: There is a hardwired tolerance of 10eps.
:seealso: :meth:`__ne__`
"""
return not l1.isequal(l2)
[docs] def __or__(l1, l2: Line3) -> bool: # type:ignore pylint: disable=no-self-argument
"""
Overloaded ``|`` operator tests for parallelism
:param l2: Second line
:type l2: ``Line3``
:return: lines are parallel
:rtype: bool
``l1 | l2`` is an operator which is true if the two lines are parallel.
.. note:: The ``|`` operator has low precendence.
.. note:: There is a hardwired tolerance of 10eps.
:seealso: :meth:`isparallel` :meth:`__xor__`
"""
return l1.isparallel(l2)
[docs] def __xor__(l1, l2: Line3) -> bool: # type:ignore pylint: disable=no-self-argument
"""
Overloaded ``^`` operator tests for intersection
:param l2: Second line
:type l2: Line3
:return: lines intersect
:rtype: bool
``l1 ^ l2`` is an operator which is true if the two lines intersect.
.. note::
- The ``^`` operator has low precendence.
- Is ``False`` if the lines are equivalent since they would intersect at
an infinite number of points.
.. note:: There is a hardwired tolerance of 10eps.
:seealso: :meth:`intersects` :meth:`isparallel` :meth:`isintersecting`
"""
return l1.isintersecting(l2)
# ------------------------------------------------------------------------- #
# PLUCKER LINE DISTANCE AND INTERSECTION
# ------------------------------------------------------------------------- #
[docs] def intersects(
l1, l2: Line3 # type:ignore
) -> Union[R3, None]: # pylint: disable=no-self-argument
"""
Intersection point of two lines
:param l2: Second line
:type l2: ``Line3``
:return: 3D intersection point
:rtype: ndarray(3) or None
``l1.intersects(l2)`` is the point of intersection of the two lines, or
``None`` if the lines do not intersect or are equivalent.
:seealso: :meth:`commonperp :meth:`eq` :meth:`__xor__`
"""
if l1 ^ l2:
# lines do intersect
return -(
np.dot(l1.v, l2.w) * np.eye(3, 3)
+ l1.w.reshape((3, 1)) @ l2.v.reshape((1, 3))
- l2.w.reshape((3, 1)) @ l1.v.reshape((1, 3))
) * base.unitvec(np.cross(l1.w, l2.w))
else:
# lines don't intersect
return None
[docs] def distance(
l1, l2: Line3, tol: float = 20 # type:ignore
) -> float: # pylint: disable=no-self-argument
"""
Minimum distance between lines
:param l2: Second line
:type l2: ``Line3``
:param tol: Tolerance in multiples of eps, defaults to 20
:type tol: float, optional
:return: Closest distance between lines
:rtype: float
``l1.distance(l2) is the minimum distance between two lines.
.. note:: Works for parallel, skew and intersecting lines.
:seealso: :meth:`closest_to_line`
"""
if l1 | l2:
# lines are parallel
l = np.cross(
l1.w, l1.v - l2.v * np.dot(l1.w, l2.w) / dot(l2.w, l2.w)
) / np.linalg.norm(l1.w)
else:
# lines are not parallel
if abs(l1 * l2) < tol * _eps:
# lines intersect at a point
l = 0
else:
# lines don't intersect, find closest distance
l = abs(l1 * l2) / np.linalg.norm(np.cross(l1.w, l2.w)) ** 2
return l
[docs] def closest_to_line(
l1, l2: Line3 # type:ignore
) -> Tuple[Points3, Rn]: # pylint: disable=no-self-argument
"""
Closest point between lines
:param l2: second line
:type l2: Line3
:return: nearest points and distance between lines at those points
:rtype: ndarray(3,N), ndarray(N)
There are four cases:
* ``len(self) == len(other) == 1`` find the point on the first line closest to the second line, as well
as the minimum distance between the lines.
* ``len(self) == 1, len(other) == N`` find the point of intersection between the first
line and the ``N`` other lines, returning ``N`` intersection points and distances.
* ``len(self) == N, len(other) == 1`` find the point of intersection between the ``N`` first
lines and the other line, returning ``N`` intersection points and distances.
* ``len(self) == N, len(other) == M`` for each of the ``N`` first
lines find the closest intersection with each of the ``M`` other lines, returning ``N``
intersection points and distances.
** this last one should be an option, default behavior would be to
test self[i] against line[i]
** maybe different function
For two sets of lines, of equal size, return an array of closest points
and distances.
Example::
.. runblock:: pycon
>>> from spatialmath import Line3
>>> line1 = Line3.Join([1, 1, 0], [1, 1, 1])
>>> line2 = Line3.Join([0, 0, 0], [2, 3, 5])
>>> line1.closest_to_line(line2)
:reference: `Plucker coordinates <https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf>`_
:seealso: :meth:`distance`
"""
# point on line closest to another line
# https://web.cs.iastate.edu/~cs577/handouts/plucker-coordinates.pdf
# but (20) (21) is the negative of correct answer
points = []
dists = []
def intersection(line1, line2):
with np.errstate(divide="ignore", invalid="ignore"):
# compute the distance between all pairs of lines
v1 = line1.v
w1 = line1.w
v2 = line2.v
w2 = line2.w
p1 = (
np.cross(v1, np.cross(w2, np.cross(w1, w2)))
- np.dot(v2, np.cross(w1, w2)) * w1
) / np.sum(np.cross(w1, w2) ** 2)
p2 = (
np.cross(-v2, np.cross(w1, np.cross(w1, w2)))
+ np.dot(v1, np.cross(w1, w2)) * w2
) / np.sum(np.cross(w1, w2) ** 2)
return p1, np.linalg.norm(p1 - p2)
if len(l1) == len(l2):
# two sets of lines of equal length
for line1, line2 in zip(l1, l2):
point, dist = intersection(line1, line2)
points.append(point)
dists.append(dist)
elif len(l1) == 1 and len(l2) > 1:
for line in l2:
point, dist = intersection(l1, line)
points.append(point)
dists.append(dist)
elif len(l1) > 1 and len(l2) == 1:
for line in l1:
point, dist = intersection(line, l2)
points.append(point)
dists.append(dist)
if len(points) == 1:
# 1D case for self or line
return points[0], dists[0]
else:
return np.array(points).T, np.array(dists)
[docs] def closest_to_point(self, x: ArrayLike3) -> Tuple[R3, float]:
"""
Point on line closest to given point
:param x: An arbitrary 3D point
:type x: array_like(3)
:return: Point on the line and distance to line
:rtype: ndarray(3), float
Find the point on the line closest to ``x`` as well as the distance
at that closest point.
Example:
.. runblock:: pycon
>>> from spatialmath import Line3
>>> line1 = Line3.Join([0, 0, 0], [2, 2, 3])
>>> line1.closest_to_point([1, 1, 1])
:seealso: meth:`point`
"""
# http://www.ahinson.com/algorithms_general/Sections/Geometry/PluckerLine.pdf
# has different equation for moment, the negative
x = base.getvector(x, 3)
lam = np.dot(x - self.pp, self.uw)
p = self.point(lam).flatten() # is the closest point on the line
d = np.linalg.norm(x - p)
return p, d
[docs] def commonperp(
l1, l2: Line3
) -> Line3: # type:ignore pylint: disable=no-self-argument
"""
Common perpendicular to two lines
:param l2: Second line
:type l2: Line3
:return: Perpendicular line
:rtype: Line3 instance or None
``l1.commonperp(l2)`` is the common perpendicular line between the two lines.
Returns ``None`` if the lines are parallel.
:seealso: :meth:`intersect`
"""
if l1 | l2:
# no common perpendicular if lines are parallel
return None
else:
# lines are skew or intersecting
w = np.cross(l1.w, l2.w)
v = (
np.cross(l1.v, l2.w)
- np.cross(l2.v, l1.w)
+ (l1 * l2) * np.dot(l1.w, l2.w) * base.unitvec(np.cross(l1.w, l2.w))
)
return l1.__class__(v, w)
[docs] def __mul__(
left, right: Line3
) -> float: # type:ignore pylint: disable=no-self-argument
r"""
Reciprocal product
:param left: Left operand
:type left: Line3
:param right: Right operand
:type right: Line3
:return: reciprocal product
:rtype: float
``left * right`` is the scalar reciprocal product :math:`\hat{w}_L \dot m_R + \hat{w}_R \dot m_R`.
.. note::
- Multiplication or composition of lines is not defined.
- Pre-multiplication by an SE3 object is supported, see ``__rmul__``.
:seealso: :meth:`__rmul__`
"""
if isinstance(right, Line3):
# reciprocal product
return np.dot(left.uw, right.v) + np.dot(right.uw, left.v)
else:
raise ValueError("bad arguments")
[docs] def __rmul__(
right, left: SE3
) -> Line3: # type:ignore pylint: disable=no-self-argument
"""
Rigid-body transformation of 3D line
:param left: Rigid-body transform
:type left: SE3
:param right: 3D line
:type right: Line
:return: transformed 3D line
:rtype: Line3 instance
``T * line`` is the line transformed by the rigid body transformation ``T``.
:seealso: :meth:`__mul__`
"""
from spatialmath.pose3d import SE3
if isinstance(left, SE3):
A = left.inv().Ad()
return right.__class__(A @ right.vec) # premultiply by SE3.Ad
else:
raise ValueError("can only premultiply Line3 by SE3")
# ------------------------------------------------------------------------- #
# PLUCKER LINE DISTANCE AND INTERSECTION
# ------------------------------------------------------------------------- #
[docs] def intersect_plane(
self, plane: Union[ArrayLike4, Plane3], tol: float = 20
) -> Tuple[R3, float]:
r"""
Line intersection with a plane
:param plane: A plane
:type plane: array_like(4) or Plane3
:param tol: Tolerance in multiples of eps, defaults to 20
:type tol: float, optional
:return: Intersection point, λ
:rtype: ndarray(3), float
- ``P, λ = line.intersect_plane(plane)`` is the point where the line
intersects the plane, and the corresponding λ value.
Return None, None if no intersection.
The plane can be specified as:
- a 4-vector :math:`[a, b, c, d]` which describes the plane :math:`\pi: ax + by + cz + d=0`.
- a ``Plane`` object
The return value is a named tuple with elements:
- ``.p`` for the point on the line as a numpy.ndarray, shape=(3,)
- ``.lam`` the `lambda` value for the point on the line.
:sealso: :meth:`point` :class:`Plane`
"""
# Line U, V
# Plane N n
# (VxN-nU:U.N)
# Note that this is in homogeneous coordinates.
# intersection of plane (n,p) with the line (v,p)
# returns point and line parameter
if not isinstance(plane, Plane3):
plane = Plane3(base.getvector(plane, 4))
den = np.dot(self.w, plane.n)
if abs(den) > (tol * _eps):
# 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 = self.lam(p)
return namedtuple("intersect_plane", "p lam")(p, t)
else:
return None
[docs] def intersect_volume(self, bounds: ArrayLike6) -> Tuple[Points3, Rn]:
"""
Line intersection with a volume
:param bounds: Bounds of an axis-aligned rectangular cuboid
:type plane: array_like(6)
:return: Intersection point, λ value
:rtype: ndarray(3,N), ndarray(N)
``P, λ = line.intersect_volume(bounds)`` is a matrix (3xN) with columns
that indicate where the line intersects the faces of the volume and
the corresponding λ values.
The volume is specified by ``bounds`` = [xmin xmax ymin ymax zmin zmax].
The number of
columns N is either:
- 0, when the line is outside the plot volume or,
- 2 when the line pierces the bounding volume.
See also :meth:`plot` :meth:`point`
"""
intersections = []
# reshape, top row is minimum, bottom row is maximum
bounds23 = bounds.reshape((3, 2))
for face in range(0, 6):
# for each face of the bounding volume
# x=xmin, x=xmax, y=ymin, y=ymax, z=zmin, z=zmax
# planes are:
# 0 normal in x direction, xmin
# 1 normal in x direction, xmax
# 2 normal in y direction, ymin
# 3 normal in y direction, ymax
# 4 normal in z direction, zmin
# 5 normal in z direction, zmax
i = face // 2 # 0, 1, 2
I = np.eye(3, 3)
p = [0, 0, 0]
p[i] = bounds[face]
plane = Plane3.PointNormal(n=I[:, i], p=p)
# find where line pierces the plane
try:
p, lam = self.intersect_plane(plane)
except TypeError:
continue # no intersection with this plane
# print('face %d: n=(%f, %f, %f)' % (face, plane.n[0], plane.n[1], plane.n[2]))
# print(' : p=(%f, %f, %f) ' % (p[0], p[1], p[2]))
# print('face', face, ' point ', p, ' plane ', plane)
# print('lamda', lam, self.point(lam))
# find if intersection point is within the cube face
# test x,y,z simultaneously
k = (p >= bounds23[:, 0]) & (p <= bounds23[:, 1])
k = np.delete(k, i) # remove the boolean corresponding to current face
if all(k):
# if within bounds, add
intersections.append(lam)
# print(' HIT');
# put them in ascending order
intersections.sort()
p = self.point(intersections)
return namedtuple("intersect_volume", "p lam")(p, intersections)
# ------------------------------------------------------------------------- #
# PLOT AND DISPLAY
# ------------------------------------------------------------------------- #
[docs] def plot(
self,
*pos,
bounds: Optional[ArrayLike] = None,
ax: Optional[plt.Axes] = None,
**kwargs,
) -> List[plt.Artist]:
"""
Plot a line
:param bounds: Bounds of an axis-aligned rectangular cuboid as [xmin xmax ymin ymax zmin zmax], optional
:type plane: 6-element array_like
:param **kwargs: Extra arguents passed to `Line2D <https://matplotlib.org/3.2.2/api/_as_gen/matplotlib.lines.Line2D.html#matplotlib.lines.Line2D>`_
:return: Plotted line
:rtype: Matplotlib artists
- ``line.plot(bounds)`` adds a line segment to the current axes, and the handle of the line is returned.
The line segment is defined by the intersection of the line and the given rectangular cuboid.
If the line does not intersect the plotting volume None is returned.
- ``line.plot()`` as above but the bounds are taken from the axis limits of the current axes.
The line color or style is specified by:
- a MATLAB-style linestyle like 'k--'
- additional arguments passed to `Line2D <https://matplotlib.org/3.2.2/api/_as_gen/matplotlib.lines.Line2D.html#matplotlib.lines.Line2D>`_
:seealso: :meth:`intersect_volume`
"""
if ax is None:
ax = plt.gca()
print(ax)
if bounds is None:
bounds = np.r_[ax.get_xlim(), ax.get_ylim(), ax.get_zlim()]
else:
bounds = base.getvector(bounds, 6)
ax.set_xlim(bounds[:2])
ax.set_ylim(bounds[2:4])
ax.set_zlim(bounds[4:6])
lines = []
for line in self:
P, lam = line.intersect_volume(bounds)
if len(lam) > 0:
l = ax.plot(
tuple(P[0, :]), tuple(P[1, :]), tuple(P[2, :]), *pos, **kwargs
)
lines.append(l)
return lines
def __str__(self) -> str:
"""
Convert Line3 to a string
:return: String representation of line parameters
:rtype: str
``str(line)`` is a string showing Plucker parameters in a compact single
line format like::
{ 0 0 0; -1 -2 -3}
where the first three numbers are the moment, and the last three are the
direction vector.
For a multi-valued ``Line3``, one line per value in ``Line3``.
"""
return "\n".join(
[
"{{ {:.5g} {:.5g} {:.5g}; {:.5g} {:.5g} {:.5g}}}".format(
*list(base.removesmall(x.vec))
)
for x in self
]
)
def __repr__(self) -> str:
"""
Display Line3
:return: String representation of line parameters
:rtype: str
Displays the line parameters in compact single line format.
For a multi-valued ``Line3``, one line per value in ``Line3``.
"""
if len(self) == 1:
return "Line3([{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}])".format(
*list(self.A)
)
else:
return (
"Line3([\n"
+ ",\n".join(
[
" [{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}]".format(
*list(tw)
)
for tw in self.data
]
)
+ "\n])"
)
def _repr_pretty_(self, p, cycle):
"""
Pretty string for IPython
:param p: pretty printer handle (ignored)
:param cycle: pretty printer flag (ignored)
Print colorized output when variable is displayed in IPython, ie. on a line by
itself.
Example::
In [1]: x
"""
if len(self) == 1:
p.text(str(self))
else:
for i, x in enumerate(self):
if i > 0:
p.break_()
p.text(f"{i:3d}: {str(x)}")
# function z = side(self1, pl2)
# Plucker.side Plucker side operator
#
# # X = SIDE(P1, P2) is the side operator which is zero whenever
# # the lines P1 and P2 intersect or are parallel.
#
# # See also Plucker.or.
#
# if ~isa(self2, 'Plucker')
# error('SMTB:Plucker:badarg', 'both arguments to | must be Plucker objects');
# end
# L1 = pl1.line(); L2 = pl2.line();
#
# z = L1([1 5 2 6 3 4]) * L2([5 1 6 2 4 3])';
# end
[docs] def side(self, other: Line3) -> float:
"""
Plucker side operator
:param other: second line
:type other: Line3
:return: permuted dot product
:rtype: float
This permuted dot product operator is zero whenever the lines intersect or are parallel.
"""
if not isinstance(other, Line3):
raise ValueError("argument must be a Line3")
return np.dot(self.A[[0, 4, 1, 5, 2, 3]], other.A[4, 0, 5, 1, 3, 2])
# Static factory methods for constructors from exotic representations
[docs]class Plucker(Line3):
[docs] def __init__(self, v=None, w=None):
import warnings
warnings.warn("use Line class instead", DeprecationWarning)
super().__init__(v, w)
if __name__ == "__main__": # pragma: no cover
import pathlib
import os.path
# L = Line3.TwoPoints((1,2,0), (1,2,1))
# print(L)
# print(L.intersect_plane([0, 0, 1, 0]))
# z = np.eye(6) * L
# L2 = SE3(2, 1, 10) * L
# print(L2)
# print(L2.intersect_plane([0, 0, 1, 0]))
# print('rx')
# L2 = SE3.Rx(np.pi/4) * L
# print(L2)
# print(L2.intersect_plane([0, 0, 1, 0]))
# print('ry')
# L2 = SE3.Ry(np.pi/4) * L
# print(L2)
# print(L2.intersect_plane([0, 0, 1, 0]))
# print('rz')
# L2 = SE3.Rz(np.pi/4) * L
# print(L2)
# print(L2.intersect_plane([0, 0, 1, 0]))
# base.plotvol3(10)
# S = Twist3.UnitRevolute([0, 0, 1], [2, 3, 2], 0.5);
# L = S.line()
# L.plot('k:', linewidth=2)
# a = Plane3([0.1, -1, -1, 2])
# base.plotvol3(5)
# a.plot(color='r', alpha=0.3)
# plt.show(block=True)
# a = SE3.Exp([2,0,0,0,0,0])
exec(
open(
pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_geom3d.py"
).read()
) # pylint: disable=exec-used