pfc-geometry 0.2.14__tar.gz → 0.2.16__tar.gz

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/.gitignore

@@ -1,5 +1,5 @@
 .vscode/
-
+_trials/
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/PKG-INFO

@@ -1,10 +1,13 @@
-Metadata-Version: 2.3
+Metadata-Version: 2.4
 Name: pfc-geometry
-Version: 0.2.14
+Version: 0.2.16
 Summary: A library for working with 3D geometry.
+License-File: LICENSE
 Requires-Python: >=3.12
+Requires-Dist: numpy-quaternion>=2024.0.7
 Requires-Dist: numpy>=2.1.3
 Requires-Dist: pandas>=2.2.3
+Requires-Dist: rowan>=1.3.2
 Description-Content-Type: text/markdown
 
 # geometry #

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/pyproject.toml

@@ -1,10 +1,15 @@
 [project]
 name = "pfc-geometry"
-version="0.2.14"
+version="0.2.16"
 description = "A library for working with 3D geometry."
 readme = "README.md"
 requires-python = ">=3.12"
-dependencies = ["numpy>=2.1.3", "pandas>=2.2.3"]
+dependencies = [
+    "numpy-quaternion>=2024.0.7",
+    "numpy>=2.1.3",
+    "pandas>=2.2.3",
+    "rowan>=1.3.2",
+]
 
 [build-system]
 requires = ["hatchling"]

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/src/geometry/base.py

@@ -10,7 +10,8 @@ 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 Self
+from __future__ import annotations
+from typing import Self, Literal
 import numpy as np
 import numpy.typing as npt
 import pandas as pd
@@ -92,6 +93,14 @@ class Base:
         else:
             raise TypeError(f"Empty {self.__class__.__name__} not allowed")
 
+    def to_numpy(self, cols: str | list = None):
+        cols = self.cols if cols is None else cols
+        return np.column_stack([getattr(self, c) for c in cols])
+
+    @classmethod
+    def from_numpy(Cls, data: npt.NDArray, cols: str | list):
+        return Cls(np.column_stack([data[:, cols.index(col)] for col in Cls.cols]))
+
     @classmethod
     def _clean_data(cls, data) -> npt.NDArray[np.float64]:
         assert isinstance(data, np.ndarray)
@@ -247,14 +256,17 @@ class Base:
     def dot(self, other: Self) -> Self:
         return np.einsum("ij,ij->i", self.data, other)
 
-    def diff(self, dt: np.array) -> Self:
+    def diff(
+        self, dt: npt.NDArray, method: Literal["gradient", "diff"] = "gradient"
+    ) -> 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
-        )
+        self, dt = Base.length_check(self, dt)
+        diff_method = np.gradient if method == "gradient" else np.diff
+
+        data = diff_method(self.data, axis=0)
+        dt = dt if method == "gradient" else dt[:-1]
+        return self.__class__(data / np.tile(dt, (len(self.__class__.cols), 1)).T)
 
     def to_pandas(self, prefix="", suffix="", columns=None, index=None):
         if columns is not None:
@@ -263,6 +275,10 @@ class Base:
             cols = [prefix + col + suffix for col in self.__class__.cols]
         return pd.DataFrame(self.data, columns=cols, index=index)
 
+    @property
+    def df(self):
+        return self.to_pandas()
+
     def tile(self, count) -> Self:
         return self.__class__(np.tile(self.data, (count, 1)))
 
@@ -347,15 +363,81 @@ class Base:
     def fill_zeros(self):
         """fills zero length rows with the previous or next non-zero value"""
         return self.__class__(
-            pd.DataFrame(np.where(
-                np.tile(abs(self) == 0, (3, 1)).T,
-                np.full(self.data.shape, np.nan),
-                self.data,
-            )).ffill().bfill().to_numpy()
+            pd.DataFrame(
+                np.where(
+                    np.tile(abs(self) == 0, (3, 1)).T,
+                    np.full(self.data.shape, np.nan),
+                    self.data,
+                )
+            )
+            .ffill()
+            .bfill()
+            .to_numpy()
         )
 
     def ffill(self):
         return self.__class__(pd.DataFrame(self.data).ffill().to_numpy())
 
     def bfill(self):
