diff --git a/src/States.cpp b/src/States.cpp index b63629df6a4194ba9cbe321c6f863c90afcf18ed..4c3d183c2ddbede88c463b1f9f870bb620342a88 100644 --- a/src/States.cpp +++ b/src/States.cpp @@ -111,6 +111,16 @@ namespace ale { State States::getState(double time, PositionInterpolation interp) const { + + + // If time is in times, don't need to interpolate! + auto candidate_time = std::lower_bound(m_ephemTimes.begin(), m_ephemTimes.end(), time); + + if ( (candidate_time != m_ephemTimes.end()) && (*candidate_time == time) ) { + int index = std::distance(m_ephemTimes.begin(), candidate_time); + return m_states[index]; + } + 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 @@ -132,7 +142,6 @@ namespace ale { } Vec3d position, velocity; - if ( interp == LINEAR || (interp == SPLINE && !hasVelocity())) { position = {interpolate(xs, interpTimes, time, interp, 0), interpolate(ys, interpTimes, time, interp, 0), @@ -145,7 +154,6 @@ namespace ale { 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); @@ -169,7 +177,7 @@ namespace ale { 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 m_states[0]; } } diff --git a/tests/ctests/StatesTests.cpp b/tests/ctests/StatesTests.cpp index 43b709f0d2b66c665595f8cf79548c1121ee5ba7..72ec4759360d644dbb580f8efcdde57fba6cc7e4 100644 --- a/tests/ctests/StatesTests.cpp +++ b/tests/ctests/StatesTests.cpp @@ -347,6 +347,31 @@ TEST(StatesTest, OneStateWithVelocity) { EXPECT_NEAR(result.velocity.z, -1.0, 1e-6); } +TEST(StatesTest, NoInterpNeeded) { + std::vector<double> ephemTimes = {0.0, 1.0, 2.0}; + + std::vector<Vec3d> positions = { + Vec3d(5.0, 3.0, 0.0), + Vec3d(6.5, 4.0, 1.0), + Vec3d(15.0, 4.0, 5.0)}; + + std::vector<Vec3d> velocities = { + Vec3d(5.0, 3.0, 0.0), + Vec3d(7.0, 6.0, 1.0), + Vec3d(15.0, 4.0, 5.0)}; + + + States testState(ephemTimes, positions, velocities); + State result = testState.getState(1.0); + EXPECT_NEAR(result.position.x, 6.5, 1e-5); + EXPECT_NEAR(result.position.y, 4.0, 1e-4); + EXPECT_NEAR(result.position.z, 1.0, 1e-5); + EXPECT_NEAR(result.velocity.x, 7.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