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   private:
60     Eigen::Vector3f mTranslationalVelocity;
61     Eigen::Vector3f mRotationalVelocity;
62 };
63 
64 /**
65  * Integrate a twist over time to obtain a pose.
66  * dt is the time over which to integration.
67  * The resulting pose represents the transformation between the starting point and the ending point
68  * of the motion over the time period.
69  */
70 Pose3f integrate(const Twist3f& twist, float dt);
71 
72 /**
73  * Differentiate pose to obtain a twist.
74  * dt is the time of the motion between the reference and the target frames of the pose.
75  */
76 Twist3f differentiate(const Pose3f& pose, float dt);
77 
78 /**
79  * Pretty-printer for twist.
80  */
81 std::ostream& operator<<(std::ostream& os, const Twist3f& twist);
82 
83 }  // namespace media
84 }  // namespace android
85