-        return self.__class__(pd.DataFrame(self.data).bfill().to_numpy())
+        return self.__class__(pd.DataFrame(self.data).bfill().to_numpy())
+
+    def linterp(
+        self,
+        index: npt.NDArray | pd.Index,
+        extrapolate: Literal["throw", "nearest"] = "throw",
+    ):
+        "linear interpolation"
+        index = pd.Index(np.arange(len(self)) if index is None else index)
+        assert len(index) == len(self)
+        assert pd.Index(index).is_monotonic_increasing
+
+        def dolinterp(ts: npt.NDArray | Number):
+            starts = index.get_indexer(ts, method="ffill")
+            stops = index.get_indexer(ts, method="bfill")
+            if np.any(starts * stops < 0) and extrapolate=="throw":
+                raise Exception("Cannot extrapolate beyond parent range")
+            return self.__class__(np.column_stack(
+                [
+                    np.interp(
+                        ts, index, self.data[:, i], self.data[0, i], self.data[-1, i]
+                    )
+                    for i, col in enumerate(self.cols)
+                ]
+            ))
+            # return lambda t: a + (b - a) * np.clip(t, 0, 1)
+        return dolinterp
+    
+    def bspline(self, index: npt.NDArray | pd.Index = None):
+        from scipy.interpolate import make_interp_spline
+
+        bspline = make_interp_spline(
+            np.arange(len(self)) if index is None else index, self.data, axis=0
+        )
+        return lambda i: self.__class__(bspline(i))
+
+    def interpolate(self, index: npt.NDArray | pd.Index = None, method:str=None):
+        if method is None:
+            match (self.__class__.__name__):
+                case "Point":
+                    method="bspline"
+                case "Quaternion":
+                    method="slerp"
+                case "Time":
+                    method="linterp"
+        return getattr(self, method)(index)
+
+    def plot(self, index=None, **kwargs):
+        import plotly.graph_objects as go
+
+        fig = go.Figure()
+        for col in self.cols:
+            fig.add_trace(
+                go.Scatter(
+                    x=np.arange(len(self)) if index is None else index,
+                    y=getattr(self, col),
+                    name=col,
+                    **kwargs,
+                )
+            )
+        # df = self.to_pandas(self.__class__.__name__[0], index=index)
+        return fig

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/src/geometry/gps.py

@@ -13,6 +13,7 @@ import math
 from geometry.base import Base
 from geometry.point import Point
 from typing import List, Union
+import numpy.typing as npt
 import numpy as np
 import pandas as pd
 
@@ -71,6 +72,15 @@ class GPS(Base):
             self.alt - pin.z
         )
 
