pfc-geometry 0.2.13__py3-none-any.whl → 0.2.15__py3-none-any.whl

geometry/base.py

@@ -10,7 +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 Self
+from __future__ import annotations
+from typing import Self, Literal
+from httpx import get
 import numpy as np
 import numpy.typing as npt
 import pandas as pd
@@ -92,6 +94,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 +257,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 +276,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 +364,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

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:

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)

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)

geometry/time.py

@@ -1,9 +1,12 @@
 from __future__ import annotations
 from geometry import Base
 from numbers import Number
-from typing import Self
+from typing import Self, Literal
 import numpy as np
+import numpy.typing as npt
+import pandas as pd
 from time import time
+from geometry.utils import get_index
 
 
 class Time(Base):
@@ -22,9 +25,13 @@ class Time(Base):
             return Time(t, dt)
 
     @staticmethod
-    def uniform(duration: float, npoints: int | None, minpoints:int=1) -> Time:
+    def uniform(duration: float, npoints: int | None, minpoints: int = 1) -> Time:
         return Time.from_t(
-            np.linspace(0, duration, npoints if npoints else max(int(np.ceil(duration * 25)), minpoints))
+            np.linspace(
+                0,
+                duration,
+                npoints if npoints else max(int(np.ceil(duration * 25)), minpoints),
+            )
         )
 
     def scale(self, duration) -> Self:
@@ -41,3 +48,39 @@ class Time(Base):
 
     def extend(self):
         return Time.concatenate([self, Time(self.t[-1] + self.dt[-1], self.dt[-1])])
+
+    def linterp(
+        self,
+        index: npt.NDArray | pd.Index,
+        extrapolate: Literal["throw", "nearest"] = "throw",
+    ):
+        """linear interpolation between two times"""
+        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 | pd.Index):
+            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")
+
+            new_time = Time.from_t(
+                np.interp(ts, index, self.t, self.t[0], self.t[-1])
+            )
+
+            last_p = self[stops][-1]
+            if last_p.t[0] == new_time.t[-1]:
+                last_dt = last_p.dt[-1]
+            else:
+                last_dt = last_p.t[0] - new_time.t[-1]
+            new_time.data[-1,-1] = last_dt    
+            return new_time
+        return dolinterp
+
+    def interpolate_t(self, t: float):
+        """get the floating point index at a given time"""
+        return get_index(self.t, t)
+
+    def __add__(self, t: float):
+        return Time.from_t(self.t + t)

geometry/transformation.py

@@ -12,8 +12,7 @@ this program. If not, see <http://www.gnu.org/licenses/>.
 from geometry import Base, Point, Quaternion, P0, Q0, Coord
 
 import numpy as np
-from typing import Union
-from typing import Self
+from typing import Self, Literal
 
 
 class Transformation(Base):
@@ -24,17 +23,23 @@ class Transformation(Base):
             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)
+                args = np.concatenate([args[0].data,Q0(len(args[0])).data],axis=1).T
             elif isinstance(args[0], Quaternion):
-                args = np.concatenate([P0().data,args[0].data],axis=1)
+                args = np.concatenate([P0(len(args[0])).data,args[0].data],axis=1).T
         elif len(args) == 2:
             _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)]
+            args = np.concatenate([_p.data, _q.data], axis=1).T
         super().__init__(*args, **kwargs)
-        self.p = Point(self.data[:,:3])
-        self.q = Quaternion(self.data[:,3:])
+
+    @property
+    def p(self):
+        return Point(self.data[:,:3])
+
+    @property
+    def q(self):    
+        return Quaternion(self.data[:,3:])
     
     def offset(self, p: Point):
         return Transformation(self.p + p, self.q)
@@ -91,7 +96,7 @@ class Transformation(Base):
         )
 
     
-    def apply(self, oin: Union[Point, Quaternion, Self, Coord]):
+    def apply(self, oin: Point | Quaternion | Self | Coord):
         if isinstance(oin, Point):
             return self.point(oin)
         elif isinstance(oin, Quaternion):
@@ -102,7 +107,7 @@ class Transformation(Base):
             return Transformation(self.apply(oin.p), self.apply(oin.q))
         
 
-    def rotate(self, oin: Union[Point, Quaternion]):
+    def rotate(self, oin: Point | Quaternion):
         if isinstance(oin, Point):
             return self.q.transform_point(oin)
         elif isinstance(oin, Quaternion):
@@ -128,3 +133,14 @@ class Transformation(Base):
         outarr[:, 3,:3] = self.translation.data
         return outarr
         
