# -*- coding: utf-8 -*-
"""
Functions to manipulate the **body kinematics** of, e.g., marine vessels.
.. dropdown:: Copyright (C) 2023-2026 Technical University of Denmark, R.E.G. Mounet
:color: primary
:icon: law
*This code is part of the NetSSE software.*
NetSSE is free software: you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free
Software Foundation, either version 3 of the License, or (at your
option) any later version.
NetSSE is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS 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 https://www.gnu.org/licenses/.
To credit the author, users are encouraged to use below reference:
.. code-block:: text
Mounet, R. E. G., & Nielsen, U. D. NetSSE: An open-source Python package
for network-based sea state estimation from ships, buoys, and other
observation platforms (version 2.2). Technical University of Denmark,
GitLab. February 2026. https://doi.org/10.11583/DTU.26379811.
*Last updated on 20-02-2026 by R.E.G. Mounet*
"""
import numpy as np
[docs]
def Rzyx(phi, theta, psi):
"""
Computes the Euler angle rotation matrix :math:`R` using the `zyx` convention,
as per Fossen (2011).
.. note::
This function can be used to transform velocity vectors from a body-fixed frame to the North-East-Down (NED) reference frame, using specified Euler angles.
Parameters
----------
phi : Scalar, or numpy array of size (N,) or (N,M)
Roll angle [rad].
theta : Scalar, or numpy array of size (N,) or (N,M)
Pitch angle [rad].
psi : Scalar, or numpy array of size (N,) or (N,M)
Yaw angle [rad].
Returns
-------
R : Numpy array of size (shape(phi),3,3)
Euler angle rotation matrix :math:`R`.
See Also
--------
Tzyx : Euler angle transformation matrix for attitude.
eulerang : Euler angle transformation matrix in full.
References
----------
Fossen, T. I. (2011). *Handbook of Marine Craft Hydrodynamics and Motion Control.*
John Wiley and Sons.
Example
-------
>>> R = Rzyx(phi, theta, psi)
"""
cphi = np.cos(phi)
sphi = np.sin(phi)
cth = np.cos(theta)
sth = np.sin(theta)
cpsi = np.cos(psi)
spsi = np.sin(psi)
R = np.array(
[
[
cpsi * cth,
-spsi * cphi + cpsi * sth * sphi,
spsi * sphi + cpsi * cphi * sth,
],
[
spsi * cth,
cpsi * cphi + sphi * sth * spsi,
-cpsi * sphi + sth * spsi * cphi,
],
[-sth, cth * sphi, cth * cphi],
]
)
if np.isscalar(phi):
return R
elif phi.ndim == 1:
R = np.transpose(R, (2, 0, 1))
elif phi.ndim == 2:
R = np.transpose(R, (2, 3, 0, 1))
return R
[docs]
def Tzyx(phi, theta):
"""
Computes the Euler angle transformation matrix :math:`T` for attitude using the `zyx` convention, as per Fossen (2011).
.. note::
This function can be used to transform velocity vectors from a body-fixed frame to the North-East-Down (NED) reference frame, using specified Euler angles.
Parameters
----------
phi : Scalar, or numpy array of size (N,) or (N,M)
Roll angle [rad].
theta : Scalar, or numpy array of size (N,) or (N,M)
Pitch angle [rad].
Raises
------
ValueError
The :math:`T` matrix has a singularity for :math:`\\theta = \pm 90^\circ`.
Returns
-------
T : Numpy array of size (shape(phi),3,3)
Euler angle transformation matrix :math:`T` for attitude.
See Also
--------
Rzyx : Euler angle rotation matrix.
eulerang : Euler angle transformation matrix.
References
----------
Fossen, T. I. (2011). *Handbook of Marine Craft Hydrodynamics and Motion Control.*
John Wiley and Sons.
Example
-------
>>> T = Tzyx(phi, theta)
"""
phishape = np.shape(phi)
cphi = np.cos(phi)
sphi = np.sin(phi)
cth = np.cos(theta)
sth = np.sin(theta)
if 0 in cth:
raise ValueError("Tzyx is singular for theta = +-90 degrees")
else:
T = np.array(
[
[np.ones(phishape), sphi * sth / cth, cphi * sth / cth],
[np.zeros(phishape), cphi, -sphi],
[np.zeros(phishape), sphi / cth, cphi / cth],
]
)
if np.isscalar(phi):
return T
elif phi.ndim == 1:
T = np.transpose(T, (2, 0, 1))
elif phi.ndim == 2:
T = np.transpose(T, (2, 3, 0, 1))
return T
[docs]
def eulerang(phi, theta, psi, unit="rad"):
"""
Computes the Euler angle transformation matrix :math:`J` in full using the `zyx`
convention, as per Fossen (2011):
.. math::
J =
\left[
\\begin{array}{cc}
J_1 & 0 \\newline
0 & J_2
\end{array}
\\right],
where :math:`J_1 = R_{zyx}` and :math:`J_2 = T_{zyx}`.
.. note::
This function can be used to transform velocity vectors from a body-fixed frame to the North-East-Down (NED) reference frame, using specified Euler angles.
Parameters
----------
phi : Scalar, or numpy array of size (N,) or (N,M)
Roll angle.
theta : Scalar, or numpy array of size (N,) or (N,M)
Pitch angle.
psi : Scalar, or numpy array of size (N,) or (N,M)
Yaw angle.
unit : {'rad','deg'}, optional
Unit of the input angles ``phi``, ``theta``, and ``psi``.
Returns
-------
J : Numpy array of size (shape(phi),6,6)
Euler angle transformation matrix.
J1 : Numpy array of size (shape(phi),3,3)
:math:`J_1 = R_{zyx}`, the Euler angle rotation matrix.
J2 : Numpy array of size (shape(phi),3,3)
:math:`J_2 = T_{zyx}`, the Euler angle transformation matrix for attitude.
See Also
--------
Rzyx : Euler angle rotation matrix.
Tzyx : Euler angle transformation matrix for attitude.
References
----------
Fossen, T. I. (2011). *Handbook of Marine Craft Hydrodynamics and Motion Control.*
John Wiley and Sons.
Example
-------
>>> [J, Rzyx, Tzyx] = eulerang(phi, theta, psi)
"""
if unit == "deg":
phi = np.radians(phi)
theta = np.radians(theta)
psi = np.radians(psi)
phishape = np.shape(phi)
J1 = Rzyx(phi, theta, psi)
J2 = Tzyx(phi, theta)
J = np.zeros(phishape + (6, 6))
if np.isscalar(phi):
J[0:3, 0:3] = J1
J[3:6, 3:6] = J2
elif phi.ndim == 1:
J[:, 0:3, 0:3] = J1
J[:, 3:6, 3:6] = J2
elif phi.ndim == 2:
J[:, :, 0:3, 0:3] = J1
J[:, :, 3:6, 3:6] = J2
return J, J1, J2
[docs]
def accel_rigid_body(a_O, r_P, omega, omega_dot, axis=0):
"""
Calculates the acceleration :math:`a_P` of a point P in a rigid body.
The acceleration, :math:`a_O` of a point O in the rigid body, as well as the body's angular velocity, :math:`\omega`, and acceleration, :math:`\dot{\omega}`, are assumed to be known.
Parameters
----------
a_O : Numpy array of vector of size (_,3,_)
Linear acceleration vector of the reference point O [m/s²].
r_P : Numpy vector of size (3,)
Position vector of point P relative to the reference point O [m].
omega : Numpy array of vector of size (_,3,_)
Angular velocity vector [rad/s].
omega_dot : Numpy array of vector of size (_,3,_)
Angular acceleration vector [rad/s²].
axis : int, optional
Axis along which to compute the cross products. Default is 0.
Returns
-------
a_P : Numpy array of size (_,3,_)
Linear acceleration vector of the target point P [m/s²].
Example
-------
>>> a_P = accel_rigid_body(a_O, r_P, omega, omega_dot)
"""
a_P = (
a_O
+ np.cross(omega_dot, r_P, axisa=axis, axisb=0, axisc=axis)
+ np.cross(
omega, np.cross(omega, r_P, axisa=axis, axisb=0, axisc=axis), axis=axis
)
)
return a_P
