feat: add state isNear method

bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/State.cpp

@@ -36,7 +36,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
             init<const Instant&, const Position&, const Velocity&>(),
             R"doc(
                  Utility constructor for Position/Velocity only.
-                 
+
                  Args:
                      instant (Instant): An instant
                      position (Position): The cartesian position at the instant
@@ -56,7 +56,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
                 const Shared<const Frame>&>(),
             R"doc(
                  Utility constructor for Position/Velocity/Attitude/Angular velocity.
-                 
+
                  Args:
                      instant (Instant): An instant
                      position (Position): The cartesian position at the instant
@@ -76,7 +76,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
             init<const Instant&, const VectorXd&, const Shared<const Frame>&, const Shared<const CoordinateBroker>&>(),
             R"doc(
                  Constructor with a pre-defined Coordinates Broker.
-                 
+
                  Args:
                      instant (Instant): An instant
                      coordinates (numpy.ndarray): The coordinates at the instant in International System of Units
@@ -96,7 +96,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
                 const Array<Shared<const CoordinateSubset>>&>(),
             R"doc(
                  Constructor with coordinate subsets.
-                 
+
                  Args:
                      instant (Instant): An instant
                      coordinates (numpy.ndarray): The coordinates at the instant in International System of Units
@@ -277,6 +277,43 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
             arg("frame")
         )
 
+        .def(
+            "is_near",
+            overload_cast<const State&, const std::unordered_map<Shared<const CoordinateSubset>, Real>&>(
+                &State::isNear, const_
+            ),
+            R"doc(
+                Check if the state is near another state.
+
+                Args:
+                    state (State): The state to compare to.
+                    tolerance_map (dict[CoordinateSubset, float]): The tolerance map for the comparison.
+
+                Returns:
+                    dict[CoordinateSubset, bool]: The map of coordinate subsets and whether or not the state is near the other state.
+            )doc",
+            arg("state"),
+            arg("tolerance_map")
+        )
+
+        .def(
+            "is_near",
+            overload_cast<const State&, const std::unordered_map<Shared<const CoordinateSubset>, VectorXd>&>(
+                &State::isNear, const_
+            ),
+            R"doc(
+                Check if the state is near another state.
+
+                Args:
+                    state (State): The state to compare to.
+                    tolerance_array_map (dict[CoordinateSubset, np.ndarray]): The tolerance array map for the comparison.
+
+                Returns:
+                    dict[CoordinateSubset, list[bool]]: The map of coordinate subsets and whether or not the state is near the other state.
+            )doc",
+            arg("state"),
+            arg("tolerance_array_map")
+        )
         .def_static(
             "undefined",
             &State::Undefined,

bindings/python/test/trajectory/test_state.py

@@ -592,3 +592,39 @@ def test_extract_coordinates(
         )
         assert len(pv_coordinates) == 6
         assert (pv_coordinates == state.get_coordinates()).all()
+
+    def test_is_near(
+        self,
+        state: State,
+    ):
+        tolerance_map: dict[CoordinateSubset, float] = {
+            CartesianPosition.default(): 1e-10,
+            CartesianVelocity.default(): 1e-10,
+        }
+        is_near_result: dict[CoordinateSubset, bool] = state.is_near(
+            state=state,
+            tolerance_map=tolerance_map,
+        )
+        expected_is_near_result: dict[CoordinateSubset, bool] = {
+            CartesianPosition.default(): True,
+            CartesianVelocity.default(): True,
+        }
+        assert is_near_result == expected_is_near_result
+
+    def test_is_near_element_wise(
+        self,
+        state: State,
+    ):
+        tolerance_array_map: dict[CoordinateSubset, np.ndarray] = {
+            CartesianPosition.default(): np.array([1e-10, 1e-10, 1e-10]),
+            CartesianVelocity.default(): np.array([1e-10, 1e-10, 1e-10]),
+        }
+        is_near_result: dict[CoordinateSubset, list[bool]] = state.is_near(
+            state=state,
+            tolerance_array_map=tolerance_array_map,
+        )
+        expected_is_near_result: dict[CoordinateSubset, list[bool]] = {
+            CartesianPosition.default(): [True, True, True],
+            CartesianVelocity.default(): [True, True, True],
+        }
+        assert is_near_result == expected_is_near_result

include/OpenSpaceToolkit/Astrodynamics/Trajectory/State.hpp

@@ -4,6 +4,7 @@
 #define __OpenSpaceToolkit_Astrodynamics_Trajectory_State__
 
 #include <OpenSpaceToolkit/Core/Container/Array.hpp>
+#include <OpenSpaceToolkit/Core/Type/Real.hpp>
 #include <OpenSpaceToolkit/Core/Type/Shared.hpp>
 #include <OpenSpaceToolkit/Core/Type/Size.hpp>
 
@@ -26,6 +27,7 @@ namespace trajectory
 {
 
 using ostk::core::container::Array;
+using ostk::core::type::Real;
 using ostk::core::type::Shared;
 using ostk::core::type::Size;
 
@@ -239,6 +241,24 @@ class State
     /// @return The transformed State
     State inFrame(const Shared<const Frame>& aFrameSPtr) const;
 
+    /// @brief Check if the State is within a given tolerance of another State per subset.
+    ///
+    /// @param aState The State to compare to
+    /// @param aToleranceMap The tolerance map for each coordinate subset
+    /// @return The map of coordinate subsets and whether or not the State is within the tolerance
+    std::unordered_map<Shared<const CoordinateSubset>, bool> isNear(
+        const State& aState, const std::unordered_map<Shared<const CoordinateSubset>, Real>& aToleranceMap
+    ) const;
+
+    /// @brief Check if the State is within a given tolerance of another State per coordinate of each subset.
+    ///
+    /// @param aState The State to compare to
+    /// @param aToleranceArrayMap The tolerance array map for each coordinate subset
+    /// @return The map of coordinate subsets and whether or not the State is within the tolerance
+    std::unordered_map<Shared<const CoordinateSubset>, Array<bool>> isNear(
+        const State& aState, const std::unordered_map<Shared<const CoordinateSubset>, VectorXd>& aToleranceArrayMap
+    ) const;
+
     /// @brief Print the State to an output stream.
     ///
     /// @param anOutputStream The output stream to print to

src/OpenSpaceToolkit/Astrodynamics/Trajectory/State.cpp

@@ -6,7 +6,6 @@
 #include <OpenSpaceToolkit/Mathematics/Geometry/3D/Transformation/Rotation/Quaternion.hpp>
 
 #include <OpenSpaceToolkit/Astrodynamics/Trajectory/State.hpp>
-#include <OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinateSubset.hpp>
 #include <OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinateSubset/AngularVelocity.hpp>
 #include <OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinateSubset/AttitudeQuaternion.hpp>
 #include <OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinateSubset/CartesianPosition.hpp>
@@ -23,7 +22,6 @@ using ostk::core::type::Index;
 
 using ostk::mathematics::geometry::d3::transformation::rotation::Quaternion;
 
-using ostk::astrodynamics::trajectory::state::CoordinateSubset;
 using ostk::astrodynamics::trajectory::state::coordinatesubset::AngularVelocity;
 using ostk::astrodynamics::trajectory::state::coordinatesubset::AttitudeQuaternion;
 using ostk::astrodynamics::trajectory::state::coordinatesubset::CartesianPosition;
@@ -499,6 +497,92 @@ State State::inFrame(const Shared<const Frame>& aFrameSPtr) const
     };
 }
 
