shoeboxpy 0.0.1__py3-none-any.whl

shoeboxpy/__init__.py

@@ -0,0 +1,3 @@
+from __future__ import annotations
+
+__version__ = "0.0.1"

shoeboxpy/__version__.py

@@ -0,0 +1,34 @@
+# file generated by setuptools-scm
+# don't change, don't track in version control
+
+__all__ = [
+    "__version__",
+    "__version_tuple__",
+    "version",
+    "version_tuple",
+    "__commit_id__",
+    "commit_id",
+]
+
+TYPE_CHECKING = False
+if TYPE_CHECKING:
+    from typing import Tuple
+    from typing import Union
+
+    VERSION_TUPLE = Tuple[Union[int, str], ...]
+    COMMIT_ID = Union[str, None]
+else:
+    VERSION_TUPLE = object
+    COMMIT_ID = object
+
+version: str
+__version__: str
+__version_tuple__: VERSION_TUPLE
+version_tuple: VERSION_TUPLE
+commit_id: COMMIT_ID
+__commit_id__: COMMIT_ID
+
+__version__ = version = '0.0.1'
+__version_tuple__ = version_tuple = (0, 0, 1)
+
+__commit_id__ = commit_id = None

shoeboxpy/animate.py

@@ -0,0 +1,233 @@
+"""Animation utilities.
+
+This module contains a (now optimized) routine for animating a vessel's
+6-DoF (or 3-DoF position only) history. The original implementation recreated
+matplotlib artists and performed Python list -> ndarray conversions every
+frame, which caused quadratic time growth and excessive CPU load for small
+``dt`` (high frame counts / high FPS).
+
+Main performance improvements:
+* Pre-allocate trace arrays instead of appending + copying each frame.
+* Keep a single ``Poly3DCollection`` instance and update its vertices in-place
+  (``set_verts``) instead of removing/adding each frame.
+* Optional frame decimation via ``max_fps`` so very small ``dt`` values do not
+  request more frames than a typical display can show.
+* Optional static axes (``follow=False``) to avoid per-frame axis limit updates.
+* Avoid ``scipy`` ``Rotation`` object construction per frame by using a tiny
+  local Euler->matrix implementation (keeps dependency optional at runtime).
+* Optional ``show`` flag so tests / scripts can build an animation without
+  blocking on GUI.
+
+Note: 3D blitting is still unreliable across backends, so ``blit=False``.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import animation
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+from typing import Tuple
+
+__all__ = ["animate_history"]
+
+
+def _euler_xyz_matrix(phi: float, theta: float, psi: float) -> np.ndarray:
+    """Return rotation matrix for XYZ (roll, pitch, yaw) intrinsic sequence.
+
+    Equivalent to Rz(psi) * Ry(theta) * Rx(phi) when using right-handed coords.
+    """
+    cphi, sphi = np.cos(phi), np.sin(phi)
+    ctheta, stheta = np.cos(theta), np.sin(theta)
+    cpsi, spsi = np.cos(psi), np.sin(psi)
+
+    # Rx
+    R_x = np.array([[1, 0, 0], [0, cphi, -sphi], [0, sphi, cphi]])
+    # Ry
+    R_y = np.array([[ctheta, 0, stheta], [0, 1, 0], [-stheta, 0, ctheta]])
+    # Rz
+    R_z = np.array([[cpsi, -spsi, 0], [spsi, cpsi, 0], [0, 0, 1]])
+    return R_z @ R_y @ R_x
+
+
+def animate_history(
+    pos_history: np.ndarray,
+    dt: float,
+    L: float = 1.0,
+    B: float = 0.5,
+    T: float = 0.3,
+    *,
+    follow: bool = True,
+    max_fps: float = 60.0,
+    show: bool = True,
+    static_margin_factor: float = 2.0,
+) -> animation.FuncAnimation:
+    """Animate a 3D vessel trajectory (position + optional Euler angles).
+
+    Parameters
+    ----------
+    pos_history : (N,3) or (N,6) ndarray
+        Columns 0:3 are x,y,z. If 6 columns, 3:6 are Euler angles (phi,theta,psi).
+    dt : float
+        Simulation time step between successive ``pos_history`` rows (seconds).
+    L, B, T : float
+        Vessel dimensions (length, beam/width, height/thickness).
+    follow : bool, default True
+        If True, axes move to keep vessel centered. If False, a static bounding
+        box that encloses the whole trajectory is used (faster for large N).
+    max_fps : float, default 60
+        Cap the displayed frames per second. If 1/dt > max_fps frames are
+        requested, frames are decimated (skipping) to not exceed this.
+    show : bool, default True
+        Call ``plt.show()`` at the end. Set False for scripts/tests.
+    static_margin_factor : float, default 2.0
+        When ``follow=False`` the static axis cube extends this multiple of the
+        vessel length beyond the min/max trajectory extents.
+
+    Returns
+    -------
+    matplotlib.animation.FuncAnimation
+        The created animation object (can be saved with ``anim.save``).
+    """
+
+    if pos_history.ndim != 2 or pos_history.shape[1] not in (3, 6):
+        raise ValueError("pos_history must have shape (N,3) or (N,6)")
+    if dt <= 0:
+        raise ValueError("dt must be positive")
+
+    n_total = pos_history.shape[0]
+
+    # Frame decimation factor if dt smaller than display refresh interval target.
+    desired_interval_ms = max(1000.0 / max_fps, 1.0)  # at least 1 ms
+    frame_interval_ms = dt * 1000.0
+    skip = int(np.ceil(desired_interval_ms / frame_interval_ms)) if frame_interval_ms < desired_interval_ms else 1
+    indices = np.arange(0, n_total, skip, dtype=int)
+    n_frames = len(indices)
+
+    # Split positions & angles (angles may be zeros if not provided).
+    pos = pos_history[:, :3]
+    if pos_history.shape[1] == 6:
+        angles = pos_history[:, 3:6]
+    else:
+        angles = np.zeros_like(pos)
+
+    # Precompute static axis limits if follow disabled.
+    margin_geom = L * static_margin_factor
+    if not follow:
+        mins = pos.min(axis=0) - margin_geom
+        maxs = pos.max(axis=0) + margin_geom
+
+    # Base vertices of vessel centered at origin.
+    base_vertices = np.array(
+        [
+            [-L / 2, -B / 2, -T / 2],
+            [L / 2, -B / 2, -T / 2],
+            [L / 2, B / 2, -T / 2],
+            [-L / 2, B / 2, -T / 2],
+            [-L / 2, -B / 2, T / 2],
+            [L / 2, -B / 2, T / 2],
+            [L / 2, B / 2, T / 2],
+            [-L / 2, B / 2, T / 2],
+        ],
+        dtype=float,
+    )
+    # Face index lists into the (8,3) vertex array.
+    face_indices = [
+        [0, 1, 2, 3],  # bottom
+        [4, 5, 6, 7],  # top
+        [0, 1, 5, 4],  # side
+        [2, 3, 7, 6],  # side
+        [1, 2, 6, 5],  # side
+        [4, 7, 3, 0],  # side
+    ]
+
+    # Pre-allocate trace arrays (avoid list append + copy each frame).
+    trace_x = np.empty(n_frames)
+    trace_y = np.empty(n_frames)
+    trace_z = np.empty(n_frames)
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+    ax.set_xlabel("X")
+    ax.set_ylabel("Y")
+    ax.set_zlabel("Z")
+    ax.set_title("Vessel animation")
+
+    if follow:
+        # Start with a reasonable cube around first position.
+        p0 = pos[0]
+        ax.set_xlim(p0[0] - margin_geom, p0[0] + margin_geom)
+        ax.set_ylim(p0[1] - margin_geom, p0[1] + margin_geom)
+        ax.set_zlim(p0[2] - margin_geom, p0[2] + margin_geom)
+    else:
+        ax.set_xlim(mins[0], maxs[0])
+        ax.set_ylim(mins[1], maxs[1])
+        ax.set_zlim(mins[2], maxs[2])
+
+    (trace_line,) = ax.plot([], [], [], "b-", lw=1.5)
+
+    # Create initial vessel poly (will update verts in-place each frame).
+    initial_faces = [[base_vertices[j] for j in face] for face in face_indices]
+    vessel_poly = Poly3DCollection(
+        initial_faces, facecolors="cyan", edgecolors="black", alpha=0.5
+    )
+    ax.add_collection3d(vessel_poly)
+
+    def init():  # noqa: D401
+        trace_line.set_data([], [])
+        trace_line.set_3d_properties([])
+        return trace_line, vessel_poly
+
+    def _frame(k: int) -> Tuple[object, object]:  # returns artists for FuncAnimation
+        i = indices[k]
+        p = pos[i]
+        a = angles[i]
+
+        # Update trace slices.
+        trace_x[k] = p[0]
+        trace_y[k] = p[1]
+        trace_z[k] = p[2]
+        trace_line.set_data(trace_x[: k + 1], trace_y[: k + 1])
+        trace_line.set_3d_properties(trace_z[: k + 1])
+
+        # Axis follow (only if enabled).
+        if follow:
+            ax.set_xlim(p[0] - margin_geom, p[0] + margin_geom)
+            ax.set_ylim(p[1] - margin_geom, p[1] + margin_geom)
+            ax.set_zlim(p[2] - margin_geom, p[2] + margin_geom)
+
+        # Rotation matrix.
+        Rm = _euler_xyz_matrix(a[0], a[1], a[2])
+        rotated = base_vertices @ Rm.T + p  # (8,3)
+        # Build face list for set_verts.
+        faces = [[rotated[j] for j in face] for face in face_indices]
+        vessel_poly.set_verts(faces)
+
+        return trace_line, vessel_poly
+
+    anim = animation.FuncAnimation(
+        fig,
+        _frame,
+        init_func=init,
+        frames=n_frames,
+        interval=max(frame_interval_ms * skip, desired_interval_ms),
+        blit=False,
+        repeat=False,
+    )
+
+    if show:
+        plt.show()
+
+    return anim
+
+
+if __name__ == "__main__":  # pragma: no cover - manual use example
+    t = np.linspace(0, 20, 2000)
+    x = 5 * np.cos(0.3 * t)
+    y = 5 * np.sin(0.3 * t)
+    z = 0.5 * np.sin(0.6 * t)
+    phi = 0.2 * np.sin(0.5 * t)
+    theta = 0.2 * np.cos(0.4 * t)
+    psi = 0.5 * t
+    hist = np.column_stack([x, y, z, phi, theta, psi])
+    animate_history(hist, dt=t[1] - t[0], L=2.0, B=1.0, T=0.5)

shoeboxpy/model3dof.py

@@ -0,0 +1,247 @@
+import numpy as np
+import numpy.typing as npt
+import typing as tp
+
+
+class Shoebox:
+    r"""
+    3-DOF planar ship model (surge, sway, yaw).
+
+    States:
+
+    .. math::
+        \eta &= [x, y, \psi] \\
+        \nu  &= [u, v, r]
+
+    Dynamics:
+
+    .. math::
+        & \dot{\eta} = J(\psi)\nu \\
+        & (M_{RB}+M_A)\dot{\nu} + \Big(C_{RB}(\nu)+C_A(\nu)\Big)\nu + D\nu = \tau
+
+    where :math:`\tau` is the control/input force/moment vector in surge, sway, and yaw.
+
+    :param L: Length of the vessel.
+    :param B: Breadth of the vessel.
+    :param T: Draft of the vessel.
+    :param rho: Density of water (default is 1000.0).
+    :param alpha_u: Added mass coefficient in surge (default is 0.1).
+    :param alpha_v: Added mass coefficient in sway (default is 0.2).
+    :param alpha_r: Added mass coefficient in yaw (default is 0.1).
+    :param beta_u: Damping factor in surge (default is 0.05).
+    :param beta_v: Damping factor in sway (default is 0.05).
+    :param beta_r: Damping factor in yaw (default is 0.05).
+    :param eta0: Initial state vector for position and orientation, i.e. [x, y, \psi]. Default is a zero array of length 3.
+    :param nu0: Initial state vector for velocities, i.e. [u, v, r]. Default is a zero array of length 3.
+    """
+
+    def __init__(
+        self,
+        L: float,
+        B: float,
+        T: float,
+        rho: float = 1000.0,
+        # Added mass coefficients for 3DOF
+        alpha_u: float = 0.1,
+        alpha_v: float = 0.2,
+        alpha_r: float = 0.1,
+        # Damping factors for 3DOF
+        beta_u: float = 0.05,
+        beta_v: float = 0.05,
+        beta_r: float = 0.05,
+        # Initial states: [x, y, psi] and [u, v, r]
+        eta0: npt.NDArray[np.float64] = np.zeros(3),
+        nu0: npt.NDArray[np.float64] = np.zeros(3),
+    ):
+        # Rigid-body mass from volume
+        self.m = rho * L * B * T
+
+        # Yaw moment of inertia for a rectangular (shoebox) vessel
+        Izz = (1.0 / 12.0) * self.m * (L**2 + B**2)
+
+        # Rigid-body mass matrix (3x3)
+        self.MRB = np.diag([self.m, self.m, Izz])
+
+        # Added mass (diagonal)
+        self.MA = np.diag(
+            [alpha_u * self.m, alpha_v * self.m, alpha_r * Izz]  # surge  # sway  # yaw
+        )
+
+        self.M_eff = self.MRB + self.MA
+
+        # Damping matrix (diagonal)
+        self.D = np.diag([beta_u * self.m, beta_v * self.m, beta_r * Izz])
+
+        self.invM_eff = np.linalg.inv(self.M_eff)
+
+        # Store states
+        self.eta = eta0.astype(float)  # [x, y, psi]
+        self.nu = nu0.astype(float)  # [u, v, r]
+        # Initialize reusable buffers
+        self._init_buffers()
+
+    def _init_buffers(self) -> None:
+        """Allocate reusable numpy arrays to reduce per-step allocations."""
+        self._k_eta = np.zeros((4, 3), dtype=float)
+        self._k_nu = np.zeros((4, 3), dtype=float)
+        self._eta_tmp = np.zeros(3, dtype=float)
+        self._nu_tmp = np.zeros(3, dtype=float)
+        self._eta_dot_buf = np.zeros(3, dtype=float)
+        self._nu_dot_buf = np.zeros(3, dtype=float)
+        self._zeros3 = np.zeros(3, dtype=float)
+
+    def J(self, eta: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]:
+        """
+        Compute the 3x3 transformation matrix from body velocities to inertial frame.
+        """
+        psi = eta[2]
+        c = np.cos(psi)
+        s = np.sin(psi)
+        return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
+
+    def C_RB(self, nu: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]:
+        """
+        Rigid-body Coriolis/centripetal matrix for 3DOF.
+        """
+        u, v, r = nu
+        m = self.m
+        Izz = self.MRB[2, 2]
+        # Standard form for surge-sway-yaw
+        return np.array([[0, 0, -m * v], [0, 0, m * u], [m * v, -m * u, 0]])
+
+    def C_A(self, nu: npt.NDArray[np.float64]) -> npt.NDArray[np.float64]:
+        """
+        Added mass Coriolis/centripetal matrix for 3DOF.
+        """
+        u, v, r = nu
+        Xu_dot = self.MA[0, 0]
+        Yv_dot = self.MA[1, 1]
+        Nr_dot = self.MA[2, 2]
+        return np.array(
+            [[0, 0, -Yv_dot * v], [0, 0, Xu_dot * u], [Yv_dot * v, -Xu_dot * u, 0]]
+        )
+
+    def dynamics(
+        self,
+        eta: npt.NDArray[np.float64],
+        nu: npt.NDArray[np.float64],
+        tau: npt.NDArray[np.float64],
+        tau_ext: npt.NDArray[np.float64] = None,
+    ) -> tp.Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64]]:
+        r"""
+        Returns the time derivatives :math:`(\dot{\eta}, \dot{\nu})` for the 3DOF model:
+
+        .. math:
+            \dot{eta} = J(\eta) \nu \\
+            (M_{RB} + M_A) \dot{\nu} + (C_{RB}(\nu) + C_A(\nu)) \nu + D \nu = \tau + \tau_{ext}.
+
+        External forces :math:`\tau_{ext}` can be provided (default is zero).
+        """
+        # Public method keeps original semantics (returns new arrays) while
+        # delegating core math to in-place implementation to reduce internal allocations.
+        eta_dot = np.empty(3, dtype=float)
+        nu_dot = np.empty(3, dtype=float)
+        self._dynamics_into(eta, nu, tau, tau_ext, eta_dot, nu_dot)
+        return eta_dot, nu_dot
+
+    # ------------------------------------------------------------------
+    # Internal optimized dynamics: writes results into provided buffers.
+    # Avoids constructing intermediate small arrays (J, C matrices, rhs).
+    # ------------------------------------------------------------------
+    def _dynamics_into(
+        self,
+        eta: npt.NDArray[np.float64],
+        nu: npt.NDArray[np.float64],
+        tau: npt.NDArray[np.float64],
+        tau_ext: npt.NDArray[np.float64],
+        eta_dot_out: npt.NDArray[np.float64],
+        nu_dot_out: npt.NDArray[np.float64],
+    ) -> None:
+        if tau_ext is None:
+            tau_ext = self._zeros3
+
+        # Unpack states
+        psi = eta[2]
+        u, v, r = nu
+
+        c = np.cos(psi)
+        s = np.sin(psi)
+
+        # eta_dot = J(psi) * nu (manual expand to avoid matrix mult allocation)
+        eta_dot_out[0] = c * u - s * v
+        eta_dot_out[1] = s * u + c * v
+        eta_dot_out[2] = r
+
+        # Coriolis * nu contributions (pre- multiplied form) for rigid-body & added mass
+        m = self.m
+        Xu_dot = self.MA[0, 0]
+        Yv_dot = self.MA[1, 1]
+        # C_total @ nu only has first two components non-zero
+        cnu0 = -(m + Yv_dot) * v * r
+        cnu1 = (m + Xu_dot) * u * r
+
+        # Damping (diagonal) * nu
+        Dnu0 = self.D[0, 0] * u
+        Dnu1 = self.D[1, 1] * v
+        Dnu2 = self.D[2, 2] * r
+
+        # rhs = tau + tau_ext - Dnu - Cnu  (no restoring forces in this planar model)
+        rhs0 = tau[0] + tau_ext[0] - Dnu0 - cnu0
+        rhs1 = tau[1] + tau_ext[1] - Dnu1 - cnu1
+        rhs2 = tau[2] + tau_ext[2] - Dnu2  # cnu2 == 0
+
+        # nu_dot = invM_eff @ rhs (manual 3x3 matvec multiply)
+        M = self.invM_eff
+        nu_dot_out[0] = M[0, 0] * rhs0 + M[0, 1] * rhs1 + M[0, 2] * rhs2
+        nu_dot_out[1] = M[1, 0] * rhs0 + M[1, 1] * rhs1 + M[1, 2] * rhs2
+        nu_dot_out[2] = M[2, 0] * rhs0 + M[2, 1] * rhs1 + M[2, 2] * rhs2
+
+    def step(
+        self,
+        tau: npt.NDArray[np.float64] = None,
+        tau_ext: npt.NDArray[np.float64] = None,
+        dt: float = 0.01,
+    ):
+        r"""
+        Advance the state :math:`(\eta, \nu)` one time step dt using 4th-order Runge-Kutta.
+        """
+        if tau is None:
+            tau = self._zeros3.copy()  # ensure user can mutate after call
+        if tau_ext is None:
+            tau_ext = self._zeros3  # safe shared read-only
+
+        eta0 = self.eta  # references (updated at end)
+        nu0 = self.nu
+
+        k_eta = self._k_eta
+        k_nu = self._k_nu
+        eta_tmp = self._eta_tmp
+        nu_tmp = self._nu_tmp
+
+        # -- k1 --
+        self._dynamics_into(eta0, nu0, tau, tau_ext, k_eta[0], k_nu[0])
+
+        # -- k2 --
+        eta_tmp[:] = eta0 + 0.5 * dt * k_eta[0]
+        nu_tmp[:] = nu0 + 0.5 * dt * k_nu[0]
+        self._dynamics_into(eta_tmp, nu_tmp, tau, tau_ext, k_eta[1], k_nu[1])
+
+        # -- k3 --
+        eta_tmp[:] = eta0 + 0.5 * dt * k_eta[1]
+        nu_tmp[:] = nu0 + 0.5 * dt * k_nu[1]
+        self._dynamics_into(eta_tmp, nu_tmp, tau, tau_ext, k_eta[2], k_nu[2])
+
+        # -- k4 --
+        eta_tmp[:] = eta0 + dt * k_eta[2]
+        nu_tmp[:] = nu0 + dt * k_nu[2]
+        self._dynamics_into(eta_tmp, nu_tmp, tau, tau_ext, k_eta[3], k_nu[3])
+
+        # Combine increments (in-place update to existing state arrays)
+        eta0 += (dt / 6.0) * (k_eta[0] + 2 * k_eta[1] + 2 * k_eta[2] + k_eta[3])
+        nu0 += (dt / 6.0) * (k_nu[0] + 2 * k_nu[1] + 2 * k_nu[2] + k_nu[3])
+
+    def get_states(self) -> tp.Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64]]:
+        r"""
+        Returns a copy of the current states: :math:`\eta, \nu`
+        """
+        return self.eta.copy(), self.nu.copy()