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