hpp-fcl 2.4.4
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
38#ifndef HPP_FCL_TRANSFORM_H
39#define HPP_FCL_TRANSFORM_H
40
41#include <hpp/fcl/data_types.h>
42
43namespace hpp {
44namespace fcl {
45
46typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
47
48static 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
56 Matrix3f R;
57
59 Vec3f T;
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
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
208template <typename Derived>
209inline 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
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Transform3f(const Vec3f &T_)
Construct transform from translation.
Definition: transform.h:87
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
Transform3f operator*(const Transform3f &other) const
multiply with another transform
Definition: transform.h:182
Transform3f & operator=(const Transform3f &tf)
operator =
Definition: transform.h:93
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:122
bool operator!=(const Transform3f &other) const
Definition: transform.h:203
Matrix3f & rotation()
get rotation
Definition: transform.h:115
Transform3f(const Quaternion3f &q_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:77
const Matrix3f & rotation() const
get rotation
Definition: transform.h:112
Vec3f & translation()
get translation
Definition: transform.h:106
const Transform3f & operator*=(const Transform3f &other)
multiply with another transform
Definition: transform.h:175
Transform3f inverseTimes(const Transform3f &other) const
inverse the transform and multiply with another
Definition: transform.h:170
Transform3f & inverseInPlace()
inverse transform
Definition: transform.h:158
const Matrix3f & getRotation() const
get rotation
Definition: transform.h:109
Transform3f(const Eigen::MatrixBase< Matrixx3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
Construct transform from rotation and translation.
Definition: transform.h:71
Transform3f(const Transform3f &tf)
Construct transform from other transform.
Definition: transform.h:90
static Transform3f Identity()
Definition: transform.h:67
bool operator==(const Transform3f &other) const
Definition: transform.h:199
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:118
bool isIdentity(const FCL_REAL &prec=Eigen::NumTraits< FCL_REAL >::dummy_precision()) const
check whether the transform is identity
Definition: transform.h:187
Transform3f()
Default transform is no movement.
Definition: transform.h:63
void setTranslation(const Eigen::MatrixBase< Derived > &T_)
set transform from translation
Definition: transform.h:142
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
Transform3f(const Quaternion3f &q_)
Construct transform from rotation.
Definition: transform.h:84
void setIdentity()
set the transform to be identity transform
Definition: transform.h:194
void setRotation(const Eigen::MatrixBase< Derived > &R_)
set transform from rotation
Definition: transform.h:136
Transform3f inverse()
inverse transform
Definition: transform.h:165
Transform3f(const Matrix3f &R_)
Construct transform from rotation.
Definition: transform.h:81
const Vec3f & translation() const
get translation
Definition: transform.h:103
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
#define HPP_FCL_DLLAPI
Definition: config.hh:88
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Quaternion3f fromAxisAngle(const Eigen::MatrixBase< Derived > &axis, FCL_REAL angle)
Definition: transform.h:209
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44