hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
44namespace hpp
45{
46namespace fcl
47{
48
49typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
50
51static 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
59{
61 Matrix3f R;
62
64 Vec3f T;
65public:
66
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>
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(const Transform3f& tf) : R(tf.R),
106 T(tf.T)
107 {}
108
110 Transform3f& operator = (const Transform3f& tf)
111 {
112 R = tf.R;
113 T = tf.T;
114 return *this;
115 }
116
118 inline const Vec3f& getTranslation() const
119 {
120 return T;
121 }
122
124 inline const Matrix3f& getRotation() const
125 {
126 return R;
127 }
128
131 {
132 return Quaternion3f (R);
133 }
134
136 template <typename Matrix3Like, typename Vector3Like>
137 inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
138 const Eigen::MatrixBase<Vector3Like>& T_)
139 {
140 R.noalias() = R_;
141 T.noalias() = T_;
142 }
143
145 inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
146 {
147 R = q_.toRotationMatrix();
148 T = T_;
149 }
150
152 template<typename Derived>
153 inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
154 {
155 R.noalias() = R_;
156 }
157
159 template<typename Derived>
160 inline void setTranslation(const Eigen::MatrixBase<Derived>& T_)
161 {
162 T.noalias() = T_;
163 }
164
166 inline void setQuatRotation(const Quaternion3f& q_)
167 {
168 R = q_.toRotationMatrix();
169 }
170
172 template <typename Derived>
173 inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
174 {
175 return R * v + T;
176 }
177
180 {
181 R.transposeInPlace();
182 T = - R * T;
183 return *this;
184 }
185
188 {
189 return Transform3f (R.transpose(), - R.transpose() * T);
190 }
191
193 inline Transform3f inverseTimes(const Transform3f& other) const
194 {
195 return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
196 }
197
199 inline const Transform3f& operator *= (const Transform3f& other)
200 {
201 T += R * other.T;
202 R *= other.R;
203 return *this;
204 }
205
207 inline Transform3f operator * (const Transform3f& other) const
208 {
209 return Transform3f(R * other.R, R * other.T + T);
210 }
211
213 inline bool isIdentity() const
214 {
215 return R.isIdentity() && T.isZero();
216 }
217
219 inline void setIdentity()
220 {
221 R.setIdentity();
222 T.setZero();
223 }
224
225 bool operator == (const Transform3f& other) const
226 {
227 return R == other.R && (T == other.getTranslation());
228 }
229
230 bool operator != (const Transform3f& other) const
231 {
232 return !(*this == other);
233 }
234};
235
236template<typename Derived>
237inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
238{
239 return Quaternion3f (Eigen::AngleAxis<FCL_REAL>(angle, axis));
240}
241
242}
243} // namespace hpp
244
245#endif
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
Transform3f(const Vec3f &T_)
Construct transform from translation.
Definition: transform.h:100
const Vec3f & getTranslation() const
get translation
Definition: transform.h:118
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:137
Transform3f(const Quaternion3f &q_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:83
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:193
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:179
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:124
Transform3f(const Eigen::MatrixBase< Matrixx3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:75
Transform3f(const Transform3f &tf)
Construct transform from other transform.
Definition: transform.h:105
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:130
Transform3f()
Default transform is no movement.
Definition: transform.h:68
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:160
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
Transform3f(const Quaternion3f &q_)
Construct transform from rotation.
Definition: transform.h:95
bool isIdentity() const
check whether the transform is identity
Definition: transform.h:213
void setIdentity()
set the transform to be identity transform
Definition: transform.h:219
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:153
Transform3f inverse()
inverse transform
Definition: transform.h:187
Transform3f(const Matrix3f &R_)
Construct transform from rotation.
Definition: transform.h:90
void setQuatRotation(const Quaternion3f &q_)
set transform from rotation
Definition: transform.h:166
void setTransform(const Quaternion3f &q_, const Vec3f &T_)
set transform from rotation and translation
Definition: transform.h:145
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:237
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:49
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44