+std::unordered_map<Shared<const CoordinateSubset>, bool> State::isNear(
+    const State& aState, const std::unordered_map<Shared<const CoordinateSubset>, Real>& aToleranceMap
+) const
+{
+    if (aToleranceMap.size() != coordinatesBrokerSPtr_->accessSubsets().getSize())
+    {
+        throw ostk::core::error::runtime::Wrong(
+            "Tolerance Map: Expected size [" + std::to_string(coordinatesBrokerSPtr_->accessSubsets().getSize()) +
+            "], but received size [" + std::to_string(aToleranceMap.size()) + "]."
+        );
+    }
+
+    const State deltaState = aState - *this;
+
+    std::unordered_map<Shared<const CoordinateSubset>, bool> isNearMap;
+    for (const auto& [subsetSPtr, tolerance] : aToleranceMap)
+    {
+        if (!coordinatesBrokerSPtr_->hasSubset(subsetSPtr))
+        {
+            throw ostk::core::error::runtime::Wrong(
+                "Tolerance map doesn't contain coordinate subset [" + subsetSPtr->getName() + "]."
+            );
+        }
+        if (tolerance <= 0.0)
+        {
+            throw ostk::core::error::runtime::Wrong(
+                "Tolerance value for subset [" + subsetSPtr->getName() + "] is less than or equal to zero."
+            );
+        }
+
+        const VectorXd deltaCoordinates = deltaState.extractCoordinate(subsetSPtr);
+
+        isNearMap[subsetSPtr] = (deltaCoordinates.norm() <= tolerance);
+    }
+    return isNearMap;
+}
+
+std::unordered_map<Shared<const CoordinateSubset>, Array<bool>> State::isNear(
+    const State& aState, const std::unordered_map<Shared<const CoordinateSubset>, VectorXd>& aToleranceArrayMap
+) const
+{
+    if (aToleranceArrayMap.size() != coordinatesBrokerSPtr_->accessSubsets().getSize())
+    {
+        throw ostk::core::error::runtime::Wrong("Tolerance array map is not of the same size as the state.");
+    }
+
+    const State deltaState = aState - *this;
+
+    std::unordered_map<Shared<const CoordinateSubset>, Array<bool>> isNearMap;
+    for (const auto& [subsetSPtr, toleranceArray] : aToleranceArrayMap)
+    {
+        if (!coordinatesBrokerSPtr_->hasSubset(subsetSPtr))
+        {
+            throw ostk::core::error::runtime::Wrong(
+                "Tolerance array map doesn't contain coordinate subset [" + subsetSPtr->getName() + "]."
+            );
+        }
+
+        const VectorXd deltaCoordinates = deltaState.extractCoordinate(subsetSPtr);
+
+        if (toleranceArray.size() != deltaCoordinates.size())
+        {
+            throw ostk::core::error::runtime::Wrong(
+                "Tolerance Array Size: Expected [" + std::to_string(deltaCoordinates.size()) + "], but got [" +
+                std::to_string(toleranceArray.size()) + "]"
+            );
+        }
+        if ((toleranceArray.array() <= 0.0).any())
+        {
+            throw ostk::core::error::runtime::Wrong(
+                "One or more elements in the tolerance array for subset [" + subsetSPtr->getName() +
+                "] is less than or equal to zero."
+            );
+        }
+
+        Array<bool> isNearArray;
+        isNearArray.reserve(deltaCoordinates.size());
+        for (int i = 0; i < deltaCoordinates.size(); ++i)
+        {
+            isNearArray.add(std::abs(deltaCoordinates[i]) <= toleranceArray[i]);
+        }
+        isNearMap[subsetSPtr] = isNearArray;
+    }
+    return isNearMap;
+}
+
 void State::print(std::ostream& anOutputStream, bool displayDecorator) const
 {
     using ostk::core::type::String;