pfc-geometry 0.1.1__py3-none-any.whl → 0.2.4__py3-none-any.whl

geometry/quaternion.py

@@ -9,21 +9,22 @@ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 You should have received a copy of the GNU General Public License along with
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
+from __future__ import annotations
 from .point import Point
 from .base import Base
 from geometry import PZ
-from typing import Union, Tuple
 import numpy as np
+import numpy.typing as npt
+import pandas as pd
 from warnings import warn
 from numbers import Number
 
 
-
 class Quaternion(Base):
     cols=["w", "x", "y", "z"]
 
     @staticmethod
-    def zero(count=1):
+    def zero(count=1) -> Quaternion:
         return Quaternion(np.tile([1,0,0,0], (count,1)))
 
     @property
@@ -31,19 +32,19 @@ class Quaternion(Base):
         return np.array([self.x, self.y, self.z, self.w]).T
 
     @property
-    def axis(self):
+    def axis(self) -> Point:
         return Point(self.data[:,1:])
 
-    def norm(self):
+    def norm(self) -> Quaternion:
         return self / abs(self)
 
-    def conjugate(self):
+    def conjugate(self) -> Quaternion:
         return Quaternion(self.w, -self.x, -self.y, -self.z)
 
     def inverse(self):
         return self.conjugate().norm()
 
-    def __mul__(self, other):
+    def __mul__(self, other: Number | Quaternion | npt.NDArray) -> Quaternion:
         if isinstance(other, Quaternion):
             a, b = Quaternion.length_check(self, Quaternion.type_check(other))
             w = a.w * b.w - a.axis.dot(b.axis)
@@ -53,15 +54,15 @@ class Quaternion(Base):
         elif isinstance(other, Number):
             return Quaternion(self.data * other)
         elif isinstance(other, np.ndarray):
-            return Quaternions(self.data * self._dprep(other))
+            return Quaternion(self.data * self._dprep(other))
                         
         raise TypeError(f"cant multiply a quaternion by a {other.__class__.__name__}")
 
-    def __rmul__(self, other):
+    def __rmul__(self, other) -> Quaternion:
         #either it should have been picked up by the left hand object or it should commute
         return self * other   
 
-    def transform_point(self, point: Point):
+    def transform_point(self, point: Point) -> Point:
         '''Transform a point by the rotation described by self'''
         a, b = Base.length_check(self, point)
         
@@ -70,9 +71,9 @@ class Quaternion(Base):
         return (a * Quaternion(qdata) * a.inverse()).axis
   
     @staticmethod
-    def from_euler(eul: Point):
-        eul = Point.type_check(eul)
-        # xyz-fixed Euler angle convention: matches ArduPilot AP_Math/Quaternion::from_euler
+    def from_euler(eul: Point) -> Quaternion:
+        '''Create a quaternion from a Point of Euler angles order z, y, x'''
+        eul = Point.type_check(eul).unwrap()
         half = eul * 0.5
         c = half.cos
         s = half.sin
@@ -86,18 +87,16 @@ class Quaternion(Base):
             ]).T
         )
 
-    def to_euler(self):
-        
-        # roll (x-axis rotation)
+    def to_euler(self) -> Point:
+        '''Create a Point of Euler angles order z,y,x'''
         sinr_cosp = 2 * (self.w * self.x + self.y * self.z)
         cosr_cosp = 1 - 2 * (self.x * self.x + self.y * self.y)
         roll = np.arctan2(sinr_cosp, cosr_cosp)
 
-        # pitch (y-axis rotation)
         sinp = 2 * (self.w * self.y - self.z * self.x)
-        pitch = np.arcsin(sinp)
+        with np.errstate(invalid='ignore'):
+            pitch = np.arcsin(sinp)
                 
-        # yaw (z-axis rotation)
         siny_cosp = 2 * (self.w * self.z + self.x * self.y)
         cosy_cosp = 1 - 2 * (self.y * self.y + self.z * self.z)
         yaw = np.arctan2(siny_cosp, cosy_cosp)
@@ -111,7 +110,7 @@ class Quaternion(Base):
         return Point(roll, pitch, yaw)
 
     @staticmethod
-    def from_axis_angle(axangles: Point):
+    def from_axis_angle(axangles: Point) -> Quaternion:
         small = 1e-6
         angles = abs(axangles)
 
@@ -132,43 +131,71 @@ class Quaternion(Base):
         #qdat[abs(Quaternions(qdat)) < .001] = np.array([[1, 0, 0, 0]])
         return Quaternion(qdat)
 
