GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/math/transform.h Lines: 48 68 70.6 %
Date: 2024-02-09 12:57:42 Branches: 25 58 43.1 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
5
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
6
 *  All rights reserved.
7
 *
8
 *  Redistribution and use in source and binary forms, with or without
9
 *  modification, are permitted provided that the following conditions
10
 *  are met:
11
 *
12
 *   * Redistributions of source code must retain the above copyright
13
 *     notice, this list of conditions and the following disclaimer.
14
 *   * Redistributions in binary form must reproduce the above
15
 *     copyright notice, this list of conditions and the following
16
 *     disclaimer in the documentation and/or other materials provided
17
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
 *     contributors may be used to endorse or promote products derived
20
 *     from this software without specific prior written permission.
21
 *
22
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
 *  POSSIBILITY OF SUCH DAMAGE.
34
 */
35
36
/** \author Jia Pan */
37
38
#ifndef HPP_FCL_TRANSFORM_H
39
#define HPP_FCL_TRANSFORM_H
40
41
#include <hpp/fcl/data_types.h>
42
43
namespace hpp {
44
namespace fcl {
45
46
typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
47
48
static inline std::ostream& operator<<(std::ostream& o, const Quaternion3f& q) {
49
  o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
50
  return o;
51
}
52
53
/// @brief Simple transform class used locally by InterpMotion
54
class HPP_FCL_DLLAPI Transform3f {
55
  /// @brief Matrix cache
56
  Matrix3f R;
57
58
  /// @brief Tranlation vector
59
  Vec3f T;
60
61
 public:
62
  /// @brief Default transform is no movement
63
1722923
  Transform3f() {
64
1722923
    setIdentity();  // set matrix_set true
65
1722923
  }
66
67
1138
  static Transform3f Identity() { return Transform3f(); }
68
69
  /// @brief Construct transform from rotation and translation
70
  template <typename Matrixx3Like, typename Vector3Like>
71
18443
  Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
72
              const Eigen::MatrixBase<Vector3Like>& T_)
73
18443
      : R(R_), T(T_) {}
74
75
  /// @brief Construct transform from rotation and translation
76
  template <typename Vector3Like>
77
77
  Transform3f(const Quaternion3f& q_, const Eigen::MatrixBase<Vector3Like>& T_)
78
77
      : R(q_.toRotationMatrix()), T(T_) {}
79
80
  /// @brief Construct transform from rotation
81
102
  Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
82
83
  /// @brief Construct transform from rotation
84
4
  Transform3f(const Quaternion3f& q_) : R(q_), T(Vec3f::Zero()) {}
85
86
  /// @brief Construct transform from translation
87
6045
  Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
88
89
  /// @brief Construct transform from other transform
90
43384
  Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {}
91
92
  /// @brief operator =
93
1488800
  Transform3f& operator=(const Transform3f& tf) {
94
1488800
    R = tf.R;
95
1488800
    T = tf.T;
96
1488800
    return *this;
97
  }
98
99
  /// @brief get translation
100
2656784
  inline const Vec3f& getTranslation() const { return T; }
101
102
  /// @brief get translation
103
  inline const Vec3f& translation() const { return T; }
104
105
  /// @brief get translation
106
  inline Vec3f& translation() { return T; }
107
108
  /// @brief get rotation
109
3980702
  inline const Matrix3f& getRotation() const { return R; }
110
111
  /// @brief get rotation
112
  inline const Matrix3f& rotation() const { return R; }
113
114
  /// @brief get rotation
115
  inline Matrix3f& rotation() { return R; }
116
117
  /// @brief get quaternion
118
1
  inline Quaternion3f getQuatRotation() const { return Quaternion3f(R); }
119
120
  /// @brief set transform from rotation and translation
121
  template <typename Matrix3Like, typename Vector3Like>
122
55353
  inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
123
                           const Eigen::MatrixBase<Vector3Like>& T_) {
124
55353
    R.noalias() = R_;
125
55353
    T.noalias() = T_;
126
55353
  }
127
128
  /// @brief set transform from rotation and translation
129
  inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) {
130
    R = q_.toRotationMatrix();
131
    T = T_;
132
  }
133
134
  /// @brief set transform from rotation
135
  template <typename Derived>
136
6002
  inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
137
6002
    R.noalias() = R_;
138
6002
  }
139
140
  /// @brief set transform from translation
141
  template <typename Derived>
142
17490
  inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
143
17490
    T.noalias() = T_;
144
17490
  }
145
146
  /// @brief set transform from rotation
147
18
  inline void setQuatRotation(const Quaternion3f& q_) {
148
18
    R = q_.toRotationMatrix();
149
18
  }
150
151
  /// @brief transform a given vector by the transform
152
  template <typename Derived>
153
201962354
  inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const {
154

201962354
    return R * v + T;
155
  }
156
157
  /// @brief inverse transform
158
  inline Transform3f& inverseInPlace() {
159
    R.transposeInPlace();
160
    T = -R * T;
161
    return *this;
162
  }
163
164
  /// @brief inverse transform
165
36
  inline Transform3f inverse() {
166


72
    return Transform3f(R.transpose(), -R.transpose() * T);
167
  }
168
169
  /// @brief inverse the transform and multiply with another
170
2711
  inline Transform3f inverseTimes(const Transform3f& other) const {
171


5422
    return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
172
  }
173
174
  /// @brief multiply with another transform
175
  inline const Transform3f& operator*=(const Transform3f& other) {
176
    T += R * other.T;
177
    R *= other.R;
178
    return *this;
179
  }
180
181
  /// @brief multiply with another transform
182
5788
  inline Transform3f operator*(const Transform3f& other) const {
183

11576
    return Transform3f(R * other.R, R * other.T + T);
184
  }
185
186
  /// @brief check whether the transform is identity
187
271
  inline bool isIdentity(
188
      const FCL_REAL& prec =
189
          Eigen::NumTraits<FCL_REAL>::dummy_precision()) const {
190

271
    return R.isIdentity(prec) && T.isZero(prec);
191
  }
192
193
  /// @brief set the transform to be identity transform
194
1728682
  inline void setIdentity() {
195
1728682
    R.setIdentity();
196
1728682
    T.setZero();
197
1728682
  }
198
199
  bool operator==(const Transform3f& other) const {
200
    return R == other.R && (T == other.getTranslation());
201
  }
202
203
  bool operator!=(const Transform3f& other) const { return !(*this == other); }
204
205
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206
};
207
208
template <typename Derived>
209
2
inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
210
                                  FCL_REAL angle) {
211
2
  return Quaternion3f(Eigen::AngleAxis<FCL_REAL>(angle, axis));
212
}
213
214
}  // namespace fcl
215
}  // namespace hpp
216
217
#endif