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