diff --git a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/StateBuilder.cpp b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/StateBuilder.cpp index cd81df008..7abdd43c7 100644 --- a/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/StateBuilder.cpp +++ b/bindings/python/src/OpenSpaceToolkitAstrodynamicsPy/Trajectory/StateBuilder.cpp @@ -14,6 +14,7 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_StateBuilder(pybind11::mo using ostk::astro::trajectory::State; using ostk::astro::trajectory::StateBuilder; using ostk::astro::trajectory::state::CoordinatesBroker; + using ostk::astro::trajectory::state::CoordinatesSubset; class_(aModule, "StateBuilder") @@ -27,16 +28,35 @@ inline void OpenSpaceToolkitAstrodynamicsPy_Trajectory_StateBuilder(pybind11::mo arg("frame"), arg("coordinates_broker") ) + .def(init(), arg("state")) .def(self == self) .def(self != self) + .def( + "__add__", + [](const StateBuilder& aStateBuilder, const Shared& aCoordinatesSubsetSPtr) + { + return aStateBuilder + aCoordinatesSubsetSPtr; + }, + is_operator() + ) + .def( + "__sub__", + [](const StateBuilder& aStateBuilder, const Shared& aCoordinatesSubsetSPtr) + { + return aStateBuilder - aCoordinatesSubsetSPtr; + }, + is_operator() + ) .def("__str__", &(shiftToString)) .def("__repr__", &(shiftToString)) .def("is_defined", &StateBuilder::isDefined) - .def("build_state", &StateBuilder::buildState) + .def("build", &StateBuilder::build) + .def("reduce", &StateBuilder::reduce) + .def("expand", &StateBuilder::expand) .def("get_coordinates_subsets", &StateBuilder::getCoordinatesSubsets) .def("get_frame", &StateBuilder::getFrame) diff --git a/bindings/python/test/trajectory/test_state_builder.py b/bindings/python/test/trajectory/test_state_builder.py index 84c956242..a6e31f1a3 100644 --- a/bindings/python/test/trajectory/test_state_builder.py +++ b/bindings/python/test/trajectory/test_state_builder.py @@ -38,17 +38,32 @@ def coordinates_subsets() -> list[CoordinatesSubset]: return [CartesianPosition.default(), CartesianVelocity.default()] +@pytest.fixture +def coordinates() -> list[float]: + return [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] + + @pytest.fixture def coordinates_broker(coordinates_subsets: list[CoordinatesSubset]) -> CoordinatesBroker: return CoordinatesBroker(coordinates_subsets) +@pytest.fixture +def state( + instant: Instant, + coordinates: list[float], + frame: Frame, + coordinates_broker: CoordinatesBroker, +) -> State: + return State(instant, coordinates, frame, coordinates_broker) + + @pytest.fixture def state_builder(frame: Frame, coordinates_broker: CoordinatesBroker) -> State: return StateBuilder(frame, coordinates_broker) -class TestState: +class TestStateBuilder: def test_broker_constructor( self, frame: Frame, @@ -69,17 +84,38 @@ def test_subsets_constructor( assert isinstance(builder, StateBuilder) assert builder.is_defined() + def test_state_constructor( + self, + state: State, + ): + builder = StateBuilder(state) + assert builder is not None + assert isinstance(builder, StateBuilder) + assert builder.is_defined() + def test_comparators(self, state_builder: StateBuilder): assert (state_builder == state_builder) is True assert (state_builder != state_builder) is False - def test_build_state( + def test_operators( + self, + state_builder: StateBuilder, + ): + added_builder: StateBuilder = state_builder + CoordinatesSubset.mass() + assert isinstance(added_builder, StateBuilder) + assert state_builder != added_builder + + subtracted_builder: StateBuilder = state_builder - CartesianPosition.default() + assert isinstance(subtracted_builder, StateBuilder) + assert state_builder != subtracted_builder + + def test_build( self, instant: Instant, state_builder: StateBuilder, ): coordinates = [1, 2, 3, 1, 2, 3] - state: State = state_builder.build_state(instant, coordinates) + state: State = state_builder.build(instant, coordinates) assert state is not None assert isinstance(state, State) @@ -89,6 +125,40 @@ def test_build_state( assert state.get_frame() == state_builder.get_frame() assert state.get_coordinates_subsets() == state_builder.get_coordinates_subsets() + def test_reduce( + self, + state: State, + ): + builder = StateBuilder(state.get_frame(), [CartesianPosition.default()]) + reduced_state: State = builder.reduce(state) + + assert isinstance(reduced_state, State) + assert state != reduced_state + + def test_expand( + self, + state: State, + ): + builder = StateBuilder( + state.get_frame(), + [ + CartesianPosition.default(), + CartesianVelocity.default(), + CoordinatesSubset.mass(), + ], + ) + default_state: State = State( + state.get_instant(), + [100], + state.get_frame(), + CoordinatesBroker([CoordinatesSubset.mass()]), + ) + expanded_state: State = builder.expand(state, default_state) + + assert isinstance(expanded_state, State) + assert state != expanded_state + assert default_state != expanded_state + def test_getters( self, state_builder: StateBuilder, diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinatesSubset.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinatesSubset.hpp index c7b2f732d..809699c39 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinatesSubset.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/State/CoordinatesSubset.hpp @@ -41,7 +41,7 @@ class CoordinatesSubset /// @brief Constructor /// /// The default CoordinatesSubset instance is frame-invariant and implements element-wise - /// addition/substraction. + /// addition/subtraction. /// @code /// CoordinateSubset coordinateSubset = {aName, aSize}; /// @endcode diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.hpp index 90abcbf28..b75042caa 100644 --- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.hpp +++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.hpp @@ -4,7 +4,9 @@ #define __OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder__ #include +#include #include +#include #include @@ -22,7 +24,9 @@ namespace astro namespace trajectory { +using ostk::core::types::Integer; using ostk::core::types::Shared; +using ostk::core::types::Size; using ostk::core::ctnr::Array; using ostk::math::obj::VectorXd; @@ -59,6 +63,12 @@ class StateBuilder StateBuilder(const Shared& aFrameSPtr, const Shared& aCoordinatesBrokerSPtr); + /// @brief Constructor. + /// + /// @param [in] aState The state to be used as a template + + StateBuilder(const State& aState); + /// @brief Equality operator. /// /// @param [in] aStateBuilder The StateBuilder to compare to @@ -73,6 +83,22 @@ class StateBuilder bool operator!=(const StateBuilder& aStateBuilder) const; + /// @brief Return a new StateBuilder with the additional CoordinatesSubset. + /// + /// @param [in] aCoordinatesSubsetSPtr The CoordinatesSubset to append + /// + /// @return A new StateBuilder + + const StateBuilder operator+(const Shared& aCoordinatesSubsetSPtr) const; + + /// @brief Return a new StateBuilder without the given CoordinatesSubset. + /// + /// @param [in] aCoordinatesSubsetSPtr The CoordinatesSubset to remove + /// + /// @return A new StateBuilder + + const StateBuilder operator-(const Shared& aCoordinatesSubsetSPtr) const; + /// @brief Stream insertion operator. /// /// @param [in] anOutputStream The output stream to insert into @@ -91,7 +117,22 @@ class StateBuilder /// /// @return A State linked to the Frame and Coordinates Broker of the StateBuilder - const State buildState(const Instant& anInstant, const VectorXd& aCoordinates) const; + const State build(const Instant& anInstant, const VectorXd& aCoordinates) const; + + /// @brief Produce a State with the CoordinatesSubsets specified by the StateBuilder. + /// + /// @param [in] aState the state from which the coordinates will be taken. + /// @return A State with the CoordinatesSubsets of the StateBuilder. + + const State reduce(const State& aState) const; + + /// @brief Produce a State with the CoordinatesSubsets specified by the StateBuilder. + /// + /// @param [in] aState the state from which the coordinates will be taken. + /// @param [in] defaultState the state from which missing coordinates will be taken. + /// @return A State with the CoordinatesSubsets of the StateBuilder. + + const State expand(const State& aState, const State& defaultState) const; /// @brief Accessor for the reference frame. /// diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/State.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/State.cpp index f562cad1a..4b14e2746 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/State.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/State.cpp @@ -101,6 +101,11 @@ bool State::operator==(const State& aState) const return false; } + if (this->getSize() != aState.getSize()) + { + return false; + } + for (const Shared& subset : this->coordinatesBrokerSPtr_->accessSubsets()) { if (!aState.coordinatesBrokerSPtr_->hasSubset(subset)) diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.cpp index 68c418fd5..2fcfaf25a 100644 --- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.cpp +++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.cpp @@ -27,6 +27,12 @@ StateBuilder::StateBuilder( { } +StateBuilder::StateBuilder(const State& aState) + : frameSPtr_(aState.accessFrame()), + coordinatesBrokerSPtr_(aState.accessCoordinatesBroker()) +{ +} + bool StateBuilder::operator==(const StateBuilder& aStateBuilder) const { if ((!this->isDefined()) || (!aStateBuilder.isDefined())) @@ -52,6 +58,48 @@ bool StateBuilder::operator!=(const StateBuilder& aStateBuilder) const return !((*this) == aStateBuilder); } +const StateBuilder StateBuilder::operator+(const Shared& aCoordinatesSubsetSPtr) const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("StateBuilder"); + } + + if (this->coordinatesBrokerSPtr_->hasSubset(aCoordinatesSubsetSPtr)) + { + throw ostk::core::error::RuntimeError("Duplicate CoordinatesSubset: [{}]", aCoordinatesSubsetSPtr->getName()); + } + + Array> expandedSubsets = coordinatesBrokerSPtr_->getSubsets(); + expandedSubsets.add(aCoordinatesSubsetSPtr); + + return StateBuilder(this->frameSPtr_, expandedSubsets); +} + +const StateBuilder StateBuilder::operator-(const Shared& aCoordinatesSubsetSPtr) const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("StateBuilder"); + } + + if (!this->coordinatesBrokerSPtr_->hasSubset(aCoordinatesSubsetSPtr)) + { + throw ostk::core::error::RuntimeError("Missing CoordinatesSubset: [{}]", aCoordinatesSubsetSPtr->getName()); + } + + Array> contractedSubsets = Array>::Empty(); + for (const auto& subset : this->coordinatesBrokerSPtr_->getSubsets()) + { + if (subset != aCoordinatesSubsetSPtr) + { + contractedSubsets.add(subset); + } + } + + return StateBuilder(this->frameSPtr_, contractedSubsets); +} + std::ostream& operator<<(std::ostream& anOutputStream, const StateBuilder& aStateBuilder) { aStateBuilder.print(anOutputStream); @@ -64,7 +112,7 @@ bool StateBuilder::isDefined() const return (this->frameSPtr_ != nullptr) && this->frameSPtr_->isDefined() && (this->coordinatesBrokerSPtr_ != nullptr); } -const State StateBuilder::buildState(const Instant& anInstant, const VectorXd& aCoordinates) const +const State StateBuilder::build(const Instant& anInstant, const VectorXd& aCoordinates) const { if (!this->isDefined()) { @@ -74,6 +122,107 @@ const State StateBuilder::buildState(const Instant& anInstant, const VectorXd& a return {anInstant, aCoordinates, this->frameSPtr_, this->coordinatesBrokerSPtr_}; } +const State StateBuilder::reduce(const State& aState) const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("StateBuilder"); + } + + if (!aState.isDefined()) + { + throw ostk::core::error::runtime::Undefined("State"); + } + + if (aState.accessFrame() != this->frameSPtr_) + { + throw ostk::core::error::runtime::Wrong("State Frame"); + } + + VectorXd coordinates = VectorXd(this->coordinatesBrokerSPtr_->getNumberOfCoordinates()); + Integer nextIndex = 0; + + for (const auto& subset : this->coordinatesBrokerSPtr_->getSubsets()) + { + if (!aState.accessCoordinatesBroker()->hasSubset(subset)) + { + throw ostk::core::error::RuntimeError("Missing CoordinatesSubset: [{}]", subset->getName()); + } + + const VectorXd subsetCoordinates = aState.extractCoordinates(subset); + coordinates.segment(nextIndex, subsetCoordinates.size()) = subsetCoordinates; + nextIndex += subsetCoordinates.size(); + } + + return this->build(aState.accessInstant(), coordinates); +} + +const State StateBuilder::expand(const State& aState, const State& defaultState) const +{ + if (!this->isDefined()) + { + throw ostk::core::error::runtime::Undefined("StateBuilder"); + } + + if (!aState.isDefined()) + { + throw ostk::core::error::runtime::Undefined("State"); + } + + if (aState.accessFrame() != this->frameSPtr_) + { + throw ostk::core::error::runtime::Wrong("State Frame"); + } + + if (!defaultState.isDefined()) + { + throw ostk::core::error::runtime::Undefined("Default State"); + } + + if (defaultState.accessFrame() != this->frameSPtr_) + { + throw ostk::core::error::runtime::Wrong("Default State Frame"); + } + + VectorXd coordinates = VectorXd(this->coordinatesBrokerSPtr_->getNumberOfCoordinates()); + Integer nextIndex = 0; + Integer nonDefaultSubsetDetections = 0; + + for (const auto& subset : this->coordinatesBrokerSPtr_->getSubsets()) + { + bool subsetDetected = false; + VectorXd subsetCoordinates; + + if (aState.accessCoordinatesBroker()->hasSubset(subset)) + { + subsetDetected = true; + nonDefaultSubsetDetections++; + subsetCoordinates = aState.extractCoordinates(subset); + } + + if (!subsetDetected && defaultState.accessCoordinatesBroker()->hasSubset(subset)) + { + subsetDetected = true; + subsetCoordinates = defaultState.extractCoordinates(subset); + } + + if (!subsetDetected) + { + throw ostk::core::error::RuntimeError("Missing CoordinatesSubset: [{}]", subset->getName()); + } + + coordinates.segment(nextIndex, subsetCoordinates.size()) = subsetCoordinates; + nextIndex += subsetCoordinates.size(); + } + + if (Size(nonDefaultSubsetDetections) != aState.accessCoordinatesBroker()->getNumberOfSubsets()) + { + throw ostk::core::error::RuntimeError("The operation is not an expansion"); + } + + return this->build(aState.accessInstant(), coordinates); +} + const Shared StateBuilder::accessFrame() const { if (!this->isDefined()) diff --git a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/AngularCondition.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/AngularCondition.test.cpp index d217c2643..6e556e57b 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/AngularCondition.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/AngularCondition.test.cpp @@ -61,7 +61,7 @@ class OpenSpaceToolkit_Astrodynamics_EventCondition_AngularCondition : public :: VectorXd coordinates(1); coordinates << coordinate; - return defaultStateBuilder_.buildState(Instant::J2000(), coordinates); + return defaultStateBuilder_.build(Instant::J2000(), coordinates); } }; diff --git a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/BooleanCondition.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/BooleanCondition.test.cpp index 9371c85be..2818defba 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/BooleanCondition.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/BooleanCondition.test.cpp @@ -65,7 +65,7 @@ class OpenSpaceToolkit_Astrodynamics_EventCondition_BooleanCondition : public :: VectorXd coordinates(1); coordinates << coordinate; - return defaultStateBuilder_.buildState(defaultInstant_, coordinates); + return defaultStateBuilder_.build(defaultInstant_, coordinates); } }; diff --git a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/RealCondition.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/RealCondition.test.cpp index c1bca020d..499708bd2 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/EventCondition/RealCondition.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/EventCondition/RealCondition.test.cpp @@ -60,7 +60,7 @@ class OpenSpaceToolkit_Astrodynamics_EventCondition_RealCondition : public ::tes VectorXd coordinates(1); coordinates << coordinate; - return defaultStateBuilder_.buildState(defaultInstant_, coordinates); + return defaultStateBuilder_.build(defaultInstant_, coordinates); } }; diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/State.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/State.test.cpp index 98d1a30bc..fc07a4e38 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/State.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/State.test.cpp @@ -122,6 +122,28 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, EqualToOperator) EXPECT_TRUE(aState == anotherState); } + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + + const Shared brokerSPtr1 = std::make_shared( + CoordinatesBroker({CartesianPosition::Default(), CartesianVelocity::Default()}) + ); + + const State aState = {instant, coordinates, Frame::GCRF(), brokerSPtr1}; + + VectorXd anotherCoordinates(7); + anotherCoordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 100.0; + const Shared brokerSPtr2 = std::make_shared( + CoordinatesBroker({CartesianPosition::Default(), CartesianVelocity::Default(), CoordinatesSubset::Mass()}) + ); + + const State anotherState = {instant, anotherCoordinates, Frame::GCRF(), brokerSPtr2}; + + EXPECT_FALSE(aState == anotherState); + } + { const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); VectorXd coordinates(6); @@ -346,6 +368,28 @@ TEST(OpenSpaceToolkit_Astrodynamics_Trajectory_State, NotEqualToOperator) EXPECT_FALSE(aState != anotherState); } + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + + const Shared brokerSPtr1 = std::make_shared( + CoordinatesBroker({CartesianPosition::Default(), CartesianVelocity::Default()}) + ); + + const State aState = {instant, coordinates, Frame::GCRF(), brokerSPtr1}; + + VectorXd anotherCoordinates(7); + anotherCoordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 100.0; + const Shared brokerSPtr2 = std::make_shared( + CoordinatesBroker({CartesianPosition::Default(), CartesianVelocity::Default(), CoordinatesSubset::Mass()}) + ); + + const State anotherState = {instant, anotherCoordinates, Frame::GCRF(), brokerSPtr2}; + + EXPECT_TRUE(aState != anotherState); + } + { const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); VectorXd coordinates(6); diff --git a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.test.cpp b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.test.cpp index 71ff97234..0be7f4391 100644 --- a/test/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.test.cpp +++ b/test/OpenSpaceToolkit/Astrodynamics/Trajectory/StateBuilder.test.cpp @@ -31,9 +31,11 @@ class OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder : public ::testing: }; const Array> posVelMassSubsets = { - CartesianPosition::Default(), - CartesianVelocity::Default(), - CoordinatesSubset::Mass(), + CartesianPosition::Default(), CartesianVelocity::Default(), CoordinatesSubset::Mass() + }; + + const Array> massPosSubsets = { + CoordinatesSubset::Mass(), CartesianPosition::Default() }; const Shared posVelBrokerSPtr = @@ -41,6 +43,9 @@ class OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder : public ::testing: const Shared posVelMassBrokerSPtr = std::make_shared(CoordinatesBroker(posVelMassSubsets)); + + const Shared massPosBrokerSPtr = + std::make_shared(CoordinatesBroker(massPosSubsets)); }; TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Constructor) @@ -52,6 +57,19 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Constructor) { EXPECT_NO_THROW(StateBuilder builder(Frame::GCRF(), posVelSubsets);); } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + const State state = State(instant, coordinates, Frame::GCRF(), posVelBrokerSPtr); + + EXPECT_NO_THROW(StateBuilder builder(state)); + } + + { + EXPECT_ANY_THROW(StateBuilder builder(State::Undefined())); + } } TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, EqualToOperator) @@ -208,6 +226,74 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, NotEqualToOperato } } +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, AdditionOperator) +{ + { + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelBrokerSPtr); + const StateBuilder expandedStateBuilder = stateBuilder + CoordinatesSubset::Mass(); + + EXPECT_FALSE(stateBuilder == expandedStateBuilder); + + const Array> subsets = stateBuilder.accessCoordinatesBroker()->accessSubsets(); + EXPECT_EQ(2, subsets.size()); + EXPECT_EQ(CartesianPosition::Default(), subsets[0]); + EXPECT_EQ(CartesianVelocity::Default(), subsets[1]); + + const Array> expandedSubsets = + expandedStateBuilder.accessCoordinatesBroker()->accessSubsets(); + EXPECT_EQ(3, expandedSubsets.size()); + EXPECT_EQ(CartesianPosition::Default(), expandedSubsets[0]); + EXPECT_EQ(CartesianVelocity::Default(), expandedSubsets[1]); + EXPECT_EQ(CoordinatesSubset::Mass(), expandedSubsets[2]); + } + + { + const StateBuilder stateBuilder = StateBuilder::Undefined(); + + EXPECT_ANY_THROW(stateBuilder + CartesianPosition::Default()); + } + + { + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder + CartesianPosition::Default()); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, SubtractionOperator) +{ + { + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + const StateBuilder contractedStateBuilder = stateBuilder - CartesianVelocity::Default(); + + EXPECT_FALSE(stateBuilder == contractedStateBuilder); + + const Array> subsets = stateBuilder.accessCoordinatesBroker()->accessSubsets(); + EXPECT_EQ(3, subsets.size()); + EXPECT_EQ(CartesianPosition::Default(), subsets[0]); + EXPECT_EQ(CartesianVelocity::Default(), subsets[1]); + EXPECT_EQ(CoordinatesSubset::Mass(), subsets[2]); + + const Array> contractedSubsets = + contractedStateBuilder.accessCoordinatesBroker()->accessSubsets(); + EXPECT_EQ(2, contractedSubsets.size()); + EXPECT_EQ(CartesianPosition::Default(), contractedSubsets[0]); + EXPECT_EQ(CoordinatesSubset::Mass(), contractedSubsets[1]); + } + + { + const StateBuilder stateBuilder = StateBuilder::Undefined(); + + EXPECT_ANY_THROW(stateBuilder - CartesianPosition::Default()); + } + + { + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder - CoordinatesSubset::Mass()); + } +} + TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, StreamOperator) { { @@ -229,7 +315,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, StreamOperator) } } -TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, BuildState) +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Build) { { const StateBuilder stateBuilder = {Frame::GCRF(), posVelBrokerSPtr}; @@ -238,7 +324,7 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, BuildState) VectorXd coordinates(6); coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; - const State state = stateBuilder.buildState(instant, coordinates); + const State state = stateBuilder.build(instant, coordinates); EXPECT_EQ(instant, state.accessInstant()); EXPECT_EQ(coordinates, state.accessCoordinates()); @@ -256,13 +342,13 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, BuildState) VectorXd coordinates1(6); coordinates1 << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; - const State state1 = stateBuilder.buildState(instant1, coordinates1); + const State state1 = stateBuilder.build(instant1, coordinates1); const Instant instant2 = Instant::DateTime(DateTime(2019, 1, 1, 0, 0, 0), Scale::UTC); VectorXd coordinates2(6); coordinates2 << 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; - const State state2 = stateBuilder.buildState(instant2, coordinates2); + const State state2 = stateBuilder.build(instant2, coordinates2); EXPECT_NE(state1.accessInstant(), state2.accessInstant()); EXPECT_NE(state1.accessCoordinates(), state2.accessCoordinates()); @@ -278,15 +364,211 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, BuildState) VectorXd coordinates(3); coordinates << 1.0, 2.0, 3.0; - EXPECT_ANY_THROW(stateBuilder.buildState(instant, coordinates)); + EXPECT_ANY_THROW(stateBuilder.build(instant, coordinates)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + + EXPECT_ANY_THROW(StateBuilder::Undefined().build(instant, coordinates)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Reduce) +{ + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(7); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 100.0; + const State aState = State(instant, coordinates, Frame::GCRF(), posVelMassBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), massPosBrokerSPtr); + + const State anotherState = stateBuilder.reduce(aState); + + VectorXd expectedAnotherCoordinates(4); + expectedAnotherCoordinates << 100.0, 1.0, 2.0, 3.0; + + EXPECT_FALSE(aState == anotherState); + EXPECT_EQ(aState.accessInstant(), anotherState.accessInstant()); + EXPECT_EQ(aState.accessFrame(), anotherState.accessFrame()); + EXPECT_EQ(coordinates, aState.accessCoordinates()); + EXPECT_EQ(expectedAnotherCoordinates, anotherState.accessCoordinates()); + } + + { + const State aState = State::Undefined(); + + const StateBuilder stateBuilder = StateBuilder(Frame::ITRF(), massPosBrokerSPtr); // Undefined + + EXPECT_ANY_THROW(stateBuilder.reduce(aState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(7); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 100.0; + const State aState = State(instant, coordinates, Frame::GCRF(), posVelMassBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder::Undefined(); // Undefined + + EXPECT_ANY_THROW(stateBuilder.reduce(aState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(7); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 100.0; + const State aState = State(instant, coordinates, Frame::GCRF(), posVelMassBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::ITRF(), massPosBrokerSPtr); // Different Frame + + EXPECT_ANY_THROW(stateBuilder.reduce(aState)); } { const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); VectorXd coordinates(6); coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + const State aState = State(instant, coordinates, Frame::GCRF(), posVelBrokerSPtr); // Missing Mass - EXPECT_ANY_THROW(StateBuilder::Undefined().buildState(instant, coordinates)); + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), massPosBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder.reduce(aState)); + } +} + +TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Expand) +{ + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::GCRF(), massPosBrokerSPtr); + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + + const State anotherState = stateBuilder.expand(aState, defaultState); + + VectorXd expectedAnotherCoordinates(7); + expectedAnotherCoordinates << 1.0, 2.0, 3.0, -4.0, -5.0, -6.0, 100.0; + + EXPECT_FALSE(aState == anotherState); + EXPECT_FALSE(defaultState == anotherState); + EXPECT_EQ(aState.accessInstant(), anotherState.accessInstant()); + EXPECT_EQ(aState.accessFrame(), anotherState.accessFrame()); + EXPECT_EQ(coordinates, aState.accessCoordinates()); + EXPECT_EQ(defaultCoordinates, defaultState.accessCoordinates()); + EXPECT_EQ(expectedAnotherCoordinates, anotherState.accessCoordinates()); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + const State aState = State::Undefined(); // Undefined + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::GCRF(), massPosBrokerSPtr); + + const State defaultState = State::Undefined(); // Undefined + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::GCRF(), massPosBrokerSPtr); + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder::Undefined(); // Undefined + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::ITRF(), massPosBrokerSPtr); // Different Frame + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::GCRF(), massPosBrokerSPtr); + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = + State(instant, defaultCoordinates, Frame::ITRF(), posVelBrokerSPtr); // Different Frame + + const StateBuilder stateBuilder = StateBuilder(Frame::GCRF(), posVelMassBrokerSPtr); + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(4); + coordinates << 100.0, 1.0, 2.0, 3.0; + const State aState = State(instant, coordinates, Frame::GCRF(), massPosBrokerSPtr); + + VectorXd defaultCoordinates(6); + defaultCoordinates << -1.0, -2.0, -3.0, -4.0, -5.0, -6.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = StateBuilder(Frame::ITRF(), posVelMassBrokerSPtr); // Different Frame + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); + } + + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + const State aState = State(instant, coordinates, Frame::GCRF(), posVelBrokerSPtr); + + VectorXd defaultCoordinates(4); + defaultCoordinates << 100.0, -1.0, -2.0, -3.0; + const State defaultState = State(instant, defaultCoordinates, Frame::GCRF(), massPosBrokerSPtr); + + const StateBuilder stateBuilder = + StateBuilder(Frame::GCRF(), massPosBrokerSPtr); // Not an expansion (missing velocity) + + EXPECT_ANY_THROW(stateBuilder.expand(aState, defaultState)); } } @@ -306,6 +588,17 @@ TEST_F(OpenSpaceToolkit_Astrodynamics_Trajectory_StateBuilder, Accessors) EXPECT_EQ(posVelSubsets, stateBuilder.accessCoordinatesBroker()->getSubsets()); } + { + const Instant instant = Instant::DateTime(DateTime(2018, 1, 1, 0, 0, 0), Scale::UTC); + VectorXd coordinates(6); + coordinates << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + const State state = State(instant, coordinates, Frame::GCRF(), posVelBrokerSPtr); + + const StateBuilder stateBuilder = {state}; + EXPECT_EQ(Frame::GCRF(), stateBuilder.accessFrame()); + EXPECT_EQ(posVelBrokerSPtr, stateBuilder.accessCoordinatesBroker()); + } + { EXPECT_ANY_THROW(StateBuilder::Undefined().accessFrame()); EXPECT_ANY_THROW(StateBuilder::Undefined().accessCoordinatesBroker());