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

geometry/__init__.py

@@ -9,6 +9,8 @@ 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 .base import Base
+from .time import Time
 from .point import *
 from .quaternion import *
 from .gps import GPS
@@ -17,11 +19,11 @@ from .transformation import Transformation
 from .mass import Mass
 
 
-def Euler(*args, **kwargs):
+def Euler(*args, **kwargs) -> Quaternion:
     return Quaternion.from_euler(Point(*args, **kwargs))
     
 
-def Euldeg(*args, **kwargs):
+def Euldeg(*args, **kwargs) -> Quaternion:
     return Quaternion.from_euler(Point(*args, **kwargs).radians())
 
 

geometry/base.py

@@ -10,8 +10,9 @@ 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 Self
 import numpy as np
+import numpy.typing as npt
 import pandas as pd
 from numbers import Number
 
@@ -47,11 +48,11 @@ class Base:
             elif "data" in kwargs:
                 args = [kwargs["data"]]
             else:
-                raise TypeError("unknown kwargs passed")
+                raise TypeError(f"unknown kwargs passed to {self.__class__.__name__}: {args}")
 
         if len(args)==1: 
-            if isinstance(args[0], np.ndarray): #data was passed directly
-                self.data = self.__class__._clean_data(args[0])
+            if isinstance(args[0], np.ndarray) or isinstance(args[0], list): #data was passed directly
+                self.data = self.__class__._clean_data(np.array(args[0]))
 
             elif all([isinstance(a, self.__class__) for a in args[0]]):
                 #a list of self.__class__ is passed, concatenate into one
@@ -60,7 +61,7 @@ class Base:
             elif isinstance(args[0], pd.DataFrame):
                 self.data = self.__class__._clean_data(np.array(args[0]))
             else:
-                raise TypeError("unknown data passed")
+                raise TypeError(f"unknown args passed to {self.__class__.__name__}: {args[0]}")
             
         elif len(args) == len(self.__class__.cols):
             #three args passed, each represents a col
@@ -70,17 +71,18 @@ 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('data must have homogeneous shape')
+            raise TypeError(f'data must have homogeneous shape for {cls.__name__}, given {data.shape}')
         if len(data.shape) == 1:
             data = data.reshape(1, len(data))
         
@@ -102,21 +104,31 @@ class Base:
         return a, b
 
     @classmethod
-    def concatenate(cls, items):
+    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
         elif name in self.__class__.from_np + self.__class__.from_np_base:
             return self.__class__(getattr(np, name)(self.data))
+        else:
+            for col in self.__class__.cols:
+                if len(name) > len(col):
+                    if name[:len(col)] == col:
+                        try:
+                            id = int(name[len(col):])
+                        except ValueError:
+                            break
+                        return getattr(self, col)[id]
+
         raise AttributeError(f"Cannot get attribute {name}")
 
     def __dir__(self):
         return self.__class__.cols
 
-    def __getitem__(self, sli):
+    def __getitem__(self, sli) -> Self:
         return self.__class__(self.data[sli,:])
 
     def _dprep(self, other):        
@@ -146,78 +158,86 @@ class Base:
         else:
             raise ValueError(f"unhandled datatype ({other.__class__.name})")
 
-    def radians(self):
+    def radians(self) -> Self:
         return self.__class__(np.radians(self.data))
 
-    def degrees(self):
+    def degrees(self) -> Self:
         return self.__class__(np.degrees(self.data))
 
-
-
-    def count(self):
+    def count(self) ->  int:
         return len(self)
 
-    def __len__(self):
+    def __len__(self) -> int:
         return self.data.shape[0]
 
+    @property
+    def ends(self) -> Self:
+        return self.__class__(self.data[[0,-1], :])
+
     @dprep
     def __eq__(self, other):
         return np.all(self.data == other)
 
     @dprep
-    def __add__(self, other):
+    def __add__(self, other) -> Self:
         return self.__class__(self.data + other)
     
     @dprep
-    def __radd__(self, other):
+    def __radd__(self, other) -> Self:
         return self.__class__(other + self.data)
 
     @dprep
-    def __sub__(self, other):
+    def __sub__(self, other) -> Self:
         return self.__class__(self.data - other)
     
     @dprep
-    def __rsub__(self, other):
+    def __rsub__(self, other) -> Self:
         return self.__class__(other - self.data)
 
     @dprep
-    def __mul__(self, other):
+    def __mul__(self, other) -> Self:
         return self.__class__(self.data * other)
 
     @dprep
-    def __rmul__(self, other):
+    def __rmul__(self, other) -> Self:
         return self.__class__(other * self.data)
 
     @dprep
-    def __rtruediv__(self, other):
+    def __rtruediv__(self, other) -> Self:
         return self.__class__(other / self.data)
 
     @dprep
-    def __truediv__(self, other):
+    def __truediv__(self, other) -> Self:
         return self.__class__(self.data / other)
 
     def __str__(self):
-        return str(pd.DataFrame(self.data, columns=self.__class__.cols))
+        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)})'
 
     def __abs__(self):
         return np.linalg.norm(self.data, axis=1)
+    
+    def abs(self) -> Self:
+        return self.__class__(np.abs(self.data))
 
-    def __neg__(self):
+    def __neg__(self) -> Self:
         return self.__class__(-self.data)
 
     @dprep
-    def dot(self, other):
+    def dot(self, other: Self) -> Self:
         return np.einsum('ij,ij->i', self.data, other)   
 
