cgse-coordinates 2023.1.0__py3-none-any.whl

egse/coordinates/point.py

@@ -0,0 +1,707 @@
+"""
+The point module provides two classes, a `Point` class which simply represents a point
+in the 3D space, and a `Points` class which is a collection of Point objects.
+
+A Point is defined with respect to a given reference frame and is given a name.
+
+Point objects defined in the same reference frame can be combined with the
+natural `+`, `-` , `+=` and `-=` operations.
+
+In order to work with 4x4 transformation matrices, the 3D [x,y,z] coordinates are
+automatically converted to a [x,y,z,1] coordinates array attribute.
+
+@author: Pierre Royer
+"""
+import logging
+import random
+import string
+
+import numpy as np
+
+import egse.coordinates.transform3d_addon as t3add
+
+LOGGER = logging.getLogger(__name__)
+
+
+class Point:
+    """
+    A Point object represents a point in 3D space and is defined with respect to
+    a given reference frame.
+
+    .. note::
+        There is no check that the randomly generated name is unique, so two Point
+        objects can be different but have the same name.
+
+    """
+    debug = 0
+
+    def __init__(self, coordinates, ref, name=None):
+        """
+        This initializes a Point object in a given reference frame.
+
+        Args:
+            coordinates (numpy.ndarray, list): 1x3 or 1x4 matrix defining this system in "ref" system
+                (1x3 being x,y,z + an additional 1 for the affine operations)
+
+            ref (ReferenceFrame): the reference system in which this Point object will be defined,
+                if not given or None the master reference frame will be used
+
+        """
+
+        # Makes sure of format [x,y,z,1] and sets coordinates
+        self.setCoordinates(coordinates)
+
+        # set the reference frame of reference
+        if ref is None:
+            raise ValueError("A Point shall be defined with a reference frame, ref can not be None.")
+        else:
+            self.ref = ref
+
+        self.setName(name)
+
+        self.definition = [self.coordinates[:-1], self.ref, self.name]
+
+    def __repr__(self):
+        return f"{self.coordinates[:-1]} (ref {self.ref.name})"
+
+    def __str__(self):
+        return f"{self.coordinates[:-1]} (ref {self.ref.name}), name {self.name}"
+
+    def __eq__(self, other):
+        """
+        Re-implements the == operator which by default checks if id(self) == id(other).
+
+        Two Point objects are equal when:
+
+        * their coordinates are equal
+        * the reference system in which they are defined is equal
+        * the name must not be equal
+        """
+        if self is other:
+            return True
+
+        if isinstance(other, Point):
+            if not np.array_equal(self.coordinates, other.coordinates):
+                return False
+            if self.ref != other.ref:
+                return False
+            return True
+        return NotImplemented
+
+    def __hash__(self):
+        return id(self.definition) // 16
+
+    def isSame(self, other):
+        """
+        This checks if a Point is the same as another Point in a different reference frame.
+
+        Two Point objects are the same when their position, i.e. coordinates, are equal
+        after they have been expressed in the same reference frame.
+
+        :param other: a Point object that you want to check
+        :type other: Point
+
+        :returns: True when the two Point objects are the same, False otherwise
+
+        :raises TypeError: when other is not a compatible type, NotImplemented will returned
+                           which will result in a ```TypeError: unsupported operand type(s) for +:```.
+        """
+
+        if isinstance(other, Point):
+            if self == other:
+                return True
+            else:
+                if np.array_equal(self.coordinates, other.expressIn(self.ref)):
+                    return True
+            return False
+        return NotImplemented
+
+    @staticmethod
+    def __coords__(coordinates):
+        """
+        Formats 1x3 or 1x4 input lists or np.arrays into 1x4 np.array coordinates
+        Static --> can be called 'from outside', without passing a Point object
+        """
+        if isinstance(coordinates, Point):
+            return coordinates.coordinates
+        elif isinstance(coordinates, (np.ndarray, list)):
+            coordinates = list(coordinates)
+            if len(coordinates) == 3:
+                coordinates.append(1)
+            return coordinates
+        else:
+            raise ValueError("input must be a list, numpy.ndarray or Point")
+
+    def setName(self, name=None):
+        """
+        Set or change the name of a Point object.
+
+        :param str name: the new name for the Point, if None, a random name will be generated.
+
+        .. todo:: Should we care about the possibility the the generation of random names does not
+                  necessarily create a unique name for the Point?
+        """
+        if name is None:
+            self.name = 'p'+''.join(random.choices(string.ascii_lowercase, k=3))
+        else:
+            self.name = name
+
+    def setCoordinates(self, coordinates):
+        """
+        Set the coordinates of this Point object.
+        """
+        coordinates = Point.__coords__(coordinates)
+        self.coordinates = np.array(coordinates)
+
+        self.x = self.coordinates[0]
+        self.y = self.coordinates[1]
+        self.z = self.coordinates[2]
+
+    def getCoordinates(self, ref=None):
+        """
+        Returns the coordinates of this Points object.
+
+        If no reference frame is given, the coordinates of the Point will just be returned,
+        other wise this method behaves the same as the ``expressIn(ref)`` method.
+
+        :param ref: the Reference Frame in which the Point shall be defined
+        :type ref: ReferenceFrame
+        """
+        if ref is None:
+            return self.coordinates
+        else:
+            return self.expressIn(ref)
+
+    def distanceTo(self, target):
+        """
+        Returns the distance of this Point object to the target. Target can be another Point,
+        a ReferenceFrame object or a Numpy dnarray or list with coordinates.
+        """
+        from egse.coordinates.referenceFrame import ReferenceFrame
+
+        if isinstance(target, Point):
+            targetCoords = target.expressIn(self.ref)[:3]
+        elif isinstance(target, ReferenceFrame):
+            return np.linalg.norm(self.expressIn(target)[:3])
+        elif isinstance(target, (np.ndarray, list)):
+            if len(target) > 3:
+                target = target[:3]
+            targetCoords = target
+        else:
+            raise ValueError("input must be a list, numpy.ndarray, Point or ReferenceFrame")
+
+        LOGGER.info(f"self={self.coordinates[:-1]}, target={targetCoords}")
+
+        return np.linalg.norm(self.coordinates[:3]-targetCoords)
+
+    def inPlaneDistanceTo(self,target,plane='xy'):
+        """
+        Returns the distance of this Point object to the target, considering 2 coordinates only!
+
+        target: can be another Point, a ReferenceFrame object or a Numpy dnarray or list with coordinates.
+
+        plane : must be in ['xy', 'xz', 'yz']
+
+        NB: The xy, yz or xz plane used to project the points coordinates before
+            computing the distances is taken from the coordinate system of "self"
+            ==> pointA.inPlaneDistanceTo(pointB) != pointB.inPlaneDistanceTo(pointA)
+            The first projects on the xy plane of pointA.ref, the second on the xy plane of pointB.ref
+        """
+        from egse.coordinates.referenceFrame import ReferenceFrame
+
+        if isinstance(target, Point):
+            targetCoords = target.expressIn(self.ref)
+        elif isinstance(target, ReferenceFrame):
+            targetCoords = target.getOrigin().expressIn(self)
+        elif isinstance(target, (np.ndarray, list)):
+            targetCoords = target
+        else:
+            raise ValueError("input must be a list, numpy.ndarray, Point or ReferenceFrame")
+
+        LOGGER.info(f"self={self.coordinates[:-1]}, target={targetCoords}")
+
+        planeSelect = {'xy':[0,1], 'xz':[0,2], 'yz':[1,2]}
+
+        LOGGER.info(f"self.coordinates[planeSelect[plane]]  {self.coordinates[planeSelect[plane]]}")
+        LOGGER.info(f"targetCoords[planeSelect[plane]]      {targetCoords[planeSelect[plane]]}")
+        LOGGER.info(f"Difference                            {self.coordinates[planeSelect[plane]]-targetCoords[planeSelect[plane]]}")
+
+        return np.linalg.norm(self.coordinates[planeSelect[plane]]-targetCoords[planeSelect[plane]])
+
+    def distanceToPlane(self,plane="xy",ref=None):
+        """
+        distanceToPlane(self,plane="xy",ref=None)
+
+        The target plane is defined by one of it coordinate planes: ["xy", "yz", "xz"]
+
+        :param ref: reference frame
+        :type ref: ReferenceFrame
+
+        :param plane: in ["xy", "xz", "yz"]
+        :type plane: str
+
+        :returns: the distance from self to plane
+        """
+        if (ref is None) or (self.ref == ref):
+            coordinates = self.coordinates[:-1]
+        elif self.ref != ref:
+            coordinates = self.expressIn(ref)
+
+        outOfPlaneIndex = {'xy':2,'xz':1,'yz':0}
+
+        return coordinates[outOfPlaneIndex[plane]]
+
+    def __sub__(self,apoint):
+        """
+        Takes care for
+        newPoint = self + point
+        """
+        if isinstance(apoint, Point):
+
+            try:
+                if apoint.ref != self.ref:
+                    raise ValueError
+            except ValueError:
+                print("WARNING: The points have different reference frames, returning NotImplemented")
+                return NotImplemented
+            newCoordinates = self.coordinates - apoint.coordinates
+
+        elif isinstance(apoint, (np.ndarray, list)):
+
+            newCoordinates = self.coordinates - Point.__coords__(apoint)
+
+        # For the affine transforms, the 4th digit must be set to 1 (it has been modified above)
+        newCoordinates[-1] = 1
+
+        return Point(coordinates=newCoordinates,ref=self.ref)
+
+    def __isub__(self,apoint):
+        """
+        Takes care for
+        self += coordinates (modifies self in place)
+        """
+        if isinstance(apoint, Point):
+
+            try:
+                if apoint.ref != self.ref:
+                    raise ValueError
+            except ValueError:
+                print("WARNING: The points have different reference frames, returning NotImplemented")
+                return NotImplemented
+            newCoordinates = self.coordinates - apoint.coordinates
+
+        elif isinstance(apoint, (np.ndarray, list)):
+
+            newCoordinates = self.coordinates - Point.__coords__(apoint)
+
+        # For the affine transforms, the 4th digit must be set to 1 (it has been modified above)
+        newCoordinates[-1] = 1
+
+        self.coordinates = newCoordinates
+
+        return self
+
+    def __add__(self,apoint):
+        """
+        Takes care for
+        newPoint = self + point
+        """
+        if isinstance(apoint, Point):
+
+            try:
+                if apoint.ref != self.ref:
+                    print(f"DEBUG: {apoint} = {apoint.expressIn(self.ref)}")
+                    raise ValueError
+            except ValueError:
+                print("WARNING: The points have different reference frames, returning NotImplemented")
+                return NotImplemented
+            newCoordinates = self.coordinates + apoint.coordinates
+
+        elif isinstance(apoint, (np.ndarray, list)):
+
+            newCoordinates = self.coordinates + Point.__coords__(apoint)
+
+        else:
+            return NotImplemented
+
+        # For the affine transforms, the 4th digit must be set to 1 (it has been modified above)
+        newCoordinates[-1] = 1
+
+        return Point(coordinates=newCoordinates,ref=self.ref)
+
+    def __iadd__(self,apoint):
+        """
+        Takes care for
+        self += coordinates (modifies self in place)
+        """
+        if isinstance(apoint, Point):
+
+            try:
+                if apoint.ref != self.ref:
+                    raise ValueError
+            except ValueError:
+                print("WARNING: The points have different reference frames, returning NotImplemented")
+                return NotImplemented
+            newCoordinates = self.coordinates + apoint.coordinates
+
+        elif isinstance(apoint, (np.ndarray, list)):
+
+            newCoordinates = self.coordinates + Point.__coords__(apoint)
+
+        # For the affine transforms, the 4th digit must be set to 1 (it has been modified above)
+        newCoordinates[-1] = 1
+
+        self.coordinates = newCoordinates
+
+        return self
+
+    def expressIn(self,targetFrame):
+        """
+        expressIn(self,targetFrame)
+        """
+        if targetFrame == self.ref:
+            """
+            targetFrame == self.ref
+            We're after the definition of self
+            """
+            result = self.coordinates
+        else:
+            """
+            We're after the coordinates of self, i.e. the definition of self in targetFrame
+
+            We know the coordinates in self.ref
+            #We need to apply the transformation from targetFrame to self.ref
+            self.ref        --> self (self.transformation)
+            """
+            #transform = targetFrame.getTransformationFrom(self.ref)
+            transform = self.ref.getPassiveTransformationTo(targetFrame)
+            if self.debug: print("transform \n{0}".format(transform))
+            result    = np.dot(transform,self.coordinates)
+        return result
+
+    def changeRef(self,targetFrame):
+        """
+        We redefine self as attached to another reference frame
+        . calculate self's coordinates in the new reference frame
+        . update the definition
+        """
+        newCoordinates = self.expressIn(targetFrame)
+        self.setCoordinates(newCoordinates)
+        self.ref = targetFrame
+        return
+
+
+
+
+class Points():
+    """
+    A Points object is a collection of Point objects.
+
+    Points can be constructed from either a numpy.ndarray of shape 3 x n or 4 x n, or
+    a list of Point objects. The coordinates of the Point objects are transferred to
+    the desired ReferenceFrame and concatenated in the list order.
+
+    When automatically generated a Points object name consists in a capital 'P' followed by
+    three lower case letters. A Point can be extracted from a Points object by its
+    position in the coordinates array (see below).
+
+    """
+    debug=0
+    def __init__(self, coordinates, ref,name=None):
+        """
+        Points.__init__(self, coordinates, ref, name=None)
+
+        Constructor
+
+        coordinates : must be of one of the following type:
+            * numpy.ndarray:
+              4xn matrix defining this system in "ref" system
+              (3 being x,y,z + an additional 1 for the affine operations)
+            * list of Point objects:
+              the coordinates of the Point objects are extracted in the order of the list
+              and concatenated in a numpy.ndarray
+
+        ref         : reference system in which self is defined
+
+        Both parameters are mandatory.
+        """
+
+        # TODO: accept a list made of Point and Points rather than strictly Point
+
+        if isinstance(coordinates, list):
+            coordinateList = []
+            for apoint in coordinates:
+                if not isinstance(apoint, Point):
+                    raise ValueError("If the input is a list, all items in it must be Point(s) objects")
+                coordinateList.append(apoint.expressIn(ref))
+            self.setCoordinates(np.array(coordinateList).T)
+        elif isinstance(coordinates, np.ndarray):
+            self.setCoordinates(coordinates)
+        else:
+            raise ValueError("The input must be either a numpy.ndarray or a list of Point objects")
+
+        self.ref = ref
+
+        self.setName(name)
+
+        return
+
+    def __repr__(self):
+        return "{0} (ref {1})".format(self.coordinates[:-1], self.ref.name)
+
+    def __str__(self):
+        return "{1} (ref {2}), name {0}".format(self.name, self.coordinates[:-1], self.ref.name)
+
+    @staticmethod
+    def __coords__(coordinates):
+        """
+        Formats 3xn or 4xn input lists or np.arrays into 4xn np.array coordinates
+        Static --> can be called 'from outside', without passing a Points object
+        """
+        if isinstance(coordinates, Point):
+            return coordinates.coordinates
+        elif isinstance(coordinates, np.ndarray):
+
+            if coordinates.shape[0] not in [3,4]:
+                raise ValueError("Input coordinates array must be 3 x n or 4 x n")
+
+            if coordinates.shape[0]==3:
+                newCoords = np.ones([4,coordinates.shape[1]])
+                newCoords[:3,:] = coordinates
+                coordinates = newCoords
+            return coordinates
+        else:
+            raise ValueError("input must be a list, numpy.ndarray or Point")
+
+    def setName(self,name=None):
+        if name is None:
+            self.name = 'P'+ ''.join(random.choices(string.ascii_lowercase, k=2))+\
+                             ''.join(random.choices(string.ascii_uppercase, k=1))
+        else:
+            self.name = name
+
+    def setCoordinates(self,coordinates):
+        #coordinates = list(coordinates)
+        #if len(coordinates)==3: coordinates.append(1)
+        coordinates = Points.__coords__(coordinates)
+        self.coordinates = coordinates
+
+        self.x = self.coordinates[0,:]
+        self.y = self.coordinates[1,:]
+        self.z = self.coordinates[2,:]
+
+        return
+
+    def getCoordinates(self,ref=None):
+        if ref is None:
+            return self.coordinates
+        else:
+            return self.expressIn(ref)
+
+    def expressIn(self,targetFrame):
+        """
+        expressIn(self,targetFrame)
+        """
+        if targetFrame == self.ref:
+            """
+            targetFrame == self.ref
+            We're after the definition of self
+            """
+            result = self.coordinates
+        else:
+            """
+            We're after the coordinates of self, i.e. the definition of self in targetFrame
+
+            We know the coordinates in self.ref
+            #We need to apply the transformation from targetFrame to self.ref
+            self.ref        --> self (self.transformation)
+            """
+            #transform = targetFrame.getTransformationFrom(self.ref)
+            transform = self.ref.getPassiveTransformationTo(targetFrame)
+            if self.debug: print("transform \n{0}".format(transform))
+            result    = np.dot(transform,self.coordinates)
+        return result
+
+    def changeRef(self,targetFrame):
+        """
+        We redefine self as attached to another reference frame
+        . calculate self's coordinates in the new reference frame
+        . update the definition
+        """
+        newCoordinates = self.expressIn(targetFrame)
+        self.setCoordinates(newCoordinates)
+        self.ref = targetFrame
+        return
+
+    def getPoint(self,index,name=None):
+        """
+        Returns the point with coordinates self.coordinates[:,index], in reference frame pts.ref
+        """
+        return Point(self.coordinates[:,index],ref=self.ref,name=name)
+
+    get = getPoint
+
+    def bestFittingPlane(self,fitPlane="xy", usesvd=False,verbose=True):
+        """
+        bestFittingPlane(self,fitPlane="xy", usesvd=False,verbose=True)
+
+        fitPlane in ['xy,'yz','zx']
+        usesvd   see transform3d_addon.affine_matrix_from_points.__doc__
+
+        OUTPUT
+        returns the reference Frame having as X-Y plane the plane best fitting all points in self
+        """
+        # Import necessary due to a circular dependency between Point and ReferenceFrame
+        from egse.coordinates.referenceFrame import ReferenceFrame
+
+        debug=True
+
+        a,b,c = self.fitPlane(fitPlane=fitPlane,verbose=verbose)
+        #print (f"a {a}, b {b}, c {c}")
+        #print()
+
+        unitaxes = Points.fromPlaneParameters(a,b,c,ref=self.ref,plane=fitPlane)
+        #print(f"Unitaxes coordinates \n{np.round(unitaxes.coordinates,3)}")
+        #print()
+
+
+        # unitaxes contain 3 unit axes and an origin
+        # => the unit vectors do NOT belong to the target plane
+        # => they must be translated before
+        unitcoords = unitaxes.coordinates
+        for ax in range(3):
+            unitcoords[:3,ax] += unitcoords[:3,3]
+
+        newaxes = Points(unitcoords, ref=self.ref)
+
+        #print(f"newaxes {np.round(newaxes.coordinates,3)}")
+
+        selfaxes = Points(np.identity(4),ref=self.ref)
+
+        transform  = t3add.affine_matrix_from_points(selfaxes.coordinates[:3,:], newaxes.coordinates[:3,:], shear=False, scale=False, usesvd=usesvd)
+
+
+        if debug:
+            transform2 =        t3add.rigid_transform_3D(selfaxes.coordinates[:3,:], newaxes.coordinates[:3,:], verbose=verbose)
+
+        if verbose:
+            print()
+            print(f"Transform  \n{np.round(transform,3)}")
+            if debug:
+                print()
+                print(f"Transform2 \n{np.round(transform2,3)}")
+                print()
+                print(f"Both methods consistent ? {np.allclose(transform, transform2)}")
+
+        return ReferenceFrame(transformation=transform, ref=self.ref)
+
+
+
+    def fitPlane(self,fitPlane="xy",verbose=True):
+        """
+        fitPlane(self,fitPlane="xy",verbose=True)
+
+        :returns: the best fitting parameters a, b, c corresponding to the fitted plane
+
+        :param fitPlane: defines the expression of the fitted plane in ['xy','yz','zx']:
+                         'xy' : z = ax + by + c
+                         'yz' : x = ay + bz + c
+                         'zx' : y = az + bx + c
+
+        :param verbose: default = True
+        """
+        xyz = [self.x,self.y,self.z]
+
+        ndata = len(xyz[0])
+
+        # Account for cases
+        startingIndex={'xy':0,'yz':1,'zx':2}[fitPlane]
+
+        # System matrix
+        A = np.vstack([xyz[startingIndex],xyz[(startingIndex+1)%3], np.ones(ndata)]).T
+
+        # Solve linear equations
+        a, b, c = np.linalg.lstsq(A, xyz[(startingIndex+2)%3],rcond=None)[0]
+
+        # Print results on screen
+        if verbose:
+            hprint = {'xy':'z = ax + by + c','yz':'x = ay + bz + c','zx':'y = az + bx + c'}
+            print(f'{hprint[fitPlane]} : \n    a = {a:7.3e}  \n    b = {b:7.3e} \n    c = {c:7.3e}')
+
+        return a,b,c
+
+    @classmethod
+    def fromPlaneParameters(cls,a,b,c,ref,plane='xy',verbose=False):
+        """fromPlaneParameters(cls,a,b,c,ref,plane='xy',verbose=False)
+
+        Returns a Points object describing the unit axes and the origin of the reference frame defined by
+        the input parameters.
+
+        :param plane: The plane definition depends on the "plane" parameter plane must be in ["xy","yz","zx"]
+                      'xy' : z = ax + by + c (origin defined at x = y = 0)
+                      'yz' : x = ay + bz + c  --> NOT IMPLEMENTED
+                      'zx' : y = az + bx + c  --> NOT IMPLEMENTED
+
+        :returns:  A Points with 4 Point objects corresponding to
+                   - the 3 unit axes defining a coordinate system with this plane as "plane" plane
+                   - the origin
+                   in this order (origin last)
+        """
+        if plane != 'xy':
+            print(f"WARNING: plane = {plane} NOT IMPLEMENTED")
+        # p0 : on the Z-axis
+        # x = y = 0
+        p0 = np.array([0,0,c])
+
+        # pxy : on the intersection between target plane and plane // to xy passing through z=c
+        if np.abs(b)>1.e-5:
+            # z = c, x = 1
+            pxy = np.array([1,-a/float(b),c])
+        else:
+            # z = c, y = 1
+            pxy = np.array([-b/float(a),1,c])
+
+        # pyz : intersection of target plane and Y-Z plane
+        # x = 0, y = 1
+        pyz = np.array([0,1,b+c])
+
+        """
+        # pzx : intersection of target plane and Z-X plane
+        if np.abs(a)>1.e-3:
+            # y = 0, z = 1
+            pzx = np.array([(1-c)/float(a),0,1])
+        else:
+            # y = 0, x = 1
+            pzx = np.array([1,0,a+c])
+        """
+        #
+        # xunit = unit vector from [0,0,0] along the intersection between target plane and plane // to xy passing through z=c
+        xunit = (pxy - p0)/np.linalg.norm(pxy-p0)
+        # ytemp = unit vector from [0,0,0] along the intersection between target plane and Y-Z plane
+        # ytemp is in the target plane, but not perpendicular to xunit
+        # its norm doesn't matter
+        ytemp = (pyz - p0)#/np.linalg.norm(pyz-p0)
+
+        # xunit and ytemp are both in the plane
+        # => zunit is perpendicular to both
+        zunit = np.cross(xunit,ytemp)
+        zunit /= np.linalg.norm(zunit)
+
+        # yunit completes the right handed reference frame
+        # The renormalisation shouldn't be necessary...
+        yunit = np.cross(zunit,xunit)
+        yunit /= np.linalg.norm(yunit)
+
+        xu = Point(xunit,ref=ref)
+        yu = Point(yunit,ref=ref)
+        zu = Point(zunit,ref=ref)
+        origin = Point(p0,ref=ref)
+
+        if verbose:
+            print(f'xunit  {xunit}')
+            print(f'yunit  {yunit}')
+            print(f'zunit  {zunit}')
+            print(f'origin {p0}')
+            print()
+
+        return cls([xu,yu,zu,origin],ref=ref)