pfc-geometry 0.1.8__tar.gz → 0.2.1__tar.gz

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/PKG-INFO

@@ -1,12 +1,15 @@
 Metadata-Version: 2.1
 Name: pfc_geometry
-Version: 0.1.8
+Version: 0.2.1
 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 #
 

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/geometry/base.py

@@ -10,9 +10,10 @@ 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 typing import Type, List
+from typing import Type, List, Self
 from typing_extensions import Self
 import numpy as np
+import numpy.typing as npt
 import pandas as pd
 from numbers import Number
 
@@ -71,14 +72,15 @@ class Base:
                 self.data = self.__class__._clean_data(np.array(args).T)
             elif all(isinstance(arg, list) for arg in args):
                 self.data = self.__class__._clean_data(np.array(args).T)
-
+            elif all(isinstance(arg, pd.Series) for arg in args):
+                self.data = self.__class__._clean_data(np.array(pd.concat(args, axis=1)))
             else:
                 raise TypeError
         else:
             raise TypeError(f"Empty {self.__class__.__name__} not allowed")
 
     @classmethod
-    def _clean_data(cls, data) -> np.ndarray:
+    def _clean_data(cls, data) -> npt.NDArray[np.float64]:
         assert isinstance(data, np.ndarray)
         if data.dtype == 'O': 
             raise TypeError(f'data must have homogeneous shape for {cls.__name__}, given {data.shape}')
@@ -106,7 +108,7 @@ class Base:
     def concatenate(cls, items) -> Self:
         return cls(np.concatenate([i.data for i in items], axis=0))
 
-    def __getattr__(self, name):
+    def __getattr__(self, name) -> npt.NDArray[np.float64]:
         if name in self.__class__.cols:
             return self.data[:,self.__class__.cols.index(name)]
             #return res[0] if len(res) == 1 else res
@@ -153,8 +155,6 @@ class Base:
     def degrees(self) -> Self:
         return self.__class__(np.degrees(self.data))
 
-
-
     def count(self) ->  int:
         return len(self)
 
@@ -197,8 +197,6 @@ class Base:
     def __truediv__(self, other) -> Self:
         return self.__class__(self.data / other)
 
-
-
     def __str__(self):
         means = ' '.join(f'{c}_={v}' for c, v in zip(self.cols, np.mean(self.data, axis=0).round(2)))
         return f'{self.__class__.__name__}({means}, len={len(self)})'
@@ -217,10 +215,12 @@ class Base:
         return np.einsum('ij,ij->i', self.data, other)   
 
     def diff(self, dt:np.array) -> Self:
+        if not pd.api.types.is_list_like(dt):
+            dt = np.full(len(self), dt)
         assert len(dt) == len(self)
         return self.__class__(
             np.gradient(self.data,axis=0) \
-                 / \
+                / \
                 np.tile(dt, (len(self.__class__.cols),1)).T)
 
     def to_pandas(self, prefix='', suffix='', columns=None, index=None):
@@ -279,4 +279,43 @@ class Base:
     
     def __repr__(self):
         return str(self)
