GCC Code Coverage Report

Directory: ./
File: include/coal/math/transform.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 85 100 85.0%
Branches: 42 88 47.7%
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 COAL_TRANSFORM_H
39 #define COAL_TRANSFORM_H
40
41 #include "coal/fwd.hh"
42 #include "coal/data_types.h"
43
44 namespace coal {
45
46 COAL_DEPRECATED typedef Eigen::Quaternion<Scalar> Quaternion3f;
47 typedef Eigen::Quaternion<Scalar> Quats;
48
49 ✗ static inline std::ostream& operator<<(std::ostream& o, const Quats& q) {
50 ✗ o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
51 ✗ return o;
52 }
53
54 /// @brief Simple transform class used locally by InterpMotion
55 class COAL_DLLAPI Transform3s {
56 /// @brief Matrix cache
57 Matrix3s R;
58
59 /// @brief Translation vector
60 Vec3s T;
61
62 public:
63 /// @brief Default transform is no movement
64 317567 Transform3s() {
65 317567 setIdentity(); // set matrix_set true
66 317567 }
67
68 1700 static Transform3s Identity() { return Transform3s(); }
69
70 /// @brief Construct transform from rotation and translation
71 template <typename Matrixx3Like, typename Vector3Like>
72 21067 Transform3s(const Eigen::MatrixBase<Matrixx3Like>& R_,
73 const Eigen::MatrixBase<Vector3Like>& T_)
74 21067 : R(R_), T(T_) {}
75
76 /// @brief Construct transform from rotation and translation
77 template <typename Vector3Like>
78 333 Transform3s(const Quats& q_, const Eigen::MatrixBase<Vector3Like>& T_)
79 333 : R(q_.toRotationMatrix()), T(T_) {}
80
81 /// @brief Construct transform from rotation
82
1/2
✓ Branch 3 taken 102 times.
✗ Branch 4 not taken.
 102 Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
83
84 /// @brief Construct transform from rotation
85
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 Transform3s(const Quats& q_) : R(q_), T(Vec3s::Zero()) {}
86
87 /// @brief Construct transform from translation
88
1/2
✓ Branch 2 taken 7075 times.
✗ Branch 3 not taken.
 7075 Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
89
90 /// @brief Construct transform from other transform
91 59700 Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {}
92
93 /// @brief operator =
94 66097 Transform3s& operator=(const Transform3s& tf) {
95 66097 R = tf.R;
96 66097 T = tf.T;
97 66097 return *this;
98 }
99
100 /// @brief get translation
101 2769028 inline const Vec3s& getTranslation() const { return T; }
102
103 /// @brief get translation
104 149 inline const Vec3s& translation() const { return T; }
105
106 /// @brief get translation
107 233 inline Vec3s& translation() { return T; }
108
109 /// @brief get rotation
110 4435669 inline const Matrix3s& getRotation() const { return R; }
111
112 /// @brief get rotation
113 348 inline const Matrix3s& rotation() const { return R; }
114
115 /// @brief get rotation
116 248 inline Matrix3s& rotation() { return R; }
117
118 /// @brief get quaternion
119 1 inline Quats getQuatRotation() const { return Quats(R); }
120
121 /// @brief set transform from rotation and translation
122 template <typename Matrix3Like, typename Vector3Like>
123 62021 inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
124 const Eigen::MatrixBase<Vector3Like>& T_) {
125
1/2
✓ Branch 2 taken 62021 times.
✗ Branch 3 not taken.
 62021 R.noalias() = R_;
126
1/2
✓ Branch 2 taken 62021 times.
✗ Branch 3 not taken.
 62021 T.noalias() = T_;
127 62021 }
128
129 /// @brief set transform from rotation and translation
130 ✗ inline void setTransform(const Quats& q_, const Vec3s& T_) {
131 ✗ R = q_.toRotationMatrix();
132 ✗ T = T_;
133 }
134
135 /// @brief set transform from rotation
136 template <typename Derived>
137 6002 inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
138
1/2
✓ Branch 2 taken 6002 times.
✗ Branch 3 not taken.
 6002 R.noalias() = R_;
139 6002 }
140
141 /// @brief set transform from translation
142 template <typename Derived>
143 26225 inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
144
1/2
✓ Branch 2 taken 16199 times.
✗ Branch 3 not taken.
 26225 T.noalias() = T_;
145 26225 }
146
147 /// @brief set transform from rotation
148 891 inline void setQuatRotation(const Quats& q_) { R = q_.toRotationMatrix(); }
149
150 /// @brief transform a given vector by the transform
151 template <typename Derived>
152 202156100 inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
153
2/4
✓ Branch 2 taken 177952893 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 177952893 times.
✗ Branch 6 not taken.
 202156100 return R * v + T;
