hpp-fcl  1.7.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& 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 
175  {
176  R.transposeInPlace();
177  T = - R * T;
178  return *this;
179  }
180 
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
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:125
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:155
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:161
void setTransform(const Quaternion3f &q_, const Vec3f &T_)
set transform from rotation and translation
Definition: transform.h:140
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:174
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Transform3f inverse()
inverse transform
Definition: transform.h:182
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:232
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:132
void setIdentity()
set the transform to be identity transform
Definition: transform.h:214
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:168
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:113
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:148
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:188
bool isIdentity() const
check whether the transform is identity
Definition: transform.h:208
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:49
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:119
#define HPP_FCL_DLLAPI
Definition: config.hh:64