-    
+    
+    def copy(self):
+        return self.__class__(self.data.copy())
+    
+
+    def unwrap(self, discont=np.pi):
+        return self.__class__(np.unwrap(self.data, discont=discont, axis=0))
+    
+    def filter(self, order, cutoff, ts: np.ndarray=None):
+        from scipy.signal import butter, freqz, filtfilt
+        if ts is None:
+            ts = np.array(range(len(self)))
+        N = len(self)
+        T = (ts[-1] - ts[0]) / N
+
+        fs = 1/T
+        b, a = butter(
+            order,
+            cutoff,
+            fs=fs,
+            btype='low', analog=False
+        )
+
+        return self.__class__(filtfilt(b, a, self.data, axis=0))
+    
+    def fft(self, ts: np.ndarray=None):
+        from scipy.fft import fft, fftfreq
+        if ts is None:
+            ts = np.array(range(len(self)))
+        N = len(self)
+        T = (ts[-1] - ts[0]) / N
+
+        yf = fft(self.data, axis=0)
+        xf = fftfreq(N, T)[:N//2]
+
+        
+        y=2.0/N * np.abs(yf[0:N//2, :])
+
+        return pd.DataFrame(np.column_stack([xf, y]), columns=['freq'] + self.cols).set_index('freq')
+

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/geometry/gps.py

@@ -29,59 +29,52 @@ LOCFAC = math.radians(erad)
 
 
 class GPS(Base):
-    cols = ["lat", "long"]
+    cols = ["lat", "long", "alt"]
     # was 6378137, extra precision removed to match ardupilot
 
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
         self._longfac = safecos(self.lat)
 
-
-    def offset(self, pin: Point):
-        assert len(pin) == len(self)
-        latb = self.lat - pin.x / LOCFAC
-
-        return GPS(
-            latb,
-            self.long + pin.y / (LOCFAC * safecos(latb))
-        )
-
     def __eq__(self, other) -> bool:
         return np.all(self.data == other.data)
 
     def __sub__(self, other) -> Point:
-        assert isinstance(other, GPS)
+        assert isinstance(other, GPS), f'Cannot offset a GPS by a {other.__class__.__name__}'
         if len(other) == len(self):
             return Point(
-                -(other.lat - self.lat) * LOCFAC,
-                -(other.long - self.long) * LOCFAC * self._longfac,
-                np.zeros(len(self))
+                (self.lat - other.lat) * LOCFAC,
+                (self.long - other.long) * LOCFAC * self._longfac,
+                other.alt - self.alt
             )
         elif len(other) == 1:
             return self - GPS.full(other, len(self))
         elif len(self) == 1:
             return GPS.full(self, len(self)) - other
         else:
-            raise ValueError(f"incompatible lengths for sub ({len(self)}) - ({len(other)})")
+            raise ValueError(f"incompatible lengths for GPS sub ({len(self)}) != ({len(other)})")
 
     def offset(self, pin: Point):
+        '''Offset by a point in NED coordinates'''
         if len(pin) == 1 and len(self) > 1:
-            pin = Point.full(pin, self.count)
+            pin = Point.full(pin, len(self))
         elif len(self) == 1 and len(pin) > 1:
-            return self.full(len(pin)).offset(pin)
+            return GPS.full(self, len(pin)).offset(pin)
         
         if not len(pin) == len(self):
-            raise ValueError(f"incompatible lengths for offset ({len(self)}) - ({len(pin)})")
+            raise ValueError(f"incompatible lengths for GPS offset ({len(self)}) != ({len(pin)})")
 
         latb = self.lat + pin.x / LOCFAC
         return GPS(
             latb,
-            self.long + pin.y / (LOCFAC * safecos(latb))
+            self.long + pin.y / (LOCFAC * safecos(latb)),
+            self.alt - pin.z
         )
 
 
-
 '''
+Extract from ardupilot:
+
 // scaling factor from 1e-7 degrees to meters at equator
 // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
 static constexpr float LOCATION_SCALING_FACTOR = 0.011131884502145034f;
@@ -102,10 +95,3 @@ float Location::long_scale() const
 }
 '''
 
-
-if __name__ == "__main__":
-    home = GPS(51.459387, -2.791393)
-
-    new = GPS(51.458876, -2.789092)
-    coord = home - new
-    print(coord.x, coord.y)

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/geometry/mass.py

@@ -50,5 +50,14 @@ class Mass(Base):
             ])) 
         ) + self
 
+    def momentum(self, v: Point):
+        return self.m * v
+    
+    def angular_momentum(self, rvel: Point):
+        return Point(
+            self.xx * rvel.x + self.xy * rvel.y + self.xz * rvel.z,
+            self.yx * rvel.x + self.yy * rvel.y + self.yz * rvel.z,
+            self.zx * rvel.x + self.zy * rvel.y + self.zz * rvel.z,
+        )
+
 
-    

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/geometry/quaternion.py

@@ -15,6 +15,8 @@ from .base import Base
 from geometry import PZ
 from typing import Union, Tuple
 import numpy as np
+import pandas as pd
+import numpy.typing as npt
 from warnings import warn
 from numbers import Number
 
@@ -44,7 +46,7 @@ class Quaternion(Base):
     def inverse(self):
         return self.conjugate().norm()
 
