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