Source code for spatialmath.geom3d

#!/usr/bin/env python3

import numpy as np
import math
from collections import namedtuple
from collections import UserList

import spatialmath.base.argcheck as arg
import spatialmath.base as sm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from spatialmath import SE3

_eps = np.finfo(np.float64).eps

   
[docs]class Plane: """ Create a plane object from linear coefficients :param c: Plane coefficients :type c: 4-element array_like :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): self.plane = arg.getvector(c, 4)
# point and normal
[docs] @staticmethod def PN(p, n): """ Create a plane object from point and normal :param p: Point in the plane :type p: 3-element array_like :param n: Normal to the plane :type n: 3-element array_like :return: a Plane object :rtype: Plane """ n = arg.getvector(n, 3) # normal to the plane p = arg.getvector(p, 3) # point on the plane return Plane(np.r_[n, -np.dot(n, p)])
# point and normal
[docs] @staticmethod def P3(p): """ Create a plane object from three points :param p: Three points in the plane :type p: numpy.ndarray, shape=(3,3) :return: a Plane object :rtype: Plane """ p = arg.ismatrix((3,3)) v1 = p[:,0] v2 = p[:,1] v3 = p[:,2] # compute a normal n = np.cross(v2-v1, v3-v1) return Plane(n, v1)
# line and point # 3 points @property def n(self): """ Normal to the plane :return: Normal to the plane :rtype: 3-element array_like For a plane :math:`\pi: ax + by + cz + d=0` this is the vector :math:`[a,b,c]`. """ # normal return self.plane[:3] @property def d(self): """ 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`. """ return self.plane[3]
[docs] def contains(self, p, tol=10*_eps): """ :param p: A 3D point :type p: 3-element array_like :param tol: Tolerance, defaults to 10*_eps :type tol: float, optional :return: if the point is in the plane :rtype: bool """ return abs(np.dot(self.n, p) - self.d) < tol
def __str__(self): """ :return: String representation of plane :rtype: str """ return str(self.plane)
[docs]class Plucker(UserList): """ Plucker coordinate class Concrete class to represent a 3D line using Plucker coordinates. Methods: Plucker Contructor from points Plucker.planes Constructor from planes Plucker.pointdir Constructor from point and direction Information and test methods:: closest closest point on line commonperp common perpendicular for two lines contains test if point is on line distance minimum distance between two lines intersects intersection point for two lines intersect_plane intersection points with a plane intersect_volume intersection points with a volume pp principal point ppd principal point distance from origin point generate point on line Conversion methods:: char convert to human readable string double convert to 6-vector skew convert to 4x4 skew symmetric matrix Display and print methods:: display display in human readable form plot plot line Operators: * multiply Plucker matrix by a general matrix | test if lines are parallel ^ test if lines intersect == test if two lines are equivalent ~= test if lines are not equivalent Notes: - This is reference (handle) class object - Plucker objects can be used in vectors and arrays References: - Ken Shoemake, "Ray Tracing News", Volume 11, Number 1 http://www.realtimerendering.com/resources/RTNews/html/rtnv11n1.html#art3 - Matt Mason lecture notes http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/lecture9.pdf - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p596-7. Implementation notes: - The internal representation is a 6-vector [v, w] where v (moment), w (direction). - There is a huge variety of notation used across the literature, as well as the ordering of the direction and moment components in the 6-vector. Copyright (C) 1993-2019 Peter I. Corke """ # w # direction vector # v # moment vector (normal of plane containing line and origin)
[docs] def __init__(self, v=None, w=None): """ Create a Plucker 3D line object :param v: Plucker vector, Plucker object, Plucker moment :type v: 6-element array_like, Plucker instance, 3-element array_like :param w: Plucker direction, optional :type w: 3-element array_like, optional :raises ValueError: bad arguments :return: Plucker line :rtype: Plucker - ``L = Plucker(X)`` creates a Plucker object from the Plucker coordinate vector ``X`` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction. - ``L = Plucker(L)`` creates a copy of the Plucker object ``L``. - ``L = Plucker(V, W)`` creates a Plucker object from moment ``V`` (3-vector) and line direction ``W`` (3-vector). Notes: - The Plucker object inherits from ``collections.UserList`` and has list-like behaviours. - A single Plucker 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 Plucker instance can be used as an iterator in a for loop or list comprehension. - Some methods support operations on the internal list. :seealso: Plucker.PQ, Plucker.Planes, Plucker.PointDir """ super().__init__() # enable list powers if w is None: # single parameter if isinstance(v, Plucker): self.data = [v.A] elif arg.isvector(v, 6): pl = arg.getvector(v) self.data = [pl] else: raise ValueError('bad argument') else: assert arg.isvector(v, 3) and arg.isvector(w, 3), 'expecting two 3-vectors' self.data = [np.r_[v, w]]
# needed to allow __rmul__ to work if left multiplied by ndarray #self.__array_priority__ = 100
[docs] @staticmethod def PQ(P=None, Q=None): """ Create Plucker line object from two 3D points :param P: First 3D point :type P: 3-element array_like :param Q: Second 3D point :type Q: 3-element array_like :return: Plucker line :rtype: Plucker ``L = Plucker(P, Q)`` create a Plucker object that represents the line joining the 3D points ``P`` (3-vector) and ``Q`` (3-vector). The direction is from ``Q`` to ``P``. :seealso: Plucker, Plucker.Planes, Plucker.PointDir """ P = arg.getvector(P, 3) Q = arg.getvector(Q, 3) # compute direction and moment w = P - Q v = np.cross(P - Q, P) return Plucker(np.r_[v, w])
[docs] @staticmethod def Planes(pi1, pi2): r""" Create Plucker line from two planes :param pi1: First plane :type pi1: 4-element array_like, or Plane :param pi2: Second plane :type pi2: 4-element array_like, or Plane :return: Plucker line :rtype: Plucker ``L = Plucker.planes(PI1, PI2)`` is a Plucker object that represents the line formed by the intersection of two planes ``PI1`` and ``PI2``. Planes are represented by the 4-vector :math:`[a, b, c, d]` which describes the plane :math:`\pi: ax + by + cz + d=0`. :seealso: Plucker, Plucker.PQ, Plucker.PointDir """ if not isinstance(pi1, Plane): pi1 = Plane(arg.getvector(pi1, 4)) if not isinstance(pi2, Plane): pi2 = Plane(arg.getvector(pi2, 4)) w = np.cross(pi1.n, pi2.n) v = pi2.d * pi1.n - pi1.d * pi2.n return Plucker(np.r_[v, w])
[docs] @staticmethod def PointDir(point, dir): """ Create Plucker line from point and direction :param point: A 3D point :type point: 3-element array_like :param dir: Direction vector :type dir: 3-element array_like :return: Plucker line :rtype: Plucker ``L = Plucker.pointdir(P, W)`` is a Plucker object that represents the line containing the point ``P`` and parallel to the direction vector ``W``. :seealso: Plucker, Plucker.Planes, Plucker.PQ """ point = arg.getvector(point, 3) dir = arg.getvector(dir, 3) return Plucker(np.r_[np.cross(dir, point), dir])
[docs] def append(self, x): """ :param x: Plucker object :type x: Plucker :raises ValueError: Attempt to append a non Plucker object :return: Plucker object with new Plucker line appended :rtype: Plucker """ #print('in append method') if not type(self) == type(x): raise ValueError("can pnly append Plucker object") if len(x) > 1: raise ValueError("cant append a Plucker sequence - use extend") super().append(x.A)
@property def A(self): # 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): """ Moment vector :return: the moment vector :rtype: numpy.ndarray, shape=(3,) """ return self.data[0][0:3] @property def w(self): """ Direction vector :return: the direction vector :rtype: numpy.ndarray, shape=(3,) :seealso: Plucker.uw """ return self.data[0][3:6] @property def uw(self): """ Line direction as a unit vector :return: Line direction :rtype: numpy.ndarray, shape=(3,) ``line.uw`` is a unit-vector parallel to the line. """ return sm.unitvec(self.w) @property def vec(self): """ Line as a Plucker coordinate vector :return: Coordinate vector :rtype: numpy.ndarray, shape=(6,) ``line.vec`` is the Plucker coordinate vector ``X`` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction. """ return np.r_[self.v, self.w] @property def skew(self): r""" Line as a Plucker skew-matrix :return: Skew-symmetric matrix form of Plucker coordinates :rtype: numpy.ndarray, shape=(4,4) ``M = line.skew()`` is the Plucker matrix, a 4x4 skew-symmetric matrix representation of the line. Notes: - 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 ] ]) @property def pp(self): """ Principal point of the line ``line.pp`` is the point on the line that is closest to the origin. Notes: - Same as Plucker.point(0) :seealso: Plucker.ppd, Plucker.point """ return np.cross(self.v, self.w) / np.dot(self.w, self.w) @property def ppd(self): """ 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: Plucker.pp """ return math.sqrt(np.dot(self.v, self.v) / np.dot(self.w, self.w) )
[docs] def point(L, lam): 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(LAMBDA)`` is a point on the line, where ``LAMBDA`` 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: Plucker.pp, Plucker.closest, Plucker.uw """ lam = arg.getvector(lam, out='row') return L.pp.reshape((3,1)) + L.uw.reshape((3,1)) * lam
# ------------------------------------------------------------------------- # # TESTS ON PLUCKER OBJECTS # ------------------------------------------------------------------------- #
[docs] def contains(self, x, tol=50*_eps): """ Test if points are on the line :param x: 3D point :type x: 3-element array_like, or numpy.ndarray, shape=(3,N) :param tol: Tolerance, defaults to 50*_eps :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 Plucker 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 arg.isvector(x, 3): x = arg.getvector(x) return np.linalg.norm( np.cross(x - self.pp, self.w) ) < tol elif arg.ismatrix(x, (3,None)): return [np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol for _ in x.T] else: raise ValueError('bad argument')
[docs] def __eq__(l1, l2): """ Test if two lines are equivalent :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: Plucker :return: line equivalence :rtype: bool ``L1 == L2`` is true if the Plucker 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. """ return abs( 1 - np.dot(sm.unitvec(l1.vec), sm.unitvec(l2.vec))) < 10*_eps
[docs] def __ne__(l1, l2): """ Test if two lines are not equivalent :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: line inequivalence :rtype: bool ``L1 != L2`` is true if the Plucker objects describe different lines in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different. """ return not l1.__eq__(l2)
[docs] def isparallel(l1, l2, tol=10*_eps): """ Test if lines are parallel :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :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: Plucker.or, Plucker.intersects """ return np.linalg.norm(np.cross(l1.w, l2.w) ) < tol
[docs] def __or__(l1, l2): """ Test if lines are parallel as a binary operator :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: lines are parallel :rtype: bool ``l1 | l2`` is an operator which is true if the two lines are parallel. :seealso: Plucker.isparallel, Plucker.__xor__ """ return l1.isparallel(l2)
[docs] def __xor__(l1, l2): """ Test if lines intersect as a binary operator :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: lines intersect :rtype: bool ``l1 ^ l2`` is an operator which is true if the two lines intersect at a point. Notes: - Is false if the lines are equivalent since they would intersect at an infinite number of points. :seealso: Plucker.intersects, Plucker.parallel """ return not l1.isparallel(l2) and (abs(l1 * l2) < 10*_eps )
# ------------------------------------------------------------------------- # # PLUCKER LINE DISTANCE AND INTERSECTION # ------------------------------------------------------------------------- #
[docs] def intersects(l1, l2): """ Intersection point of two lines :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: 3D intersection point :rtype: numpy.ndarray, shape=(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: Plucker.commonperp, Plucker.eq, Plucker.__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))) * sm.unitvec(np.cross(l1.w, l2.w)) else: # lines don't intersect return None
[docs] def distance(l1, l2): """ Minimum distance between lines :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: Closest distance :rtype: float ``l1.distance(l2) is the minimum distance between two lines. Notes: - Works for parallel, skew and intersecting lines. """ 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) < 10*_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(line, x): """ Point on line closest to given point :param line: A line :type l1: Plucker :param l2: An arbitrary 3D point :type l2: 3-element array_like :return: Point on the line and distance to line :rtype: collections.namedtuple - ``line.closest(x).p`` is the coordinate of a point on the line that is closest to ``x``. - ``line.closest(x).d`` is the distance between the point on the line and ``x``. The return value is a named tuple with elements: - ``.p`` for the point on the line as a numpy.ndarray, shape=(3,) - ``.d`` for the distance to the point from ``x`` - ``.lam`` the `lambda` value for the point on the line. :seealso: Plucker.point """ # http://www.ahinson.com/algorithms_general/Sections/Geometry/PluckerLine.pdf # has different equation for moment, the negative x = arg.getvector(x, 3) lam = np.dot(x - line.pp, line.uw) p = line.point(lam).flatten() # is the closest point on the line d = np.linalg.norm( x - p) return namedtuple('closest', 'p d lam')(p, d, lam)
[docs] def commonperp(l1, l2): """ Common perpendicular to two lines :param l1: First line :type l1: Plucker :param l2: Second line :type l2: Plucker :return: Perpendicular line :rtype: Plucker or None ``l1.commonperp(l2)`` is the common perpendicular line between the two lines. Returns ``None`` if the lines are parallel. :seealso: Plucker.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) * sm.unitvec(np.cross(l1.w, l2.w)) return Plucker(v, w)
[docs] def __mul__(left, right): """ Reciprocal product :param left: Left operand :type left: Plucker :param right: Right operand :type right: Plucker :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`. Notes: - Multiplication or composition of Plucker lines is not defined. - Pre-multiplication by an SE3 object is supported, see ``__rmul__``. :seealso: Plucker.__rmul__ """ if isinstance(right, Plucker): # 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): """ Line transformation :param left: Rigid-body transform :type left: SE3 :param right: Right operand :type right: Plucker :return: transformed line :rtype: Plucker ``T * line`` is the line transformed by the rigid body transformation ``T``. :seealso: Plucker.__mul__ """ if isinstance(left, SE3): A = np.r_[ np.c_[left.R, sm.skew(-left.t) @ left.R], np.c_[np.zeros((3,3)), left.R] ] return Plucker( A @ right.vec) # premultiply by SE3 else: raise ValueError('bad arguments')
# ------------------------------------------------------------------------- # # PLUCKER LINE DISTANCE AND INTERSECTION # ------------------------------------------------------------------------- #
[docs] def intersect_plane(line, plane): r""" Line intersection with a plane :param line: A line :type line: Plucker :param plane: A plane :type plane: 4-element array_like or Plane :return: Intersection point :rtype: collections.namedtuple - ``line.intersect_plane(plane).p`` is the point where the line intersects the plane, or None if no intersection. - ``line.intersect_plane(plane).lam`` is the `lambda` value for the point on the line that intersects the plane. 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. See also Plucker.point. """ # 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, Plane): plane = Plane(arg.getvector(plane, 4)) den = np.dot(line.w, plane.n) if abs(den) > (100*_eps): # P = -(np.cross(line.v, plane.n) + plane.d * line.w) / den p = (np.cross(line.v, plane.n) - plane.d * line.w) / den t = np.dot( line.pp - p, plane.n) return namedtuple('intersect_plane', 'p lam')(p, t) else: return None
[docs] def intersect_volume(line, bounds): """ Line intersection with a volume :param line: A line :type line: Plucker :param bounds: Bounds of an axis-aligned rectangular cuboid :type plane: 6-element array_like :return: Intersection point :rtype: collections.namedtuple ``line.intersect_volume(bounds).p`` is a matrix (3xN) with columns that indicate where the line intersects the faces of the volume 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. ``line.intersect_volume(bounds).lam`` is an array of shape=(N,) where N is as above. The return value is a named tuple with elements: - ``.p`` for the points on the line as a numpy.ndarray, shape=(3,N) - ``.lam`` for the `lambda` values for the intersection points as a numpy.ndarray, shape=(N,). See also Plucker.plot, Plucker.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 i = face // 2 # 0, 1, 2 I = np.eye(3,3) p = [0, 0, 0] p[i] = bounds[face] plane = Plane.PN(n=I[:,i], p=p) # find where line pierces the plane try: p, lam = line.intersect_plane(plane) except TypeError: continue # no intersection with this plane # # print('face %d: n=(%f, %f, %f), p=(%f, %f, %f)' % (face, plane.n, plane.p)) # print(' : p=(%f, %f, %f) ' % p) # print('face', face, ' point ', p, ' plane ', plane) # 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 = line.point(intersections) return namedtuple('intersect_volume', 'p lam')(p, intersections)
# ------------------------------------------------------------------------- # # PLOT AND DISPLAY # ------------------------------------------------------------------------- #
[docs] def plot(line, bounds=None, **kwargs): """ Plot a line :param line: A line :type line: Plucker :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: Line3D or None - ``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: Plucker.intersect_volume """ if bounds is None: ax = plt.gca() bounds = np.r_[ax.get_xlim(), ax.get_ylim(), ax.get_zlim()] else: ax.set_xlim(bounds[:2]) ax.set_ylim(bounds[2:4]) ax.set_zlim(bounds[4:6]) #U = self.Q - self.P; #line.p = self.P; line.v = unit(U); P, lam = line.intersect_volume(bounds) if len(lam) > 0: return ax.plot(P[0,:], P[1,:], P[2,:], **kwargs) else: return None
def __str__(self): """ Convert 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. """ return '\n'.join(['{{ {:.5g} {:.5g} {:.5g}; {:.5g} {:.5g} {:.5g}}}'.format(*list(x.vec)) for x in self]) def __repr__(self): """ %Twist.display Display parameters % L.display() displays the twist parameters in compact single line format. If L is a vector of Twist objects displays one line per element. % Notes:: - This method is invoked implicitly at the command line when the result of an expression is a Twist object and the command has no trailing semicolon. % See also Twist.char. """ if len(self) == 1: return "Plucker([{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}])".format(*list(self)) else: return "Plucker([\n" + \ ',\n'.join([" [{:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}, {:.5g}]".format(*list(tw)) for tw in self]) +\ "\n])"
# 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 # # function z = intersect(self1, pl2) # Plucker.intersect Line intersection # # PL1.intersect(self2) is zero if the lines intersect. It is positive if PL2 # passes counterclockwise and negative if PL2 passes clockwise. Defined as # looking in direction of PL1 # # ----------> # o o # ----------> # counterclockwise clockwise # # z = dot(self1.w, pl1.v) + dot(self2.w, pl2.v); # end # Static factory methods for constructors from exotic representations if __name__ == '__main__': # pragma: no cover import pathlib import os.path a = SE3.Exp([2,0,0,0,0,0]) exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_geom3d.py")).read())