ale::States implementation (#314)

* Adds an initial header file for the States

* Small updates to State header re: disucssion about not including units

* Adds most of an implementation and tests for just the constructors

* Actually add tests

* Adding tests for all except reduceCache

* Adding in tests for minimizeCache

* Add rough draft of minimizecache to states

* Update error throws and a bit more cleanup

* Clean up weird way I was creating vectors of states for tests

* Restore Lauren's test data values

* Add tests to bring test coverage up

* Fix tests I didn't test before pushing up

* Update for review comments 1

* Make other updates based on review comments

* Update to make minimizeCache return a new States object and change a lot of function signatures to use const references for vectors

* Move interpolation-related utils out of States into ale.h and update tests appropriately

* Updated state interpolation to reduce needed times

* Changed the position and velocities in the test fixture to values from functions

* Updated the spline interpolation to use the interpolation subset

* Removed parenthese again

* Added tests for vector size checks

* Addressed PR feedback

* Removed the interpolateState function as it was a copy of interpolate function

* Updated States to use interpolate and not interpolateState

* Removed interpolateState tests

* Updated state tests

* Removed interpolateState from ale header

* Updated interpolation tests

Co-authored-by: Adoram-Kershner <ladoramkershner@igswzawglt0046.gs.doi.net>
Co-authored-by: Jesse Mapel <jam826@nau.edu>
Co-authored-by: Adam Paquette <acpaquette@usgs.gov>
Co-authored-by: Jesse Mapel <jmapel@usgs.gov>

CMakeLists.txt

@@ -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

+#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

@@ -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

+#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

@@ -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) {

tests/ctests/AleTest.cpp

@@ -347,8 +347,14 @@ TEST(VelocityInterpTest, InvalidTime) {
   EXPECT_THROW(ale::getVelocity(coords, times, 3, ale::LINEAR), invalid_argument);
 }
 
-TEST(Interpolation, Derivative2)
-{
+TEST(Interpolation, Derivative1) {
+  vector<double> points = {0, 2, 4};
+  vector<double> times = {0, 1, 2};
+
+  EXPECT_DOUBLE_EQ(ale::interpolate(points, times, 1, ale::LINEAR, 1), 2);
+}
+
+TEST(Interpolation, Derivative2) {
   //only checks that case 2 of the switch block is hit
   vector<double> points = {0, 0, 0};
   vector<double> times = {0, 1, 2};
@@ -362,3 +368,53 @@ TEST(Interpolation, InvalidDerivative) {
 
   EXPECT_THROW(ale::interpolate(points, times, 1, ale::LINEAR, 3), invalid_argument);
 }
+
+TEST(InterpIndex, InvalidTimes) {
+  std::vector<double> times = {};
+
+  EXPECT_THROW(ale::interpolationIndex(times, 0), invalid_argument);
+}
+
+TEST(EvaluateCubicHermite, SimplyPolynomial) {
+  // Cubic function is y = x^3 - 2x^2 + 1
+  // derivative is dy/dx = 3x^2 - 4x
+  std::vector<double> derivs = {7.0, -1.0};
+  std::vector<double> x = {-1.0, 1.0};
+  std::vector<double> y = {-2.0, 0.0};
+
+  EXPECT_DOUBLE_EQ(ale::evaluateCubicHermite(0.0, derivs, x, y), 1.0);
+}
+
+TEST(EvaluateCubicHermite, InvalidDervisXY) {
+  std::vector<double> derivs = {};
+  std::vector<double> x = {1.0};
+  std::vector<double> y = {1.0};
+
+  EXPECT_THROW(ale::evaluateCubicHermite(0.0, derivs, x, y), invalid_argument);
+}
+
+TEST(EvaluateCubicHermiteFirstDeriv, SimplyPolynomial) {
+  // Cubic function is y = x^3 - 2x^2 + 1
+  // derivative is dy/dx = 3x^2 - 4x
+  std::vector<double> derivs = {7.0, -1.0};
+  std::vector<double> x = {-1.0, 1.0};
+  std::vector<double> y = {-2.0, 0.0};
+
+  EXPECT_DOUBLE_EQ(ale::evaluateCubicHermiteFirstDeriv(0.5, derivs, x, y), -1.25);
+}
+
+TEST(EvaluateCubicHermiteFirstDeriv, InvalidDervisTimes) {
+  std::vector<double> derivs = {};
+  std::vector<double> times = {1.0};
+  std::vector<double> y = {1.0};
+
+  EXPECT_THROW(ale::evaluateCubicHermiteFirstDeriv(0.0, derivs, times, y), invalid_argument);
+}
+
+TEST(EvaluateCubicHermiteFirstDeriv, InvalidVelocities) {
+  std::vector<double> derivs = {5.0, 6.0};
+  std::vector<double> times = {1.0, 1.0};
+  std::vector<double> y = {1.0};
+
+  EXPECT_THROW(ale::evaluateCubicHermiteFirstDeriv(0.0, derivs, times, y), invalid_argument);
+}