1 /* 2 * Copyright (C) 2021 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 #pragma once 17 18 #include <Eigen/Geometry> 19 20 #include "Pose.h" 21 22 namespace android { 23 namespace media { 24 25 /** 26 * A 6-DoF twist. 27 * This class represents the translational and rotational velocity of a rigid object, typically 28 * relative to its own coordinate-frame. 29 * It is created by two 3-vectors, one representing linear motion per time-unit and the other, a 30 * rotation-vector in radians per time-unit (right-handed). 31 */ 32 class Twist3f { 33 public: Twist3f(const Eigen::Vector3f & translationalVelocity,const Eigen::Vector3f & rotationalVelocity)34 Twist3f(const Eigen::Vector3f& translationalVelocity, const Eigen::Vector3f& rotationalVelocity) 35 : mTranslationalVelocity(translationalVelocity), mRotationalVelocity(rotationalVelocity) {} 36 Twist3f()37 Twist3f() : Twist3f(Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()) {} 38 Twist3f(const Twist3f & other)39 Twist3f(const Twist3f& other) { *this = other; } 40 41 Twist3f& operator=(const Twist3f& other) { 42 mTranslationalVelocity = other.mTranslationalVelocity; 43 mRotationalVelocity = other.mRotationalVelocity; 44 return *this; 45 } 46 translationalVelocity()47 Eigen::Vector3f translationalVelocity() const { return mTranslationalVelocity; } rotationalVelocity()48 Eigen::Vector3f rotationalVelocity() const { return mRotationalVelocity; } 49 scalarTranslationalVelocity()50 float scalarTranslationalVelocity() const { return mTranslationalVelocity.norm(); } scalarRotationalVelocity()51 float scalarRotationalVelocity() const { return mRotationalVelocity.norm(); } 52 53 bool isApprox(const Twist3f& other, 54 float prec = Eigen::NumTraits<float>::dummy_precision()) const { 55 return mTranslationalVelocity.isApprox(other.mTranslationalVelocity, prec) && 56 mRotationalVelocity.isApprox(other.mRotationalVelocity, prec); 57 } 58 59 template<typename T> 60 Twist3f operator*(const T& s) const { 61 return Twist3f(mTranslationalVelocity * s, mRotationalVelocity * s); 62 } 63 64 template<typename T> 65 Twist3f operator/(const T& s) const { 66 return Twist3f(mTranslationalVelocity / s, mRotationalVelocity / s); 67 } 68 69 // Convert instance to a string representation. 70 std::string toString() const; 71 72 private: 73 Eigen::Vector3f mTranslationalVelocity; 74 Eigen::Vector3f mRotationalVelocity; 75 }; 76 77 /** 78 * Integrate a twist over time to obtain a pose. 79 * dt is the time over which to integration. 80 * The resulting pose represents the transformation between the starting point and the ending point 81 * of the motion over the time period. 82 */ 83 Pose3f integrate(const Twist3f& twist, float dt); 84 85 /** 86 * Differentiate pose to obtain a twist. 87 * dt is the time of the motion between the reference and the target frames of the pose. 88 */ 89 Twist3f differentiate(const Pose3f& pose, float dt); 90 91 /** 92 * Pretty-printer for twist. 93 */ 94 std::ostream& operator<<(std::ostream& os, const Twist3f& twist); 95 96 } // namespace media 97 } // namespace android 98