+
+    def plot_3d(self, size: float=3, vis:Literal["coord", "plane"]="coord"):
+        import plotly.graph_objects as go
+        from plotting.traces import axestrace, meshes
+        import plotting.templates
+        fig = go.Figure(layout=dict(template="generic3d+clean_paper"))
+        if vis=="coord":
+            fig.add_traces(axestrace(self, length=size))
+        elif vis=="plane":
+            fig.add_traces(meshes(len(self), self, scale=size))
+        return fig

geometry/utils.py

@@ -0,0 +1,99 @@
+from numbers import Number
+from typing import Callable, Literal
+import numpy.typing as npt
+import numpy as np
+import pandas as pd
+
+
+def handle_slice(fun: Callable[[npt.NDArray, Number], Number]):
+    def inner(
+        arr: npt.NDArray, value: slice | Number | npt.ArrayLike | None, *args, **kwargs
+    ) -> slice | Number | None:
+        if isinstance(value, slice):
+            start = (
+                fun(arr, value.start, *args, **kwargs)
+                if value.start is not None
+                else None
+            )
+            stop = (
+                fun(arr, value.stop, *args, **kwargs)
+                if value.stop is not None
+                else None
+            )
+            step = None  # TODO not sure how to handle this
+            return slice(start, stop, step)
+        elif pd.api.types.is_list_like(value):
+            return [fun(arr, val, *args, **kwargs) for val in value]
+        else:
+            return None if value is None else fun(arr, value, *args, **kwargs)
+
+    return inner
+
+
+@handle_slice
+def get_index(
+    arr: npt.NDArray,
+    value: Number,
+    missing: float | Literal["throw"] = "throw",
+    direction: Literal["forward", "backward"] = "forward",
+):
+    """given a value, find the index of the first location in the aray,
+    if no exact match, linearly interpolate in the index
+    assumes arr is monotonic increasing
+    raise value error outside of bounds and missing == "throw", else return missing"""
+    increasing = np.sign(np.diff(arr).mean())
+    res = np.argwhere(arr == value)
+    if len(res):
+        return res[0 if direction == "forward" else -1, 0]
+        # res[:,0]
+    if value > arr.max() or value < arr.min():
+        if missing == "throw":
+            raise ValueError(f"Time {value} is out of bounds")
+        else:
+            return missing
+
+    i0 = np.nonzero(arr <= value if increasing > 0 else arr >= value)[0][-1]
+    i1 = i0 + 1
+    t0 = arr[i0]
+    t1 = arr[i1]
+
+    return i0 + (value - t0) / (t1 - t0)
+
+
+@handle_slice
+def get_value(arr: npt.NDArray, index: Number):
+    """given an index, find the value in the array
+    linearly interpolate if no exact match,
+    assumes arr is monotonic increasing"""
+    if index > len(arr) - 1:
+        raise ValueError(f"Index {index} is out of bounds")
+    elif index < 0:
+        index = len(arr) + index
+    frac = index % 1
+    if frac == 0:
+        return arr[int(index)]
+
+    i0 = np.trunc(index)
+    i1 = i0 + 1
+
+    v0 = arr[int(i0)]
+    v1 = arr[int(i1)]
+    return v0 + (v1 - v0) * frac
+
+
+def apply_index_slice(index: npt.NDArray, value: slice | Number | npt.ArrayLike | None):
+    if isinstance(value, slice):
+        middle = pd.Index(index)[
+            int(np.ceil(value.start)) if value.start is not None else None : int(
+                np.ceil(value.stop)
+            )
+            if value.stop is not None
+            else None
+        ]
+        if value.start is not None and middle[0] != value.start and value.start > index[0]:
+            middle = np.concatenate([[get_value(index, value.start)], middle])
+        if value.stop is not None and middle[-1] != value.stop and value.stop < index[-1]:
+            middle = np.concatenate([middle, [get_value(index, value.stop)]])
+        return middle
+    else:
+        return index[value]

{pfc_geometry-0.2.13.dist-info → pfc_geometry-0.2.15.dist-info}/METADATA

@@ -1,10 +1,13 @@
-Metadata-Version: 2.3
+Metadata-Version: 2.4
 Name: pfc-geometry
-Version: 0.2.13
+Version: 0.2.15
 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.15.dist-info/RECORD

