diff --git a/include/simcoon/Simulation/Maths/rotation.hpp b/include/simcoon/Simulation/Maths/rotation.hpp index 02274599..1922a775 100755 --- a/include/simcoon/Simulation/Maths/rotation.hpp +++ b/include/simcoon/Simulation/Maths/rotation.hpp @@ -450,8 +450,36 @@ class Rotation { // Identity quaternion is [0, 0, 0, 1] or [0, 0, 0, -1] return std::abs(std::abs(_quat(3)) - 1.0) < tol; } + + /** + * @brief Compute derivatives of the rotation matrix w.r.t. rotation vector components. + * + * Uses the exact differentiation of the Rodrigues formula + * (Gallego & Yezzi, J. Math. Imaging Vis., 2015). + * + * @return 3x3x3 cube where slice(k) is dR/d(omega_k) + * (Python exposure: result[:, :, k]). + */ + arma::cube dR_drotvec() const; }; +/** + * @brief Compute derivatives of R(omega) w.r.t. rotation vector components. + * + * Given a rotation vector omega (axis * angle), computes the three 3x3 + * matrices dR/d(omega_k) for k = 0, 1, 2 using exact differentiation of + * the Rodrigues formula. + * + * @param omega Rotation vector (3 elements) + * @return 3x3x3 cube where slice(k) is dR/d(omega_k) + * (Python exposure: result[:, :, k]). + * + * @details Reference: Gallego & Yezzi, "A Compact Formula for the + * Derivative of a 3-D Rotation in Exponential Coordinates", + * J. Math. Imaging Vis., 2015. + */ +arma::cube dR_drotvec(const arma::vec::fixed<3>& omega); + // ========================================================================= // Convenience Free Functions // ========================================================================= diff --git a/python-setup/simcoon/rotation.py b/python-setup/simcoon/rotation.py index cdc6cc7e..58d4a887 100644 --- a/python-setup/simcoon/rotation.py +++ b/python-setup/simcoon/rotation.py @@ -10,7 +10,7 @@ import numpy as np from scipy.spatial.transform import Rotation as ScipyRotation -from simcoon._core import _CppRotation +from simcoon._core import _CppRotation, dR_drotvec as _dR_drotvec class Rotation(ScipyRotation): @@ -379,6 +379,30 @@ def as_voigt_strain_rotation(self, active=True): """ return self._voigt_strain_matrices(active) + # ------------------------------------------------------------------ + # Rotation vector derivatives + # ------------------------------------------------------------------ + + def dR_drotvec(self): + """Derivatives of the rotation matrix w.r.t. rotation vector components. + + Uses the exact differentiation of the Rodrigues formula + (Gallego & Yezzi, J. Math. Imaging Vis., 2015). + + Returns + ------- + numpy.ndarray + Single: (3, 3, 3) where ``result[:, :, k]`` is dR/d(omega_k). + Batch: (3, 3, 3, N) where ``result[:, :, k, n]`` is dR_n/d(omega_k). + + The slice axis is last, matching simcoon's project-wide cube + convention ((3,3,N), (6,6,N), ...). + """ + rotvec = self.as_rotvec() + if rotvec.ndim == 1: + return _dR_drotvec(rotvec) + return np.stack([_dR_drotvec(r) for r in rotvec], axis=-1) + # ------------------------------------------------------------------ # Compatibility helpers # ------------------------------------------------------------------ diff --git a/simcoon-python-builder/include/simcoon/docs/Libraries/Maths/doc_rotation.hpp b/simcoon-python-builder/include/simcoon/docs/Libraries/Maths/doc_rotation.hpp index a9cdc0df..2886d905 100644 --- a/simcoon-python-builder/include/simcoon/docs/Libraries/Maths/doc_rotation.hpp +++ b/simcoon-python-builder/include/simcoon/docs/Libraries/Maths/doc_rotation.hpp @@ -275,4 +275,51 @@ constexpr auto apply_stress_concentration = R"pbdoc( The rotated stress concentration tensor, same shape as input. )pbdoc"; +constexpr auto dR_drotvec = R"pbdoc( + Compute the derivatives of the rotation matrix w.r.t. rotation vector components. + + Uses the exact differentiation of the Rodrigues formula + (Gallego & Yezzi, J. Math. Imaging Vis., 2015). + + Returns + ------- + numpy.ndarray + A (3, 3, 3) array where ``result[:, :, k]`` is + :math:`\partial R / \partial \omega_k`. The slice axis is last, + matching simcoon's project-wide cube convention. + + Examples + -------- + .. code-block:: python + + import simcoon as smc + import numpy as np + + r = smc.Rotation.from_rotvec([0.1, 0.2, 0.3]) + dR = r.dR_drotvec() # (3, 3, 3) + # dR[:, :, 0] = dR/d(omega_0), dR[:, :, 1] = dR/d(omega_1), dR[:, :, 2] = dR/d(omega_2) +)pbdoc"; + +constexpr auto dR_drotvec_free = R"pbdoc( + Compute the derivatives of R(omega) w.r.t. rotation vector components. + + Free function version — takes a rotation vector directly. + + Parameters + ---------- + rotvec : numpy.ndarray + A 1D array of 3 elements: rotation vector (axis * angle). + + Returns + ------- + numpy.ndarray + A (3, 3, 3) array where ``result[:, :, k]`` is + :math:`\partial R / \partial \omega_k`. + + References + ---------- + Gallego & Yezzi, "A Compact Formula for the Derivative of a 3-D + Rotation in Exponential Coordinates", J. Math. Imaging Vis., 2015. +)pbdoc"; + } // namespace simcoon_docs diff --git a/simcoon-python-builder/src/python_wrappers/Libraries/Maths/rotation.cpp b/simcoon-python-builder/src/python_wrappers/Libraries/Maths/rotation.cpp index 33d3bdae..c08bf633 100644 --- a/simcoon-python-builder/src/python_wrappers/Libraries/Maths/rotation.cpp +++ b/simcoon-python-builder/src/python_wrappers/Libraries/Maths/rotation.cpp @@ -139,7 +139,23 @@ void register_rotation(py::module_& m) { py::arg("B"), py::arg("active") = true, simcoon_docs::apply_stress_concentration) + .def("dR_drotvec", + [](const simcoon::Rotation& self) { + return carma::cube_to_arr(self.dR_drotvec()); + }, + simcoon_docs::dR_drotvec) + ; + + m.def("dR_drotvec", + [](py::array_t rotvec) { + validate_vector_size(rotvec, 3, "rotvec"); + auto r = rotvec.unchecked<1>(); + vec::fixed<3> omega = {r(0), r(1), r(2)}; + return carma::cube_to_arr(simcoon::dR_drotvec(omega)); + }, + py::arg("rotvec"), + simcoon_docs::dR_drotvec_free); } } // namespace simpy diff --git a/simcoon-python-builder/test/test_core/test_dR_drotvec.py b/simcoon-python-builder/test/test_core/test_dR_drotvec.py new file mode 100644 index 00000000..aeddeeb1 --- /dev/null +++ b/simcoon-python-builder/test/test_core/test_dR_drotvec.py @@ -0,0 +1,141 @@ +"""Tests for ``dR_drotvec``: derivatives of R(omega) w.r.t. rotation-vector components. + +Layout convention (matches simcoon's project-wide cube convention): + single : shape (3, 3, 3), ``result[:, :, k]`` is dR/d(omega_k) + batch : shape (3, 3, 3, N), ``result[:, :, k, n]`` is dR_n/d(omega_k) +""" + +import numpy as np +import pytest + +import simcoon as sim + + +# --------------------------------------------------------------------------- +# Fixtures +# --------------------------------------------------------------------------- + +@pytest.fixture +def rotvec_general(): + """Generic non-axis-aligned rotvec (moderate angle ~0.37 rad).""" + return np.array([0.1, -0.2, 0.3]) + + +@pytest.fixture +def rotvecs_batch(): + """A batch of diverse rotvecs, including identity.""" + return np.array([ + [0.1, -0.2, 0.3], + [0.0, 0.0, 0.5], + [0.7, 0.0, 0.0], + [0.0, 0.0, 0.0], # identity + [np.pi / 2 - 0.05, 0.0, 0.0], # near pi/2 + ]) + + +# Reference skew matrices [e_k]x for k=0,1,2 +E_SKEW = np.array([ + [[0.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], # [e_0]x + [[0.0, 0.0, 1.0], [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]], # [e_1]x + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 0.0]], # [e_2]x +]) + + +# --------------------------------------------------------------------------- +# Shape + API symmetry +# --------------------------------------------------------------------------- + +class TestShape: + def test_single_shape(self, rotvec_general): + assert sim.dR_drotvec(rotvec_general).shape == (3, 3, 3) + + def test_member_shape(self, rotvec_general): + r = sim.Rotation.from_rotvec(rotvec_general) + assert r.dR_drotvec().shape == (3, 3, 3) + + def test_batch_shape(self, rotvecs_batch): + r = sim.Rotation.from_rotvec(rotvecs_batch) + assert r.dR_drotvec().shape == (3, 3, 3, len(rotvecs_batch)) + + def test_free_eq_member(self, rotvec_general): + r = sim.Rotation.from_rotvec(rotvec_general) + np.testing.assert_array_equal(sim.dR_drotvec(rotvec_general), r.dR_drotvec()) + + +# --------------------------------------------------------------------------- +# Small-angle / identity branch +# --------------------------------------------------------------------------- + +class TestSmallAngle: + def test_zero_rotvec_equals_basis_skews(self): + """At omega=0: dR/d(omega_k) == [e_k]x for each k.""" + result = sim.dR_drotvec(np.zeros(3)) + for k in range(3): + np.testing.assert_allclose(result[:, :, k], E_SKEW[k], atol=0.0) + + def test_identity_rotation_matches_zero_rotvec(self): + result = sim.Rotation.identity().dR_drotvec() + expected = sim.dR_drotvec(np.zeros(3)) + np.testing.assert_allclose(result, expected, atol=0.0) + + def test_below_iota_threshold_uses_small_angle_branch(self): + """Very small rotvec should give near-basis-skew slices.""" + tiny = np.array([1e-14, 0.0, 0.0]) + result = sim.dR_drotvec(tiny) + for k in range(3): + np.testing.assert_allclose(result[:, :, k], E_SKEW[k], atol=1e-12) + + +# --------------------------------------------------------------------------- +# Finite-difference agreement +# --------------------------------------------------------------------------- + +def _R(omega): + return sim.Rotation.from_rotvec(omega).as_matrix() + + +def _fd_slice(omega, k, eps=1e-6): + pert = omega.copy() + pert[k] += eps + return (_R(pert) - _R(omega - np.eye(3)[k] * eps)) / (2.0 * eps) + + +class TestFiniteDifference: + @pytest.mark.parametrize("rotvec", [ + np.array([0.1, -0.2, 0.3]), + np.array([0.0, 0.0, 0.9]), + np.array([1.2, -0.4, 0.3]), # |omega| ~ 1.3 rad + np.array([np.pi / 2 - 0.01, 0.0, 0.0]), + ]) + def test_fd_matches_analytic(self, rotvec): + analytic = sim.dR_drotvec(rotvec) + for k in range(3): + fd = _fd_slice(rotvec, k) + np.testing.assert_allclose(analytic[:, :, k], fd, atol=1e-7, rtol=1e-7) + + +# --------------------------------------------------------------------------- +# Batch consistency +# --------------------------------------------------------------------------- + +class TestBatch: + def test_batch_slice_matches_single_call(self, rotvecs_batch): + r = sim.Rotation.from_rotvec(rotvecs_batch) + batch = r.dR_drotvec() + for n, rv in enumerate(rotvecs_batch): + single = sim.dR_drotvec(rv) + np.testing.assert_allclose(batch[:, :, :, n], single, atol=0.0) + + +# --------------------------------------------------------------------------- +# Input validation +# --------------------------------------------------------------------------- + +class TestValidation: + def test_rejects_wrong_size(self): + with pytest.raises(Exception): + sim.dR_drotvec(np.zeros(2)) + + def test_rejects_2d_input(self): + with pytest.raises(Exception): + sim.dR_drotvec(np.zeros((3, 1))) diff --git a/src/Simulation/Maths/rotation.cpp b/src/Simulation/Maths/rotation.cpp index c349efed..220a79b3 100755 --- a/src/Simulation/Maths/rotation.cpp +++ b/src/Simulation/Maths/rotation.cpp @@ -972,4 +972,59 @@ mat rotate_stress_concentration(const mat &B, const mat &DR, const bool &active) return vs*(B*trans(ve)); } +cube Rotation::dR_drotvec() const { + return simcoon::dR_drotvec(as_rotvec()); +} + +namespace { + // Cross-product matrix [v]x such that [v]x * u = v x u. + inline mat::fixed<3,3> skew(const vec::fixed<3>& v) { + mat::fixed<3,3> S; + S.zeros(); + S(0,1) = -v(2); S(0,2) = v(1); + S(1,0) = v(2); S(1,2) = -v(0); + S(2,0) = -v(1); S(2,1) = v(0); + return S; + } + + inline mat::fixed<3,3> skew_basis(int k) { + vec::fixed<3> e; + e.zeros(); + e(k) = 1.0; + return skew(e); + } +} + +cube dR_drotvec(const vec::fixed<3>& omega) { + double theta = norm(omega); + cube result(3, 3, 3); + + if (theta < simcoon::iota) { + // d/d(omega_k) [ exp([omega]x) ] at omega=0 is [e_k]x. + for (int k = 0; k < 3; ++k) { + result.slice(k) = skew_basis(k); + } + return result; + } + + mat::fixed<3,3> W = skew(omega); + mat::fixed<3,3> W2 = W * W; + + double s = sin(theta), c = cos(theta); + double t2 = theta * theta; + double a = s / theta; + double b = (1.0 - c) / t2; + double da = (c * theta - s) / t2; + double db = (s * theta - 2.0 * (1.0 - c)) / (t2 * theta); + + for (int k = 0; k < 3; ++k) { + mat::fixed<3,3> dW = skew_basis(k); + mat::fixed<3,3> dW2 = dW * W + W * dW; + double dtk = omega(k) / theta; + result.slice(k) = da * dtk * W + a * dW + db * dtk * W2 + b * dW2; + } + + return result; +} + } //namespace simcoon