-    def to_axis_angle(self):
+    def to_axis_angle(self) -> Point:
+        a = self._to_axis_angle()
+        b = (-self)._to_axis_angle()
+
+        res = a.data
+        replocs = abs(a)>abs(b)
+        res[replocs, :] = b.data[replocs, :]
+
+        return Point(res)
+
+    def _to_axis_angle(self) -> Point:
         """to a point of axis angles. must be normalized first."""
         angle = 2 * np.arccos(self.w)
         s = np.sqrt(1 - self.w**2)
         np.array(s)[np.array(s) < 1e-6] = 1.0
-        return self.axis * angle / s
-
+        with np.errstate(divide="ignore", invalid='ignore'):
+            sangle = angle / s
+            sangle[sangle==np.inf] = 0
+            sangle[np.isnan(sangle)] = 0
+        res = self.axis * sangle
+        return res
 
     @staticmethod
-    def axis_rates(q, qdot) -> Point:
+    def axis_rates(q: Quaternion, qdot: Quaternion) -> Point:
         wdash = qdot * q.conjugate()
         return wdash.norm().to_axis_angle() 
 
     @staticmethod
-    def body_axis_rates(q, qdot) -> Point:
+    def _axis_rates(q: Quaternion, qdot: Quaternion) -> Point:
+        wdash = qdot * q.conjugate()
+        return wdash.norm()._to_axis_angle() 
+
+    @staticmethod
+    def body_axis_rates(q: Quaternion, qdot: Quaternion) -> Point:
         wdash = q.conjugate() * qdot
         return wdash.norm().to_axis_angle() 
 
-    def rotate(self, rate: Point):
+    @staticmethod
+    def _body_axis_rates(q: Quaternion, qdot: Quaternion) -> Point:
+        wdash = q.conjugate() * qdot
+        return wdash.norm()._to_axis_angle() 
+
+    def rotate(self, rate: Point) -> Quaternion:
         return (Quaternion.from_axis_angle(rate) * self).norm()
 
-    def body_rotate(self, rate: Point):
+    def body_rotate(self, rate: Point) -> Quaternion:
         return (self * Quaternion.from_axis_angle(rate)).norm()
 
-    def diff(self, dt: np.array) -> Point:
+    def diff(self, dt: Number | npt.NDArray) -> Point:
         """differentiate in the world frame"""
+        if not pd.api.types.is_list_like(dt):
+            dt = np.full(len(self), dt)
         assert len(dt) == len(self)
         dt = dt * len(dt) / (len(dt) - 1)
 
-        ps = Quaternion.axis_rates(
+        ps = Quaternion._axis_rates(
             Quaternion(self.data[:-1, :]),
             Quaternion(self.data[1:, :])
         ) / dt[:-1]
-        return Point(np.vstack([ps.data, ps.data[-1,:]]))#.remove_outliers(2)  # Bodge to get rid of phase jump
+        return Point(np.vstack([ps.data, ps.data[-1,:]]))
 
-    def body_diff(self, dt: np.array) -> Point:
+    def body_diff(self, dt: Number | npt.NDArray) -> Point:
         """differentiate in the body frame"""
+        if not pd.api.types.is_list_like(dt):
+            dt = np.full(len(self), dt)
         assert len(dt) == len(self)
         dt = dt * len(dt) / (len(dt) - 1)
 
@@ -179,7 +206,7 @@ class Quaternion(Base):
         return Point(np.vstack([ps.data, ps.data[-1,:]]))
 
     
-    def to_rotation_matrix(self):
+    def to_rotation_matrix(self) -> npt.NDArray[np.float64]:
         """http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
         https://github.com/mortlind/pymath3d/blob/master/math3d/quaternion.py
         """
@@ -193,7 +220,7 @@ class Quaternion(Base):
         ]).T
 
     @staticmethod
-    def from_rotation_matrix(matrix: np.ndarray):
+    def from_rotation_matrix(matrix: npt.NDArray[np.float64]) -> Quaternion:
         # This method assumes row-vector and postmultiplication of that vector
         m = matrix.conj().transpose()
         if m[2, 2] < 0:
@@ -217,15 +244,12 @@ class Quaternion(Base):
         q *= 0.5 / np.sqrt(t)
         return Quaternion(*q)
 
-    def __str__(self):
-        return "W:{w:.2f}\nX:{x:.2f}\nY:{y:.2f}\nZ:{z:.2f}".format(w=self.w, x=self.x, y=self.y, z=self.z)
-
-    def closest_principal(self):
+    def closest_principal(self) -> Quaternion:
         eul = self.to_euler()
         rads = eul * (2 / np.pi)
         return Quaternion.from_euler(rads.round(0) * np.pi/2)
 
-    def is_inverted(self):
+    def is_inverted(self) -> bool:
         # does the rotation reverse the Z axis?
         return np.sign(self.transform_point(PZ()).z) > 0
 

