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 
38 #ifndef HPP_FCL_TRANSFORM_H
39 #define HPP_FCL_TRANSFORM_H
40 
41 #include <hpp/fcl/data_types.h>
42 
43 namespace hpp {
44 namespace fcl {
45 
46 typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
47 
48 static inline std::ostream& operator<<(std::ostream& o, const Quaternion3f& q) {
49  o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
50  return o;
51 }
52 
54 class HPP_FCL_DLLAPI Transform3f {
57 
60 
61  public:
64  setIdentity(); // set matrix_set true
65  }
66 
67  static Transform3f Identity() { return Transform3f(); }
68 
70  template <typename Matrixx3Like, typename Vector3Like>
71  Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
72  const Eigen::MatrixBase<Vector3Like>& T_)
73  : R(R_), T(T_) {}
74 
76  template <typename Vector3Like>
77  Transform3f(const Quaternion3f& q_, const Eigen::MatrixBase<Vector3Like>& T_)
78  : R(q_.toRotationMatrix()), T(T_) {}
79 
81  Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
82 
84  Transform3f(const Quaternion3f& q_) : R(q_), T(Vec3f::Zero()) {}
85 
87  Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
88 
90  Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {}
91 
94  R = tf.R;
95  T = tf.T;
96  return *this;
97  }
98 
100  inline const Vec3f& getTranslation() const { return T; }
101 
103  inline const Vec3f& translation() const { return T; }
104 
106  inline Vec3f& translation() { return T; }
107 
109  inline const Matrix3f& getRotation() const { return R; }
110 
112  inline const Matrix3f& rotation() const { return R; }
113 
115  inline Matrix3f& rotation() { return R; }
116 
118  inline Quaternion3f getQuatRotation() const { return Quaternion3f(R); }
119 
121  template <typename Matrix3Like, typename Vector3Like>
122  inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
123  const Eigen::MatrixBase<Vector3Like>& T_) {
124  R.noalias() = R_;
125  T.noalias() = T_;
126  }
127 
129  inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) {
130  R = q_.toRotationMatrix();
131  T = T_;
132  }
133 
135  template <typename Derived>
136  inline void setRotation(const Eigen::MatrixBase<Derived>& R_) {
137  R.noalias() = R_;
138  }
139 
141  template <typename Derived>
142  inline void setTranslation(const Eigen::MatrixBase<Derived>& T_) {
143  T.noalias() = T_;
144  }
145 
147  inline void setQuatRotation(const Quaternion3f& q_) {
148  R = q_.toRotationMatrix();
149  }
150 
152  template <typename Derived>
153  inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const {
154  return R * v + T;
155  }
156 
159  R.transposeInPlace();
160  T = -R * T;
161  return *this;
162  }
163 
165  inline Transform3f inverse() {
166  return Transform3f(R.transpose(), -R.transpose() * T);
167  }
168 
170  inline Transform3f inverseTimes(const Transform3f& other) const {
171  return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
172  }
173 
175  inline const Transform3f& operator*=(const Transform3f& other) {
176  T += R * other.T;
177  R *= other.R;
178  return *this;
179  }
180 
182  inline Transform3f operator*(const Transform3f& other) const {
183  return Transform3f(R * other.R, R * other.T + T);
184  }
185 
187  inline bool isIdentity(
188  const FCL_REAL& prec =
189  Eigen::NumTraits<FCL_REAL>::dummy_precision()) const {
190  return R.isIdentity(prec) && T.isZero(prec);
191  }
192 
194  inline void setIdentity() {
195  R.setIdentity();
196  T.setZero();
197  }
198 
199  bool operator==(const Transform3f& other) const {
200  return R == other.R && (T == other.getTranslation());
201  }
202 
203  bool operator!=(const Transform3f& other) const { return !(*this == other); }
204 
205  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
206 };
207 
208 template <typename Derived>
209 inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
210  FCL_REAL angle) {
211  return Quaternion3f(Eigen::AngleAxis<FCL_REAL>(angle, axis));
212 }
213 
214 } // namespace fcl
215 } // namespace hpp
216 
217 #endif
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:118
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Transform3f(const Quaternion3f &q_)
Construct transform from rotation.
Definition: transform.h:84
Transform3f(const Eigen::MatrixBase< Matrixx3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:71
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
Transform3f()
Default transform is no movement.
Definition: transform.h:63
Transform3f operator*(const Transform3f &other) const
multiply with another transform
Definition: transform.h:182
Main namespace.
void setQuatRotation(const Quaternion3f &q_)
set transform from rotation
Definition: transform.h:147
void setTransform(const Quaternion3f &q_, const Vec3f &T_)
set transform from rotation and translation
Definition: transform.h:129
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:158
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Transform3f inverse()
inverse transform
Definition: transform.h:165
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:209
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:122
void setIdentity()
set the transform to be identity transform
Definition: transform.h:194
const Transform3f & operator*=(const Transform3f &other)
multiply with another transform
Definition: transform.h:175
bool operator!=(const Transform3f &other) const
Definition: transform.h:203
const Vec3f & translation() const
get translation
Definition: transform.h:103
list v
Definition: obb.py:45
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
Transform3f(const Vec3f &T_)
Construct transform from translation.
Definition: transform.h:87
Vec3f T
Tranlation vector.
Definition: transform.h:59
double FCL_REAL
Definition: data_types.h:65
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
Definition: transform.h:48
Matrix3f R
Matrix cache.
Definition: transform.h:56
Transform3f & operator=(const Transform3f &tf)
operator =
Definition: transform.h:93
Vec3f & translation()
get translation
Definition: transform.h:106
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:136
Transform3f(const Quaternion3f &q_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:77
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:170
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
Transform3f(const Transform3f &tf)
Construct transform from other transform.
Definition: transform.h:90
Transform3f(const Matrix3f &R_)
Construct transform from rotation.
Definition: transform.h:81
const Matrix3f & rotation() const
get rotation
Definition: transform.h:112
static Transform3f Identity()
Definition: transform.h:67
Matrix3f & rotation()
get rotation
Definition: transform.h:115
bool isIdentity(const FCL_REAL &prec=Eigen::NumTraits< FCL_REAL >::dummy_precision()) const
check whether the transform is identity
Definition: transform.h:187
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
bool operator==(const Transform3f &other) const
Definition: transform.h:199


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:02