hpp-fcl  1.8.1
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 
59 {
61  Matrix3f R;
62 
64  Vec3f T;
65 public:
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>
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(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 
130  inline Quaternion3f getQuatRotation() const
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 
236 template<typename Derived>
237 inline 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
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:130
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
Transform3f(const Quaternion3f &q_)
Construct transform from rotation.
Definition: transform.h:95
Transform3f(const Eigen::MatrixBase< Matrixx3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:75
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:160
Transform3f()
Default transform is no movement.
Definition: transform.h:68
Main namespace.
Definition: AABB.h:43
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
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:179
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Transform3f inverse()
inverse transform
Definition: transform.h:187
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:237
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:137
void setIdentity()
set the transform to be identity transform
Definition: transform.h:219
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:173
Transform3f(const Vec3f &T_)
Construct transform from translation.
Definition: transform.h:100
double FCL_REAL
Definition: data_types.h:66
const Vec3f & getTranslation() const
get translation
Definition: transform.h:118
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:153
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
bool isIdentity() const
check whether the transform is identity
Definition: transform.h:213
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:49
Transform3f(const Transform3f &tf)
Construct transform from other transform.
Definition: transform.h:105
Transform3f(const Matrix3f &R_)
Construct transform from rotation.
Definition: transform.h:90
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:124
#define HPP_FCL_DLLAPI
Definition: config.hh:64