From 0f2739f7edd936c5edc1be8c6cebbc4e1f65a1ee Mon Sep 17 00:00:00 2001
From: Kristin <kberry@usgs.gov>
Date: Fri, 19 Jun 2020 16:21:35 -0700
Subject: [PATCH] Update State::getState() to handle the case of one state
 (like a framing camera.) (#364)

* Update format of range conversion coefficients isd output to include a separate list for times. Also update driver, test data, and remove unneeded semicolons

* Fix documentaion error

* fix other documentation error

* Added the case of 1 state (1 position, time, and maybe velocity) to getState()

* Roll back ale version # and accidental json submodule deletion
---
 include/ale/InterpUtils.h    |  2 +-
 src/States.cpp               | 96 ++++++++++++++++++++----------------
 tests/ctests/StatesTests.cpp | 34 +++++++++++++
 3 files changed, 89 insertions(+), 43 deletions(-)

diff --git a/include/ale/InterpUtils.h b/include/ale/InterpUtils.h
index 3bb0e84..f0e9099 100644
--- a/include/ale/InterpUtils.h
+++ b/include/ale/InterpUtils.h
@@ -75,7 +75,7 @@ namespace ale {
                                      double time, int order=8);
 
    /**
-    *@brief Interpolates the spacecrafts position along a path generated from a set of points,
+    *@brief Interpolates the spacecraft's position along a path generated from a set of points,
               times, and a time of observation
     *@param points A double vector of points
     *@param times A double vector of times
diff --git a/src/States.cpp b/src/States.cpp
index 9a498cc..b63629d 100644
--- a/src/States.cpp
+++ b/src/States.cpp
@@ -111,54 +111,66 @@ namespace ale {
 
 
   State States::getState(double time, PositionInterpolation interp) const {
-    int lowerBound = interpolationIndex(m_ephemTimes, time);
-    // try to copy the surrounding 8 points as that's the most possibly needed
-    int interpStart = std::max(0, lowerBound - 3);
-    int interpStop = std::min(lowerBound + 4, (int) m_ephemTimes.size() - 1);
-
-    State state;
-    std::vector<double> xs, ys, zs, vxs, vys, vzs, interpTimes;
-
-    for (int i = interpStart; i <= interpStop; i++) {
-      state = m_states[i];
-      interpTimes.push_back(m_ephemTimes[i]);
-      xs.push_back(state.position.x);
-      ys.push_back(state.position.y);
-      zs.push_back(state.position.z);
-      vxs.push_back(state.velocity.x);
-      vys.push_back(state.velocity.y);
-      vzs.push_back(state.velocity.z);
-    }
+    if (m_ephemTimes.size() > 1) {
+      int lowerBound = interpolationIndex(m_ephemTimes, time); 
+      // try to copy the surrounding 8 points as that's the most possibly needed
+      int interpStart = std::max(0, lowerBound - 3);
+      int interpStop = std::min(lowerBound + 4, (int) m_ephemTimes.size() - 1);
+
+      State state;
+      std::vector<double> xs, ys, zs, vxs, vys, vzs, interpTimes;
+
+      for (int i = interpStart; i <= interpStop; i++) {
+        state = m_states[i];
+        interpTimes.push_back(m_ephemTimes[i]);
+        xs.push_back(state.position.x);
+        ys.push_back(state.position.y);
+        zs.push_back(state.position.z);
+        vxs.push_back(state.velocity.x);
+        vys.push_back(state.velocity.y);
+        vzs.push_back(state.velocity.z);
+      }
 
-    Vec3d position, velocity;
+      Vec3d position, velocity;
 
-    if ( interp == LINEAR || (interp == SPLINE && !hasVelocity())) {
-      position = {interpolate(xs,  interpTimes, time, interp, 0),
-                  interpolate(ys,  interpTimes, time, interp, 0),
-                  interpolate(zs,  interpTimes, time, interp, 0)};
+      if ( interp == LINEAR || (interp == SPLINE && !hasVelocity())) {
+        position = {interpolate(xs,  interpTimes, time, interp, 0),
+                    interpolate(ys,  interpTimes, time, interp, 0),
+                    interpolate(zs,  interpTimes, time, interp, 0)};
 
-      velocity = {interpolate(xs, interpTimes, time, interp, 1),
-                  interpolate(ys, interpTimes, time, interp, 1),
-                  interpolate(zs, interpTimes, time, interp, 1)};
-    }
-    else if (interp == SPLINE && hasVelocity()){
-      // Do hermite spline if velocities are available
-      double baseTime = (interpTimes.front() + interpTimes.back()) / 2;
+        velocity = {interpolate(xs, interpTimes, time, interp, 1),
+                    interpolate(ys, interpTimes, time, interp, 1),
+                    interpolate(zs, interpTimes, time, interp, 1)};
+      }
+      else if (interp == SPLINE && hasVelocity()){
+        // Do hermite spline if velocities are available
+        double baseTime = (interpTimes.front() + interpTimes.back()) / 2;
 
-      std::vector<double> scaledEphemTimes;
-      for(unsigned int i = 0; i < interpTimes.size(); i++) {
-        scaledEphemTimes.push_back(interpTimes[i] - baseTime);
+        std::vector<double> scaledEphemTimes;
+        for(unsigned int i = 0; i < interpTimes.size(); i++) {
+          scaledEphemTimes.push_back(interpTimes[i] - baseTime);
+        }
+        double sTime = time - baseTime;
+        position.x = evaluateCubicHermite(sTime, vxs, scaledEphemTimes, xs);
+        position.y = evaluateCubicHermite(sTime, vys, scaledEphemTimes, ys);
+        position.z = evaluateCubicHermite(sTime, vzs, scaledEphemTimes, zs);
+
+        velocity.x = evaluateCubicHermiteFirstDeriv(sTime, vxs, scaledEphemTimes, xs);
+        velocity.y = evaluateCubicHermiteFirstDeriv(sTime, vys, scaledEphemTimes, ys);
+        velocity.z = evaluateCubicHermiteFirstDeriv(sTime, vzs, scaledEphemTimes, zs);
       }
-      double sTime = time - baseTime;
-      position.x = evaluateCubicHermite(sTime, vxs, scaledEphemTimes, xs);
-      position.y = evaluateCubicHermite(sTime, vys, scaledEphemTimes, ys);
-      position.z = evaluateCubicHermite(sTime, vzs, scaledEphemTimes, zs);
-
-      velocity.x = evaluateCubicHermiteFirstDeriv(sTime, vxs, scaledEphemTimes, xs);
-      velocity.y = evaluateCubicHermiteFirstDeriv(sTime, vys, scaledEphemTimes, ys);
-      velocity.z = evaluateCubicHermiteFirstDeriv(sTime, vzs, scaledEphemTimes, zs);
+      return State(position, velocity);
+    }
+    else if (hasVelocity()) {
+      // Here we have: 1 state (1 time, 1 position, 1 velocity)
+      // x_f = x_i + v * (t_f - t-i)
+      Vec3d position = m_states[0].position + m_states[0].velocity*(time - m_ephemTimes[0]);
+      Vec3d velocity = m_states[0].velocity;
+      return State(position, velocity);
+    }
+    else { // Here we have: only 1 time and 1 state, so just return the only state.
+      return State(m_states[0]);
     }
-    return State(position, velocity);
   }
 
 
diff --git a/tests/ctests/StatesTests.cpp b/tests/ctests/StatesTests.cpp
index 56e8087..43b709f 100644
--- a/tests/ctests/StatesTests.cpp
+++ b/tests/ctests/StatesTests.cpp
@@ -313,6 +313,40 @@ TEST(StatesTest, minimizeCache_false) {
 }
 
 
+TEST(StatesTest, OneStateNoVelocity) {
+  std::vector<double> ephemTimes = {0.0};
+  std::vector<Vec3d> positions = {Vec3d(2.0, 3.0, 4.0)};
+
+  States testState(ephemTimes, positions);
+  State result = testState.getState(1.0);
+  EXPECT_NEAR(result.position.x, 2.0, 1e-5);
+  EXPECT_NEAR(result.position.y, 3.0, 1e-4);
+  EXPECT_NEAR(result.position.z, 4.0, 1e-5);
+}
+
+TEST(StatesTest, OneStateWithVelocity) {
+  std::vector<double> ephemTimes = {1.0};
+  std::vector<Vec3d> positions = {Vec3d(2.0, 3.0, 4.0)};
+  std::vector<Vec3d> velocities = {Vec3d(5.0, 6.0, -1.0)};
+
+  States testState(ephemTimes, positions, velocities);
+  State result = testState.getState(2.0);
+  EXPECT_NEAR(result.position.x, 7.0, 1e-5);
+  EXPECT_NEAR(result.position.y, 9.0, 1e-4);
+  EXPECT_NEAR(result.position.z, 3.0, 1e-5);
+  EXPECT_NEAR(result.velocity.x, 5.0, 1e-6);
+  EXPECT_NEAR(result.velocity.y, 6.0, 1e-6);
+  EXPECT_NEAR(result.velocity.z, -1.0, 1e-6);
+
+  result = testState.getState(-1.0);
+  EXPECT_NEAR(result.position.x, -8.0, 1e-5);
+  EXPECT_NEAR(result.position.y, -9.0, 1e-4);
+  EXPECT_NEAR(result.position.z, 6.0, 1e-5);
+  EXPECT_NEAR(result.velocity.x, 5.0, 1e-6);
+  EXPECT_NEAR(result.velocity.y, 6.0, 1e-6);
+  EXPECT_NEAR(result.velocity.z, -1.0, 1e-6);
+}
+
 // This test checks to see if the minimized cache looks identical to ISIS's minimized cache for
 // a Dawn IR image VIR_IR_1A_1_362681634_1 (located in dawnvir2isis's IR app test.)
 // Values were obtained by adding strategic couts to SpicePosition.cpp, running spiceinit, and
-- 
GitLab