"""Elliptic Fourier Analysis"""
# Copyright 2020 Koji Noshita
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from __future__ import annotations
from abc import ABCMeta
from typing import Optional
import numpy as np
import numpy.typing as npt
import pandas as pd
from scipy.interpolate import make_interp_spline
from sklearn.base import (
BaseEstimator,
ClassNamePrefixFeaturesOutMixin,
TransformerMixin,
)
from sklearn.decomposition import PCA
from sklearn.utils.parallel import Parallel, delayed
[docs]
class EllipticFourierAnalysis(
ClassNamePrefixFeaturesOutMixin, TransformerMixin, BaseEstimator, metaclass=ABCMeta
):
r"""
Elliptic Fourier Analysis (EFA)
Parameters
----------
n_harmonics: int, default=20
Number of harmonics
n_dim: int, default=2
Dimension of the coordinate space.
Must be 2 (for planar curves) or 3 (for space curves).
n_jobs: int, default=None
The number of jobs to run in parallel. None means 1 unless in a
joblib.parallel_backend context. -1 means using all processors.
verbose: int, default=0
The verbosity level.
norm_method : str, default="area"
Normalization method for 3D EFA coefficients when ``norm=True``.
Only affects ``n_dim=3``.
- ``"area"``: Scale by ``sqrt(pi * a1 * b1)`` where ``a1`` and ``b1``
are the semi-major and semi-minor axis lengths of the 1st harmonic
ellipse (Godefroy et al. 2012).
- ``"semi_major_axis"``: Scale by the semi-major axis length ``a1``
of the 1st harmonic ellipse, consistent with the 2D normalization
convention (Kuhl & Giardina 1982).
Notes
----------
EFA is widely applied for outline shape analysis
in two-dimensional space [Kuhl_Giardina_1982]_.
.. math::
\begin{align}
x(l) &=
\frac{a_0}{2} + \sum_{i=1}^{n}
\left[ a_i \cos\left(\frac{2\pi i t}{T}\right)
+ b_i \sin\left(\frac{2\pi i t}{T}\right) \right]\\
y(l) &=
\frac{c_0}{2} + \sum_{i=1}^{n}
\left[ c_i \cos\left(\frac{2\pi i t}{T}\right)
+ d_i \sin\left(\frac{2\pi i t}{T}\right) \right]\\
\end{align}
EFA is also applied for a closed curve in the three-dimensional space
(e.g., [Lestrel_1997]_, [Lestrel_et_al_1997]_, and [Godefroy_et_al_2012]_).
For 3D data (``n_dim=3``), normalization (``norm=True``) follows
Godefroy et al. (2012) §3.1: rescaling by the 1st harmonic ellipse area,
reorientation using ZXZ Euler angles, phase shift, and direction correction.
When ``return_orientation_scale=True`` with 3D normalized data, 5 values
are appended to the output: ``[alpha, beta, gamma, phi, scale]``, where
``(alpha, beta, gamma)`` are ZXZ Euler angles (in radians) of the 1st
harmonic ellipse orientation, ``phi`` is the phase angle, and ``scale``
is the normalization factor (``sqrt(pi * a1 * b1)`` for
``norm_method="area"``, or ``a1`` for ``norm_method="semi_major_axis"``).
References
----------
.. [Kuhl_Giardina_1982] Kuhl, F.P., Giardina, C.R. (1982) Elliptic Fourier features of a closed contour. Comput. Graph. Image Process. 18: 236–258. https://doi.org/10.1016/0146-664X(82)90034-X
.. [Lestrel_1997] Lestrel, P.E., 1997. Introduction and overview of Fourier descriptors, in: Fourier Descriptors and Their Applications in Biology. Cambridge University Press, pp. 22–44. https://doi.org/10.1017/cbo9780511529870.003
.. [Lestrel_et_al_1997] Lestrel, P.E., Read, D.W., Wolfe, C., 1997. Size and shape of the rabbit orbit: 3-D Fourier descriptors, in: Lestrel, P.E. (Ed.), Fourier Descriptors and Their Applications in Biology. Cambridge University Press, pp. 359–378. https://doi.org/10.1017/cbo9780511529870.017
.. [Godefroy_et_al_2012] Godefroy, J.E., Bornert, F., Gros, C.I., Constantinesco, A., 2012. Elliptical Fourier descriptors for contours in three dimensions: A new tool for morphometrical analysis in biology. C. R. Biol. 335, 205–213. https://doi.org/10.1016/j.crvi.2011.12.004
"""
_VALID_NORM_METHODS = {"area", "semi_major_axis"}
def __init__(
self,
n_harmonics: int = 20,
n_dim: int = 2,
n_jobs: Optional[int] = None,
verbose: int = 0,
norm_method: str = "area",
):
self.n_harmonics = n_harmonics
if n_dim not in (2, 3):
raise ValueError("n_dim must be 2 or 3")
else:
self.n_dim = n_dim
self.n_jobs = n_jobs
self.verbose = verbose
if norm_method not in self._VALID_NORM_METHODS:
raise ValueError(
f"norm_method must be 'area' or 'semi_major_axis', got '{norm_method}'"
)
self.norm_method = norm_method
###########################################################
#
# 2D
#
###########################################################
def _transform_single_2d(
self,
X: np.ndarray,
t: np.ndarray | None = None,
norm=True,
return_orientation_scale: bool = False,
duplicated_points="infinitesimal",
):
"""Fit the model with a single outline.
Parameters
----------
X: ndarray of shape (n_coords, 2)
Coordinate values of an 2D outline.
t: ndarray of shape (n_coords, ), optional
A parameter indicating the position on the outline.
If `t=None`, then t is calculated based on
the coordinate values with the linear interpolation.
Returns
----------
X_transformed: ndarray of shape (4*(n_harmonics+1), )
Coefficients of Fourier series.
"""
n_harmonics = self.n_harmonics
X_arr = np.append(
X[-1].reshape(1, self.n_dim), np.array(X), axis=0
) # 1 <= i <= k
dx = X_arr[1:, 0] - X_arr[:-1, 0] # 1 <= i <= k
dy = X_arr[1:, 1] - X_arr[:-1, 1] # 1 <= i <= k
if t is None:
dt = np.sqrt(dx**2 + dy**2)
tp = np.cumsum(dt)
else:
t_ = np.append(0, t)
dt = t_[1:] - t_[:-1] # 1 <= i <= k
tp = np.cumsum(dt)
if duplicated_points == "infinitesimal":
dt[dt < 10**-10] = 10**-10
elif duplicated_points == "deletion":
idx_duplicated_points = np.where(dt == 0)[0]
if len(idx_duplicated_points) > 0:
dx = np.delete(dx, idx_duplicated_points)
dy = np.delete(dy, idx_duplicated_points)
dt = np.delete(dt, idx_duplicated_points)
tp = np.delete(
tp,
(np.array(idx_duplicated_points) + 1).tolist(),
)
X_arr = np.delete(X_arr, idx_duplicated_points, 0)
else:
raise ValueError(
"'duplicated_points' must be 'infinitesimal' or 'deletion'"
)
if len(tp) != len(X):
raise ValueError(
"len(t) must have a same len(X), len(t): "
+ str(len(tp))
+ ", len(X): "
+ str(len(X))
)
# Fourier series expansion
T = tp[-1]
a0 = 2 * np.sum(X_arr[1:, 0] * dt) / T
c0 = 2 * np.sum(X_arr[1:, 1] * dt) / T
an = np.append(a0, _cse(dx, dt, n_harmonics))
bn = np.append(0, _sse(dx, dt, n_harmonics))
cn = np.append(c0, _cse(dy, dt, n_harmonics))
dn = np.append(0, _sse(dy, dt, n_harmonics))
# Normalize
if norm:
an, bn, cn, dn, psi, scale = self._normalize_2d(an, bn, cn, dn)
if return_orientation_scale:
X_transformed = np.hstack([an, bn, cn, dn, psi, scale])
else:
X_transformed = np.hstack([an, bn, cn, dn])
return X_transformed
def _normalize_2d(self, an, bn, cn, dn, keep_start_point=False):
"""Normalize Fourier coefficients.
Todo:
- [x] 1st ellipse, major axis
- [ ] 1st ellipse, area
- [ ] Procrustes alignment -> in coordinate values?
"""
a1 = an[1]
b1 = bn[1]
c1 = cn[1]
d1 = dn[1]
theta = (1 / 2) * np.arctan(
2 * (a1 * b1 + c1 * d1) / (a1**2 + c1**2 - b1**2 - d1**2)
)
[[a_s, b_s], [c_s, d_s]] = np.array([[a1, b1], [c1, d1]]).dot(
rotation_matrix_2d(theta)
)
s1 = a_s**2 + c_s**2
s2 = b_s**2 + d_s**2
if s1 < s2:
if theta < 0:
theta = theta + np.pi / 2
else:
theta = theta - np.pi / 2
a_s = a1 * np.cos(theta) + b1 * np.sin(theta)
c_s = c1 * np.cos(theta) + d1 * np.sin(theta)
scale = np.sqrt(a_s**2 + c_s**2)
psi = np.arctan2(c_s, a_s)
if keep_start_point:
theta = 0
coef_norm_list = []
r_psi = rotation_matrix_2d(-psi)
for n in range(1, len(an)):
r_ntheta = rotation_matrix_2d(n * theta)
coef_orig = np.array([[an[n], bn[n]], [cn[n], dn[n]]])
coef_norm_tmp = (1 / scale) * np.dot(np.dot(r_psi, coef_orig), r_ntheta)
coef_norm_list.append(coef_norm_tmp.reshape(-1))
coef_norm = np.stack(coef_norm_list)
An = np.append(an[0], coef_norm[:, 0])
Bn = np.append(bn[0], coef_norm[:, 1])
Cn = np.append(cn[0], coef_norm[:, 2])
Dn = np.append(dn[0], coef_norm[:, 3])
return An, Bn, Cn, Dn, psi, scale
def _inverse_transform_single_2d(
self,
X_transformed,
t_num=100,
norm=True,
as_frame=False,
):
coef = X_transformed
if as_frame:
a0 = coef["an"][0]
c0 = coef["cn"][0]
an = coef["an"][1:]
bn = coef["bn"][1:]
cn = coef["cn"][1:]
dn = coef["dn"][1:]
else:
an, bn, cn, dn = coef.reshape([2 * self.n_dim, -1])
a0 = an[0]
c0 = cn[0]
an = an[1:]
bn = bn[1:]
cn = cn[1:]
dn = dn[1:]
if norm:
a0 = 0
c0 = 0
n_max = len(an)
theta = np.linspace(2 * np.pi / t_num, 2 * np.pi, t_num)
cos = np.cos(np.tensordot(np.arange(1, n_max + 1, 1), theta, 0))
sin = np.sin(np.tensordot(np.arange(1, n_max + 1, 1), theta, 0))
x = a0 / 2 + np.dot(an, cos) + np.dot(bn, sin)
y = c0 / 2 + np.dot(cn, cos) + np.dot(dn, sin)
X_coords = np.stack([x, y], 1)
return X_coords
###########################################################
#
# 3D
#
###########################################################
def _transform_single_3d(
self,
X: np.ndarray,
t: np.ndarray | None = None,
norm: bool = False,
return_orientation_scale: bool = False,
duplicated_points: str = "infinitesimal",
):
"""Fit the model with a single outline.
Parameters
----------
X: ndarray of shape (n_coords, 3)
Coordinate values of an 3D outline.
t: ndarray of shape (n_coords+1, ), optional
A parameter indicating the position on the outline.
Both t[0] and t[n_coords] corresponds to X[0].
If `t=None`, then t is calculated based on
the coordinate values with the linear interpolation.
norm: bool, default=True
Normalize the elliptic Fourier coefficients by the 1st harmonic
ellipse following Godefroy et al. (2012).
return_orientation_scale: bool, default=False
If True and norm=True, append 5 orientation/scale values to the
output: [alpha, beta, gamma, phi, scale] where (alpha, beta, gamma)
are ZXZ Euler angles, phi is the phase angle, and scale is the
normalization factor (``sqrt(pi * a1 * b1)`` for ``norm_method="area"``,
or ``a1`` for ``norm_method="semi_major_axis"``).
Returns
----------
X_transformed: ndarray of shape (6*(n_harmonics+1),) or (6*(n_harmonics+1)+5,)
Coefficients of Fourier series. When return_orientation_scale=True,
5 additional values are appended.
"""
n_harmonics = self.n_harmonics
X_arr = np.append(
X[-1].reshape(1, self.n_dim), np.array(X), axis=0
) # 1 <= i <= k
dx = X_arr[1:, 0] - X_arr[:-1, 0] # 1 <= i <= k
dy = X_arr[1:, 1] - X_arr[:-1, 1] # 1 <= i <= k
dz = X_arr[1:, 2] - X_arr[:-1, 2] # 1 <= i <= k
if t is None:
dt = np.sqrt(dx**2 + dy**2 + dz**2)
tp = np.cumsum(dt)
else:
t_ = np.append(0, t)
dt = t_[1:] - t_[:-1] # 1 <= i <= k
tp = np.cumsum(dt)
if duplicated_points == "infinitesimal":
dt[dt < 10**-10] = 10**-10
elif duplicated_points == "deletion":
idx_duplicated_points = np.where(dt == 0)[0]
if len(idx_duplicated_points) > 0:
dx = np.delete(dx, idx_duplicated_points)
dy = np.delete(dy, idx_duplicated_points)
dz = np.delete(dz, idx_duplicated_points)
dt = np.delete(dt, idx_duplicated_points)
tp = np.delete(
tp,
(np.array(idx_duplicated_points) + 1).tolist(),
)
X_arr = np.delete(X_arr, idx_duplicated_points, 0)
else:
raise ValueError(
"'duplicated_points' must be 'infinitesimal' or 'deletion'"
)
if len(tp) != len(X):
raise ValueError(
"len(t) must have a same len(X), len(t): "
+ str(len(tp))
+ ", len(X): "
+ str(len(X))
)
# Fourier series expansion
T = tp[-1]
a0 = 2 * np.sum(X_arr[1:, 0] * dt) / T
c0 = 2 * np.sum(X_arr[1:, 1] * dt) / T
e0 = 2 * np.sum(X_arr[1:, 2] * dt) / T
an = np.append(a0, _cse(dx, dt, n_harmonics))
bn = np.append(0, _sse(dx, dt, n_harmonics))
cn = np.append(c0, _cse(dy, dt, n_harmonics))
dn = np.append(0, _sse(dy, dt, n_harmonics))
en = np.append(e0, _cse(dz, dt, n_harmonics))
fn = np.append(0, _sse(dz, dt, n_harmonics))
# Normalize
if norm:
an, bn, cn, dn, en, fn, alpha, beta, gamma, phi, scale = self._normalize_3d(
an, bn, cn, dn, en, fn
)
if return_orientation_scale:
X_transformed = np.hstack(
[an, bn, cn, dn, en, fn, alpha, beta, gamma, phi, scale]
)
else:
X_transformed = np.hstack([an, bn, cn, dn, en, fn])
return X_transformed
def _normalize_3d(self, an, bn, cn, dn, en, fn):
"""Normalize 3D EFA coefficients.
Applies the 4-step normalization algorithm:
1. Rescaling by a scale factor determined by ``self.norm_method``:
- ``"area"``: ``scale = sqrt(pi * a1 * b1)``
- ``"semi_major_axis"``: ``scale = a1``
2. Reorientation using the 1st harmonic's Euler angles
3. Phase shift using the 1st harmonic's phase angle
4. Direction correction (sign of y-sine component)
Parameters
----------
an, bn, cn, dn, en, fn : np.ndarray of shape (n_harmonics+1,)
Raw Fourier coefficient arrays. Index 0 is the offset.
Returns
----------
An, Bn, Cn, Dn, En, Fn : np.ndarray of shape (n_harmonics+1,)
Normalized coefficient arrays.
alpha, beta, gamma : float
ZXZ Euler angles of the 1st harmonic ellipse.
phi : float
Phase angle of the 1st harmonic ellipse.
scale : float
Scaling factor. ``sqrt(pi * a1 * b1)`` when ``norm_method="area"``,
or ``a1`` when ``norm_method="semi_major_axis"``.
"""
# Extract geometric parameters of the 1st harmonic
phi1, a1, b1, alpha1, beta1, gamma1 = _compute_ellipse_geometry_3d(
an[1], bn[1], cn[1], dn[1], en[1], fn[1]
)
# Handle degenerate 1st harmonic
if a1 < 1e-15:
raise ValueError(
"Degenerate 1st harmonic: the ellipse has near-zero semi-axes. "
"Cannot normalize 3D EFA coefficients."
)
# 1. Rescaling
if self.norm_method == "semi_major_axis":
scale = a1
else:
# Default: area-based (Godefroy et al. 2012)
area1 = np.pi * a1 * b1
scale = np.sqrt(area1)
# 2. Reorientation matrix (Omega1_inv = Omega1^T)
Omega1 = rotation_matrix_3d_euler_zxz(alpha1, beta1, gamma1)
Omega1_inv = Omega1.T
n_harmonics = len(an) - 1
An = np.empty_like(an)
Bn = np.empty_like(bn)
Cn = np.empty_like(cn)
Dn = np.empty_like(dn)
En = np.empty_like(en)
Fn = np.empty_like(fn)
An[0] = an[0]
Bn[0] = bn[0]
Cn[0] = cn[0]
Dn[0] = dn[0]
En[0] = en[0]
Fn[0] = fn[0]
for k in range(1, n_harmonics + 1):
# Build 3x2 coefficient matrix
# C_k = [[an_k, bn_k], [cn_k, dn_k], [en_k, fn_k]]
C_k = np.array(
[
[an[k], bn[k]],
[cn[k], dn[k]],
[en[k], fn[k]],
]
)
# 3. Phase rotation uses k*phi1 for harmonic k
# Removing phase phi1 means substituting t -> t + phi1:
# new_xc = xc*cos(k*phi1) + xs*sin(k*phi1)
# new_xs = -xc*sin(k*phi1) + xs*cos(k*phi1)
# In matrix form: C_k @ R(-k*phi1) where R is the standard rotation matrix
angle_k = k * phi1
cos_k = np.cos(angle_k)
sin_k = np.sin(angle_k)
R_phase_k = np.array(
[
[cos_k, sin_k],
[-sin_k, cos_k],
]
)
# Apply: C'_k = (1/scale) * Omega1_inv @ C_k @ R_phase_k
C_norm = (1.0 / scale) * Omega1_inv @ C_k @ R_phase_k
An[k] = C_norm[0, 0]
Bn[k] = C_norm[0, 1]
Cn[k] = C_norm[1, 0]
Dn[k] = C_norm[1, 1]
En[k] = C_norm[2, 0]
Fn[k] = C_norm[2, 1]
# 4. Direction correction
# If the y-sine coefficient of the 1st harmonic is negative,
# negate all sine columns
if Dn[1] < 0:
Bn = -Bn
Dn = -Dn
Fn = -Fn
return An, Bn, Cn, Dn, En, Fn, alpha1, beta1, gamma1, phi1, scale
def _inverse_transform_single_3d(
self,
X_transformed,
t_num: int = 100,
norm=True,
as_frame: bool = False,
):
coef = X_transformed
if as_frame:
a0 = coef["an"][0]
c0 = coef["cn"][0]
e0 = coef["en"][0]
an = coef["an"][1:]
bn = coef["bn"][1:]
cn = coef["cn"][1:]
dn = coef["dn"][1:]
en = coef["en"][1:]
fn = coef["fn"][1:]
else:
an, bn, cn, dn, en, fn = coef.reshape([2 * self.n_dim, -1])
a0 = an[0]
c0 = cn[0]
e0 = en[0]
an = an[1:]
bn = bn[1:]
cn = cn[1:]
dn = dn[1:]
en = en[1:]
fn = fn[1:]
n_max = len(an)
theta = np.linspace(2 * np.pi / t_num, 2 * np.pi, t_num)
if norm:
a0 = 0
c0 = 0
e0 = 0
cos = np.cos(np.tensordot(np.arange(1, n_max + 1, 1), theta, 0))
sin = np.sin(np.tensordot(np.arange(1, n_max + 1, 1), theta, 0))
x = a0 / 2 + np.dot(an, cos) + np.dot(bn, sin)
y = c0 / 2 + np.dot(cn, cos) + np.dot(dn, sin)
z = e0 / 2 + np.dot(en, cos) + np.dot(fn, sin)
X_coords = np.stack([x, y, z], 1)
return X_coords
###########################################################
#
# set_output API
#
###########################################################
[docs]
def get_feature_names_out(
self, input_features: None | npt.ArrayLike = None
) -> np.ndarray:
"""Get output feature names.
Parameters
----------
input_features : None | npt.ArrayLike, optional
Input feature names, by default None
Returns
----------
feature_names_out : ndarray of str objects
Transformed feature names.
"""
an = ["a_" + str(i) for i in range(self.n_harmonics + 1)]
bn = ["b_" + str(i) for i in range(self.n_harmonics + 1)]
cn = ["c_" + str(i) for i in range(self.n_harmonics + 1)]
dn = ["d_" + str(i) for i in range(self.n_harmonics + 1)]
feature_names = an + bn + cn + dn
if self.n_dim == 3:
en = ["e_" + str(i) for i in range(self.n_harmonics + 1)]
fn = ["f_" + str(i) for i in range(self.n_harmonics + 1)]
feature_names = feature_names + en + fn
feature_names_out = np.asarray(feature_names, dtype=str)
return feature_names_out
@property
def _n_features_out(self):
"""Number of transformed output features."""
return (self.n_harmonics + 1) * (2 * self.n_dim)
###########################################################
#
# utility functions
#
###########################################################
def rotation_matrix_2d(theta):
rot_mat = np.array(
[[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]
)
return rot_mat
def rotation_matrix_3d_euler_zxz(alpha: float, beta: float, gamma: float) -> np.ndarray:
"""Construct 3x3 rotation matrix from ZXZ Euler angles.
The rotation is composed as Omega = R_gamma @ R_beta @ R_alpha,
following the convention in Godefroy et al. (2012) Fig. 1.
Parameters
----------
alpha, beta, gamma : float
ZXZ Euler angles in radians.
Returns
----------
rotation_matrix : np.ndarray of shape (3, 3)
Orthogonal rotation matrix with determinant +1.
"""
ca, sa = np.cos(alpha), np.sin(alpha)
cb, sb = np.cos(beta), np.sin(beta)
cg, sg = np.cos(gamma), np.sin(gamma)
return np.array(
[
[ca * cg - sa * cb * sg, -ca * sg - sa * cb * cg, sa * sb],
[sa * cg + ca * cb * sg, -sa * sg + ca * cb * cg, -ca * sb],
[sb * sg, sb * cg, cb],
]
)
def _compute_ellipse_geometry_3d(
xc: float,
xs: float,
yc: float,
ys: float,
zc: float,
zs: float,
) -> tuple[float, float, float, float, float, float]:
"""Compute geometric parameters of a 3D ellipse from Fourier coefficients.
Returns the unique solution satisfying:
phi in ]-pi/4, pi/4[, a >= b > 0, beta in [0, pi].
Parameters
----------
xc, xs, yc, ys, zc, zs : float
Cosine and sine Fourier coefficients for x, y, z coordinates.
Returns
----------
phi : float
Phase angle in ]-pi/4, pi/4[.
a : float
Semi-major axis length (a > 0).
b : float
Semi-minor axis length (b > 0).
alpha : float
First Euler angle (ZXZ convention).
beta : float
Second Euler angle, beta in [0, pi].
gamma : float
Third Euler angle (ZXZ convention).
"""
sum_c2 = xc**2 + yc**2 + zc**2
sum_s2 = xs**2 + ys**2 + zs**2
dot_cs = xc * xs + yc * ys + zc * zs
# phase angle phi
# 2*dot_cs / denom = -tan(2*phi)
# -> phi_0 = -(1/2) * arctan2(2*dot_cs, denom)
denom = sum_c2 - sum_s2
if abs(denom) < 1e-15 and abs(dot_cs) < 1e-15:
phi_0 = 0.0
else:
phi_0 = -0.5 * np.arctan2(2 * dot_cs, denom)
cos_phi = np.cos(phi_0)
sin_phi = np.sin(phi_0)
sin_2phi = np.sin(2 * phi_0)
a2 = sum_c2 * cos_phi**2 + sum_s2 * sin_phi**2 - dot_cs * sin_2phi
b2 = sum_c2 * sin_phi**2 + sum_s2 * cos_phi**2 + dot_cs * sin_2phi
# Enforce a >= b; if not, shift phi by pi/2
if a2 < b2:
a2, b2 = b2, a2
phi_0 = phi_0 + np.pi / 2 if phi_0 < 0 else phi_0 - np.pi / 2
# Normalize phi to ]-pi/4, pi/4[
phi = phi_0
if phi > np.pi / 4:
phi -= np.pi / 2
a2, b2 = b2, a2
elif phi <= -np.pi / 4:
phi += np.pi / 2
a2, b2 = b2, a2
a = np.sqrt(max(a2, 0.0))
b = np.sqrt(max(b2, 0.0))
if a < 1e-15:
return 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
cos_phi = np.cos(phi)
sin_phi = np.sin(phi)
# Recover rotation matrix Omega columns from coefficient vectors.
# The parametric equation relates coefficients to Omega and local-frame values:
# [xc, yc, zc]^T = Omega @ [a*cos(phi), b*sin(phi), 0]^T
# [xs, ys, zs]^T = Omega @ [-a*sin(phi), b*cos(phi), 0]^T
# Solving for the first two columns of Omega:
# col0 = (cos(phi)*[xc,yc,zc] - sin(phi)*[xs,ys,zs]) / a
# col1 = (sin(phi)*[xc,yc,zc] + cos(phi)*[xs,ys,zs]) / b
coef_c = np.array([xc, yc, zc])
coef_s = np.array([xs, ys, zs])
col0 = (cos_phi * coef_c - sin_phi * coef_s) / a
if b < 1e-15:
col1 = np.zeros(3)
col2 = np.zeros(3)
else:
col1 = (sin_phi * coef_c + cos_phi * coef_s) / b
col2 = np.cross(col0, col1)
# Rotation matrix entries
Omega_11, Omega_21, Omega_31 = col0
Omega_12, Omega_22, Omega_32 = col1
Omega_13, Omega_23, Omega_33 = col2
# Extract Euler angles (ZXZ) from the rotation matrix
cos_beta = np.clip(Omega_33, -1.0, 1.0)
beta = float(np.arccos(cos_beta))
_GIMBAL_TOL = 1e-10
if abs(np.sin(beta)) < _GIMBAL_TOL:
# Gimbal lock
gamma = 0.0
if beta < np.pi / 2:
# beta ≈ 0: R reduces to rotation about Z by (alpha+gamma)
alpha = float(np.arctan2(Omega_21, Omega_11))
else:
# beta ≈ pi: R reduces to rotation about Z by (alpha-gamma)
alpha = float(np.arctan2(Omega_21, Omega_11))
else:
sin_beta = np.sin(beta)
gamma = float(np.arctan2(Omega_31 / sin_beta, Omega_32 / sin_beta))
alpha = float(np.arctan2(Omega_13 / sin_beta, -Omega_23 / sin_beta))
return float(phi), float(a), float(b), alpha, beta, float(gamma)
def _cse(dx: np.ndarray, dt: np.ndarray, n_harmonics: int) -> np.ndarray:
"""Cos series expansion n>=1
Parameters
----------
dx : np.ndarray
differences of coordinates
dt : np.ndarray
differences of parameter
n_harmonics : int
number of harmonics
Returns
----------
coef : np.ndarray
coefficients of cos series expansion
"""
# t = np.concatenate([[0], np.cumsum(dt)]) - dt[0] # t_{i-1}
# T = t[-1] + dt[0]
t = np.concatenate([[0], np.cumsum(dt)])
T = t[-1]
cn = [
(T / (2 * (np.pi**2) * (n**2)))
* np.sum(
(dx / dt)
* (np.cos(2 * np.pi * n * t[1:] / T) - np.cos(2 * np.pi * n * t[:-1] / T))
)
for n in range(1, n_harmonics + 1, 1)
]
coef = np.array(cn)
return coef
def _sse(dx: np.ndarray, dt: np.ndarray, n_harmonics: int) -> np.ndarray:
"""Sin series expansion n>=1"""
# t = np.concatenate([[np.sum(dt)], np.cumsum(dt)]) - dt[0] # t_{i-1}
# T = t[-1] + dt[0]
t = np.concatenate([[0], np.cumsum(dt)])
T = t[-1]
cn = [
(T / (2 * (np.pi**2) * (n**2)))
* np.sum(
(dx / dt)
* (np.sin(2 * np.pi * n * t[1:] / T) - np.sin(2 * np.pi * n * t[:-1] / T))
)
for n in range(1, n_harmonics + 1, 1)
]
coef = np.array(cn)
return coef
