feat: address feedback and add tests

Author: apaletta3

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

@@ -279,7 +279,9 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
 
         .def(
             "is_near",
-            &State::isNear,
+            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.
 
@@ -288,12 +290,30 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_State(pybind11::module& a
                     tolerance_map (dict[CoordinateSubset, float]): The tolerance map for the comparison.
 
                 Returns:
-                    bool: True if the states are near, False otherwise.
+                    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, 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,12 +241,23 @@ class State
     /// @return The transformed State
     State inFrame(const Shared<const Frame>& aFrameSPtr) const;
 
-    /// @brief Check if the State is within a given tolerance per subset of another State.
+    /// @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 True if the States are within the tolerance, false otherwise
-    bool isNear(const State& aState, const std::unordered_map<CoordinateSubset, Real>& aToleranceMap) const;
+    /// @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.
     ///

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,29 +497,75 @@ State State::inFrame(const Shared<const Frame>& aFrameSPtr) const
     };
 }
 
-bool State::isNear(const State& aState, const std::unordered_map<CoordinateSubset, Real>& aToleranceMap) 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())
+    if (aToleranceMap.size() != coordinatesBrokerSPtr_->accessSubsets().getSize())
     {
         throw ostk::core::error::runtime::Wrong("Tolerance Map");
     }
 
     const State deltaState = aState - *this;
 
-    bool isNear = true;
-    for (const auto& [subset, tolerance] : aToleranceMap) {
-        if (!coordinatesBrokerSPtr_->hasSubset(subset)) {
+    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 Coordinate Subset");
+        }
+        if (tolerance <= 0.0)
+        {
+            throw ostk::core::error::runtime::Wrong("Tolerance");
+        }
+
+        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 Map");
+    }
+
+    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 Map Coordinate Subset");
         }
 
-        const VectorXd deltaCoordinates = deltaState.extractCoordinate(subset);
+        const VectorXd deltaCoordinates = deltaState.extractCoordinate(subsetSPtr);
 
-        if (deltaCoordinates.norm() > tolerance) {
-            isNear = false;
-            break;
+        if (toleranceArray.size() != deltaCoordinates.size())
+        {
+            throw ostk::core::error::runtime::Wrong("Tolerance Array Size");
+        }
+        if ((toleranceArray.array() <= 0.0).any())
+        {
+            throw ostk::core::error::runtime::Wrong("Tolerance Array");
+        }
+
+        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 isNear;
+    return isNearMap;
 }
 
 void State::print(std::ostream& anOutputStream, bool displayDecorator) const

test/OpenSpaceToolkit/Astrodynamics/Trajectory/State.test.cpp

@@ -11,6 +11,7 @@
 #include <Global.test.hpp>
 
 using ostk::core::container::Array;
+using ostk::core::type::Real;
 using ostk::core::type::Shared;
 
 using ostk::mathematics::geometry::d3::transformation::rotation::Quaternion;
@@ -1323,6 +1324,149 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, InFrame)
     }
 }
 
+TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, isNear)
+{
+    const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC);
+    Shared<const Frame> frameSPtr = Frame::GCRF();
+    VectorXd coordinates(4);
+    coordinates << 0.0, 0.0, 0.0, 0.0;
+    VectorXd otherCoordinates(4);
+    otherCoordinates << -1.0, 0.0, 0.0, 0.1;
+    const Array<Shared<const CoordinateSubset>> subsets = {CartesianPosition::Default(), CoordinateSubset::Mass()};
+
+    const State state = {instant, coordinates, frameSPtr, subsets};
+    const State otherState = {instant, otherCoordinates, frameSPtr, subsets};
+
+    // Test success
+    {
+        const std::unordered_map<Shared<const CoordinateSubset>, Real> toleranceMap = {
+            {CartesianPosition::Default(), 2.0}, {CoordinateSubset::Mass(), 0.05}
+        };
+
+        const std::unordered_map<Shared<const CoordinateSubset>, bool> isNearMap = {
+            {CartesianPosition::Default(), true}, {CoordinateSubset::Mass(), false}
+        };
+        EXPECT_EQ(state.isNear(otherState, toleranceMap), isNearMap);
+    }
+    // Test failure wrong tolerance map
+    {
+        const std::unordered_map<Shared<const CoordinateSubset>, Real> toleranceMap = {
+            {CartesianPosition::Default(), 2.0}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceMap));
+    }
+
+    // Test failure wrong tolerance map coordinate subset
+    {
+        const std::unordered_map<Shared<const CoordinateSubset>, Real> toleranceMap = {
+            {CartesianPosition::Default(), 2.0}, {CoordinateSubset::SurfaceArea(), 0.05}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceMap));
+    }
+
+    // Test failure negative tolerance
+    {
+        const std::unordered_map<Shared<const CoordinateSubset>, Real> toleranceMap = {
+            {CartesianPosition::Default(), -2.0}, {CoordinateSubset::Mass(), -0.05}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceMap));
+    }
+}
+
+TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, isNearElementWise)
+{
+    const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC);
+    Shared<const Frame> frameSPtr = Frame::GCRF();
+    VectorXd coordinates(4);
+    coordinates << 0.0, 0.0, 0.0, 0.0;
+    VectorXd otherCoordinates(4);
+    otherCoordinates << -1.0, 2.0, 0.0, 0.1;
+    const Array<Shared<const CoordinateSubset>> subsets = {CartesianPosition::Default(), CoordinateSubset::Mass()};
+
+    const State state = {instant, coordinates, frameSPtr, subsets};
+    const State otherState = {instant, otherCoordinates, frameSPtr, subsets};
+
+    // Test success
+    {
+        VectorXd positionTolerance(3);
+        positionTolerance << 2.0, 0.5, 0.5;
+
+        VectorXd massTolerance(1);
+        massTolerance << 0.05;
+
+        const std::unordered_map<Shared<const CoordinateSubset>, VectorXd> toleranceArrayMap = {
+            {CartesianPosition::Default(), positionTolerance}, {CoordinateSubset::Mass(), massTolerance}
+        };
+
+        const std::unordered_map<Shared<const CoordinateSubset>, Array<bool>> isNearArrayMap = {
+            {CartesianPosition::Default(), Array<bool>({true, false, true})},
+            {CoordinateSubset::Mass(), Array<bool>({false})}
+        };
+
+        EXPECT_EQ(state.isNear(otherState, toleranceArrayMap), isNearArrayMap);
+    }
+
+    // Test failure wrong tolerance map
+    {
+        VectorXd positionTolerance(3);
+        positionTolerance << 2.0, 0.5, 0.5;
+
+        const std::unordered_map<Shared<const CoordinateSubset>, VectorXd> toleranceArrayMap = {
+            {CartesianPosition::Default(), positionTolerance}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceArrayMap));
+    }
+
+    // Test failure wrong tolerance map coordinate subset
+    {
+        VectorXd positionTolerance(3);
+        positionTolerance << 2.0, 0.5, 0.5;
+
+        VectorXd areaTolerance(1);
+        areaTolerance << 0.05;
+
+        const std::unordered_map<Shared<const CoordinateSubset>, VectorXd> toleranceArrayMap = {
+            {CartesianPosition::Default(), positionTolerance}, {CoordinateSubset::SurfaceArea(), areaTolerance}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceArrayMap));
+    }
+
+    // Test failure wrong tolerance array size
+    {
+        VectorXd positionTolerance(2);
+        positionTolerance << 2.0, 0.5;
+
+        VectorXd massTolerance(1);
+        massTolerance << 0.05;
+
+        const std::unordered_map<Shared<const CoordinateSubset>, VectorXd> toleranceArrayMap = {
+            {CartesianPosition::Default(), positionTolerance}, {CoordinateSubset::Mass(), massTolerance}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceArrayMap));
+    }
+
+    // Test failure negative tolerance
+    {
+        VectorXd positionTolerance(3);
+        positionTolerance << -2.0, 0.5, 0.5;
+
+        VectorXd massTolerance(1);
+        massTolerance << 0.05;
+
+        const std::unordered_map<Shared<const CoordinateSubset>, VectorXd> toleranceArrayMap = {
+            {CartesianPosition::Default(), positionTolerance}, {CoordinateSubset::Mass(), massTolerance}
+        };
+
+        EXPECT_ANY_THROW(state.isNear(otherState, toleranceArrayMap));
+    }
+}
+
 TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, Undefined)
 {
     {