-    def diff(self, dt:np.array):
+    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):
-        if not columns is None:
+        if columns is not None:
             cols = columns
         else:
             cols = [prefix + col + suffix for col in self.__class__.cols]
@@ -227,7 +247,7 @@ class Base:
             index=index
         )
 
-    def tile(self, count):
+    def tile(self, count) -> Self:
         return self.__class__(np.tile(self.data, (count, 1)))
 
     def to_dict(self):
@@ -235,6 +255,17 @@ class Base:
             return {key: getattr(self, key)[0] for key in self.cols}
         else:
             return {key: getattr(self, key) for key in self.cols}
+    
+    @classmethod
+    def from_dict(Cls, data):
+        return Cls(**data)
+
+    def to_dicts(self):
+        return self.to_pandas().to_dict('records')
+    
+    @classmethod
+    def from_dicts(Cls, data: dict):
+        return Cls(pd.DataFrame.from_dict(data))
 
     @classmethod
     def full(cls, val, count):
@@ -257,4 +288,47 @@ class Base:
         return self.__class__(np.cumsum(self.data,axis=0))
 
     def round(self, decimals=0):
-        return self.__class__(self.data.round(decimals))
+        return self.__class__(self.data.round(decimals))
+    
+    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)*2
+        T = (ts[-1] - ts[0]) / len(self)
+
+        yf = fft(self.data, axis=0, n=N)
+        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')
+

geometry/coordinate_frame.py

@@ -19,7 +19,7 @@ from geometry.base import Base
 
 class Coord(Base):
     cols = [
-        "ox", "oy", "ox",
+        "ox", "oy", "oz",
         "x1", "y1", "z1",
         "x2", "y2", "z2",
         "x3", "y3", "z3",
@@ -42,6 +42,10 @@ class Coord(Base):
             z.unit().data
         ],axis=1))
 
+    @staticmethod
+    def zero(count=1):
+        return Coord.from_nothing(count)
+
     @staticmethod
     def from_nothing(count=1):
         return Coord.from_axes(P0(count), PX(1,count), PY(1,count), PZ(1,count))

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)

geometry/mass.py

@@ -33,6 +33,10 @@ class Mass(Base):
     def matrix(self):
         return self.data[:,1:].reshape((len(self), 3, 3))
 
+    @property
+    def I(self):
+        return self.matrix()
+
     def offset(self, v: Point):
         xx = v.y**2 + v.z**2
         yy = v.z**2 + v.x**2
@@ -46,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,
+        )
+
 
-    

geometry/point.py

@@ -9,6 +9,7 @@ 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 .base import Base
 import numpy as np
 import pandas as pd
@@ -23,15 +24,13 @@ class Point(Base):
         "arcsin","arccos","arctan",
     ]
 
-    def scale(self, value):
-        a, b=value, abs(self)
-        res = a/b
-        res[b==0] = 0
-        res = self * res
+    def scale(self, value) -> Point:
+        with np.errstate(divide="ignore"):
+            res = value/abs(self)
+        res[res==np.inf] = 0
+        return self * res
         
-        return res
-        
-    def unit(self):
+    def unit(self) -> Point:
         return self.scale(1)
 
     def remove_outliers(self, nstds = 2):
@@ -43,7 +42,7 @@ class Point(Base):
 
         data[abs(ab - mean) > nstds * std, :] = [np.nan, np.nan, np.nan]
 
-        return Point(pd.DataFrame(data).fillna(method="ffill").to_numpy())
+        return Point(pd.DataFrame(data).ffill().bfill().to_numpy())
 
     def mean(self):
         return Point(np.mean(self.data, axis=0))
@@ -141,31 +140,27 @@ def cross(a, b) -> Point:
 
 @ppmeth
 def cos_angle_between(a: Point, b: Point) -> np.ndarray:
-    if a == 0 or b == 0:
-        raise ValueError("cannot measure the angle to a zero length vector")
     return a.unit().dot(b.unit())
 
-
 @ppmeth
 def angle_between(a: Point, b: Point) -> np.ndarray:
     return np.arccos(a.cos_angle_between(b))
 
 @ppmeth
 def scalar_projection(a: Point, b: Point) -> Point:
-    if a==0 or b==0:
-        return 0
     return a.cos_angle_between(b) * abs(a)
 
 @ppmeth
 def vector_projection(a: Point, b: Point) -> Point:
-    if abs(a) == 0:
-        return Point.zeros()
     return b.scale(a.scalar_projection(b))
 
+@ppmeth
+def vector_rejection(a: Point, b: Point) -> Point:
+    return a - ((Point.dot(a, b)) / Point.dot(b,b)) * b
+
+
 @ppmeth
 def is_parallel(a: Point, b: Point, tolerance=1e-6):
-    if a.unit() == b.unit():
-        return True
     return abs(a.cos_angle_between(b) - 1) < tolerance
 
 @ppmeth
@@ -182,9 +177,7 @@ def angle_between(a: Point, b: Point) -> float:
     return np.arccos(cos_angle_between(a, b))
 
 def arbitrary_perpendicular(v: Point) -> Point:
-    if v.x == 0 and v.y == 0:
-        return Point(0, 1, 0)
-    return Point(-v.y, v.x, 0).unit
+    return Point(-v.y, v.x, 0).unit()
 
 def vector_norm(point: Point):
     return abs(point)