+    def bspline(self, index: npt.NDArray | pd.Index = None):
+        
+        def interpolator(i):
+            ps: Point = self - self[0]
+            ips = ps.bspline(index)(i)
+            return self[0].offset(ips)
+
+        return interpolator
+
 
 '''
 Extract from ardupilot:

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/src/geometry/point.py

@@ -9,7 +9,9 @@ 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 typing import Literal
 from .base import Base
 import numpy as np
 import pandas as pd
@@ -19,22 +21,38 @@ import numpy.typing as npt
 
 
 class Point(Base):
-    cols=["x", "y", "z"]
+    cols = ["x", "y", "z"]
     from_np = [
-        "sin","cos","tan",
-        "arcsin","arccos","arctan",
+        "sin",
+        "cos",
+        "tan",
+        "arcsin",
+        "arccos",
+        "arctan",
     ]
 
+    @property
+    def xy(self):
+        return Point(self.x, self.y, np.zeros(len(self)))
+
+    @property
+    def yz(self):
+        return Point(np.zeros(len(self)), self.y, self.z)
+
+    @property
+    def zx(self):
+        return Point(self.x, np.zeros(len(self)), self.z)
+
     def scale(self, value) -> Point:
         with np.errstate(divide="ignore"):
-            res = value/abs(self)
-        res[res==np.inf] = 0
+            res = value / abs(self)
+        res[res == np.inf] = 0
         return self * res
-        
+
     def unit(self) -> Point:
         return self.scale(1)
 
-    def remove_outliers(self, nstds = 2):
+    def remove_outliers(self, nstds=2):
         ab = abs(self)
         std = np.nanstd(ab)
         mean = np.nanmean(ab)
@@ -50,69 +68,123 @@ class Point(Base):
 
     def max(self):
         return Point(np.max(self.data, axis=0))
-    
+
     def min(self):
         return Point(np.min(self.data, axis=0))
 
     def angles(self, p2):
         return (self.cross(p2) / (abs(self) * abs(p2))).arcsin
-    
+
     def planar_angles(self):
-        return Point(np.arctan2(self.y, self.z), np.arctan2(self.z, self.x), np.arctan2(self.x, self.y))
+        return Point(
+            np.arctan2(self.y, self.z),
+            np.arctan2(self.z, self.x),
+            np.arctan2(self.x, self.y),
+        )
 
     def angle(self, p2):
         return abs(Point.angles(self, p2))
-    
+
     @staticmethod
-    def X(value: Number | npt.NDArray=1, count=1):
-        return np.tile(value, count) * Point(1,0,0)
+    def X(value: Number | npt.NDArray = 1, count=1):
+        return np.tile(value, count) * Point(1, 0, 0)
 
     @staticmethod
     def Y(value=1, count=1):
-        return np.tile(value, count) * Point(0,1,0)
+        return np.tile(value, count) * Point(0, 1, 0)
 
     @staticmethod
     def Z(value=1, count=1):
-        return np.tile(value, count) * Point(0,0,1)
+        return np.tile(value, count) * Point(0, 0, 1)
 
     def rotate(self, rmat=np.ndarray):
         if len(rmat.shape) == 3:
             pass
         elif len(rmat.shape) == 2:
-            rmat = np.reshape(rmat, (1, 3, 3 ))
+            rmat = np.reshape(rmat, (1, 3, 3))
         else:
             raise TypeError("expected a 3x3 matrix")
-        
+
         return self.dot(rmat)
 
     def to_rotation_matrix(self):
-        '''returns the rotation matrix based on a point representing Euler angles'''
+        """returns the rotation matrix based on a point representing Euler angles"""
         s = self.sin
         c = self.cos
-        return np.array([
-            [
-                c.z * c.y, 
-                c.z * s.y * s.x - c.x * s.z, 
-                c.x * c.z * s.y + s.x * s.z
-            ], [
-                c.y * s.z, 
-                c.x * c.z + s.x * s.y * s.z, 
-                -1 * c.z * s.x + c.x * s.y * s.z
-            ],
-            [
-                -1 * s.y, 
-                c.y * s.x, 
-                c.x * c.y
-            ]
-        ]).T
+        return np.transpose(
+            np.array(
+                [
+                    [
+                        c.z * c.y,
+                        c.z * s.y * s.x - c.x * s.z,
+                        c.x * c.z * s.y + s.x * s.z,
+                    ],
+                    [
+                        c.y * s.z,
+                        c.x * c.z + s.x * s.y * s.z,
+                        -1 * c.z * s.x + c.x * s.y * s.z,
+                    ],
+                    [-1 * s.y, c.y * s.x, c.x * c.y],
+                ]
+            ),
+            (2, 0, 1),
+        )
+
+    def matrix(self):
+        return np.einsum("i...,...->i...", self.data, np.identity(3))
+
+    @staticmethod
+    def from_matrix(matrix):
+        return Point(matrix[:, 0, 0], matrix[:, 1, 1], matrix[:, 2, 2])
+
+    def skew_symmetric(self):
+        o = np.zeros(len(self))
+        return np.transpose(
+            np.array(
+                [[o, -self.z, self.y], [self.z, o, -self.x], [-self.y, self.x, o]]
+            ),
+            (2, 0, 1),
+        )
 
     @staticmethod
     def zeros(count=1):
-        return Point(np.zeros((count,3)))
+        return Point(np.zeros((count, 3)))
 
     def bearing(self):
         return np.arctan2(self.y, self.x)
 
+    def plot3d(self, **kwargs):
+        import plotly.graph_objects as go
+        fig = go.Figure()
+
+        fig.add_trace(go.Scatter3d(x=self.x, y=self.y, z=self.z, **kwargs))
+        fig.update_layout(
+            scene=dict(aspectmode="data"),
+        )
+        return fig
+
+    def plotxy(self):
+        import plotly.express as px
+
+        return px.line(self.df, x="x", y="y").update_layout(
+            yaxis=dict(scaleanchor="x", scaleratio=1)
+        )
+
+    def plotyz(self):
+        import plotly.express as px
+
+        return px.line(self.df, x="y", y="z").update_layout(
+            yaxis=dict(scaleanchor="x", scaleratio=1, title="z"), xaxis=dict(title="y")
+        )
+
+    def plotzx(self):
+        import plotly.express as px
+
+        return px.line(self.df, x="z", y="x").update_layout(
+            yaxis=dict(scaleanchor="x", scaleratio=1, title="x"), xaxis=dict(title="z")
+        )
+
+
 def Points(*args, **kwargs):
     warn("Points is deprecated, you can now just use Point", DeprecationWarning)
     return Point(*args, **kwargs)
@@ -121,15 +193,19 @@ def Points(*args, **kwargs):
 def PX(length=1, count=1):
     return Point.X(length, count)
 
+
 def PY(length=1, count=1):
     return Point.Y(length, count)
 
+
 def PZ(length=1, count=1):
     return Point.Z(length, count)
 
+
 def P0(count=1):
     return Point.zeros(count)
 
+
 def ppmeth(func):
     def wrapper(a, b, *args, **kwargs):
         assert all([isinstance(arg, Point) for arg in args])
@@ -143,47 +219,56 @@ def ppmeth(func):
 @ppmeth
 def cross(a: Point, b: Point) -> Point:
     return Point(np.cross(a.data, b.data))
- 
+
 
 @ppmeth
 def cos_angle_between(a: Point, b: Point) -> np.ndarray:
     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:
     return a.cos_angle_between(b) * abs(a)
 
+
 @ppmeth
 def vector_projection(a: Point, b: Point) -> Point:
     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
+    return a - ((Point.dot(a, b)) / Point.dot(b, b)) * b
 
 
 @ppmeth
 def is_parallel(a: Point, b: Point, tolerance=1e-6):
     return abs(a.cos_angle_between(b) - 1) < tolerance
 
+
 @ppmeth
 def is_perpendicular(a: Point, b: Point, tolerance=1e-6):
     return abs(a.dot(b)) < tolerance
 
+
 @ppmeth
 def min_angle_between(p1: Point, p2: Point):
     angle = angle_between(p1, p2) % np.pi
     return np.minimum(angle, np.pi - angle)
 
+
 def arbitrary_perpendicular(v: Point) -> Point:
     return Point(-v.y, v.x, 0).unit()
 
+
 def vector_norm(point: Point):
     return abs(point)
 
+
 def normalize_vector(point: Point):
-    return point / abs(point)
+    return point / abs(point)

{pfc_geometry-0.2.14 → pfc_geometry-0.2.16}/src/geometry/quaternion.py

@@ -11,13 +11,14 @@ this program. If not, see <http://www.gnu.org/licenses/>.
 """
 from __future__ import annotations
 from .point import Point
-from .base import Base, dprep
+from .base import Base
 from geometry import PZ
 import numpy as np
 import numpy.typing as npt
 import pandas as pd
 from warnings import warn
 from numbers import Number
+from typing import Callable, Literal
 
 
 class Quaternion(Base):
@@ -179,10 +180,10 @@ class Quaternion(Base):
     def body_rotate(self, rate: Point) -> Quaternion:
         return (self * Quaternion.from_axis_angle(rate)).norm()
 
-    def diff(self, dt: Number | npt.NDArray) -> Point:
+    def diff(self, dt: Number | npt.NDArray = None) -> Point:
         """differentiate in the world frame"""
         if not pd.api.types.is_list_like(dt):
-            dt = np.full(len(self), dt)
+            dt = np.full(len(self), 1 if not dt else dt)
         assert len(dt) == len(self)
         dt = dt * len(dt) / (len(dt) - 1)
 
@@ -192,10 +193,10 @@ class Quaternion(Base):
         ) / dt[:-1]
         return Point(np.vstack([ps.data, ps.data[-1,:]]))
 
-    def body_diff(self, dt: Number | npt.NDArray) -> Point:
+    def body_diff(self, dt: Number | npt.NDArray = None) -> Point:
         """differentiate in the body frame"""
         if not pd.api.types.is_list_like(dt):
-            dt = np.full(len(self), dt)
+            dt = np.full(len(self), 1 if not dt else dt)
         assert len(dt) == len(self)
         dt = dt * len(dt) / (len(dt) - 1)
 
@@ -258,7 +259,68 @@ class Quaternion(Base):
             p = Point.X()
         return self.transform_point(p).bearing()
     
-        
+    def slerp(self, index: pd.Index | npt.NDArray = None, extrapolate:Literal["throw", "nearest"]="throw"):
+        index = pd.Index(np.arange(len(self)) if index is None else index)
+
+        assert len(index) == len(self)
+        assert pd.Index(index).is_monotonic_increasing
+        from rowan.interpolate import slerp
+        def doslerp(ts: npt.NDArray | Number) -> Quaternion:
+            starts = index.get_indexer(ts, method='ffill')
+            stops = index.get_indexer(ts, method='bfill')
+            
+            #case interpolate match (start == stop - 1)
+            odata = slerp(
+                self[starts].to_numpy("xyzw"),
+                self[stops].to_numpy("xyzw"),
+                (ts - index[starts]) / (index[stops] - index[starts]),
+                True 
+            )
+
+            #case exact match (start == stop)
+            exacts = starts == stops
+            odata[exacts] = self.to_numpy("xyzw")[starts[exacts]]
+
+            #case outside range above (start == index[-1], stop== -1)
+            aboves = stops==-1
+            if np.any(aboves):
+                if extrapolate=="throw":
+                    raise Exception("Cannot slerp beyond range")
+                else:
+                    odata[aboves] = self.to_numpy("xyzw")[-1, :]
+            #case outside range below (start == -1, stop==index[0])
+            belows = starts==-1
+            if np.any(belows):
+                if extrapolate=="throw":
+                    raise Exception("Cannot slerp beyond range")
+                else:
+                    odata[belows] = self.to_numpy("xyzw")[0, :]
+
+            return Quaternion.from_numpy( odata, "xyzw")
+            
+        return doslerp
+
+    
+#    @staticmethod
+#    def slerp(a: Quaternion, b: Quaternion):
+#        """spherical linear interpolation"""
+#        from rowan.interpolate import slerp
+#        def doslerp(t):
+#            xyzw = slerp(a.xyzw, b.xyzw, np.clip(t, 0, 1))
+#            return Quaternion(xyzw[:,3], xyzw[:,0], xyzw[:,1], xyzw[:,2])
+#        return doslerp
+
+    @staticmethod
+    def squad(p: Quaternion, a: Quaternion, b: Quaternion, q: Quaternion):
+        from rowan.interpolate import squad
+        def dosquad(t):
+            xyzq = squad(p.xyzw, a.xyzw, b.xyzw, q.xyzw, np.clip(t, 0, 1))
+            return Quaternion(xyzq[:,3], xyzq[:,0], xyzq[:,1], xyzq[:,2])
+        return dosquad
+
+    def plot_3d(self, size: float=3, vis:Literal["coord", "plane"]="coord"):
+        from geometry import Transformation
+        return Transformation(self).plot_3d(size, vis)
 
 def Q0(count=1):
     return Quaternion.zero(count)