geometry/time.py

@@ -0,0 +1,40 @@
+from geometry import Base
+from numbers import Number
+from typing import Self
+import numpy as np
+from time import time
+
+
+class Time(Base):
+    cols=["t", "dt"]
+    
+    @staticmethod
+    def from_t(t: np.ndarray) -> Self:
+        if isinstance(t, Number):
+            return Time(t, 1/30)
+        else:
+            if len(t) == 1:
+                dt = np.array([1/30]) 
+            else:
+                arr = np.diff(t)
+                dt = np.concatenate([arr, [arr[-1]]])
+            return Time(t, dt)
+
+    def scale(self, duration) -> Self:
+        old_duration = self.t[-1] - self.t[0]
+        sfac = duration / old_duration
+        return Time(
+            self.t[0] + (self.t - self.t[0]) * sfac,
+            self.dt * sfac
+        )
+
+    def reset_zero(self):
+        return Time(self.t - self.t[0], self.dt)
+
+    @staticmethod
+    def now():
+        return Time.from_t(time())
+
+    def extend(self):
+        return Time.concatenate([self, Time(self.t[-1] + self.dt[-1], self.dt[-1])])
+

geometry/transformation.py

@@ -9,10 +9,11 @@ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 You should have received a copy of the GNU General Public License along with
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
-from geometry import Base, Point, Quaternion, Point, P0, Q0, Coord
+from geometry import Base, Point, Quaternion, P0, Q0, Coord
 
 import numpy as np
 from typing import Union
+from typing import Self
 
 
 class Transformation(Base):
@@ -21,8 +22,16 @@ class Transformation(Base):
     def __init__(self, *args, **kwargs):
         if len(args) == len(kwargs) == 0:
             args = np.concatenate([P0().data,Q0().data],axis=1)
+        elif len(args) == 1:
+            if isinstance(args[0], Point):
+                args = np.concatenate([args[0].data,Q0().data],axis=1)
+            elif isinstance(args[0], Quaternion):
+                args = np.concatenate([P0().data,args[0].data],axis=1)
         if len(args) == 2:
-            args = np.concatenate([args[0].data, args[1].data], axis=1)
+            _q = args[0] if isinstance(args[0], Quaternion) else args[1]
+            _p = args[0] if isinstance(args[0], Point) else args[1]
+            assert isinstance(_q, Quaternion) and isinstance(_p, Point), f'expected a Point and a Quaternion, got a {_p.__class__.__name__} and a {_q.__class__.__name__}'
+            args = np.concatenate([_p.data, _q.data], axis=1)
         super().__init__(*args, **kwargs)
         self.p = Point(self.data[:,:3])
         self.q = Quaternion(self.data[:,3:])
@@ -82,19 +91,24 @@ class Transformation(Base):
         )
 
     
-    def apply(self, oin: Union[Point, Quaternion]):
+    def apply(self, oin: Union[Point, Quaternion, Self, Coord]):
         if isinstance(oin, Point):
             return self.point(oin)
         elif isinstance(oin, Quaternion):
             return self.rotate(oin)
+        elif isinstance(oin, Coord):
+            return self.coord(oin)
         elif isinstance(oin, self.__class__):
             return Transformation(self.apply(oin.p), self.apply(oin.q))
+        
 
     def rotate(self, oin: Union[Point, Quaternion]):
         if isinstance(oin, Point):
             return self.q.transform_point(oin)
         elif isinstance(oin, Quaternion):
             return self.q * oin
+        else:
+            raise TypeError(f"expected a Point or a Quaternion, got a {oin.__class__.__name__}")
 
     def translate(self, point: Point):
         return point + self.p
@@ -102,8 +116,10 @@ class Transformation(Base):
     def point(self, point: Point):
         return self.translate(self.rotate(point))       
 
-    def coord(self, coord):
-        return coord.translate(self.p).rotate(self.q.to_rotation_matrix())
+    def coord(self, coord=None):
+        if coord is None:
+            coord = Coord.zero()
+        return coord.translate(self.p).rotate(self.q)
 
 
     def to_matrix(self):

pfc_geometry-0.2.4.dist-info/METADATA