@@ -0,0 +1,16 @@
+geometry/__init__.py,sha256=HNhMyemIJzDq1nDjrr09eX5PS7q9ULscSbYsXss3JRM,1253
+geometry/base.py,sha256=lJfqFJz7thRoG1U7WnaykbLMIQjYQMJZBHtRN3nif78,14891
+geometry/checks.py,sha256=o8yMBAdU5Vy0EspBYaof4fPGgRSFZhRDhzBjRPsLd0M,375
+geometry/coordinate_frame.py,sha256=YTCAtCUuIq5LAsO-P9FwFs55f4qpWP9pUP6mf-Nhk54,3145
+geometry/gps.py,sha256=EsokABt40ZoltpAQfKrRc4kA-Lc2ScP_ltJNF7pvAWc,3654
+geometry/mass.py,sha256=BUWBSITwpdRfpJR5-oJTd16BI7FLZt8rhxdzr0cx1HY,1675
+geometry/point.py,sha256=FR2TBAV-YyKei0VA4ECDGA7ah5KRUhSKa2u_pVfAx-s,7057
+geometry/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+geometry/quaternion.py,sha256=J7Yk7qAUsH8SfjhcqQAF7Lw5hk_LGfpotVaVRyJnmeQ,12137
+geometry/time.py,sha256=VTfMHLxhcws8YESYvxxP8W_vSePvl4lwKRXHxFWhJeA,2695
+geometry/transformation.py,sha256=pEJXS7JYxu-f05ELTUUU4w7swcQPAgAxQNI6vPp5VCg,5242
+geometry/utils.py,sha256=N9AQsd1ZOYnOy5QdzpvaKIPRmzMlKsD6KTQersgEUFE,3278
+pfc_geometry-0.2.15.dist-info/METADATA,sha256=UXGOntYl4x1QSSGlCrfiLI-_mcbFGbB8ZKZqVXngWs4,1629
+pfc_geometry-0.2.15.dist-info/WHEEL,sha256=qtCwoSJWgHk21S1Kb4ihdzI2rlJ1ZKaIurTj_ngOhyQ,87
+pfc_geometry-0.2.15.dist-info/licenses/LICENSE,sha256=z72U6pv-bQgJ_Svr4uCXnMjemsp38aSerhHEdEAOMJ4,7632
+pfc_geometry-0.2.15.dist-info/RECORD,,

{pfc_geometry-0.2.13.dist-info → pfc_geometry-0.2.15.dist-info}/WHEEL

@@ -1,4 +1,4 @@
 Wheel-Version: 1.0
-Generator: hatchling 1.26.3
+Generator: hatchling 1.27.0
 Root-Is-Purelib: true
 Tag: py3-none-any

pfc_geometry-0.2.13.dist-info/RECORD

@@ -1,15 +0,0 @@
-geometry/__init__.py,sha256=HNhMyemIJzDq1nDjrr09eX5PS7q9ULscSbYsXss3JRM,1253
-geometry/base.py,sha256=yNiO7Fe-S5c4wtvgNmQ2jrX7Yt4tB3eBekYHK0_psYc,11926
-geometry/checks.py,sha256=o8yMBAdU5Vy0EspBYaof4fPGgRSFZhRDhzBjRPsLd0M,375
-geometry/coordinate_frame.py,sha256=YTCAtCUuIq5LAsO-P9FwFs55f4qpWP9pUP6mf-Nhk54,3145
-geometry/gps.py,sha256=Fs3hakSQ754HUqRsA7NWg_MSEdYxNqyiu4gu6EDrFqI,3381
-geometry/mass.py,sha256=BUWBSITwpdRfpJR5-oJTd16BI7FLZt8rhxdzr0cx1HY,1675
-geometry/point.py,sha256=XF7sdCaCD-si9UAumpyvxzMOTX3utVogJ2tt_7vt7b0,5185
-geometry/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-geometry/quaternion.py,sha256=nggmRu_8xCQNYlf8GcilC4zJZLsac3dwUQKu5pxL_38,9522
-geometry/time.py,sha256=ljQxAYxphHU4AE_gdUT7DDTXOF1QvYKpWyGpXw_zWQA,1239
-geometry/transformation.py,sha256=eunEC924zPBLhaVSEWE-IClrRfitvUKjSdOTaa2tdDQ,4705
-pfc_geometry-0.2.13.dist-info/METADATA,sha256=5H5SFGIU_UzRoAY3uUDF5EVvcDBUksjpv-Miw-TO-c0,1537
-pfc_geometry-0.2.13.dist-info/WHEEL,sha256=C2FUgwZgiLbznR-k0b_5k3Ai_1aASOXDss3lzCUsUug,87
-pfc_geometry-0.2.13.dist-info/licenses/LICENSE,sha256=z72U6pv-bQgJ_Svr4uCXnMjemsp38aSerhHEdEAOMJ4,7632
-pfc_geometry-0.2.13.dist-info/RECORD,,