-    def __mul__(self, other):
+    def __mul__(self, other: Union[Number, Quaternion, np.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)
@@ -58,7 +60,7 @@ class Quaternion(Base):
                         
         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   
 
@@ -72,8 +74,8 @@ class Quaternion(Base):
   
     @staticmethod
     def from_euler(eul: Point) -> Quaternion:
-        eul = Point.type_check(eul)
-        # xyz-fixed Euler angle convention: matches ArduPilot AP_Math/Quaternion::from_euler
+        '''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
@@ -88,17 +90,15 @@ class Quaternion(Base):
         )
 
     def to_euler(self) -> Point:
-        
-        # roll (x-axis rotation)
+        '''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)
@@ -133,7 +133,7 @@ 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()
 
@@ -181,8 +181,10 @@ class Quaternion(Base):
     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: Union[Number, np.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)
 
@@ -190,10 +192,12 @@ class Quaternion(Base):
             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: Union[Number, np.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)
 
@@ -204,7 +208,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
         """
@@ -218,7 +222,7 @@ class Quaternion(Base):
         ]).T
 
     @staticmethod
-    def from_rotation_matrix(matrix: np.ndarray) -> Quaternion:
+    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:

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/geometry/transformation.py

@@ -21,8 +21,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)
+            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:])

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/pfc_geometry.egg-info/PKG-INFO

@@ -1,12 +1,15 @@
 Metadata-Version: 2.1
 Name: pfc-geometry
-Version: 0.1.8
+Version: 0.2.1
 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 #
 

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/setup.cfg

@@ -5,7 +5,7 @@ author_email = thomasdavid0@gmail.com
 description = A package for handling 3D geometry with a nice interface
 long_description = file: README.md
 long_description_content_type = text/markdown
-version = 0.1.8
+version = 0.2.1
 url = https://github.com/PyFlightCoach/geometry
 
 [options]

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/tests/test_base.py

@@ -66,7 +66,7 @@ def test_init_values():
     abc = ABC(*[np.ones(10) for _ in range(3)])
     np.testing.assert_array_equal(abc.data, np.ones((10,3)))
 
-    with raises(TypeError):
+    with raises(ValueError):
         abc = ABC([1,2,3], [1,2], [1,2,3,4])
 
     abc = ABC([1,2], [1,2], [1,2])
@@ -89,10 +89,6 @@ def test_init_kwargs():
     with raises(TypeError):
         ABC(ggg=234342)
 
-def test_init_list_of_self():
-    abc = ABC([ABC(1,2,3), ABC(np.ones((4,3)))])
-    np.testing.assert_array_equal(abc.b, np.array([2,1,1,1,1]))
-
 def test_init_empty():
     with raises(TypeError):
         ABC()
@@ -210,7 +206,12 @@ def test_concatenate():
 
 def test_repr__():
     p = ABC(1,2,3)
-    rpr = p.__repr__().split("\n")
-    assert rpr[0] == "ABC"
-    assert np.all(rpr[1:] == str(p.to_pandas()).split("\n"))
-    
+    rpr = p.__repr__()
+    assert rpr == "ABC(a_=1.0 b_=2.0 c_=3.0, len=1)"
+    
+
+def test_three_pandas_series():
+    df = pd.DataFrame(np.random.random((10,3)), columns=list("abc"))
+    abc = ABC(df.a, df.b, df.c)
+
+    pd.testing.assert_frame_equal(abc.to_pandas(), df)

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/tests/test_gps.py

@@ -4,8 +4,8 @@ from geometry.gps import GPS
 import numpy as np
 
 def test_offset():
-    c = GPS( 52.542375, -1.631038)
-    p = GPS( 52.542264, -1.631817)
+    c = GPS( 52.542375, -1.631038, 0)
+    p = GPS( 52.542264, -1.631817, 0)
     diff = c - p
     c2 = p.offset(diff)
 
@@ -14,27 +14,27 @@ def test_offset():
     np.testing.assert_array_almost_equal(diff.data, diff2.data, 1e-4)
 
 def test_diff():
-    p0 = GPSPosition( 50.206, 4.1941755999999994)
-    p0n = GPSPosition( 50.201, 4.1941755999999994)
+    p0 = GPS( 50.206, 4.1941755999999994, 0)
+    p0n = GPS( 50.201, 4.1941755999999994, 0)
     
     diff= p0 - p0n # should be south vector
-    pytest.approx(diff.y, 0)
+    approx(diff.y, 0)
     assert diff.x >  0
     
-    p0e= GPSPosition( 50.206, 4.195)
+    p0e= GPS( 50.206, 4.195, 0)
     diff= p0 - p0e # should be west vector
-    pytest.approx(diff.x, 0)
+    approx(diff.x, 0)
     assert diff.y < 0
 
 def test_diff():
-    p0 = GPS( 50.201, 4.195)
-    p0n = GPS( 50.206, 4.195) #directly north of p0
+    p0 = GPS( 50.201, 4.195, 0)
+    p0n = GPS( 50.206, 4.195, 0) #directly north of p0
     
     diff= p0 - p0n # should be south vector
     assert diff.y == approx(0)
     assert diff.x < 0
     
-    p0e= GPS( 50.201, 4.196)
+    p0e= GPS( 50.201, 4.196, 0)
     diff= p0 - p0e # should be west vector
     assert diff.x == approx(0)
     assert diff.y < 0
@@ -43,8 +43,8 @@ def test_diff():
 
 
 def test_sub():
-    centre = GPS( 52.542375, -1.631038)
-    pilot = GPS( 52.542264, -1.631817)
+    centre = GPS( 52.542375, -1.631038, 0)
+    pilot = GPS( 52.542264, -1.631817, 0)
 
     vec = centre - pilot 
 

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/tests/test_quaternion.py

@@ -3,8 +3,10 @@ from pytest import mark, approx, raises
 from geometry.quaternion import Quaternion, Q0
 from geometry.point import Point, PX, PY, PZ, P0
 from geometry import Euler, Euldeg
+from geometry.testing import assert_almost_equal
 import pandas as pd
 import numpy as np
+import quaternion
 from scipy.spatial.transform import Rotation as R
 
 
@@ -44,6 +46,13 @@ def test_from_euler():
     )
 
 
+def test_from_euler_unwrap():    
+    q = Q0(10).rotate(PZ()*np.radians(np.linspace(0,360, 10)))
+    np.testing.assert_array_almost_equal(
+        Quaternion.from_euler(q.to_euler()).diff(1).data, 
+        q.diff(1).data
+    )
+
 
 def test_to_euler():
     qarr = Quaternion(np.random.random((2, 4))).norm()
@@ -111,18 +120,6 @@ def test_body_diff():
         Point.X(np.pi/100, 100).data
     )
 
-def test_transform_point2():
-    tqs = Quaternion(np.random.random((10,4))).norm()
-
-    np.testing.assert_array_almost_equal(
-        tqs.transform_point(Point(1, 1, 1)).data,
-        Quaternion.rotate_vectors(
-            np.array([np.quaternion(*q.data[0]) for q in tqs]),
-            Point(1,1,1).tile(1).data,
-            axis=1
-        ).reshape(10,3)
-    )
-
 def test_tp_2():
     np.testing.assert_array_equal(
         Quaternion(0.1, 0, 0,0).norm().transform_point(Point(1,0,0)).data,
@@ -162,7 +159,8 @@ def test_to_from_axis_angle():
 
 def test_to_axis_angle():
     q1 = Quaternion.from_euler(PZ(np.pi/4))
-    assert q1.to_axis_angle() == PZ(np.pi/4)
+    
+    assert_almost_equal( q1.to_axis_angle(), PZ(np.pi/4))
 
 
 

{pfc_geometry-0.1.8 → pfc_geometry-0.2.1}/tests/test_transform.py

@@ -1,7 +1,7 @@
 from pytest import mark, approx
 from geometry import Coord, Transformation, Point, Quaternion, PX, PY, PZ, P0
 import numpy as np
-
+from geometry.testing import assert_almost_equal
 
 
 def test_from_coords():
@@ -12,13 +12,14 @@ def test_from_coords():
     
     ps = Point(np.random.random((100, 3)))
     
-    np.testing.assert_array_almost_equal(
-        ps.data,
-        trans_from.translate(trans_to.translate(ps)).data
+    assert_almost_equal(
+        ps,
+        trans_from.translate(trans_to.translate(ps))
     )
 
     qs = Quaternion.from_euler(ps)
-    np.testing.assert_array_almost_equal(
+    
+    assert_almost_equal(
         qs,
         trans_from.rotate(trans_to.rotate(qs))
     )