ale::States implementation (#314) (ad614ca6) · Commits · aflab / astrogeology / Ale

CMakeLists.txt

+2 −0

Original line number	Diff line number	Diff line
		@@ -33,10 +33,12 @@ set(ALE_INSTALL_INCLUDE_DIR "include/ale")
		add_library(ale SHARED
		${CMAKE_CURRENT_SOURCE_DIR}/src/ale.cpp
		${CMAKE_CURRENT_SOURCE_DIR}/src/Rotation.cpp
		${CMAKE_CURRENT_SOURCE_DIR}/src/States.cpp
		${CMAKE_CURRENT_SOURCE_DIR}/src/Isd.cpp
		${CMAKE_CURRENT_SOURCE_DIR}/src/Util.cpp)
		set(ALE_HEADERS "${ALE_BUILD_INCLUDE_DIR}/ale.h"
		"${ALE_BUILD_INCLUDE_DIR}/Rotation.h"
		"${ALE_BUILD_INCLUDE_DIR}/States.h"
		"${ALE_BUILD_INCLUDE_DIR}/Isd.h"
		"${ALE_BUILD_INCLUDE_DIR}/Util.h")
		set_target_properties(ale PROPERTIES

include/States.h

0 → 100644

+133 −0

Original line number	Diff line number	Diff line
		#ifndef ALE_STATES_H
		#define ALE_STATES_H

		#include<vector>
		#include <stdexcept>
		#include <gsl/gsl_interp.h>
		#include <ale.h>

		namespace ale {

		/** A 3D cartesian vector */
		struct Vec3d {
		double x;
		double y;
		double z;

		// Accepts an {x,y,z} vector
		Vec3d(const std::vector<double>& vec) {
		if (vec.size() != 3) {
		throw std::invalid_argument("Input vector must have 3 entries.");
		}
		x = vec[0];
		y = vec[1];
		z = vec[2];
		};

		Vec3d(double x, double y, double z) : x(x), y(y), z(z) {};
		Vec3d() : x(0.0), y(0.0), z(0.0) {};
		};

		/** A state vector with position and velocity*/
		struct State {
		Vec3d position;
		Vec3d velocity;

		// Accepts a {x, y, z, vx, vy, vz} vector
		State(const std::vector<double>& vec) {
		if (vec.size() != 6) {
		throw std::invalid_argument("Input vector must have 6 entries.");
		}
		position = {vec[0], vec[1], vec[2]};
		velocity = {vec[3], vec[4], vec[5]};
		};

		State(ale::Vec3d position, ale::Vec3d velocity) : position(position), velocity(velocity) {};
		State() {};
		};

		class States {
		public:
		// Constructors
		States();
		States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
		int refFrame=1);

		States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
		const std::vector<ale::Vec3d>& velocities, int refFrame=1);

		States(const std::vector<double>& ephemTimes, const std::vector<ale::State>& states,
		int refFrame=1);

		~States();

		// Getters
		std::vector<ale::State> getStates() const; //! Returns state vectors (6-element positions&velocities)
		std::vector<ale::Vec3d> getPositions() const; //! Returns the current positions
		std::vector<ale::Vec3d> getVelocities() const; //! Returns the current velocities
		std::vector<double> getTimes() const; //! Returns the current times
		int getReferenceFrame() const; //! Returns reference frame as NAIF ID
		bool hasVelocity() const; //! Returns true if any velocities have been provided

		/**
		* Returns a single state by interpolating state.
		* If the Cache has been minimized, a cubic hermite is used to interpolate the
		* position and velocity over the reduced cache.
		* If not, a standard gsl interpolation will be done.
		*
		* @param time Time to get a value at
		* @param interp Interpolation type to use. Will be ignored if cache is minimized.
		*
		* @return ale::State
		*/
		ale::State getState(double time, ale::interpolation interp=LINEAR) const;

		/ Gets a position at a single time. Operates the same way as getState() /
		ale::Vec3d getPosition(double time, ale::interpolation interp=LINEAR) const;

		/ Gets a velocity at a single time. Operates the same way as getState() /
		ale::Vec3d getVelocity(double time, ale::interpolation interp=LINEAR) const;

		/ Returns the first ephemeris time /
		double getStartTime();

		/ Returns the last ephemeris time /
		double getStopTime();

		/**
		* Perform a cache reduction. After running this, getStates(), getPositions(),
		* and getVelocities() will return vectors of reduced size, and getState(),
		* getPosition(), and getVelocity() will
		* returns values interpolated over the reduced cache using a cubic hermite spline
		*
		* Adapted from Isis::SpicePosition::reduceCache().
		*
		* @param tolerance Maximum error between hermite approximation and original value.
		*/
		States minimizeCache(double tolerance=0.01);

		private:

		/**
		* Calculates the points (indicies) which need to be kept for the hermite spline to
		* interpolate between to mantain a maximum error of tolerance.
		*
		* Adapted from Isis::SpicePosition::HermiteIndices.
		*
		* @param tolerance Maximum error between hermite approximation and original value.
		* @param indexList The list of indicies that need to be kept.
		* @param baseTime Scaled base time for fit
		* @param timeScale Time scale for fit.
		*
		* @return std::vector<int>
		*/
		std::vector<int> hermiteIndices(double tolerance, std::vector <int> indexList,
		double baseTime, double timeScale);
		std::vector<ale::State> m_states; //! Represent at states internally to keep pos, vel together
		std::vector<double> m_ephemTimes; //! Time in seconds
		int m_refFrame; //! Naif IDs for reference frames
		};
		}

		#endif

