Commit 33caa804 authored by acpaquette's avatar acpaquette Committed by GitHub
Browse files

Orientation (#343) 



* Added first pass orientations class

* Added stub Orientations tests

* More Orientation tests and fixed interpUtils

* Fixed new enum name

* minimal docs

* Added vec merge

* Added Orientation multiplications

* Moved Vec3d struct into utils

* Removed interpolationIndex from ale.cpp/h as it's now located in interpUtils

* Updated Orientation/Rotation to use State and Vec3d structs

* Updated associated tests

* Updated things based on PR feedback

Co-authored-by: default avatarJesse Mapel <jmapel@usgs.gov>
parent f2879b36

CMakeLists.txt

+4 −0
Original line number Diff line number Diff line
@@ -32,12 +32,16 @@ set(ALE_BUILD_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include/") 
set(ALE_INSTALL_INCLUDE_DIR "include/ale")

add_library(ale SHARED

            ${CMAKE_CURRENT_SOURCE_DIR}/src/ale.cpp

            ${CMAKE_CURRENT_SOURCE_DIR}/src/InterpUtils.cpp

            ${CMAKE_CURRENT_SOURCE_DIR}/src/Rotation.cpp

            ${CMAKE_CURRENT_SOURCE_DIR}/src/Orientations.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}/InterpUtils.h"

                "${ALE_BUILD_INCLUDE_DIR}/Rotation.h"

                "${ALE_BUILD_INCLUDE_DIR}/Orientations.h"

                "${ALE_BUILD_INCLUDE_DIR}/States.h"

                "${ALE_BUILD_INCLUDE_DIR}/Isd.h"

                "${ALE_BUILD_INCLUDE_DIR}/Util.h")

cmake/gtest.cmake

+21 −1
Original line number Diff line number Diff line
@@ -17,3 +17,23 @@ if (NOT TARGET gtest) 


  add_library(gtest ${GOOGLETEST_SOURCES})

endif()



if (NOT TARGET gmock)

  set(GOOGLEMOCK_ROOT googletest/googlemock CACHE STRING "Google Mock source root")



  include_directories(SYSTEM

      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}

      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/include

      )



  set(GOOGLEMOCK_SOURCES

      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/src/gmock-all.cc

      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/src/gmock_main.cc

      )



  foreach(_source ${GOOGLEMOCK_SOURCES})

      set_source_files_properties(${_source} PROPERTIES GENERATED 1)

  endforeach()



  add_library(gmock ${GOOGLEMOCK_SOURCES})

endif()

include/InterpUtils.h

0 → 100644
+45 −0
Original line number Diff line number Diff line 
#ifndef ALE_INCLUDE_INTERP_UTILS_H

#define ALE_INCLUDE_INTERP_UTILS_H



#include <vector>



#include "Util.h"



namespace ale {

  /**

   * Linearly interpolate between two values.

   * @param x The first value.

   * @param y The second value.

   * @param t The distance to interpolate. 0 is x and 1 is y.

   */

  double linearInterpolate(double x, double y, double t);



  /**

   * Linearly interpolate between two vectors.

   * @param x The first vectors.

   * @param y The second vectors.

   * @param t The distance to interpolate. 0 is x and 1 is y.

   */

  std::vector<double> linearInterpolate(const std::vector<double> &x, const std::vector<double> &y, double t);



  ale::Vec3d linearInterpolate(const ale::Vec3d &x, const ale::Vec3d &y, double t);



  /**

   * Compute the index of the first time to use when interpolating at a given time.

   * @param times The ordered vector of times to search. Must have at least 2 times.

   * @param interpTime The time to search for the interpolation index of.

   * @return int The index of the time that comes before interpTime. If there is

   *             no time that comes before interpTime, then returns 0. If all

   *             times come before interpTime, then returns the second to last

   *             index.

   */

  int interpolationIndex(const std::vector<double> &times, double interpTime);



  /**

   * Merge, sort, and remove duplicates from two vectors

   */

   std::vector<double> orderedVecMerge(const std::vector<double> &x, const std::vector<double> &y);



}



#endif

include/Orientations.h

0 → 100644
+81 −0
Original line number Diff line number Diff line 
#ifndef ALE_ORIENTATIONS_H

#define ALE_ORIENTATIONS_H



#include <vector>



#include "Rotation.h"



namespace ale {

  class Orientations {

  public:

    /**

     * Construct a default empty orientation object

     */

    Orientations() {};

    /**

     * Construct an orientation object give a set of rotations

     * and optionally angular velocities at specific times.

     */

    Orientations(

      const std::vector<Rotation> &rotations,

      const std::vector<double> &times,

      const std::vector<Vec3d> &avs = std::vector<ale::Vec3d>()

    );

    /**

     * Orientations destructor

     */

    ~Orientations() {};



    /**

     * Const accessor methods

     */

    std::vector<Rotation> rotations() const;

    std::vector<ale::Vec3d> angularVelocities() const;

    std::vector<double> times() const;



    /**

     * Get the interpolated rotation at a specific time.

     */

    Rotation interpolate(

      double time,

      RotationInterpolation interpType=SLERP

    ) const;

    /**

     * Get the interpolated angular velocity at a specific time

     */

    ale::Vec3d interpolateAV(double time) const;



    ale::Vec3d rotateVectorAt(

      double time,

      const ale::Vec3d &vector,

      RotationInterpolation interpType=SLERP,

      bool invert=false

    ) const;



    /**

     * Rotate a position or state vector at a specific time

     */

    ale::State rotateStateAt(

      double time,

      const ale::State &state,

      RotationInterpolation interpType=SLERP,

      bool invert=false

    ) const;



    /**

     * Multiply this orientation by another orientation

     */

    Orientations &operator*=(const Orientations &rhs);

    /**

     * Multiply this orientation by a constant rotation

     */

    Orientations &operator*=(const Rotation &rhs);



  private:

    std::vector<Rotation> m_rotations;

    std::vector<ale::Vec3d> m_avs;

    std::vector<double> m_times;

  };

}



#endif

include/Rotation.h

+8 −3
Original line number Diff line number Diff line
@@ -4,6 +4,9 @@ 
#include <memory>

#include <vector>



#include "States.h"

#include "Util.h"



namespace ale {



  enum RotationInterpolation {
@@ -79,7 +82,7 @@ namespace ale { 
       *

       * @return The state rotation matrix in row-major order.

       */

      std::vector<double> toStateRotationMatrix(const std::vector<double> &av) const;

      std::vector<double> toStateRotationMatrix(const ale::Vec3d &av) const;

      /**

       * The rotation as Euler angles.

       *
@@ -99,12 +102,14 @@ namespace ale { 
      /**

       * Rotate a vector

       *

       * @param vector The vector to rotate. Cab be a 3 element position or 6 element state.

       * @param vector The vector to rotate. Can be a 3 element position or 6 element state.

       * @param av The angular velocity to use when rotating state vectors. Defaults to 0.

       *

       * @return The rotated vector.

       */

      std::vector<double> operator()(const std::vector<double>& vector, const std::vector<double>& av = {0.0, 0.0, 0.0}) const;

      ale::Vec3d operator()(const ale::Vec3d &av) const;



      ale::State operator()(const ale::State &state, const ale::Vec3d& av = Vec3d(0.0, 0.0, 0.0)) const;

      /**

       * Get the inverse rotation.

       */