1e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#ifndef ANDROID_DVR_POSE_H_ 2e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#define ANDROID_DVR_POSE_H_ 3e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 4e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <private/dvr/eigen.h> 5e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 6e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkonamespace android { 7e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkonamespace dvr { 8e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 9e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Encapsulates a 3D pose (rotation and position). 10e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// 11e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @tparam T Data type for storing the position coordinate and rotation 12e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// quaternion. 13e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 14e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoclass Pose { 15e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko public: 16e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Creates identity pose. 17e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose() 18e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko : rotation_(Eigen::Quaternion<T>::Identity()), 19e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko position_(Eigen::Vector3<T>::Zero()) {} 20e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 21e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Initializes a pose with given rotation and position. 22e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // 23e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // rotation Initial rotation. 24e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // position Initial position. 25e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose(Eigen::Quaternion<T> rotation, Eigen::Vector3<T> position) 26e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko : rotation_(rotation), position_(position) {} 27e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 28e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko void Invert() { 29e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko rotation_ = rotation_.inverse(); 30e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko position_ = rotation_ * -position_; 31e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 32e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 33e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose Inverse() const { 34e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose result(*this); 35e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko result.Invert(); 36e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return result; 37e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 38e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 39e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Compute the composition of this pose with another, storing the result 40e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // in the current object 41e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko void ComposeInPlace(const Pose& other) { 42e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko position_ = position_ + rotation_ * other.position_; 43e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko rotation_ = rotation_ * other.rotation_; 44e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 45e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 46e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Computes the composition of this pose with another, and returns the result 47e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose Compose(const Pose& other) const { 48e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose result(*this); 49e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko result.ComposeInPlace(other); 50e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return result; 51e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 52e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 53e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Vector3<T> TransformPoint(const Eigen::Vector3<T>& v) const { 54e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return rotation_ * v + position_; 55e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 56e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 57e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Vector3<T> Transform(const Eigen::Vector3<T>& v) const { 58e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return rotation_ * v; 59e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 60e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 61e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose& operator*=(const Pose& other) { 62e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko ComposeInPlace(other); 63e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return *this; 64e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 65e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 66e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Pose operator*(const Pose& other) const { return Compose(other); } 67e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 68e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Gets the rotation of the 3D pose. 69e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Quaternion<T> GetRotation() const { return rotation_; } 70e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 71e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Gets the position of the 3D pose. 72e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Vector3<T> GetPosition() const { return position_; } 73e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 74e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Sets the rotation of the 3D pose. 75e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko void SetRotation(Eigen::Quaternion<T> rotation) { rotation_ = rotation; } 76e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 77e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Sets the position of the 3D pose. 78e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko void SetPosition(Eigen::Vector3<T> position) { position_ = position; } 79e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 80e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Gets a 4x4 matrix representing a transform from the reference space (that 81e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // the rotation and position of the pose are relative to) to the object space. 82e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::AffineMatrix<T, 4> GetObjectFromReferenceMatrix() const; 83e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 84e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // Gets a 4x4 matrix representing a transform from the object space to the 85e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // reference space (that the rotation and position of the pose are relative 86e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // to). 87e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::AffineMatrix<T, 4> GetReferenceFromObjectMatrix() const; 88e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 89e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko private: 90e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Quaternion<T> rotation_; 91e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::Vector3<T> position_; 92e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko}; 93e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 94e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 95e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoEigen::AffineMatrix<T, 4> Pose<T>::GetObjectFromReferenceMatrix() const { 96e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // The transfrom from the reference is the inverse of the pose. 97e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::AffineMatrix<T, 4> matrix(rotation_.inverse().toRotationMatrix()); 98e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return matrix.translate(-position_); 99e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 100e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 101e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 102e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoEigen::AffineMatrix<T, 4> Pose<T>::GetReferenceFromObjectMatrix() const { 103e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko // The transfrom to the reference. 104e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko Eigen::AffineMatrix<T, 4> matrix(rotation_.toRotationMatrix()); 105e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return matrix.pretranslate(position_); 106e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 107e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 108e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko//------------------------------------------------------------------------------ 109e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Type-specific typedefs. 110e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko//------------------------------------------------------------------------------ 111e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 112e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkousing Posef = Pose<float>; 113e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkousing Posed = Pose<double>; 114e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 115e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} // namespace dvr 116e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} // namespace android 117e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 118e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#endif // ANDROID_DVR_POSE_H_ 119