include/ale.h

+22 −3

Original line number	Diff line number	Diff line
		@@ -14,11 +14,30 @@ namespace ale {
		/// Interpolation enum for defining different methods of interpolation
		enum interpolation {
		/// Interpolate using linear interpolation
		LINEAR,
		LINEAR = 0,
		/// Interpolate using spline interpolation
		SPLINE
		SPLINE = 1,
		};


		// Temporarily moved interpolation and associated helper functions over from States. Will be
		// moved to interpUtils in the future.

		/** The following helper functions are used to calculate the reduced states cache and cubic hermite
		to interpolate over it. They were migrated, with minor modifications, from
		Isis::NumericalApproximation **/

		/** Determines the lower index for the interpolation interval. */
		int interpolationIndex(const std::vector<double> &times, double interpTime);

		/ Evaluates a cubic hermite at time, interpTime, between the appropriate two points in x. /
		double evaluateCubicHermite(const double interpTime, const std::vector<double>& derivs,
		const std::vector<double>& x, const std::vector<double>& y);

		/ Evaluate velocities using a Cubic Hermite Spline at a time a, within some interval in x, /
		double evaluateCubicHermiteFirstDeriv(const double interpTime, const std::vector<double>& deriv,
		const std::vector<double>& times, const std::vector<double>& y);

		/**
		*@brief Get the position of the spacecraft at a given time based on a set of coordinates, and their associated times
		*@param coords A vector of double vectors of coordinates

src/States.cpp

0 → 100644

+289 −0

Original line number	Diff line number	Diff line
		#include "States.h"

		#include <iostream>
		#include <algorithm>
		#include <gsl/gsl_interp.h>
		#include <gsl/gsl_spline.h>
		#include <gsl/gsl_poly.h>
		#include <cmath>
		#include <float.h>

		namespace ale {

		// Empty constructor
		States::States() : m_refFrame(0) {
		m_states = {};
		m_ephemTimes = {};
		}


		States::States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
		int refFrame) :
		m_ephemTimes(ephemTimes), m_refFrame(refFrame) {
		// Construct State vector from position and velocity vectors

		if (positions.size() != ephemTimes.size()) {
		throw std::invalid_argument("Length of times must match number of positions");
		}

		ale::Vec3d velocities = {0.0, 0.0, 0.0};
		for (ale::Vec3d position : positions) {
		m_states.push_back(ale::State(position, velocities));
		}
		}


		States::States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
		const std::vector<ale::Vec3d>& velocities, int refFrame) :
		m_ephemTimes(ephemTimes), m_refFrame(refFrame) {

		if ((positions.size() != ephemTimes.size())\|\|(ephemTimes.size() != velocities.size())) {
		throw std::invalid_argument("Length of times must match number of positions and velocities.");
		}

		for (int i=0; i < positions.size() ;i++) {
		m_states.push_back(ale::State(positions[i], velocities[i]));
		}
		}


		States::States(const std::vector<double>& ephemTimes, const std::vector<ale::State>& states,
		int refFrame) :
		m_ephemTimes(ephemTimes), m_states(states), m_refFrame(refFrame) {
		if (states.size() != ephemTimes.size()) {
		throw std::invalid_argument("Length of times must match number of states.");
		}
		}

		// Default Destructor
		States::~States() {}

		// Getters
		std::vector<ale::State> States::getStates() const {
		return m_states;
		}

		std::vector<ale::Vec3d> States::getPositions() const {
		// extract positions from state vector
		std::vector<ale::Vec3d> positions;

		for(ale::State state : m_states) {
		positions.push_back(state.position);
		}
		return positions;
		}


		std::vector<ale::Vec3d> States::getVelocities() const {
		// extract velocities from state vector
		std::vector<ale::Vec3d> velocities;

		for(ale::State state : m_states) {
		velocities.push_back(state.velocity);
		}
		return velocities;
		}


		std::vector<double> States::getTimes() const {
		return m_ephemTimes;
		}


		int States::getReferenceFrame() const {
		return m_refFrame;
		}


		bool States::hasVelocity() const {
		std::vector<ale::Vec3d> velocities = getVelocities();
		bool allZero = std::all_of(velocities.begin(), velocities.end(), [](ale::Vec3d vec)
		{ return vec.x==0.0 && vec.y==0.0 && vec.z==0.0; });
		return !allZero;
		}


		ale::State States::getState(double time, ale::interpolation interp) const {
		int lowerBound = ale::interpolationIndex(m_ephemTimes, time);
		int interpStart;
		int interpStop;

		if (m_ephemTimes.size() <= 3) {
		interpStart = 0;
		interpStop = m_ephemTimes.size() - 1;
		}
		else if (lowerBound == 0) {
		interpStart = lowerBound;
		interpStop = lowerBound + 3;
		}
		else if (lowerBound == m_ephemTimes.size() - 1) {
		interpStart = lowerBound - 3;
		interpStop = lowerBound;
		}
		else if (lowerBound == m_ephemTimes.size() - 2) {
		interpStart = lowerBound - 2;
		interpStop = lowerBound + 1;
		}
		else {
		interpStart = lowerBound - 1;
		interpStop = lowerBound + 2;
		}

		ale::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);
		}

		ale::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)};

		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);
		}
		double sTime = time - baseTime;
		position.x = ale::evaluateCubicHermite(sTime, vxs, scaledEphemTimes, xs);
		position.y = ale::evaluateCubicHermite(sTime, vys, scaledEphemTimes, ys);
		position.z = ale::evaluateCubicHermite(sTime, vzs, scaledEphemTimes, zs);

		velocity.x = ale::evaluateCubicHermiteFirstDeriv(sTime, vxs, scaledEphemTimes, xs);
		velocity.y = ale::evaluateCubicHermiteFirstDeriv(sTime, vys, scaledEphemTimes, ys);
		velocity.z = ale::evaluateCubicHermiteFirstDeriv(sTime, vzs, scaledEphemTimes, zs);
		}
		return ale::State(position, velocity);
		}


		ale::Vec3d States::getPosition(double time, ale::interpolation interp) const {
		ale::State interpState = getState(time, interp);
		return interpState.position;
		}


		ale::Vec3d States::getVelocity(double time, ale::interpolation interp) const {
		ale::State interpState = getState(time, interp);
		return interpState.velocity;
		}


		double States::getStartTime() {
		return m_ephemTimes[0];
		}


		double States::getStopTime() {
		int len = m_ephemTimes.size();
		return m_ephemTimes.back();
		}


		States States::minimizeCache(double tolerance) {
		if (m_states.size() <= 2) {
		throw std::invalid_argument("Cache size is 2, cannot minimize.");
		}
		if (!hasVelocity()) {
		throw std::invalid_argument("The cache can only be minimized if velocity is provided.");
		}

		// Compute scaled time to use for fitting.
		double baseTime = (m_ephemTimes.at(0) + m_ephemTimes.back())/ 2.0;
		double timeScale = 1.0;

		// Find current size of m_states
		int currentSize = m_ephemTimes.size() - 1;

		// Create 3 starting values for the new size-minimized cache
		std::vector <int> inputIndices;
		inputIndices.push_back(0);
		inputIndices.push_back(currentSize / 2);
		inputIndices.push_back(currentSize);

		// find all indices needed to make a hermite table within the appropriate tolerance
		std::vector <int> indexList = hermiteIndices(tolerance, inputIndices, baseTime, timeScale);

		// Update m_states and m_ephemTimes to only save the necessary indicies in the index list
		std::vector<ale::State> tempStates;
		std::vector<double> tempTimes;

		for(int i : indexList) {
		tempStates.push_back(m_states[i]);
		tempTimes.push_back(m_ephemTimes[i]);
		}
		return States(tempTimes, tempStates, m_refFrame);
		}

		std::vector<int> States::hermiteIndices(double tolerance, std::vector<int> indexList,
		double baseTime, double timeScale) {
		unsigned int n = indexList.size();
		double sTime;

		std::vector<double> x, y, z, vx, vy, vz, scaledEphemTimes;
		for(unsigned int i = 0; i < indexList.size(); i++) {
		scaledEphemTimes.push_back((m_ephemTimes[indexList[i]] - baseTime) / timeScale);
		x.push_back(m_states[indexList[i]].position.x);
		y.push_back(m_states[indexList[i]].position.y);
		z.push_back(m_states[indexList[i]].position.z);
		vx.push_back(m_states[i].velocity.x);
		vy.push_back(m_states[i].velocity.y);
		vz.push_back(m_states[i].velocity.z);
		}

		// loop through the saved indices from the end
		for(unsigned int i = indexList.size() - 1; i > 0; i--) {
		double xerror = 0;
		double yerror = 0;
		double zerror = 0;

		// check every value of the original kernel values within interval
		for(int line = indexList[i-1] + 1; line < indexList[i]; line++) {
		sTime = (m_ephemTimes[line] - baseTime) / timeScale;

		// find the errors at each value
		xerror = fabs(ale::evaluateCubicHermite(sTime, vx, scaledEphemTimes, x) - m_states[line].position.x);
		yerror = fabs(ale::evaluateCubicHermite(sTime, vy, scaledEphemTimes, y) - m_states[line].position.y);
		zerror = fabs(ale::evaluateCubicHermite(sTime, vz, scaledEphemTimes, z) - m_states[line].position.z);

		if(xerror > tolerance \|\| yerror > tolerance \|\| zerror > tolerance) {
		// if any error is greater than tolerance, no need to continue looking, break
		break;
		}
		}

		if(xerror < tolerance && yerror < tolerance && zerror < tolerance) {
		// if errors are less than tolerance after looping interval, no new point is necessary
		continue;
		}
		else {
		// if any error is greater than tolerance, add midpoint of interval to indexList vector
		indexList.push_back((indexList[i] + indexList[i-1]) / 2);
		}
		}

		if(indexList.size() > n) {
		std::sort(indexList.begin(), indexList.end());
		indexList = hermiteIndices(tolerance, indexList, baseTime, timeScale);
		}
		return indexList;
		}
		}

src/ale.cpp

+87 −0

Original line number	Diff line number	Diff line
		@@ -21,6 +21,93 @@ using namespace std;

		namespace ale {


		// Temporarily moved over from States.cpp. Will be moved into interpUtils in the future.

		/** The following helper functions are used to calculate the reduced states cache and cubic hermite
		to interpolate over it. They were migrated, with minor modifications, from
		Isis::NumericalApproximation **/

		/** Determines the lower index for the interpolation interval. */
		int interpolationIndex(const std::vector<double> &times, double interpTime) {
		if (times.size() < 2){
		throw std::invalid_argument("There must be at least two times.");
		}
		auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime);
		if (nextTimeIt == times.end()) {
		--nextTimeIt;
		}
		if (nextTimeIt != times.begin()) {
		--nextTimeIt;
		}
		return std::distance(times.begin(), nextTimeIt);
		}


		/ Evaluates a cubic hermite at time, interpTime, between the appropriate two points in x. /
		double evaluateCubicHermite(const double interpTime, const std::vector<double>& derivs,
		const std::vector<double>& x, const std::vector<double>& y) {
		if( (derivs.size() != x.size()) \|\| (derivs.size() != y.size()) ) {
		throw std::invalid_argument("EvaluateCubicHermite - The size of the first derivative vector does not match the number of (x,y) data points.");
		}

		// Find the interval in which "a" exists
		int lowerIndex = ale::interpolationIndex(x, interpTime);

		double x0, x1, y0, y1, m0, m1;
		// interpTime is contained within the interval (x0,x1)
		x0 = x[lowerIndex];
		x1 = x[lowerIndex+1];
		// the corresponding known y-values for x0 and x1
		y0 = y[lowerIndex];
		y1 = y[lowerIndex+1];
		// the corresponding known tangents (slopes) at (x0,y0) and (x1,y1)
		m0 = derivs[lowerIndex];
		m1 = derivs[lowerIndex+1];

		double h, t;
		h = x1 - x0;
		t = (interpTime - x0) / h;
		return (2 * t * t * t - 3 * t * t + 1) * y0 + (t * t * t - 2 * t * t + t) * h * m0 + (-2 * t * t * t + 3 * t * t) * y1 + (t * t * t - t * t) * h * m1;
		}

		/ Evaluate velocities using a Cubic Hermite Spline at a time a, within some interval in x, /
		double evaluateCubicHermiteFirstDeriv(const double interpTime, const std::vector<double>& deriv,
		const std::vector<double>& times, const std::vector<double>& y) {
		if(deriv.size() != times.size()) {
		throw std::invalid_argument("EvaluateCubicHermiteFirstDeriv - The size of the first derivative vector does not match the number of (x,y) data points.");
		}

		// find the interval in which "interpTime" exists
		int lowerIndex = ale::interpolationIndex(times, interpTime);

		double x0, x1, y0, y1, m0, m1;

		// interpTime is contained within the interval (x0,x1)
		x0 = times[lowerIndex];
		x1 = times[lowerIndex+1];

		// the corresponding known y-values for x0 and x1
		y0 = y[lowerIndex];
		y1 = y[lowerIndex+1];

		// the corresponding known tangents (slopes) at (x0,y0) and (x1,y1)
		m0 = deriv[lowerIndex];
		m1 = deriv[lowerIndex+1];

		double h, t;
		h = x1 - x0;
		t = (interpTime - x0) / h;
		if(h != 0.0) {
		return ((6 * t * t - 6 * t) * y0 + (3 * t * t - 4 * t + 1) * h * m0 + (-6 * t * t + 6 * t) * y1 + (3 * t * t - 2 * t) * h * m1) / h;

		}
		else {
		throw std::invalid_argument("Error in evaluating cubic hermite velocities, values at"
		"lower and upper indicies are exactly equal.");
		}
		}

		// Position Data Functions
		vector<double> getPosition(vector<vector<double>> coords, vector<double> times, double time,
		interpolation interp) {