blob: 97944e8928b6b64a57e42bf0ff36b64bf1671711 [file] [log] [blame]
 #ifndef ANDROID_DVR_POSE_H_ #define ANDROID_DVR_POSE_H_ #include namespace android { namespace dvr { // Encapsulates a 3D pose (rotation and position). // // @tparam T Data type for storing the position coordinate and rotation // quaternion. template class Pose { public: // Creates identity pose. Pose() : rotation_(Eigen::Quaternion::Identity()), position_(Eigen::Vector3::Zero()) {} // Initializes a pose with given rotation and position. // // rotation Initial rotation. // position Initial position. Pose(Eigen::Quaternion rotation, Eigen::Vector3 position) : rotation_(rotation), position_(position) {} void Invert() { rotation_ = rotation_.inverse(); position_ = rotation_ * -position_; } Pose Inverse() const { Pose result(*this); result.Invert(); return result; } // Compute the composition of this pose with another, storing the result // in the current object void ComposeInPlace(const Pose& other) { position_ = position_ + rotation_ * other.position_; rotation_ = rotation_ * other.rotation_; } // Computes the composition of this pose with another, and returns the result Pose Compose(const Pose& other) const { Pose result(*this); result.ComposeInPlace(other); return result; } Eigen::Vector3 TransformPoint(const Eigen::Vector3& v) const { return rotation_ * v + position_; } Eigen::Vector3 Transform(const Eigen::Vector3& v) const { return rotation_ * v; } Pose& operator*=(const Pose& other) { ComposeInPlace(other); return *this; } Pose operator*(const Pose& other) const { return Compose(other); } // Gets the rotation of the 3D pose. Eigen::Quaternion GetRotation() const { return rotation_; } // Gets the position of the 3D pose. Eigen::Vector3 GetPosition() const { return position_; } // Sets the rotation of the 3D pose. void SetRotation(Eigen::Quaternion rotation) { rotation_ = rotation; } // Sets the position of the 3D pose. void SetPosition(Eigen::Vector3 position) { position_ = position; } // Gets a 4x4 matrix representing a transform from the reference space (that // the rotation and position of the pose are relative to) to the object space. Eigen::AffineMatrix GetObjectFromReferenceMatrix() const; // Gets a 4x4 matrix representing a transform from the object space to the // reference space (that the rotation and position of the pose are relative // to). Eigen::AffineMatrix GetReferenceFromObjectMatrix() const; private: Eigen::Quaternion rotation_; Eigen::Vector3 position_; }; template Eigen::AffineMatrix Pose::GetObjectFromReferenceMatrix() const { // The transfrom from the reference is the inverse of the pose. Eigen::AffineMatrix matrix(rotation_.inverse().toRotationMatrix()); return matrix.translate(-position_); } template Eigen::AffineMatrix Pose::GetReferenceFromObjectMatrix() const { // The transfrom to the reference. Eigen::AffineMatrix matrix(rotation_.toRotationMatrix()); return matrix.pretranslate(position_); } //------------------------------------------------------------------------------ // Type-specific typedefs. //------------------------------------------------------------------------------ using Posef = Pose; using Posed = Pose; } // namespace dvr } // namespace android #endif // ANDROID_DVR_POSE_H_