154 }
155
156 /// @brief transform a given vector by the inverse of the transform
157 template <typename Derived>
158 203 inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
159
3/6
✓ Branch 2 taken 195 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 195 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 195 times.
✗ Branch 9 not taken.
 203 return R.transpose() * (v - T);
160 }
161
162 /// @brief inverse transform
163 ✗ inline Transform3s& inverseInPlace() {
164 ✗ R.transposeInPlace();
165 ✗ T = -R * T;
166 ✗ return *this;
167 }
168
169 /// @brief inverse transform
170 164 inline Transform3s inverse() {
171
4/8
✓ Branch 2 taken 164 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 164 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 164 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 164 times.
✗ Branch 12 not taken.
 328 return Transform3s(R.transpose(), -R.transpose() * T);
172 }
173
174 /// @brief inverse the transform and multiply with another
175 3051 inline Transform3s inverseTimes(const Transform3s& other) const {
176
5/10
✓ Branch 2 taken 3051 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3051 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3051 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3051 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3051 times.
✗ Branch 15 not taken.
 6102 return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T));
177 }
178
179 /// @brief multiply with another transform
180 ✗ inline const Transform3s& operator*=(const Transform3s& other) {
181 ✗ T += R * other.T;
182 ✗ R *= other.R;
183 ✗ return *this;
184 }
185
186 /// @brief multiply with another transform
187 6866 inline Transform3s operator*(const Transform3s& other) const {
188
3/6
✓ Branch 2 taken 6866 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6866 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6866 times.
✗ Branch 9 not taken.
 13732 return Transform3s(R * other.R, R * other.T + T);
189 }
190
191 /// @brief check whether the transform is identity
192 371 inline bool isIdentity(
193 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
194
3/4
✓ Branch 1 taken 280 times.
✓ Branch 2 taken 91 times.
✓ Branch 4 taken 280 times.
✗ Branch 5 not taken.
 371 return R.isIdentity(prec) && T.isZero(prec);
195 }
196
197 /// @brief set the transform to be identity transform
198 323481 inline void setIdentity() {
199 323481 R.setIdentity();
200 323481 T.setZero();
201 323481 }
202
203 /// @brief return a random transform
204 100 static Transform3s Random() {
205 100 Transform3s tf = Transform3s();
206 100 tf.setRandom();
207 100 return tf;
208 }
209
210 /// @brief set the transform to a random transform
211 inline void setRandom();
212
213 48 bool operator==(const Transform3s& other) const {
214
2/4
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 48 times.
✗ Branch 7 not taken.
 48 return (R == other.getRotation()) && (T == other.getTranslation());
215 }
216
217 ✗ bool operator!=(const Transform3s& other) const { return !(*this == other); }
218
219 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
220 };
221
222 template <typename Derived>
223 2 inline Quats fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
224 Scalar angle) {
225
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 return Quats(Eigen::AngleAxis<Scalar>(angle, axis));
226 }
227
228 /// @brief Uniformly random quaternion sphere.
229 /// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
230 100 inline Quats uniformRandomQuaternion() {
231 // Rotational part
232 100 const Scalar u1 = (Scalar)rand() / Scalar(RAND_MAX);
233 100 const Scalar u2 = (Scalar)rand() / Scalar(RAND_MAX);
234 100 const Scalar u3 = (Scalar)rand() / Scalar(RAND_MAX);
235
236 100 const Scalar mult1 = std::sqrt(Scalar(1.0) - u1);
237 100 const Scalar mult2 = std::sqrt(u1);
238
239 static const Scalar PI_value = static_cast<Scalar>(EIGEN_PI);
240 100 Scalar s2 = std::sin(2 * PI_value * u2);
241 100 Scalar c2 = std::cos(2 * PI_value * u2);
242 100 Scalar s3 = std::sin(2 * PI_value * u3);
243 100 Scalar c3 = std::cos(2 * PI_value * u3);
244
245 100 Quats q;
246 100 q.w() = mult1 * s2;
247 100 q.x() = mult1 * c2;
248 100 q.y() = mult2 * s3;
249 100 q.z() = mult2 * c3;
250 100 return q;
251 }
252
253 100 inline void Transform3s::setRandom() {
254
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 const Quats q = uniformRandomQuaternion();
255
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 this->rotation() = q.matrix();
256
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 this->translation().setRandom();
257 100 }
258
259 /// @brief Construct othonormal basis from vector.
260 /// The z-axis is the normalized input vector.
261 42 inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
262
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 Matrix3s basis = Matrix3s::Zero();
263
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 42 basis.col(2) = vec.normalized();
264
3/6
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
 42 basis.col(1) = -vec.unitOrthogonal();
265
3/6
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
 42 basis.col(0) = basis.col(1).cross(vec);
266 42 return basis;
267 }
268
269 } // namespace coal
270
271 #endif
272