GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
Generated by: GCOVR (Version 4.2) |