hpp-fcl: include/hpp/fcl/math/transform.h Source File

hpp-fcl 1.7.0 HPP fork of FCL -- The Flexible Collision Library

transform.h
Go to the documentation of this file.
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
39 #ifndef HPP_FCL_TRANSFORM_H
40 #define HPP_FCL_TRANSFORM_H
41
42 #include <hpp/fcl/data_types.h>
43
44 namespace hpp
45 {
46 namespace fcl
47 {
48
49 typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
50
51 static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
52 {
53 o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
54 return o;
55 }
56
58 class HPP_FCL_DLLAPI Transform3f
59 {
61 Matrix3f R;
62
64 Vec3f T;
65 public:
66
68 Transform3f()
69 {
70 setIdentity(); // set matrix_set true
71 }
72
74 template <typename Matrixx3Like, typename Vector3Like>
75 Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
76 const Eigen::MatrixBase<Vector3Like>& T_) :
77 R(R_),
78 T(T_)
79 {}
80
82 template <typename Vector3Like>
83 Transform3f(const Quaternion3f& q_,
84 const Eigen::MatrixBase<Vector3Like>& T_) :
85 R(q_.toRotationMatrix()),
86 T(T_)
87 {}
88
90 Transform3f(const Matrix3f& R_) : R(R_),
91 T(Vec3f::Zero())
92 {}
93
95 Transform3f(const Quaternion3f& q_) : R(q_),
96 T(Vec3f::Zero())
97 {}
98
100 Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()),
101 T(T_)
102 {}
103
105 Transform3f& operator = (const Transform3f& tf)
106 {
107 R = tf.R;
108 T = tf.T;
109 return *this;
110 }
111
113 inline const Vec3f& getTranslation() const
114 {
115 return T;
116 }
117
119 inline const Matrix3f& getRotation() const
120 {
121 return R;
122 }
123
125 inline Quaternion3f getQuatRotation() const
126 {
127 return Quaternion3f (R);
128 }
129
131 template <typename Matrix3Like, typename Vector3Like>
132 inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
133 const Eigen::MatrixBase<Vector3Like>& T_)
134 {
135 R.noalias() = R_;
136 T.noalias() = T_;
137 }
138
140 inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
141 {
142 R = q_.toRotationMatrix();
143 T = T_;
144 }
145
147 template<typename Derived>
148 inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
149 {
150 R.noalias() = R_;
151 }
152
154 template<typename Derived>
155 inline void setTranslation(const Eigen::MatrixBase<Derived>& T_)
156 {
157 T.noalias() = T_;
158 }
159
161 inline void setQuatRotation(const Quaternion3f& q_)
162 {
163 R = q_.toRotationMatrix();
164 }
165
167 template <typename Derived>
168 inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
169 {
170 return R * v + T;
171 }
172
174 inline Transform3f& inverseInPlace()
175 {
176 R.transposeInPlace();
177 T = - R * T;
178 return *this;
179 }
180
182 inline Transform3f inverse()
183 {
184 return Transform3f (R.transpose(), - R.transpose() * T);
185 }
186
188 inline Transform3f inverseTimes(const Transform3f& other) const
189 {
190 return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
191 }
192
194 inline const Transform3f& operator *= (const Transform3f& other)
195 {
196 T += R * other.T;
197 R *= other.R;
198 return *this;
199 }
200
202 inline Transform3f operator * (const Transform3f& other) const
203 {
204 return Transform3f(R * other.R, R * other.T + T);
205 }
206
208 inline bool isIdentity() const
209 {
210 return R.isIdentity() && T.isZero();
211 }
212
214 inline void setIdentity()
215 {
216 R.setIdentity();
217 T.setZero();
218 }
219
220 bool operator == (const Transform3f& other) const
221 {
222 return R == other.R && (T == other.getTranslation());
223 }
224
225 bool operator != (const Transform3f& other) const
226 {
227 return !(*this == other);
228 }
229 };
230
231 template<typename Derived>
232 inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
233 {
234 return Quaternion3f (Eigen::AngleAxis<FCL_REAL>(angle, axis));
235 }
236
237 }
238 } // namespace hpp
239
240 #endif
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
hpp::fcl::Transform3f::Transform3f
Transform3f(const Vec3f &T_)
Construct transform from translation.
Definition: transform.h:100
hpp::fcl::Transform3f::setTransform
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:132
hpp::fcl::Transform3f::getTranslation
const Vec3f & getTranslation() const
get translation
Definition: transform.h:113
hpp::fcl::Transform3f::Transform3f
Transform3f(const Quaternion3f &q_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:83
hpp::fcl::Transform3f::getRotation
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:119
hpp::fcl::Transform3f::inverseTimes
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:188
hpp::fcl::Transform3f::Transform3f
Transform3f(const Eigen::MatrixBase< Matrixx3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:75
hpp::fcl::Transform3f::getQuatRotation
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:125
hpp::fcl::Transform3f::Transform3f
Transform3f()
Default transform is no movement.
Definition: transform.h:68
hpp::fcl::Transform3f::setTranslation
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:155
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:168
hpp::fcl::Transform3f::Transform3f
Transform3f(const Quaternion3f &q_)
Construct transform from rotation.
Definition: transform.h:95
hpp::fcl::Transform3f::isIdentity
bool isIdentity() const
check whether the transform is identity
Definition: transform.h:208
hpp::fcl::Transform3f::setIdentity
void setIdentity()
set the transform to be identity transform
Definition: transform.h:214
hpp::fcl::Transform3f::setRotation
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:148
hpp::fcl::Transform3f::inverseInPlace
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:174
hpp::fcl::Transform3f::inverse
Transform3f inverse()
inverse transform
Definition: transform.h:182
hpp::fcl::Transform3f::Transform3f
Transform3f(const Matrix3f &R_)
Construct transform from rotation.
Definition: transform.h:90
hpp::fcl::Transform3f::setQuatRotation
void setQuatRotation(const Quaternion3f &q_)
set transform from rotation
Definition: transform.h:161
hpp::fcl::Transform3f::setTransform
void setTransform(const Quaternion3f &q_, const Vec3f &T_)
set transform from rotation and translation
Definition: transform.h:140
data_types.h
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::fromAxisAngle
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:232
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::Quaternion3f
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:49
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44