@@ -0,0 +1,30 @@
+Metadata-Version: 2.1
+Name: pfc_geometry
+Version: 0.2.4
+Summary: A package for handling 3D geometry with a nice interface
+Home-page: https://github.com/PyFlightCoach/geometry
+Author: Thomas David
+Author-email: thomasdavid0@gmail.com
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: setuptools
+Requires-Dist: numpy
+Requires-Dist: pandas
+
+# geometry #
+
+Tools for handling 3D geometry, mostly just adds a nice interface to various geometric enterties. Each geometric entity can also be a vector of geometric entities. Each entity wraps a numpy array with the relevant number of columns labelled according to the cols class property and rows equal to the number of elements in the vector. Attribute access to each column is available and returns a numpy array. 
+
+Where operations are supported between geometric types the size of the output is inferred based on the length of the inputs. Where the two vectors of entities are of the same length, elementwise operations are performed. Where one vector is length one and the other is greater than one then the operation will be performed on every element of the longer vector. 
+
+Magic methods are used extensively and the function of operators are logical for each type. If unsure what the logical option is then check the code where it should be pretty clear. 
+
+Many convenience methods and constructors are available. Documentation is limited but if you need something it has probably already been written so check the code first. 
+
+
+Some examples are available here: https://pfcdocumentation.readthedocs.io/pyflightcoach/geometry.html
+
+now available on pypi:
+```bash
+    pip install pfc-geometry
+```

pfc_geometry-0.2.4.dist-info/RECORD

@@ -0,0 +1,15 @@
+geometry/__init__.py,sha256=HNhMyemIJzDq1nDjrr09eX5PS7q9ULscSbYsXss3JRM,1253
+geometry/base.py,sha256=ZgjOPv2QU6nBSEFurBXCIos0YVjEZhjjT-NTz06DCvo,11277
+geometry/coordinate_frame.py,sha256=qO3jqUWF4PW4BOkw5rBMTXfOzBjlA1HYwGYmPtu2M8M,3039
+geometry/gps.py,sha256=Fs3hakSQ754HUqRsA7NWg_MSEdYxNqyiu4gu6EDrFqI,3381
+geometry/mass.py,sha256=BUWBSITwpdRfpJR5-oJTd16BI7FLZt8rhxdzr0cx1HY,1675
+geometry/point.py,sha256=gwP9Jn80eSKmWE9Q2efS5oFYEGoO6XpBmPCFSOQ2j3Q,5025
+geometry/quaternion.py,sha256=rw_9Fx9uKMMuGAWJjH1vU-iZzA15Vbn3HyzS4qV3oxY,9367
+geometry/testing.py,sha256=o8yMBAdU5Vy0EspBYaof4fPGgRSFZhRDhzBjRPsLd0M,375
+geometry/time.py,sha256=NdzVWdL7F3HpJb4MXYJ0uPHvgHqipVMTDTR-UVNx_wc,995
+geometry/transformation.py,sha256=iCQRTC1sbH_TCRpm9d_PmGS8qxAne5LpGcCSeRpmcpM,4701
+pfc_geometry-0.2.4.dist-info/LICENSE,sha256=z72U6pv-bQgJ_Svr4uCXnMjemsp38aSerhHEdEAOMJ4,7632
+pfc_geometry-0.2.4.dist-info/METADATA,sha256=uyHMoGMgnSpAmPEcwQHjh_6AxVVqCZMQP0SebSx3HUE,1675
+pfc_geometry-0.2.4.dist-info/WHEEL,sha256=GJ7t_kWBFywbagK5eo9IoUwLW6oyOeTKmQ-9iHFVNxQ,92
+pfc_geometry-0.2.4.dist-info/top_level.txt,sha256=RWbWhWYclEM-OFtXLCB4eLv-jxNnlJB-NPC2YM587Fo,9
+pfc_geometry-0.2.4.dist-info/RECORD,,

{pfc_geometry-0.1.1.dist-info → pfc_geometry-0.2.4.dist-info}/WHEEL

@@ -1,5 +1,5 @@
 Wheel-Version: 1.0
-Generator: bdist_wheel (0.38.4)
+Generator: bdist_wheel (0.43.0)
 Root-Is-Purelib: true
 Tag: py3-none-any
 

geometry/circle.py

@@ -1,28 +0,0 @@
-"""
-This program is free software: you can redistribute it and/or modify it under
-the terms of the GNU General Public License as published by the Free Software
-Foundation, either version 3 of the License, or (at your option) any later
-version.
-This program is distributed in the hope that it will be useful, but WITHOUT
-ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-You should have received a copy of the GNU General Public License along with
-this program. If not, see <http://www.gnu.org/licenses/>.
-"""
-from geometry import Point, Quaternion, Transformation
-
-
-
-class Circle():
-    def __init__(self, radius: float, transform: Transformation):
-        """The circle is about the Z axis, centre on the origin. The transformation 
-        defines the location of the axis.
-
-        Args:
-            radius (float): The radius of the circle
-            transform (Transformation): The transformation to the circles axis (0,0,1)
-        """
-        self.radius = radius
-        self.transform = transform
-    
-#change