Feature/1540 add binary asteroid

src/simulation/environment/spiceInterface/_UnitTest/test_unitSpicePlanetOffset.py

@@ -0,0 +1,187 @@
+# SPDX-License-Identifier: ISC
+# Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder
+#
+
+import inspect
+import os
+
+import numpy as np
+import numpy.testing as npt
+import pytest
+
+# Exercises the SecondaryBody capability of SpiceInterface:
+#   - the secondary state output message equals the primary's state plus
+#     the configured offset, and the body is renamed
+#   - the primary planet message itself is untouched
+#   - when an orbital period is set, the secondary traces a circle of radius |offset|
+#     centered on the primary, in the equator-tilted plane normal to
+#     offset x (offset x J2000 Z)
+
+filename = inspect.getframeinfo(inspect.currentframe()).filename
+path = os.path.dirname(os.path.abspath(filename))
+from xmera import __path__
+bsk_path = __path__[0]
+
+from xmera.utilities import SimulationBaseClass
+from xmera.simulation import spiceInterface
+from xmera.utilities import macros
+from xmera.architecture import messaging  # noqa: F401  registers Message<X> Python wrappers
+
+
+planet_names = ["earth", "moon"]
+primary_name = "moon"
+primary_index = 1
+secondary_name = "binaryAsteroid"
+default_offset = np.array([1000.0, 2000.0, -3000.0])  # m
+date_spice = "2015 February 10, 00:00:00.0 TDB"
+# Position-difference tolerance: secondary - primary involves subtracting two
+# SPICE-derived positions of order 1e8 m, so the float-precision floor is ~1e-8 m.
+# 1e-3 m (1 mm) is well below any physically meaningful threshold.
+position_atol = 1e-3  # m
+
+
+def build_sim(orbital_period, stop_time_sec, time_step_sec=0.1, attach_secondary=True,
+              offset=default_offset):
+    """Configure a SpiceInterface, attach recorders, run the sim, and return
+    (spice, secondary_rec, primary_rec)."""
+    sim = SimulationBaseClass.SimBaseClass()
+    proc = sim.CreateNewProcess("TestProcess")
+    proc.addTask(sim.CreateNewTask("unitTask", macros.sec2nano(time_step_sec)))
+
+    spice = spiceInterface.SpiceInterface()
+    spice.modelTag = "SpiceInterface"
+    spice.SPICEDataPath = bsk_path + "/supportData/EphemerisData/"
+    spice.addPlanetNames(planet_names)
+    spice.UTCCalInit = date_spice
+
+    if attach_secondary:
+        secondary = spiceInterface.SecondaryBody()
+        secondary.secondaryName = secondary_name
+        secondary.positionOffset = np.asarray(offset, dtype=float)
+        secondary.orbitalPeriod = orbital_period
+        spice.setOffsetBody(primary_name, secondary)
+
+    secondary_rec = spice.secondaryStateOutMsg.recorder()
+    primary_rec = spice.planetStateOutMsgs[primary_index].recorder()
+
+    sim.AddModelToTask("unitTask", spice)
+    sim.AddModelToTask("unitTask", secondary_rec)
+    sim.AddModelToTask("unitTask", primary_rec)
+    sim.ConfigureStopTime(macros.sec2nano(stop_time_sec))
+    sim.InitializeSimulation()
+    sim.ExecuteSimulation()
+
+    return spice, secondary_rec, primary_rec
+
+
+def test_secondary_body_static_offset(show_plots):
+    """Period=0: secondary position = primary position + constant offset; name is renamed."""
+    spice, secondary_rec, primary_rec = build_sim(orbital_period=0.0, stop_time_sec=0.5)
+
+    delta = np.array(secondary_rec.PositionVector) - np.array(primary_rec.PositionVector)
+    npt.assert_allclose(delta, np.broadcast_to(default_offset, delta.shape), atol=position_atol,
+                        err_msg="static offset mismatch in secondary message")
+
+    name = spice.secondaryStateOutMsg.read().PlanetName
+    npt.assert_equal(name, secondary_name)
+
+
+def test_primary_body_unchanged(show_plots):
+    """Attaching a SecondaryBody must not perturb the primary's planet message."""
+    _, _, primary_with_rec = build_sim(orbital_period=0.0, stop_time_sec=0.5,
+                                       attach_secondary=True)
+    _, _, primary_ref_rec = build_sim(orbital_period=0.0, stop_time_sec=0.5,
+                                      attach_secondary=False)
+
+    npt.assert_allclose(np.array(primary_with_rec.PositionVector),
+                        np.array(primary_ref_rec.PositionVector),
+                        atol=position_atol,
+                        err_msg="primary planet message changed when SecondaryBody attached")
+
+
+def _expected_orbit_basis(offset):
+    """Replicates the C++ orbit-plane construction: plane_normal = offset x (offset x up),
+    which tilts the orbit toward the equator rather than through the poles. Falls back
+    to the J2000 X axis when offset is parallel to up."""
+    parallel_tol = 8.0 * np.finfo(np.float64).eps
+    up = np.array([0.0, 0.0, 1.0])
+    intermediate = np.cross(offset, up)
+    if np.linalg.norm(intermediate) < parallel_tol * np.linalg.norm(offset):
+        intermediate = np.cross(offset, np.array([1.0, 0.0, 0.0]))
+    plane_normal = np.cross(offset, intermediate)
+    plane_normal /= np.linalg.norm(plane_normal)
+    return plane_normal, np.cross(plane_normal, offset)
+
+
+# Offsets cover: generic 3-component, pure +X, pure +Y, asymmetric in-plane,
+# and the parallel-to-up degenerate case (forces the X-axis fallback in the
+# C++ basis construction).
+orbit_offsets = [
+    pytest.param(np.array([1000.0, 2000.0, -3000.0]), id="mixed"),
+    pytest.param(np.array([5000.0, 0.0, 0.0]),        id="pure_x"),
+    pytest.param(np.array([0.0, 4000.0, 0.0]),        id="pure_y"),
+    pytest.param(np.array([-500.0, 1500.0, 0.0]),     id="asymmetric_xy"),
+    pytest.param(np.array([0.0, 0.0, 2500.0]),        id="parallel_to_up"),
+]
+
+
+@pytest.mark.parametrize("offset", orbit_offsets)
+def test_secondary_body_circular_orbit(show_plots, offset):
+    """Period=T: secondary orbits the primary in the equator-tilted plane normal to
+    offset x (offset x J2000 Z) (or offset x (offset x X) when offset is parallel to Z),
+    and returns to start at t=T."""
+    period = 10.0  # s
+    time_step = period / 20.0
+    _, secondary_rec, primary_rec = build_sim(
+        orbital_period=period, stop_time_sec=period, time_step_sec=time_step,
+        offset=offset,
+    )
+
+    times = np.array(secondary_rec.times()) * macros.NANO2SEC
+    delta = np.array(secondary_rec.PositionVector) - np.array(primary_rec.PositionVector)
+
+    angles = 2.0 * np.pi * times / period
+    plane_normal, perp = _expected_orbit_basis(offset)
+    expected = (np.outer(np.cos(angles), offset)
+                + np.outer(np.sin(angles), perp))
+    npt.assert_allclose(delta, expected, atol=position_atol,
+                        err_msg="circular-orbit rotation mismatch")
+
+    # Radius is conserved.
+    r_3d = np.linalg.norm(delta, axis=1)
+    npt.assert_allclose(r_3d, np.linalg.norm(offset), atol=position_atol,
+                        err_msg="orbital radius drifted")
+
+    # Trajectory stays in the plane (projection onto the plane normal is zero).
+    out_of_plane = delta @ plane_normal
+    npt.assert_allclose(out_of_plane, 0.0, atol=position_atol,
+                        err_msg="secondary drifted out of orbital plane")
+
+    # Closes the loop: t=0 and t=T positions match.
+    npt.assert_allclose(delta[-1], delta[0], atol=position_atol,
+                        err_msg="orbit did not close after one period")
+
+    if show_plots:
+        import matplotlib.pyplot as plt
+        from mpl_toolkits.mplot3d import Axes3D  # noqa: F401
+        fig = plt.figure(figsize=(7, 6))
+        ax = fig.add_subplot(111, projection="3d")
+        ax.plot(delta[:, 0], delta[:, 1], delta[:, 2], "o-",
+                label="secondary - primary")
+        ax.scatter([offset[0]], [offset[1]], [offset[2]],
+                   color="r", s=80, label="initial offset")
+        ax.scatter([0], [0], [0], color="k", s=40, label="primary")
+        ax.set_xlabel("X offset [m]")
+        ax.set_ylabel("Y offset [m]")
+        ax.set_zlabel("Z offset [m]")
+        ax.set_title(f"Secondary body trajectory (T={period}s, offset={offset.tolist()})")
+        ax.legend()
+        plt.show()
+
+
+if __name__ == "__main__":
+    test_secondary_body_static_offset(True)
+    test_primary_body_unchanged(True)
+    for params in orbit_offsets:
+        test_secondary_body_circular_orbit(True, params.values[0])
+    print(" \n PASSED ")