tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
Transform.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3 
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14 
15 
16 
17 #ifndef tf2_Transform_H
18 #define tf2_Transform_H
19 
20 
21 #include "Matrix3x3.h"
22 #include "tf2/visibility_control.h"
23 
24 
25 namespace tf2
26 {
27 
28 #define TransformData TransformDoubleData
29 
30 
33 class Transform {
34 
36  Matrix3x3 m_basis;
38  Vector3 m_origin;
39 
40 public:
41 
44  Transform() {}
49  const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
50  : m_basis(q),
51  m_origin(c)
52  {}
53 
58  const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
59  : m_basis(b),
60  m_origin(c)
61  {}
64  : m_basis(other.m_basis),
65  m_origin(other.m_origin)
66  {
67  }
70  {
71  m_basis = other.m_basis;
72  m_origin = other.m_origin;
73  return *this;
74  }
75 
80  TF2SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) {
81  m_basis = t1.m_basis * t2.m_basis;
82  m_origin = t1(t2.m_origin);
83  }
84 
85 /* void multInverseLeft(const Transform& t1, const Transform& t2) {
86  Vector3 v = t2.m_origin - t1.m_origin;
87  m_basis = tf2MultTransposeLeft(t1.m_basis, t2.m_basis);
88  m_origin = v * t1.m_basis;
89  }
90  */
91 
93  TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const
94  {
95  return Vector3(m_basis[0].dot(x) + m_origin.x(),
96  m_basis[1].dot(x) + m_origin.y(),
97  m_basis[2].dot(x) + m_origin.z());
98  }
99 
101  TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const
102  {
103  return (*this)(x);
104  }
105 
108  {
109  return getRotation() * q;
110  }
111 
113  TF2SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; }
115  TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; }
116 
118  TF2SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; }
120  TF2SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; }
121 
123  TF2_PUBLIC
125  Quaternion q;
126  m_basis.getRotation(q);
127  return q;
128  }
129 
130 
133  TF2_PUBLIC
135  {
136  m_basis.setFromOpenGLSubMatrix(m);
137  m_origin.setValue(m[12],m[13],m[14]);
138  }
139 
142  TF2_PUBLIC
143  void getOpenGLMatrix(tf2Scalar *m) const
144  {
145  m_basis.getOpenGLSubMatrix(m);
146  m[12] = m_origin.x();
147  m[13] = m_origin.y();
148  m[14] = m_origin.z();
149  m[15] = tf2Scalar(1.0);
150  }
151 
154  TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin)
155  {
156  m_origin = origin;
157  }
158 
159  TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const;
160 
161 
164  {
165  m_basis = basis;
166  }
167 
170  {
171  m_basis.setRotation(q);
172  }
173 
174 
176  TF2_PUBLIC
177  void setIdentity()
178  {
179  m_basis.setIdentity();
180  m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
181  }
182 
185  TF2_PUBLIC
187  {
188  m_origin += m_basis * t.m_origin;
189  m_basis *= t.m_basis;
190  return *this;
191  }
192 
194  TF2_PUBLIC
196  {
197  Matrix3x3 inv = m_basis.transpose();
198  return Transform(inv, inv * -m_origin);
199  }
200 
204  TF2_PUBLIC
205  Transform inverseTimes(const Transform& t) const;
206 
208  TF2_PUBLIC
209  Transform operator*(const Transform& t) const;
210 
212  TF2_PUBLIC
213  static const Transform& getIdentity()
214  {
215  static const Transform identityTransform(Matrix3x3::getIdentity());
216  return identityTransform;
217  }
218 
219  TF2_PUBLIC
220  void serialize(struct TransformData& dataOut) const;
221 
222  TF2_PUBLIC
223  void serializeFloat(struct TransformFloatData& dataOut) const;
224 
225  TF2_PUBLIC
226  void deSerialize(const struct TransformData& dataIn);
227 
228  TF2_PUBLIC
229  void deSerializeDouble(const struct TransformDoubleData& dataIn);
230 
231  TF2_PUBLIC
232  void deSerializeFloat(const struct TransformFloatData& dataIn);
233 
234 };
235 
236 
237 TF2SIMD_FORCE_INLINE Vector3
238 Transform::invXform(const Vector3& inVec) const
239 {
240  Vector3 v = inVec - m_origin;
241  return (m_basis.transpose() * v);
242 }
243 
246 {
247  Vector3 v = t.getOrigin() - m_origin;
248  return Transform(m_basis.transposeTimes(t.m_basis),
249  v * m_basis);
250 }
251 
254 {
255  return Transform(m_basis * t.m_basis,
256  (*this)(t.m_origin));
257 }
258 
261 {
262  return ( t1.getBasis() == t2.getBasis() &&
263  t1.getOrigin() == t2.getOrigin() );
264 }
265 
266 
269 {
272 };
273 
275 {
278 };
279 
280 
281 
283 {
284  m_basis.serialize(dataOut.m_basis);
285  m_origin.serialize(dataOut.m_origin);
286 }
287 
289 {
290  m_basis.serializeFloat(dataOut.m_basis);
291  m_origin.serializeFloat(dataOut.m_origin);
292 }
293 
294 
296 {
297  m_basis.deSerialize(dataIn.m_basis);
298  m_origin.deSerialize(dataIn.m_origin);
299 }
300 
302 {
303  m_basis.deSerializeFloat(dataIn.m_basis);
304  m_origin.deSerializeFloat(dataIn.m_origin);
305 }
306 
308 {
309  m_basis.deSerializeDouble(dataIn.m_basis);
310  m_origin.deSerializeDouble(dataIn.m_origin);
311 }
312 
313 }
314 
315 #endif
316 
317 
318 
319 
320 
321 
tf2::Transform::serializeFloat
void serializeFloat(struct TransformFloatData &dataOut) const
Definition: Transform.h:288
tf2::dot
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:395
tf2::TransformFloatData::m_origin
Vector3FloatData m_origin
Definition: Transform.h:271
tf2::Transform::Transform
TF2SIMD_FORCE_INLINE Transform(const Transform &other)
Copy constructor.
Definition: Transform.h:63
Matrix3x3.h
tf2::Transform::getOpenGLMatrix
void getOpenGLMatrix(tf2Scalar *m) const
Fill an array representation.
Definition: Transform.h:143
tf2::Transform::deSerializeFloat
void deSerializeFloat(const struct TransformFloatData &dataIn)
Definition: Transform.h:301
tf2::Transform::serialize
void serialize(struct TransformData &dataOut) const
Definition: Transform.h:282
tf2::Transform::inverse
Transform inverse() const
Return the inverse of this transform.
Definition: Transform.h:195
tf2::Matrix3x3::deSerializeFloat
void deSerializeFloat(const struct Matrix3x3FloatData &dataIn)
Definition: Matrix3x3.h:690
tf2::Transform::operator*
TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3 &x) const
Return the transform of the vector.
Definition: Transform.h:101
tf2::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:247
tf2::Transform::Transform
Transform()
No initialization constructor.
Definition: Transform.h:44
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
Set the rotational element by Quaternion.
Definition: Transform.h:169
tf2::Transform::Transform
TF2SIMD_FORCE_INLINE Transform(const Matrix3x3 &b, const Vector3 &c=Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
Constructor from Matrix3x3 (optional Vector3)
Definition: Transform.h:57
tf2::Transform::operator*
TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion &q) const
Return the transform of the Quaternion.
Definition: Transform.h:107
tf2::Transform::getOrigin
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:118
tf2::Transform::operator*=
Transform & operator*=(const Transform &t)
Multiply this Transform by another(this = this * another)
Definition: Transform.h:186
tf2::Vector3DoubleData
Definition: Vector3.h:686
tf2::Transform::invXform
TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3 &inVec) const
Definition: Transform.h:238
tf2::Matrix3x3::getIdentity
static const Matrix3x3 & getIdentity()
Definition: Matrix3x3.h:217
tf2::Matrix3x3::getOpenGLSubMatrix
void getOpenGLSubMatrix(tf2Scalar *m) const
Fill the values of the matrix into a 9 element array.
Definition: Matrix3x3.h:228
tf2::Matrix3x3::setRotation
void setRotation(const Quaternion &q)
Set the matrix from a quaternion.
Definition: Matrix3x3.h:155
tf2::Matrix3x3::transposeTimes
Matrix3x3 transposeTimes(const Matrix3x3 &m) const
Definition: Matrix3x3.h:586
tf2::Vector3FloatData
Definition: Vector3.h:681
tf2::Matrix3x3FloatData
for serialization
Definition: Matrix3x3.h:657
tf2::TransformDoubleData::m_origin
Vector3DoubleData m_origin
Definition: Transform.h:277
tf2::Transform::operator()
TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3 &x) const
Return the transform of the vector.
Definition: Transform.h:93
tf2::Matrix3x3::deSerialize
void deSerialize(const struct Matrix3x3Data &dataIn)
Definition: Matrix3x3.h:684
tf2::Transform::operator=
TF2SIMD_FORCE_INLINE Transform & operator=(const Transform &other)
Assignment Operator.
Definition: Transform.h:69
tf2::Transform::getIdentity
static const Transform & getIdentity()
Return an identity transform.
Definition: Transform.h:213
tf2::operator==
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Definition: Matrix3x3.h:649
tf2::Matrix3x3::deSerializeDouble
void deSerializeDouble(const struct Matrix3x3DoubleData &dataIn)
Definition: Matrix3x3.h:696
tf2::Transform::deSerialize
void deSerialize(const struct TransformData &dataIn)
Definition: Transform.h:295
tf2::TransformFloatData
for serialization
Definition: Transform.h:268
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Set the translational element.
Definition: Transform.h:154
tf2::TransformDoubleData::m_basis
Matrix3x3DoubleData m_basis
Definition: Transform.h:276
tf2::Transform::setBasis
TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3 &basis)
Set the rotational element by Matrix3x3.
Definition: Transform.h:163
tf2::Transform::setFromOpenGLMatrix
void setFromOpenGLMatrix(const tf2Scalar *m)
Set from an array.
Definition: Transform.h:134
tf2::Transform
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:33
tf2::Transform::getRotation
Quaternion getRotation() const
Return a quaternion representing the rotation.
Definition: Transform.h:124
TF2SIMD_FORCE_INLINE
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
tf2::Transform::Transform
TF2SIMD_FORCE_INLINE Transform(const Quaternion &q, const Vector3 &c=Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
Constructor from Quaternion (optional Vector3 )
Definition: Transform.h:48
tf2::Matrix3x3DoubleData
for serialization
Definition: Matrix3x3.h:663
visibility_control.h
tf2::Transform::inverseTimes
Transform inverseTimes(const Transform &t) const
Return the inverse of this transform times the other transform.
Definition: Transform.h:245
tf2::Matrix3x3::setIdentity
void setIdentity()
Set the matrix to the identity.
Definition: Matrix3x3.h:209
tf2::Transform::getBasis
const TF2SIMD_FORCE_INLINE Matrix3x3 & getBasis() const
Return the basis matrix for the rotation.
Definition: Transform.h:115
tf2
Definition: buffer_core.h:53
tf2::Matrix3x3::setFromOpenGLSubMatrix
void setFromOpenGLSubMatrix(const tf2Scalar *m)
Set from a carray of tf2Scalars.
Definition: Matrix3x3.h:125
tf2::Matrix3x3::serializeFloat
void serializeFloat(struct Matrix3x3FloatData &dataOut) const
Definition: Matrix3x3.h:677
TF2_PUBLIC
#define TF2_PUBLIC
Definition: visibility_control.h:57
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:29
tf2::Transform::deSerializeDouble
void deSerializeDouble(const struct TransformDoubleData &dataIn)
Definition: Transform.h:307
tf2::Matrix3x3::serialize
void serialize(struct Matrix3x3Data &dataOut) const
Definition: Matrix3x3.h:671
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
Return the transpose of the matrix.
Definition: Matrix3x3.h:558
tf2Scalar
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
tf2::Transform::getOrigin
const TF2SIMD_FORCE_INLINE Vector3 & getOrigin() const
Return the origin vector translation.
Definition: Transform.h:120
TransformData
#define TransformData
Definition: Transform.h:28
tf2::Matrix3x3
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:33
tf2::TransformFloatData::m_basis
Matrix3x3FloatData m_basis
Definition: Transform.h:270
tf2::Transform::mult
TF2SIMD_FORCE_INLINE void mult(const Transform &t1, const Transform &t2)
Set the current transform as the value of the product of two transforms.
Definition: Transform.h:80
tf2::TransformDoubleData
Definition: Transform.h:274
tf2::Transform::getBasis
TF2SIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:113
tf2::Transform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: Transform.h:177