From 9b7810aac14a3d4053c92a04818fdbfa2156ec70 Mon Sep 17 00:00:00 2001
From: kyle-cochran <kyle.ray.cochran@gmail.com>
Date: Tue, 10 Oct 2023 15:50:49 -0700
Subject: [PATCH] refactor: Use the built-in State method to extract
 coordinates (#235)

* chore: rebase conflicts

* feat: add bindings (and fix typo bug

* refactor: leverage State subsets extraction to simplify

* chore: remove artifact from rebase
---
 .../Astrodynamics/Trajectory/Propagator.hpp   |  2 --
 .../Astrodynamics/Trajectory/Propagator.cpp   | 29 ++++---------------
 2 files changed, 6 insertions(+), 25 deletions(-)

diff --git a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp
index f0f8bfb22..66cf2db4a 100644
--- a/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp
+++ b/include/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.hpp
@@ -232,8 +232,6 @@ class Propagator
     mutable NumericalSolver numericalSolver_;
 
     void registerDynamicsContext(const Shared<Dynamics>& aDynamicsSPtr);
-
-    NumericalSolver::StateVector extractCoordinatesFromState(const State& aState) const;
 };
 
 }  // namespace trajectory
diff --git a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp
index 7f1b68003..a2af230c2 100644
--- a/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp
+++ b/src/OpenSpaceToolkit/Astrodynamics/Trajectory/Propagator.cpp
@@ -145,8 +145,8 @@ State Propagator::calculateStateAt(const State& aState, const Instant& anInstant
 
     const State state = {
         aState.accessInstant(),
-        extractCoordinatesFromState(aState.inFrame(Propagator::IntegrationFrameSPtr)),
-        aState.accessFrame(),
+        aState.inFrame(Propagator::IntegrationFrameSPtr).extractCoordinates(this->coordinatesBrokerSPtr_->getSubsets()),
+        Propagator::IntegrationFrameSPtr,
         this->coordinatesBrokerSPtr_,
     };
 
@@ -170,8 +170,8 @@ NumericalSolver::ConditionSolution Propagator::calculateStateToCondition(
 
     const State state = {
         aState.accessInstant(),
-        extractCoordinatesFromState(aState.inFrame(Propagator::IntegrationFrameSPtr)),
-        aState.accessFrame(),
+        aState.inFrame(Propagator::IntegrationFrameSPtr).extractCoordinates(this->coordinatesBrokerSPtr_->getSubsets()),
+        Propagator::IntegrationFrameSPtr,
         this->coordinatesBrokerSPtr_,
     };
 
@@ -212,8 +212,8 @@ Array<State> Propagator::calculateStatesAt(const State& aState, const Array<Inst
 
     const State state = {
         aState.accessInstant(),
-        extractCoordinatesFromState(aState.inFrame(Propagator::IntegrationFrameSPtr)),
-        aState.accessFrame(),
+        aState.inFrame(Propagator::IntegrationFrameSPtr).extractCoordinates(this->coordinatesBrokerSPtr_->getSubsets()),
+        Propagator::IntegrationFrameSPtr,
         this->coordinatesBrokerSPtr_,
     };
 
@@ -327,23 +327,6 @@ void Propagator::registerDynamicsContext(const Shared<Dynamics>& aDynamicsSPtr)
     this->dynamicsContexts_.add({aDynamicsSPtr, readInfo, writeInfo});
 }
 
-NumericalSolver::StateVector Propagator::extractCoordinatesFromState(const State& aState) const
-{
-    const State state = aState.inFrame(Propagator::IntegrationFrameSPtr);
-
-    Index offset = 0;
-    NumericalSolver::StateVector extractedStateVector = NumericalSolver::StateVector(this->getNumberOfCoordinates());
-
-    for (const Shared<const CoordinatesSubset>& subset : this->coordinatesBrokerSPtr_->getSubsets())
-    {
-        const Size subsetSize = subset->getSize();
-        extractedStateVector.segment(offset, subsetSize) = state.extractCoordinates(subset);
-        offset += subsetSize;
-    }
-
-    return extractedStateVector;
-}
-
 }  // namespace trajectory
 }  // namespace astro
 }  // namespace ostk